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

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