2 * Copyright (c) 2010 Broadcom Corporation
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.
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.
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.
28 if (op > (s32) 0x7fff) {
30 } else if (op < (s32) 0xffff8000) {
31 result = (s16) (0x8000);
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).
44 s32 qm_mul321616(s16 op1, s16 op2)
46 return (s32) (op1) * (s32) (op2);
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
54 s16 qm_mul16(s16 op1, s16 op2)
57 result = ((s32) (op1) * (s32) (op2));
58 return (s16) (result >> 16);
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.
68 s32 qm_muls321616(s16 op1, s16 op2)
71 if (op1 == (s16) (0x8000) && op2 == (s16) (0x8000)) {
74 result = ((s32) (op1) * (s32) (op2));
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.
84 u16 qm_mulu16(u16 op1, u16 op2)
86 return (u16) (((u32) op1 * (u32) op2) >> 16);
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.
96 s16 qm_muls16(s16 op1, s16 op2)
99 if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000) {
102 result = ((s32) (op1) * (s32) (op2));
104 return (s16) (result >> 15);
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.
111 s32 qm_add32(s32 op1, s32 op2)
115 if (op1 < 0 && op2 < 0 && result > 0) {
117 } else if (op1 > 0 && op2 > 0 && result < 0) {
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.
127 s16 qm_add16(s16 op1, s16 op2)
130 s32 temp = (s32) op1 + (s32) op2;
131 if (temp > (s32) 0x7fff) {
132 result = (s16) 0x7fff;
133 } else if (temp < (s32) 0xffff8000) {
134 result = (s16) 0xffff8000;
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.
145 s16 qm_sub16(s16 op1, s16 op2)
148 s32 temp = (s32) op1 - (s32) op2;
149 if (temp > (s32) 0x7fff) {
150 result = (s16) 0x7fff;
151 } else if (temp < (s32) 0xffff8000) {
152 result = (s16) 0xffff8000;
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.
163 s32 qm_sub32(s32 op1, s32 op2)
167 if (op1 >= 0 && op2 < 0 && result < 0) {
169 } else if (op1 < 0 && op2 > 0 && result > 0) {
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.
180 s32 qm_mac321616(s32 acc, s16 op1, s16 op2)
183 result = qm_add32(acc, qm_mul321616(op1, op2));
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.
192 s32 qm_shl32(s32 op, int shift)
199 else if (shift < -31)
202 for (i = 0; i < shift; i++) {
203 result = qm_add32(result, result);
206 result = result >> (-shift);
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.
216 s32 qm_shr32(s32 op, int shift)
218 return qm_shl32(op, -shift);
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.
226 s16 qm_shl16(s16 op, int shift)
233 else if (shift < -15)
236 for (i = 0; i < shift; i++) {
237 result = qm_add16(result, result);
240 result = result >> (-shift);
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.
250 s16 qm_shr16(s16 op, int shift)
252 return qm_shl16(op, -shift);
256 Description: This function return the number of redundant sign bits in a 16 bit number.
257 Example: qm_norm16(0x0080) = 7.
259 s16 qm_norm16(s16 op)
261 u16 u16extraSignBits;
265 u16extraSignBits = 0;
266 while ((op >> 15) == (op >> 14)) {
271 return u16extraSignBits;
275 Description: This function return the number of redundant sign bits in a 32 bit number.
276 Example: qm_norm32(0x00000080) = 23
278 s16 qm_norm32(s32 op)
280 u16 u16extraSignBits;
284 u16extraSignBits = 0;
285 while ((op >> 31) == (op >> 30)) {
290 return u16extraSignBits;
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.
298 s16 qm_div_s(s16 num, s16 denom)
305 L_denom = (denom) << 15;
306 for (iteration = 0; iteration < 15; iteration++) {
308 if (L_num >= L_denom) {
309 L_num = qm_sub32(L_num, L_denom);
310 L_num = qm_add32(L_num, 1);
313 var_out = (s16) (L_num & 0x7fff);
318 Description: This function compute the absolute value of a 16 bit number.
323 if (op == (s16) 0xffff8000) {
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.
340 s16 qm_div16(s16 num, s16 denom, s16 *qQuotient)
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;
353 return qm_div_s(num, denom);
355 return -qm_div_s(num, denom);
360 Description: This function compute absolute value of a 32 bit number.
365 if (op == (s32) 0x80000000) {
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.
383 s16 qm_div163232(s32 num, s32 denom, s16 *qquotient)
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;
396 return qm_div_s((s16) (num >> 16), (s16) (denom >> 16));
398 return -qm_div_s((s16) (num >> 16), (s16) (denom >> 16));
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
407 s32 qm_mul323216(s32 op1, s16 op2)
413 lo = (s16) (op1 & 0xffff);
414 result = qm_mul321616(hi, op2);
415 result = result + (qm_mulsu321616(op2, lo) >> 16);
420 Description: This function multiply signed 16 bit number with unsigned 16 bit number and return
421 the result in 32 bits.
423 s32 qm_mulsu321616(s16 op1, u16 op2)
425 return (s32) (op1) * op2;
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.
434 s32 qm_muls323216(s32 op1, s16 op2)
440 lo = (s16) (op1 & 0xffff);
441 result = qm_muls321616(hi, op2);
442 result = qm_add32(result, (qm_mulsu321616(op2, lo) >> 15));
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.
451 s32 qm_mul32(s32 a, s32 b)
458 lo1 = (u16) (a & 0xffff);
459 lo2 = (u16) (b & 0xffff);
460 result = qm_mul321616(hi1, hi2);
461 result = result + (qm_mulsu321616(hi1, lo2) >> 16);
462 result = result + (qm_mulsu321616(hi2, lo1) >> 16);
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
474 s32 qm_muls32(s32 a, s32 b)
481 lo1 = (u16) (a & 0xffff);
482 lo2 = (u16) (b & 0xffff);
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));
490 /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */
491 static const s16 log_table[] = {
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 */
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.
541 N - number to which log10 has to be found.
543 log10N - address where log10(N) will be written.
544 qLog10N - address where log10N qformat will be written.
546 For accurate results input should be in normalized or near normalized form.
548 void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
550 s16 s16norm, s16tableIndex, s16errorApproximation;
554 /* Logerithm of negative values is undefined.
555 * assert N is greater than 0.
559 /* normalize the N. */
560 s16norm = qm_norm32(N);
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.
569 qN = qN + s16norm - 30;
571 /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */
572 s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE)));
574 /* remove the MSB. the MSB is always 1 after normalization. */
576 s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
578 /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
579 N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1);
581 /* take the offset as the 16 MSBS after table index.
583 u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16)));
585 /* look the log value in the table. */
586 s32log = log_table[s16tableIndex]; /* q.15 format */
588 /* interpolate using the offset. */
589 s16errorApproximation = (s16) qm_mulu16(u16offset, (u16) (log_table[s16tableIndex + 1] - log_table[s16tableIndex])); /* q.15 */
591 s32log = qm_add16((s16) s32log, s16errorApproximation); /* q.15 format */
593 /* adjust for the qformat of the N as
594 * log2(mag * 2^x) = log2(mag) + x
596 s32log = qm_add32(s32log, ((s32) -qN) << 15); /* q.15 format */
598 /* normalize the result. */
599 s16norm = qm_norm32(s32log);
601 /* bring all the important bits into lower 16 bits */
602 s32log = qm_shl32(s32log, s16norm - 16); /* q.15+s16norm-16 format */
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)
608 *log10N = qm_muls16((s16) s32log, (s16) LOG10_2);
610 /* write the q format of the result. */
611 *qLog10N = 15 + s16norm - 16 + 1;
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
628 N - number to which 1/N has to be found.
630 sqrtN - address where 1/N has to be written.
631 qsqrtN - address where q format of 1/N has to be written.
634 void qm_1byN(s32 N, s16 qN, s32 *result, s16 *qResult)
637 s32 s32firstTerm, s32secondTerm, x;
640 normN = qm_norm32(N);
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 */
647 /* Take the initial guess as 1/0.75 in qx format with appropriate sign. */
649 x = (s32) ((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. */
652 x = (s32) ((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. */
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 */
660 qm_muls321616((s16) (s32firstTerm >> 16),
661 (s16) (s32firstTerm >> 16));
662 /* s32secondTerm = x*x in q.(29+1-16)*2+1 */
664 qm_muls321616((s16) (s32secondTerm >> 16), (s16) 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 */
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. */
674 /* compute the final q format of the result. */
675 *qResult = -qN + 30; /* adjusting the q format of actual output */