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.
17 #include <linux/types.h>
19 #include "wlc_phy_qmath.h"
22 Description: This function saturate input 32 bit number into a 16 bit number.
23 If input number is greater than 0x7fff then output is saturated to 0x7fff.
24 else if input number is less than 0xffff8000 then output is saturated to 0xffff8000
25 else output is same as input.
30 if (op
> (s32
) 0x7fff) {
32 } else if (op
< (s32
) 0xffff8000) {
33 result
= (s16
) (0x8000);
41 Description: This function multiply two input 16 bit numbers and return the 32 bit result.
42 This multiplication is similar to compiler multiplication. This operation is defined if
43 16 bit multiplication on the processor platform is cheaper than 32 bit multiplication (as
44 the most of qmath functions can be replaced with processor intrinsic instructions).
46 s32
qm_mul321616(s16 op1
, s16 op2
)
48 return (s32
) (op1
) * (s32
) (op2
);
52 Description: This function make 16 bit multiplication and return the result in 16 bits.
53 To fit the result into 16 bits the 32 bit multiplication result is right
56 s16
qm_mul16(s16 op1
, s16 op2
)
59 result
= ((s32
) (op1
) * (s32
) (op2
));
60 return (s16
) (result
>> 16);
64 Description: This function multiply two 16 bit numbers and return the result in 32 bits.
65 This function remove the extra sign bit created by the multiplication by leftshifting the
66 32 bit multiplication result by 1 bit before returning the result. So the output is
67 twice that of compiler multiplication. (i.e. qm_muls321616(2,3)=12).
68 When both input 16 bit numbers are 0x8000, then the result is saturated to 0x7fffffff.
70 s32
qm_muls321616(s16 op1
, s16 op2
)
73 if (op1
== (s16
) (0x8000) && op2
== (s16
) (0x8000)) {
76 result
= ((s32
) (op1
) * (s32
) (op2
));
83 Description: This function make 16 bit unsigned multiplication. To fit the output into
84 16 bits the 32 bit multiplication result is right shifted by 16 bits.
86 u16
qm_mulu16(u16 op1
, u16 op2
)
88 return (u16
) (((u32
) op1
* (u32
) op2
) >> 16);
92 Description: This function make 16 bit multiplication and return the result in 16 bits.
93 To fit the multiplication result into 16 bits the multiplication result is right shifted by
94 15 bits. Right shifting 15 bits instead of 16 bits is done to remove the extra sign bit formed
95 due to the multiplication.
96 When both the 16bit inputs are 0x8000 then the output is saturated to 0x7fffffff.
98 s16
qm_muls16(s16 op1
, s16 op2
)
101 if (op1
== (s16
) 0x8000 && op2
== (s16
) 0x8000) {
104 result
= ((s32
) (op1
) * (s32
) (op2
));
106 return (s16
) (result
>> 15);
110 Description: This function add two 32 bit numbers and return the 32bit result.
111 If the result overflow 32 bits, the output will be saturated to 32bits.
113 s32
qm_add32(s32 op1
, s32 op2
)
117 if (op1
< 0 && op2
< 0 && result
> 0) {
119 } else if (op1
> 0 && op2
> 0 && result
< 0) {
126 Description: This function add two 16 bit numbers and return the 16bit result.
127 If the result overflow 16 bits, the output will be saturated to 16bits.
129 s16
qm_add16(s16 op1
, s16 op2
)
132 s32 temp
= (s32
) op1
+ (s32
) op2
;
133 if (temp
> (s32
) 0x7fff) {
134 result
= (s16
) 0x7fff;
135 } else if (temp
< (s32
) 0xffff8000) {
136 result
= (s16
) 0xffff8000;
144 Description: This function make 16 bit subtraction and return the 16bit result.
145 If the result overflow 16 bits, the output will be saturated to 16bits.
147 s16
qm_sub16(s16 op1
, s16 op2
)
150 s32 temp
= (s32
) op1
- (s32
) op2
;
151 if (temp
> (s32
) 0x7fff) {
152 result
= (s16
) 0x7fff;
153 } else if (temp
< (s32
) 0xffff8000) {
154 result
= (s16
) 0xffff8000;
162 Description: This function make 32 bit subtraction and return the 32bit result.
163 If the result overflow 32 bits, the output will be saturated to 32bits.
165 s32
qm_sub32(s32 op1
, s32 op2
)
169 if (op1
>= 0 && op2
< 0 && result
< 0) {
171 } else if (op1
< 0 && op2
> 0 && result
> 0) {
178 Description: This function multiply input 16 bit numbers and accumulate the result
179 into the input 32 bit number and return the 32 bit accumulated result.
180 If the accumulation result in overflow, then the output will be saturated.
182 s32
qm_mac321616(s32 acc
, s16 op1
, s16 op2
)
185 result
= qm_add32(acc
, qm_mul321616(op1
, op2
));
190 Description: This function make a 32 bit saturated left shift when the specified shift
191 is +ve. This function will make a 32 bit right shift when the specified shift is -ve.
192 This function return the result after shifting operation.
194 s32
qm_shl32(s32 op
, int shift
)
201 else if (shift
< -31)
204 for (i
= 0; i
< shift
; i
++) {
205 result
= qm_add32(result
, result
);
208 result
= result
>> (-shift
);
214 Description: This function make a 32 bit right shift when shift is +ve.
215 This function make a 32 bit saturated left shift when shift is -ve. This function
216 return the result of the shift operation.
218 s32
qm_shr32(s32 op
, int shift
)
220 return qm_shl32(op
, -shift
);
224 Description: This function make a 16 bit saturated left shift when the specified shift
225 is +ve. This function will make a 16 bit right shift when the specified shift is -ve.
226 This function return the result after shifting operation.
228 s16
qm_shl16(s16 op
, int shift
)
235 else if (shift
< -15)
238 for (i
= 0; i
< shift
; i
++) {
239 result
= qm_add16(result
, result
);
242 result
= result
>> (-shift
);
248 Description: This function make a 16 bit right shift when shift is +ve.
249 This function make a 16 bit saturated left shift when shift is -ve. This function
250 return the result of the shift operation.
252 s16
qm_shr16(s16 op
, int shift
)
254 return qm_shl16(op
, -shift
);
258 Description: This function return the number of redundant sign bits in a 16 bit number.
259 Example: qm_norm16(0x0080) = 7.
261 s16
qm_norm16(s16 op
)
263 u16 u16extraSignBits
;
267 u16extraSignBits
= 0;
268 while ((op
>> 15) == (op
>> 14)) {
273 return u16extraSignBits
;
277 Description: This function return the number of redundant sign bits in a 32 bit number.
278 Example: qm_norm32(0x00000080) = 23
280 s16
qm_norm32(s32 op
)
282 u16 u16extraSignBits
;
286 u16extraSignBits
= 0;
287 while ((op
>> 31) == (op
>> 30)) {
292 return u16extraSignBits
;
296 Description: This function divide two 16 bit unsigned numbers.
297 The numerator should be less than denominator. So the quotient is always less than 1.
298 This function return the quotient in q.15 format.
300 s16
qm_div_s(s16 num
, s16 denom
)
307 L_denom
= (denom
) << 15;
308 for (iteration
= 0; iteration
< 15; iteration
++) {
310 if (L_num
>= L_denom
) {
311 L_num
= qm_sub32(L_num
, L_denom
);
312 L_num
= qm_add32(L_num
, 1);
315 var_out
= (s16
) (L_num
& 0x7fff);
320 Description: This function compute the absolute value of a 16 bit number.
325 if (op
== (s16
) 0xffff8000) {
336 Description: This function divide two 16 bit numbers.
337 The quotient is returned through return value.
338 The qformat of the quotient is returned through the pointer (qQuotient) passed
339 to this function. The qformat of quotient is adjusted appropriately such that
340 the quotient occupies all 16 bits.
342 s16
qm_div16(s16 num
, s16 denom
, s16
*qQuotient
)
348 denom
= qm_abs16(denom
);
349 nNum
= qm_norm16(num
);
350 nDenom
= qm_norm16(denom
);
351 num
= qm_shl16(num
, nNum
- 1);
352 denom
= qm_shl16(denom
, nDenom
);
353 *qQuotient
= nNum
- 1 - nDenom
+ 15;
355 return qm_div_s(num
, denom
);
357 return -qm_div_s(num
, denom
);
362 Description: This function compute absolute value of a 32 bit number.
367 if (op
== (s32
) 0x80000000) {
378 Description: This function divide two 32 bit numbers. The division is performed
379 by considering only important 16 bits in 32 bit numbers.
380 The quotient is returned through return value.
381 The qformat of the quotient is returned through the pointer (qquotient) passed
382 to this function. The qformat of quotient is adjusted appropriately such that
383 the quotient occupies all 16 bits.
385 s16
qm_div163232(s32 num
, s32 denom
, s16
*qquotient
)
391 denom
= qm_abs32(denom
);
392 nNum
= qm_norm32(num
);
393 nDenom
= qm_norm32(denom
);
394 num
= qm_shl32(num
, nNum
- 1);
395 denom
= qm_shl32(denom
, nDenom
);
396 *qquotient
= nNum
- 1 - nDenom
+ 15;
398 return qm_div_s((s16
) (num
>> 16), (s16
) (denom
>> 16));
400 return -qm_div_s((s16
) (num
>> 16), (s16
) (denom
>> 16));
405 Description: This function multiply a 32 bit number with a 16 bit number.
406 The multiplicaton result is right shifted by 16 bits to fit the result
409 s32
qm_mul323216(s32 op1
, s16 op2
)
415 lo
= (s16
) (op1
& 0xffff);
416 result
= qm_mul321616(hi
, op2
);
417 result
= result
+ (qm_mulsu321616(op2
, lo
) >> 16);
422 Description: This function multiply signed 16 bit number with unsigned 16 bit number and return
423 the result in 32 bits.
425 s32
qm_mulsu321616(s16 op1
, u16 op2
)
427 return (s32
) (op1
) * op2
;
431 Description: This function multiply 32 bit number with 16 bit number. The multiplication result is
432 right shifted by 15 bits to fit the result into 32 bits. Right shifting by only 15 bits instead of
433 16 bits is done to remove the extra sign bit formed by multiplication from the return value.
434 When the input numbers are 0x80000000, 0x8000 the return value is saturated to 0x7fffffff.
436 s32
qm_muls323216(s32 op1
, s16 op2
)
442 lo
= (s16
) (op1
& 0xffff);
443 result
= qm_muls321616(hi
, op2
);
444 result
= qm_add32(result
, (qm_mulsu321616(op2
, lo
) >> 15));
449 Description: This function multiply two 32 bit numbers. The multiplication result is right
450 shifted by 32 bits to fit the multiplication result into 32 bits. The right shifted
451 multiplication result is returned as output.
453 s32
qm_mul32(s32 a
, s32 b
)
460 lo1
= (u16
) (a
& 0xffff);
461 lo2
= (u16
) (b
& 0xffff);
462 result
= qm_mul321616(hi1
, hi2
);
463 result
= result
+ (qm_mulsu321616(hi1
, lo2
) >> 16);
464 result
= result
+ (qm_mulsu321616(hi2
, lo1
) >> 16);
469 Description: This function multiply two 32 bit numbers. The multiplication result is
470 right shifted by 31 bits to fit the multiplication result into 32 bits. The right
471 shifted multiplication result is returned as output. Right shifting by only 31 bits
472 instead of 32 bits is done to remove the extra sign bit formed by multiplication.
473 When the input numbers are 0x80000000, 0x80000000 the return value is saturated to
476 s32
qm_muls32(s32 a
, s32 b
)
483 lo1
= (u16
) (a
& 0xffff);
484 lo2
= (u16
) (b
& 0xffff);
485 result
= qm_muls321616(hi1
, hi2
);
486 result
= qm_add32(result
, (qm_mulsu321616(hi1
, lo2
) >> 15));
487 result
= qm_add32(result
, (qm_mulsu321616(hi2
, lo1
) >> 15));
488 result
= qm_add32(result
, (qm_mulu16(lo1
, lo2
) >> 15));
492 /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */
493 static const s16 log_table
[] = {
528 #define LOG_TABLE_SIZE 32 /* log_table size */
529 #define LOG2_LOG_TABLE_SIZE 5 /* log2(log_table size) */
530 #define Q_LOG_TABLE 15 /* qformat of log_table */
531 #define LOG10_2 19728 /* log10(2) in q.16 */
535 This routine takes the input number N and its q format qN and compute
536 the log10(N). This routine first normalizes the input no N. Then N is in mag*(2^x) format.
537 mag is any number in the range 2^30-(2^31 - 1). Then log2(mag * 2^x) = log2(mag) + x is computed.
538 From that log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
539 This routine looks the log2 value in the table considering LOG2_LOG_TABLE_SIZE+1 MSBs.
540 As the MSB is always 1, only next LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup.
541 Next 16 MSBs are used for interpolation.
543 N - number to which log10 has to be found.
545 log10N - address where log10(N) will be written.
546 qLog10N - address where log10N qformat will be written.
548 For accurate results input should be in normalized or near normalized form.
550 void qm_log10(s32 N
, s16 qN
, s16
*log10N
, s16
*qLog10N
)
552 s16 s16norm
, s16tableIndex
, s16errorApproximation
;
556 /* normalize the N. */
557 s16norm
= qm_norm32(N
);
560 /* The qformat of N after normalization.
561 * -30 is added to treat the no as between 1.0 to 2.0
562 * i.e. after adding the -30 to the qformat the decimal point will be
563 * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e.
564 * at the right side of 30th bit.
566 qN
= qN
+ s16norm
- 30;
568 /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */
569 s16tableIndex
= (s16
) (N
>> (32 - (2 + LOG2_LOG_TABLE_SIZE
)));
571 /* remove the MSB. the MSB is always 1 after normalization. */
573 s16tableIndex
& (s16
) ((1 << LOG2_LOG_TABLE_SIZE
) - 1);
575 /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
576 N
= N
& ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE
))) - 1);
578 /* take the offset as the 16 MSBS after table index.
580 u16offset
= (u16
) (N
>> (32 - (2 + LOG2_LOG_TABLE_SIZE
+ 16)));
582 /* look the log value in the table. */
583 s32log
= log_table
[s16tableIndex
]; /* q.15 format */
585 /* interpolate using the offset. */
586 s16errorApproximation
= (s16
) qm_mulu16(u16offset
, (u16
) (log_table
[s16tableIndex
+ 1] - log_table
[s16tableIndex
])); /* q.15 */
588 s32log
= qm_add16((s16
) s32log
, s16errorApproximation
); /* q.15 format */
590 /* adjust for the qformat of the N as
591 * log2(mag * 2^x) = log2(mag) + x
593 s32log
= qm_add32(s32log
, ((s32
) -qN
) << 15); /* q.15 format */
595 /* normalize the result. */
596 s16norm
= qm_norm32(s32log
);
598 /* bring all the important bits into lower 16 bits */
599 s32log
= qm_shl32(s32log
, s16norm
- 16); /* q.15+s16norm-16 format */
601 /* compute the log10(N) by multiplying log2(N) with log10(2).
602 * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
603 * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16)
605 *log10N
= qm_muls16((s16
) s32log
, (s16
) LOG10_2
);
607 /* write the q format of the result. */
608 *qLog10N
= 15 + s16norm
- 16 + 1;
615 This routine compute 1/N.
616 This routine reformates the given no N as N * 2^qN where N is in between 0.5 and 1.0
617 in q.15 format in 16 bits. So the problem now boils down to finding the inverse of a
618 q.15 no in 16 bits which is in the range of 0.5 to 1.0. The output is always between
619 2.0 to 1. So the output is 2.0 to 1.0 in q.30 format. Once the final output format is found
620 by taking the qN into account. Inverse is found with newton rapson method. Initially
621 inverse (x) is guessed as 1/0.75 (with appropriate sign). The new guess is calculated
622 using the formula x' = 2*x - N*x*x. After 4 or 5 iterations the inverse is very close to
625 N - number to which 1/N has to be found.
627 sqrtN - address where 1/N has to be written.
628 qsqrtN - address where q format of 1/N has to be written.
631 void qm_1byN(s32 N
, s16 qN
, s32
*result
, s16
*qResult
)
634 s32 s32firstTerm
, s32secondTerm
, x
;
637 normN
= qm_norm32(N
);
639 /* limit N to least significant 16 bits. 15th bit is the sign bit. */
640 N
= qm_shl32(N
, normN
- 16);
641 qN
= qN
+ normN
- 16 - 15;
642 /* -15 is added to treat N as 16 bit q.15 number in the range from 0.5 to 1 */
644 /* Take the initial guess as 1/0.75 in qx format with appropriate sign. */
646 x
= (s32
) ((1 / 0.75) * (1 << qx
));
647 /* input no is in the range 0.5 to 1. So 1/0.75 is taken as initial guess. */
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. */
653 /* iterate the equation x = 2*x - N*x*x for 4 times. */
654 for (i
= 0; i
< 4; i
++) {
655 s32firstTerm
= qm_shl32(x
, 1); /* s32firstTerm = 2*x in q.29 */
657 qm_muls321616((s16
) (s32firstTerm
>> 16),
658 (s16
) (s32firstTerm
>> 16));
659 /* s32secondTerm = x*x in q.(29+1-16)*2+1 */
661 qm_muls321616((s16
) (s32secondTerm
>> 16), (s16
) N
);
662 /* s32secondTerm = N*x*x in q.((29+1-16)*2+1)-16+15+1 i.e. in q.29 */
663 x
= qm_sub32(s32firstTerm
, s32secondTerm
);
664 /* can be added directly as both are in q.29 */
667 /* Bring the x to q.30 format. */
668 *result
= qm_shl32(x
, 1);
669 /* giving the output in q.30 format for q.15 input in 16 bits. */
671 /* compute the final q format of the result. */
672 *qResult
= -qN
+ 30; /* adjusting the q format of actual output */