staging: brcm80211: move qmath sources to phy directory
[linux-2.6/linux-acpi-2.6/ibm-acpi-2.6.git] / drivers / staging / brcm80211 / brcmsmac / phy / wlc_phy_qmath.c
blob06172921a79b950d2379dd766fc889b7e493f252
1 /*
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.
27 s16 qm_sat32(s32 op)
29 s16 result;
30 if (op > (s32) 0x7fff) {
31 result = 0x7fff;
32 } else if (op < (s32) 0xffff8000) {
33 result = (s16) (0x8000);
34 } else {
35 result = (s16) op;
37 return result;
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
54 shifted by 16 bits.
56 s16 qm_mul16(s16 op1, s16 op2)
58 s32 result;
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)
72 s32 result;
73 if (op1 == (s16) (0x8000) && op2 == (s16) (0x8000)) {
74 result = 0x7fffffff;
75 } else {
76 result = ((s32) (op1) * (s32) (op2));
77 result = result << 1;
79 return result;
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)
100 s32 result;
101 if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000) {
102 result = 0x7fffffff;
103 } else {
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)
115 s32 result;
116 result = op1 + op2;
117 if (op1 < 0 && op2 < 0 && result > 0) {
118 result = 0x80000000;
119 } else if (op1 > 0 && op2 > 0 && result < 0) {
120 result = 0x7fffffff;
122 return result;
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)
131 s16 result;
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;
137 } else {
138 result = (s16) temp;
140 return result;
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)
149 s16 result;
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;
155 } else {
156 result = (s16) temp;
158 return result;
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)
167 s32 result;
168 result = op1 - op2;
169 if (op1 >= 0 && op2 < 0 && result < 0) {
170 result = 0x7fffffff;
171 } else if (op1 < 0 && op2 > 0 && result > 0) {
172 result = 0x80000000;
174 return result;
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)
184 s32 result;
185 result = qm_add32(acc, qm_mul321616(op1, op2));
186 return result;
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)
196 int i;
197 s32 result;
198 result = op;
199 if (shift > 31)
200 shift = 31;
201 else if (shift < -31)
202 shift = -31;
203 if (shift >= 0) {
204 for (i = 0; i < shift; i++) {
205 result = qm_add32(result, result);
207 } else {
208 result = result >> (-shift);
210 return result;
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)
230 int i;
231 s16 result;
232 result = op;
233 if (shift > 15)
234 shift = 15;
235 else if (shift < -15)
236 shift = -15;
237 if (shift > 0) {
238 for (i = 0; i < shift; i++) {
239 result = qm_add16(result, result);
241 } else {
242 result = result >> (-shift);
244 return result;
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;
264 if (op == 0) {
265 return 15;
266 } else {
267 u16extraSignBits = 0;
268 while ((op >> 15) == (op >> 14)) {
269 u16extraSignBits++;
270 op = op << 1;
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;
283 if (op == 0) {
284 return 31;
285 } else {
286 u16extraSignBits = 0;
287 while ((op >> 31) == (op >> 30)) {
288 u16extraSignBits++;
289 op = op << 1;
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)
302 s16 var_out;
303 s16 iteration;
304 s32 L_num;
305 s32 L_denom;
306 L_num = (num) << 15;
307 L_denom = (denom) << 15;
308 for (iteration = 0; iteration < 15; iteration++) {
309 L_num <<= 1;
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);
316 return var_out;
320 Description: This function compute the absolute value of a 16 bit number.
322 s16 qm_abs16(s16 op)
324 if (op < 0) {
325 if (op == (s16) 0xffff8000) {
326 return 0x7fff;
327 } else {
328 return -op;
330 } else {
331 return op;
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)
344 s16 sign;
345 s16 nNum, nDenom;
346 sign = num ^ denom;
347 num = qm_abs16(num);
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;
354 if (sign >= 0) {
355 return qm_div_s(num, denom);
356 } else {
357 return -qm_div_s(num, denom);
362 Description: This function compute absolute value of a 32 bit number.
364 s32 qm_abs32(s32 op)
366 if (op < 0) {
367 if (op == (s32) 0x80000000) {
368 return 0x7fffffff;
369 } else {
370 return -op;
372 } else {
373 return op;
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)
387 s32 sign;
388 s16 nNum, nDenom;
389 sign = num ^ denom;
390 num = qm_abs32(num);
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;
397 if (sign >= 0) {
398 return qm_div_s((s16) (num >> 16), (s16) (denom >> 16));
399 } else {
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
407 into 32 bit output.
409 s32 qm_mul323216(s32 op1, s16 op2)
411 s16 hi;
412 u16 lo;
413 s32 result;
414 hi = op1 >> 16;
415 lo = (s16) (op1 & 0xffff);
416 result = qm_mul321616(hi, op2);
417 result = result + (qm_mulsu321616(op2, lo) >> 16);
418 return result;
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)
438 s16 hi;
439 u16 lo;
440 s32 result;
441 hi = op1 >> 16;
442 lo = (s16) (op1 & 0xffff);
443 result = qm_muls321616(hi, op2);
444 result = qm_add32(result, (qm_mulsu321616(op2, lo) >> 15));
445 return result;
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)
455 s16 hi1, hi2;
456 u16 lo1, lo2;
457 s32 result;
458 hi1 = a >> 16;
459 hi2 = b >> 16;
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);
465 return result;
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
474 0x7fffffff.
476 s32 qm_muls32(s32 a, s32 b)
478 s16 hi1, hi2;
479 u16 lo1, lo2;
480 s32 result;
481 hi1 = a >> 16;
482 hi2 = b >> 16;
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));
489 return result;
492 /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */
493 static const s16 log_table[] = {
495 1455,
496 2866,
497 4236,
498 5568,
499 6863,
500 8124,
501 9352,
502 10549,
503 11716,
504 12855,
505 13968,
506 15055,
507 16117,
508 17156,
509 18173,
510 19168,
511 20143,
512 21098,
513 22034,
514 22952,
515 23852,
516 24736,
517 25604,
518 26455,
519 27292,
520 28114,
521 28922,
522 29717,
523 30498,
524 31267,
525 32024
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 */
534 Description:
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.
542 Inputs:
543 N - number to which log10 has to be found.
544 qN - q format of N
545 log10N - address where log10(N) will be written.
546 qLog10N - address where log10N qformat will be written.
547 Note/Problem:
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;
553 u16 u16offset;
554 s32 s32log;
556 /* normalize the N. */
557 s16norm = qm_norm32(N);
558 N = N << s16norm;
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. */
572 s16tableIndex =
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;
610 return;
614 Description:
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
623 inverse of N.
624 Inputs:
625 N - number to which 1/N has to be found.
626 qn - q format of N.
627 sqrtN - address where 1/N has to be written.
628 qsqrtN - address where q format of 1/N has to be written.
630 #define qx 29
631 void qm_1byN(s32 N, s16 qN, s32 *result, s16 *qResult)
633 s16 normN;
634 s32 s32firstTerm, s32secondTerm, x;
635 int i;
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. */
645 if (N >= 0) {
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. */
648 } else {
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 */
656 s32secondTerm =
657 qm_muls321616((s16) (s32firstTerm >> 16),
658 (s16) (s32firstTerm >> 16));
659 /* s32secondTerm = x*x in q.(29+1-16)*2+1 */
660 s32secondTerm =
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 */
674 return;
677 #undef qx