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