drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_qmath.c

   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 "phy_qmath.h"
  18
  19 /*
  20  * Description: This function make 16 bit unsigned multiplication.
  21  * To fit the output into 16 bits the 32 bit multiplication result is right
  22  * shifted by 16 bits.
  23  */
  24 u16 qm_mulu16(u16 op1, u16 op2)
  25 {
  26         return (u16) (((u32) op1 * (u32) op2) >> 16);
  27 }
  28
  29 /*
  30  * Description: This function make 16 bit multiplication and return the result
  31  * in 16 bits. To fit the multiplication result into 16 bits the multiplication
  32  * result is right shifted by 15 bits. Right shifting 15 bits instead of 16 bits
  33  * is done to remove the extra sign bit formed due to the multiplication.
  34  * When both the 16bit inputs are 0x8000 then the output is saturated to
  35  * 0x7fffffff.
  36  */
  37 s16 qm_muls16(s16 op1, s16 op2)
  38 {
  39         s32 result;
  40         if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000)
  41                 result = 0x7fffffff;
  42         else
  43                 result = ((s32) (op1) * (s32) (op2));
  44
  45         return (s16) (result >> 15);
  46 }
  47
  48 /*
  49  * Description: This function add two 32 bit numbers and return the 32bit
  50  * result. If the result overflow 32 bits, the output will be saturated to
  51  * 32bits.
  52  */
  53 s32 qm_add32(s32 op1, s32 op2)
  54 {
  55         s32 result;
  56         result = op1 + op2;
  57         if (op1 < 0 && op2 < 0 && result > 0)
  58                 result = 0x80000000;
  59         else if (op1 > 0 && op2 > 0 && result < 0)
  60                 result = 0x7fffffff;
  61
  62         return result;
  63 }
  64
  65 /*
  66  * Description: This function add two 16 bit numbers and return the 16bit
  67  * result. If the result overflow 16 bits, the output will be saturated to
  68  * 16bits.
  69  */
  70 s16 qm_add16(s16 op1, s16 op2)
  71 {
  72         s16 result;
  73         s32 temp = (s32) op1 + (s32) op2;
  74         if (temp > (s32) 0x7fff)
  75                 result = (s16) 0x7fff;
  76         else if (temp < (s32) 0xffff8000)
  77                 result = (s16) 0xffff8000;
  78         else
  79                 result = (s16) temp;
  80
  81         return result;
  82 }
  83
  84 /*
  85  * Description: This function make 16 bit subtraction and return the 16bit
  86  * result. If the result overflow 16 bits, the output will be saturated to
  87  * 16bits.
  88  */
  89 s16 qm_sub16(s16 op1, s16 op2)
  90 {
  91         s16 result;
  92         s32 temp = (s32) op1 - (s32) op2;
  93         if (temp > (s32) 0x7fff)
  94                 result = (s16) 0x7fff;
  95         else if (temp < (s32) 0xffff8000)
  96                 result = (s16) 0xffff8000;
  97         else
  98                 result = (s16) temp;
  99
 100         return result;
 101 }
 102
 103 /*
 104  * Description: This function make a 32 bit saturated left shift when the
 105  * specified shift is +ve. This function will make a 32 bit right shift when
 106  * the specified shift is -ve. This function return the result after shifting
 107  * operation.
 108  */
 109 s32 qm_shl32(s32 op, int shift)
 110 {
 111         int i;
 112         s32 result;
 113         result = op;
 114         if (shift > 31)
 115                 shift = 31;
 116         else if (shift < -31)
 117                 shift = -31;
 118         if (shift >= 0) {
 119                 for (i = 0; i < shift; i++)
 120                         result = qm_add32(result, result);
 121         } else {
 122                 result = result >> (-shift);
 123         }
 124
 125         return result;
 126 }
 127
 128 /*
 129  * Description: This function make a 16 bit saturated left shift when the
 130  * specified shift is +ve. This function will make a 16 bit right shift when
 131  * the specified shift is -ve. This function return the result after shifting
 132  * operation.
 133  */
 134 s16 qm_shl16(s16 op, int shift)
 135 {
 136         int i;
 137         s16 result;
 138         result = op;
 139         if (shift > 15)
 140                 shift = 15;
 141         else if (shift < -15)
 142                 shift = -15;
 143         if (shift > 0) {
 144                 for (i = 0; i < shift; i++)
 145                         result = qm_add16(result, result);
 146         } else {
 147                 result = result >> (-shift);
 148         }
 149
 150         return result;
 151 }
 152
 153 /*
 154  * Description: This function make a 16 bit right shift when shift is +ve.
 155  * This function make a 16 bit saturated left shift when shift is -ve. This
 156  * function return the result of the shift operation.
 157  */
 158 s16 qm_shr16(s16 op, int shift)
 159 {
 160         return qm_shl16(op, -shift);
 161 }
 162
 163 /*
 164  * Description: This function return the number of redundant sign bits in a
 165  * 32 bit number. Example: qm_norm32(0x00000080) = 23
 166  */
 167 s16 qm_norm32(s32 op)
 168 {
 169         u16 u16extraSignBits;
 170         if (op == 0) {
 171                 return 31;
 172         } else {
 173                 u16extraSignBits = 0;
 174                 while ((op >> 31) == (op >> 30)) {
 175                         u16extraSignBits++;
 176                         op = op << 1;
 177                 }
 178         }
 179         return u16extraSignBits;
 180 }
 181
 182 /* This table is log2(1+(i/32)) where i=[0:1:32], in q.15 format */
 183 static const s16 log_table[] = {
 184         0,
 185         1455,
 186         2866,
 187         4236,
 188         5568,
 189         6863,
 190         8124,
 191         9352,
 192         10549,
 193         11716,
 194         12855,
 195         13968,
 196         15055,
 197         16117,
 198         17156,
 199         18173,
 200         19168,
 201         20143,
 202         21098,
 203         22034,
 204         22952,
 205         23852,
 206         24736,
 207         25604,
 208         26455,
 209         27292,
 210         28114,
 211         28922,
 212         29717,
 213         30498,
 214         31267,
 215         32024,
 216         32767
 217 };
 218
 219 #define LOG_TABLE_SIZE 32       /* log_table size */
 220 #define LOG2_LOG_TABLE_SIZE 5   /* log2(log_table size) */
 221 #define Q_LOG_TABLE 15          /* qformat of log_table */
 222 #define LOG10_2         19728   /* log10(2) in q.16 */
 223
 224 /*
 225  * Description:
 226  * This routine takes the input number N and its q format qN and compute
 227  * the log10(N). This routine first normalizes the input no N.  Then N is in
 228  * mag*(2^x) format. mag is any number in the range 2^30-(2^31 - 1).
 229  * Then log2(mag * 2^x) = log2(mag) + x is computed. From that
 230  * log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
 231  * This routine looks the log2 value in the table considering
 232  * LOG2_LOG_TABLE_SIZE+1 MSBs. As the MSB is always 1, only next
 233  * LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. Next 16 MSBs are used
 234  * for interpolation.
 235  * Inputs:
 236  * N - number to which log10 has to be found.
 237  * qN - q format of N
 238  * log10N - address where log10(N) will be written.
 239  * qLog10N - address where log10N qformat will be written.
 240  * Note/Problem:
 241  * For accurate results input should be in normalized or near normalized form.
 242  */
 243 void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
 244 {
 245         s16 s16norm, s16tableIndex, s16errorApproximation;
 246         u16 u16offset;
 247         s32 s32log;
 248
 249         /* normalize the N. */
 250         s16norm = qm_norm32(N);
 251         N = N << s16norm;
 252
 253         /* The qformat of N after normalization.
 254          * -30 is added to treat the no as between 1.0 to 2.0
 255          * i.e. after adding the -30 to the qformat the decimal point will be
 256          * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e.
 257          * at the right side of 30th bit.
 258          */
 259         qN = qN + s16norm - 30;
 260
 261         /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the
 262          * MSB */
 263         s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE)));
 264
 265         /* remove the MSB. the MSB is always 1 after normalization. */
 266         s16tableIndex =
 267                 s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
 268
 269         /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
 270         N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1);
 271
 272         /* take the offset as the 16 MSBS after table index.
 273          */
 274         u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16)));
 275
 276         /* look the log value in the table. */
 277         s32log = log_table[s16tableIndex];      /* q.15 format */
 278
 279         /* interpolate using the offset. q.15 format. */
 280         s16errorApproximation = (s16) qm_mulu16(u16offset,
 281                                 (u16) (log_table[s16tableIndex + 1] -
 282                                        log_table[s16tableIndex]));
 283
 284          /* q.15 format */
 285         s32log = qm_add16((s16) s32log, s16errorApproximation);
 286
 287         /* adjust for the qformat of the N as
 288          * log2(mag * 2^x) = log2(mag) + x
 289          */
 290         s32log = qm_add32(s32log, ((s32) -qN) << 15);   /* q.15 format */
 291
 292         /* normalize the result. */
 293         s16norm = qm_norm32(s32log);
 294
 295         /* bring all the important bits into lower 16 bits */
 296         /* q.15+s16norm-16 format */
 297         s32log = qm_shl32(s32log, s16norm - 16);
 298
 299         /* compute the log10(N) by multiplying log2(N) with log10(2).
 300          * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
 301          * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16)
 302          */
 303         *log10N = qm_muls16((s16) s32log, (s16) LOG10_2);
 304
 305         /* write the q format of the result. */
 306         *qLog10N = 15 + s16norm - 16 + 1;
 307
 308         return;
 309 }