]>
Commit | Line | Data |
---|---|---|
a9533e7e HP |
1 | /* |
2 | * Copyright (c) 2010 Broadcom Corporation | |
3 | * | |
4 | * Permission to use, copy, modify, and/or distribute this software for any | |
5 | * purpose with or without fee is hereby granted, provided that the above | |
6 | * copyright notice and this permission notice appear in all copies. | |
7 | * | |
8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES | |
9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF | |
10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY | |
11 | * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES | |
12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION | |
13 | * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN | |
14 | * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. | |
15 | */ | |
16 | ||
17 | #include "qmath.h" | |
18 | ||
19 | /* | |
20 | Description: This function saturate input 32 bit number into a 16 bit number. | |
21 | If input number is greater than 0x7fff then output is saturated to 0x7fff. | |
22 | else if input number is less than 0xffff8000 then output is saturated to 0xffff8000 | |
23 | else output is same as input. | |
24 | */ | |
25 | int16 qm_sat32(int32 op) | |
26 | { | |
27 | int16 result; | |
28 | if (op > (int32) 0x7fff) { | |
29 | result = 0x7fff; | |
30 | } else if (op < (int32) 0xffff8000) { | |
31 | result = (int16) (0x8000); | |
32 | } else { | |
33 | result = (int16) op; | |
34 | } | |
35 | return result; | |
36 | } | |
37 | ||
38 | /* | |
39 | Description: This function multiply two input 16 bit numbers and return the 32 bit result. | |
40 | This multiplication is similar to compiler multiplication. This operation is defined if | |
41 | 16 bit multiplication on the processor platform is cheaper than 32 bit multiplication (as | |
42 | the most of qmath functions can be replaced with processor intrinsic instructions). | |
43 | */ | |
44 | int32 qm_mul321616(int16 op1, int16 op2) | |
45 | { | |
90ea2296 | 46 | return (int32) (op1) * (int32) (op2); |
a9533e7e HP |
47 | } |
48 | ||
49 | /* | |
50 | Description: This function make 16 bit multiplication and return the result in 16 bits. | |
51 | To fit the result into 16 bits the 32 bit multiplication result is right | |
52 | shifted by 16 bits. | |
53 | */ | |
54 | int16 qm_mul16(int16 op1, int16 op2) | |
55 | { | |
56 | int32 result; | |
57 | result = ((int32) (op1) * (int32) (op2)); | |
90ea2296 | 58 | return (int16) (result >> 16); |
a9533e7e HP |
59 | } |
60 | ||
61 | /* | |
62 | Description: This function multiply two 16 bit numbers and return the result in 32 bits. | |
63 | This function remove the extra sign bit created by the multiplication by leftshifting the | |
64 | 32 bit multiplication result by 1 bit before returning the result. So the output is | |
65 | twice that of compiler multiplication. (i.e. qm_muls321616(2,3)=12). | |
66 | When both input 16 bit numbers are 0x8000, then the result is saturated to 0x7fffffff. | |
67 | */ | |
68 | int32 qm_muls321616(int16 op1, int16 op2) | |
69 | { | |
70 | int32 result; | |
71 | if (op1 == (int16) (0x8000) && op2 == (int16) (0x8000)) { | |
72 | result = 0x7fffffff; | |
73 | } else { | |
74 | result = ((int32) (op1) * (int32) (op2)); | |
75 | result = result << 1; | |
76 | } | |
77 | return result; | |
78 | } | |
79 | ||
80 | /* | |
81 | Description: This function make 16 bit unsigned multiplication. To fit the output into | |
82 | 16 bits the 32 bit multiplication result is right shifted by 16 bits. | |
83 | */ | |
7d4df48e | 84 | u16 qm_mulu16(u16 op1, u16 op2) |
a9533e7e | 85 | { |
7d4df48e | 86 | return (u16) (((uint32) op1 * (uint32) op2) >> 16); |
a9533e7e HP |
87 | } |
88 | ||
89 | /* | |
90 | Description: This function make 16 bit multiplication and return the result in 16 bits. | |
91 | To fit the multiplication result into 16 bits the multiplication result is right shifted by | |
92 | 15 bits. Right shifting 15 bits instead of 16 bits is done to remove the extra sign bit formed | |
93 | due to the multiplication. | |
94 | When both the 16bit inputs are 0x8000 then the output is saturated to 0x7fffffff. | |
95 | */ | |
96 | int16 qm_muls16(int16 op1, int16 op2) | |
97 | { | |
98 | int32 result; | |
99 | if (op1 == (int16) 0x8000 && op2 == (int16) 0x8000) { | |
100 | result = 0x7fffffff; | |
101 | } else { | |
102 | result = ((int32) (op1) * (int32) (op2)); | |
103 | } | |
90ea2296 | 104 | return (int16) (result >> 15); |
a9533e7e HP |
105 | } |
106 | ||
107 | /* | |
108 | Description: This function add two 32 bit numbers and return the 32bit result. | |
109 | If the result overflow 32 bits, the output will be saturated to 32bits. | |
110 | */ | |
111 | int32 qm_add32(int32 op1, int32 op2) | |
112 | { | |
113 | int32 result; | |
114 | result = op1 + op2; | |
115 | if (op1 < 0 && op2 < 0 && result > 0) { | |
116 | result = 0x80000000; | |
117 | } else if (op1 > 0 && op2 > 0 && result < 0) { | |
118 | result = 0x7fffffff; | |
119 | } | |
120 | return result; | |
121 | } | |
122 | ||
123 | /* | |
124 | Description: This function add two 16 bit numbers and return the 16bit result. | |
125 | If the result overflow 16 bits, the output will be saturated to 16bits. | |
126 | */ | |
127 | int16 qm_add16(int16 op1, int16 op2) | |
128 | { | |
129 | int16 result; | |
130 | int32 temp = (int32) op1 + (int32) op2; | |
131 | if (temp > (int32) 0x7fff) { | |
132 | result = (int16) 0x7fff; | |
133 | } else if (temp < (int32) 0xffff8000) { | |
134 | result = (int16) 0xffff8000; | |
135 | } else { | |
136 | result = (int16) temp; | |
137 | } | |
138 | return result; | |
139 | } | |
140 | ||
141 | /* | |
142 | Description: This function make 16 bit subtraction and return the 16bit result. | |
143 | If the result overflow 16 bits, the output will be saturated to 16bits. | |
144 | */ | |
145 | int16 qm_sub16(int16 op1, int16 op2) | |
146 | { | |
147 | int16 result; | |
148 | int32 temp = (int32) op1 - (int32) op2; | |
149 | if (temp > (int32) 0x7fff) { | |
150 | result = (int16) 0x7fff; | |
151 | } else if (temp < (int32) 0xffff8000) { | |
152 | result = (int16) 0xffff8000; | |
153 | } else { | |
154 | result = (int16) temp; | |
155 | } | |
156 | return result; | |
157 | } | |
158 | ||
159 | /* | |
160 | Description: This function make 32 bit subtraction and return the 32bit result. | |
161 | If the result overflow 32 bits, the output will be saturated to 32bits. | |
162 | */ | |
163 | int32 qm_sub32(int32 op1, int32 op2) | |
164 | { | |
165 | int32 result; | |
166 | result = op1 - op2; | |
167 | if (op1 >= 0 && op2 < 0 && result < 0) { | |
168 | result = 0x7fffffff; | |
169 | } else if (op1 < 0 && op2 > 0 && result > 0) { | |
170 | result = 0x80000000; | |
171 | } | |
172 | return result; | |
173 | } | |
174 | ||
175 | /* | |
176 | Description: This function multiply input 16 bit numbers and accumulate the result | |
177 | into the input 32 bit number and return the 32 bit accumulated result. | |
178 | If the accumulation result in overflow, then the output will be saturated. | |
179 | */ | |
180 | int32 qm_mac321616(int32 acc, int16 op1, int16 op2) | |
181 | { | |
182 | int32 result; | |
183 | result = qm_add32(acc, qm_mul321616(op1, op2)); | |
184 | return result; | |
185 | } | |
186 | ||
187 | /* | |
188 | Description: This function make a 32 bit saturated left shift when the specified shift | |
189 | is +ve. This function will make a 32 bit right shift when the specified shift is -ve. | |
190 | This function return the result after shifting operation. | |
191 | */ | |
192 | int32 qm_shl32(int32 op, int shift) | |
193 | { | |
194 | int i; | |
195 | int32 result; | |
196 | result = op; | |
197 | if (shift > 31) | |
198 | shift = 31; | |
199 | else if (shift < -31) | |
200 | shift = -31; | |
201 | if (shift >= 0) { | |
202 | for (i = 0; i < shift; i++) { | |
203 | result = qm_add32(result, result); | |
204 | } | |
205 | } else { | |
206 | result = result >> (-shift); | |
207 | } | |
208 | return result; | |
209 | } | |
210 | ||
211 | /* | |
212 | Description: This function make a 32 bit right shift when shift is +ve. | |
213 | This function make a 32 bit saturated left shift when shift is -ve. This function | |
214 | return the result of the shift operation. | |
215 | */ | |
216 | int32 qm_shr32(int32 op, int shift) | |
217 | { | |
218 | return qm_shl32(op, -shift); | |
219 | } | |
220 | ||
221 | /* | |
222 | Description: This function make a 16 bit saturated left shift when the specified shift | |
223 | is +ve. This function will make a 16 bit right shift when the specified shift is -ve. | |
224 | This function return the result after shifting operation. | |
225 | */ | |
226 | int16 qm_shl16(int16 op, int shift) | |
227 | { | |
228 | int i; | |
229 | int16 result; | |
230 | result = op; | |
231 | if (shift > 15) | |
232 | shift = 15; | |
233 | else if (shift < -15) | |
234 | shift = -15; | |
235 | if (shift > 0) { | |
236 | for (i = 0; i < shift; i++) { | |
237 | result = qm_add16(result, result); | |
238 | } | |
239 | } else { | |
240 | result = result >> (-shift); | |
241 | } | |
242 | return result; | |
243 | } | |
244 | ||
245 | /* | |
246 | Description: This function make a 16 bit right shift when shift is +ve. | |
247 | This function make a 16 bit saturated left shift when shift is -ve. This function | |
248 | return the result of the shift operation. | |
249 | */ | |
250 | int16 qm_shr16(int16 op, int shift) | |
251 | { | |
252 | return qm_shl16(op, -shift); | |
253 | } | |
254 | ||
255 | /* | |
256 | Description: This function return the number of redundant sign bits in a 16 bit number. | |
257 | Example: qm_norm16(0x0080) = 7. | |
258 | */ | |
259 | int16 qm_norm16(int16 op) | |
260 | { | |
7d4df48e | 261 | u16 u16extraSignBits; |
a9533e7e HP |
262 | if (op == 0) { |
263 | return 15; | |
264 | } else { | |
265 | u16extraSignBits = 0; | |
266 | while ((op >> 15) == (op >> 14)) { | |
267 | u16extraSignBits++; | |
268 | op = op << 1; | |
269 | } | |
270 | } | |
271 | return u16extraSignBits; | |
272 | } | |
273 | ||
274 | /* | |
275 | Description: This function return the number of redundant sign bits in a 32 bit number. | |
276 | Example: qm_norm32(0x00000080) = 23 | |
277 | */ | |
278 | int16 qm_norm32(int32 op) | |
279 | { | |
7d4df48e | 280 | u16 u16extraSignBits; |
a9533e7e HP |
281 | if (op == 0) { |
282 | return 31; | |
283 | } else { | |
284 | u16extraSignBits = 0; | |
285 | while ((op >> 31) == (op >> 30)) { | |
286 | u16extraSignBits++; | |
287 | op = op << 1; | |
288 | } | |
289 | } | |
290 | return u16extraSignBits; | |
291 | } | |
292 | ||
293 | /* | |
294 | Description: This function divide two 16 bit unsigned numbers. | |
295 | The numerator should be less than denominator. So the quotient is always less than 1. | |
296 | This function return the quotient in q.15 format. | |
297 | */ | |
298 | int16 qm_div_s(int16 num, int16 denom) | |
299 | { | |
300 | int16 var_out; | |
301 | int16 iteration; | |
302 | int32 L_num; | |
303 | int32 L_denom; | |
304 | L_num = (num) << 15; | |
305 | L_denom = (denom) << 15; | |
306 | for (iteration = 0; iteration < 15; iteration++) { | |
307 | L_num <<= 1; | |
308 | if (L_num >= L_denom) { | |
309 | L_num = qm_sub32(L_num, L_denom); | |
310 | L_num = qm_add32(L_num, 1); | |
311 | } | |
312 | } | |
313 | var_out = (int16) (L_num & 0x7fff); | |
90ea2296 | 314 | return var_out; |
a9533e7e HP |
315 | } |
316 | ||
317 | /* | |
318 | Description: This function compute the absolute value of a 16 bit number. | |
319 | */ | |
320 | int16 qm_abs16(int16 op) | |
321 | { | |
322 | if (op < 0) { | |
323 | if (op == (int16) 0xffff8000) { | |
324 | return 0x7fff; | |
325 | } else { | |
326 | return -op; | |
327 | } | |
328 | } else { | |
329 | return op; | |
330 | } | |
331 | } | |
332 | ||
333 | /* | |
334 | Description: This function divide two 16 bit numbers. | |
335 | The quotient is returned through return value. | |
336 | The qformat of the quotient is returned through the pointer (qQuotient) passed | |
337 | to this function. The qformat of quotient is adjusted appropriately such that | |
338 | the quotient occupies all 16 bits. | |
339 | */ | |
7cc4a4c0 | 340 | int16 qm_div16(int16 num, int16 denom, int16 *qQuotient) |
a9533e7e HP |
341 | { |
342 | int16 sign; | |
343 | int16 nNum, nDenom; | |
344 | sign = num ^ denom; | |
345 | num = qm_abs16(num); | |
346 | denom = qm_abs16(denom); | |
347 | nNum = qm_norm16(num); | |
348 | nDenom = qm_norm16(denom); | |
349 | num = qm_shl16(num, nNum - 1); | |
350 | denom = qm_shl16(denom, nDenom); | |
351 | *qQuotient = nNum - 1 - nDenom + 15; | |
352 | if (sign >= 0) { | |
353 | return qm_div_s(num, denom); | |
354 | } else { | |
355 | return -qm_div_s(num, denom); | |
356 | } | |
357 | } | |
358 | ||
359 | /* | |
360 | Description: This function compute absolute value of a 32 bit number. | |
361 | */ | |
362 | int32 qm_abs32(int32 op) | |
363 | { | |
364 | if (op < 0) { | |
365 | if (op == (int32) 0x80000000) { | |
366 | return 0x7fffffff; | |
367 | } else { | |
368 | return -op; | |
369 | } | |
370 | } else { | |
371 | return op; | |
372 | } | |
373 | } | |
374 | ||
375 | /* | |
376 | Description: This function divide two 32 bit numbers. The division is performed | |
377 | by considering only important 16 bits in 32 bit numbers. | |
378 | The quotient is returned through return value. | |
379 | The qformat of the quotient is returned through the pointer (qquotient) passed | |
380 | to this function. The qformat of quotient is adjusted appropriately such that | |
381 | the quotient occupies all 16 bits. | |
382 | */ | |
7cc4a4c0 | 383 | int16 qm_div163232(int32 num, int32 denom, int16 *qquotient) |
a9533e7e HP |
384 | { |
385 | int32 sign; | |
386 | int16 nNum, nDenom; | |
387 | sign = num ^ denom; | |
388 | num = qm_abs32(num); | |
389 | denom = qm_abs32(denom); | |
390 | nNum = qm_norm32(num); | |
391 | nDenom = qm_norm32(denom); | |
392 | num = qm_shl32(num, nNum - 1); | |
393 | denom = qm_shl32(denom, nDenom); | |
394 | *qquotient = nNum - 1 - nDenom + 15; | |
395 | if (sign >= 0) { | |
396 | return qm_div_s((int16) (num >> 16), (int16) (denom >> 16)); | |
397 | } else { | |
398 | return -qm_div_s((int16) (num >> 16), (int16) (denom >> 16)); | |
399 | } | |
400 | } | |
401 | ||
402 | /* | |
403 | Description: This function multiply a 32 bit number with a 16 bit number. | |
404 | The multiplicaton result is right shifted by 16 bits to fit the result | |
405 | into 32 bit output. | |
406 | */ | |
407 | int32 qm_mul323216(int32 op1, int16 op2) | |
408 | { | |
409 | int16 hi; | |
7d4df48e | 410 | u16 lo; |
a9533e7e HP |
411 | int32 result; |
412 | hi = op1 >> 16; | |
413 | lo = (int16) (op1 & 0xffff); | |
414 | result = qm_mul321616(hi, op2); | |
415 | result = result + (qm_mulsu321616(op2, lo) >> 16); | |
416 | return result; | |
417 | } | |
418 | ||
419 | /* | |
420 | Description: This function multiply signed 16 bit number with unsigned 16 bit number and return | |
421 | the result in 32 bits. | |
422 | */ | |
7d4df48e | 423 | int32 qm_mulsu321616(int16 op1, u16 op2) |
a9533e7e HP |
424 | { |
425 | return (int32) (op1) * op2; | |
426 | } | |
427 | ||
428 | /* | |
429 | Description: This function multiply 32 bit number with 16 bit number. The multiplication result is | |
430 | right shifted by 15 bits to fit the result into 32 bits. Right shifting by only 15 bits instead of | |
431 | 16 bits is done to remove the extra sign bit formed by multiplication from the return value. | |
432 | When the input numbers are 0x80000000, 0x8000 the return value is saturated to 0x7fffffff. | |
433 | */ | |
434 | int32 qm_muls323216(int32 op1, int16 op2) | |
435 | { | |
436 | int16 hi; | |
7d4df48e | 437 | u16 lo; |
a9533e7e HP |
438 | int32 result; |
439 | hi = op1 >> 16; | |
440 | lo = (int16) (op1 & 0xffff); | |
441 | result = qm_muls321616(hi, op2); | |
442 | result = qm_add32(result, (qm_mulsu321616(op2, lo) >> 15)); | |
443 | return result; | |
444 | } | |
445 | ||
446 | /* | |
447 | Description: This function multiply two 32 bit numbers. The multiplication result is right | |
448 | shifted by 32 bits to fit the multiplication result into 32 bits. The right shifted | |
449 | multiplication result is returned as output. | |
450 | */ | |
451 | int32 qm_mul32(int32 a, int32 b) | |
452 | { | |
453 | int16 hi1, hi2; | |
7d4df48e | 454 | u16 lo1, lo2; |
a9533e7e HP |
455 | int32 result; |
456 | hi1 = a >> 16; | |
457 | hi2 = b >> 16; | |
7d4df48e GKH |
458 | lo1 = (u16) (a & 0xffff); |
459 | lo2 = (u16) (b & 0xffff); | |
a9533e7e HP |
460 | result = qm_mul321616(hi1, hi2); |
461 | result = result + (qm_mulsu321616(hi1, lo2) >> 16); | |
462 | result = result + (qm_mulsu321616(hi2, lo1) >> 16); | |
463 | return result; | |
464 | } | |
465 | ||
466 | /* | |
467 | Description: This function multiply two 32 bit numbers. The multiplication result is | |
468 | right shifted by 31 bits to fit the multiplication result into 32 bits. The right | |
469 | shifted multiplication result is returned as output. Right shifting by only 31 bits | |
470 | instead of 32 bits is done to remove the extra sign bit formed by multiplication. | |
471 | When the input numbers are 0x80000000, 0x80000000 the return value is saturated to | |
472 | 0x7fffffff. | |
473 | */ | |
474 | int32 qm_muls32(int32 a, int32 b) | |
475 | { | |
476 | int16 hi1, hi2; | |
7d4df48e | 477 | u16 lo1, lo2; |
a9533e7e HP |
478 | int32 result; |
479 | hi1 = a >> 16; | |
480 | hi2 = b >> 16; | |
7d4df48e GKH |
481 | lo1 = (u16) (a & 0xffff); |
482 | lo2 = (u16) (b & 0xffff); | |
a9533e7e HP |
483 | result = qm_muls321616(hi1, hi2); |
484 | result = qm_add32(result, (qm_mulsu321616(hi1, lo2) >> 15)); | |
485 | result = qm_add32(result, (qm_mulsu321616(hi2, lo1) >> 15)); | |
486 | result = qm_add32(result, (qm_mulu16(lo1, lo2) >> 15)); | |
487 | return result; | |
488 | } | |
489 | ||
490 | /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */ | |
491 | static const int16 log_table[] = { | |
492 | 0, | |
493 | 1455, | |
494 | 2866, | |
495 | 4236, | |
496 | 5568, | |
497 | 6863, | |
498 | 8124, | |
499 | 9352, | |
500 | 10549, | |
501 | 11716, | |
502 | 12855, | |
503 | 13968, | |
504 | 15055, | |
505 | 16117, | |
506 | 17156, | |
507 | 18173, | |
508 | 19168, | |
509 | 20143, | |
510 | 21098, | |
511 | 22034, | |
512 | 22952, | |
513 | 23852, | |
514 | 24736, | |
515 | 25604, | |
516 | 26455, | |
517 | 27292, | |
518 | 28114, | |
519 | 28922, | |
520 | 29717, | |
521 | 30498, | |
522 | 31267, | |
523 | 32024 | |
524 | }; | |
525 | ||
526 | #define LOG_TABLE_SIZE 32 /* log_table size */ | |
527 | #define LOG2_LOG_TABLE_SIZE 5 /* log2(log_table size) */ | |
528 | #define Q_LOG_TABLE 15 /* qformat of log_table */ | |
529 | #define LOG10_2 19728 /* log10(2) in q.16 */ | |
530 | ||
531 | /* | |
532 | Description: | |
533 | This routine takes the input number N and its q format qN and compute | |
534 | the log10(N). This routine first normalizes the input no N. Then N is in mag*(2^x) format. | |
535 | mag is any number in the range 2^30-(2^31 - 1). Then log2(mag * 2^x) = log2(mag) + x is computed. | |
536 | From that log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed. | |
537 | This routine looks the log2 value in the table considering LOG2_LOG_TABLE_SIZE+1 MSBs. | |
538 | As the MSB is always 1, only next LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. | |
539 | Next 16 MSBs are used for interpolation. | |
540 | Inputs: | |
541 | N - number to which log10 has to be found. | |
542 | qN - q format of N | |
543 | log10N - address where log10(N) will be written. | |
544 | qLog10N - address where log10N qformat will be written. | |
545 | Note/Problem: | |
546 | For accurate results input should be in normalized or near normalized form. | |
547 | */ | |
7cc4a4c0 | 548 | void qm_log10(int32 N, int16 qN, int16 *log10N, int16 *qLog10N) |
a9533e7e HP |
549 | { |
550 | int16 s16norm, s16tableIndex, s16errorApproximation; | |
7d4df48e | 551 | u16 u16offset; |
a9533e7e HP |
552 | int32 s32log; |
553 | ||
554 | /* Logerithm of negative values is undefined. | |
555 | * assert N is greater than 0. | |
556 | */ | |
557 | /* ASSERT(N > 0); */ | |
558 | ||
559 | /* normalize the N. */ | |
560 | s16norm = qm_norm32(N); | |
561 | N = N << s16norm; | |
562 | ||
563 | /* The qformat of N after normalization. | |
564 | * -30 is added to treat the no as between 1.0 to 2.0 | |
565 | * i.e. after adding the -30 to the qformat the decimal point will be | |
566 | * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e. | |
567 | * at the right side of 30th bit. | |
568 | */ | |
569 | qN = qN + s16norm - 30; | |
570 | ||
571 | /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */ | |
572 | s16tableIndex = (int16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE))); | |
573 | ||
574 | /* remove the MSB. the MSB is always 1 after normalization. */ | |
575 | s16tableIndex = | |
576 | s16tableIndex & (int16) ((1 << LOG2_LOG_TABLE_SIZE) - 1); | |
577 | ||
578 | /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */ | |
579 | N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1); | |
580 | ||
581 | /* take the offset as the 16 MSBS after table index. | |
582 | */ | |
7d4df48e | 583 | u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16))); |
a9533e7e HP |
584 | |
585 | /* look the log value in the table. */ | |
586 | s32log = log_table[s16tableIndex]; /* q.15 format */ | |
587 | ||
588 | /* interpolate using the offset. */ | |
7d4df48e | 589 | s16errorApproximation = (int16) qm_mulu16(u16offset, (u16) (log_table[s16tableIndex + 1] - log_table[s16tableIndex])); /* q.15 */ |
a9533e7e HP |
590 | |
591 | s32log = qm_add16((int16) s32log, s16errorApproximation); /* q.15 format */ | |
592 | ||
593 | /* adjust for the qformat of the N as | |
594 | * log2(mag * 2^x) = log2(mag) + x | |
595 | */ | |
29c4275a | 596 | s32log = qm_add32(s32log, ((int32) -qN) << 15); /* q.15 format */ |
a9533e7e HP |
597 | |
598 | /* normalize the result. */ | |
599 | s16norm = qm_norm32(s32log); | |
600 | ||
601 | /* bring all the important bits into lower 16 bits */ | |
602 | s32log = qm_shl32(s32log, s16norm - 16); /* q.15+s16norm-16 format */ | |
603 | ||
604 | /* compute the log10(N) by multiplying log2(N) with log10(2). | |
605 | * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2) | |
606 | * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16) | |
607 | */ | |
608 | *log10N = qm_muls16((int16) s32log, (int16) LOG10_2); | |
609 | ||
610 | /* write the q format of the result. */ | |
611 | *qLog10N = 15 + s16norm - 16 + 1; | |
612 | ||
613 | return; | |
614 | } | |
615 | ||
616 | /* | |
617 | Description: | |
618 | This routine compute 1/N. | |
619 | This routine reformates the given no N as N * 2^qN where N is in between 0.5 and 1.0 | |
620 | in q.15 format in 16 bits. So the problem now boils down to finding the inverse of a | |
621 | q.15 no in 16 bits which is in the range of 0.5 to 1.0. The output is always between | |
622 | 2.0 to 1. So the output is 2.0 to 1.0 in q.30 format. Once the final output format is found | |
623 | by taking the qN into account. Inverse is found with newton rapson method. Initially | |
624 | inverse (x) is guessed as 1/0.75 (with appropriate sign). The new guess is calculated | |
625 | using the formula x' = 2*x - N*x*x. After 4 or 5 iterations the inverse is very close to | |
626 | inverse of N. | |
627 | Inputs: | |
628 | N - number to which 1/N has to be found. | |
629 | qn - q format of N. | |
630 | sqrtN - address where 1/N has to be written. | |
631 | qsqrtN - address where q format of 1/N has to be written. | |
632 | */ | |
633 | #define qx 29 | |
7cc4a4c0 | 634 | void qm_1byN(int32 N, int16 qN, int32 *result, int16 *qResult) |
a9533e7e HP |
635 | { |
636 | int16 normN; | |
637 | int32 s32firstTerm, s32secondTerm, x; | |
638 | int i; | |
639 | ||
640 | normN = qm_norm32(N); | |
641 | ||
642 | /* limit N to least significant 16 bits. 15th bit is the sign bit. */ | |
643 | N = qm_shl32(N, normN - 16); | |
644 | qN = qN + normN - 16 - 15; | |
645 | /* -15 is added to treat N as 16 bit q.15 number in the range from 0.5 to 1 */ | |
646 | ||
647 | /* Take the initial guess as 1/0.75 in qx format with appropriate sign. */ | |
648 | if (N >= 0) { | |
649 | x = (int32) ((1 / 0.75) * (1 << qx)); | |
650 | /* input no is in the range 0.5 to 1. So 1/0.75 is taken as initial guess. */ | |
651 | } else { | |
652 | x = (int32) ((1 / -0.75) * (1 << qx)); | |
653 | /* input no is in the range -0.5 to -1. So 1/-0.75 is taken as initial guess. */ | |
654 | } | |
655 | ||
656 | /* iterate the equation x = 2*x - N*x*x for 4 times. */ | |
657 | for (i = 0; i < 4; i++) { | |
658 | s32firstTerm = qm_shl32(x, 1); /* s32firstTerm = 2*x in q.29 */ | |
659 | s32secondTerm = | |
660 | qm_muls321616((int16) (s32firstTerm >> 16), | |
661 | (int16) (s32firstTerm >> 16)); | |
662 | /* s32secondTerm = x*x in q.(29+1-16)*2+1 */ | |
663 | s32secondTerm = | |
664 | qm_muls321616((int16) (s32secondTerm >> 16), (int16) N); | |
665 | /* s32secondTerm = N*x*x in q.((29+1-16)*2+1)-16+15+1 i.e. in q.29 */ | |
666 | x = qm_sub32(s32firstTerm, s32secondTerm); | |
667 | /* can be added directly as both are in q.29 */ | |
668 | } | |
669 | ||
670 | /* Bring the x to q.30 format. */ | |
671 | *result = qm_shl32(x, 1); | |
672 | /* giving the output in q.30 format for q.15 input in 16 bits. */ | |
673 | ||
674 | /* compute the final q format of the result. */ | |
675 | *qResult = -qN + 30; /* adjusting the q format of actual output */ | |
676 | ||
677 | return; | |
678 | } | |
679 | ||
680 | #undef qx |