brcmsmac/phy/phy_qmath.c

0001 // SPDX-License-Identifier: ISC
0002 /*
0003  * Copyright (c) 2010 Broadcom Corporation
0004  */
0005
0006 #include "phy_qmath.h"
0007
0008 /*
0009  * Description: This function make 16 bit unsigned multiplication.
0010  * To fit the output into 16 bits the 32 bit multiplication result is right
0011  * shifted by 16 bits.
0012  */
0013 u16 qm_mulu16(u16 op1, u16 op2)
0014 {
0015     return (u16) (((u32) op1 * (u32) op2) >> 16);
0016 }
0017
0018 /*
0019  * Description: This function make 16 bit multiplication and return the result
0020  * in 16 bits. To fit the multiplication result into 16 bits the multiplication
0021  * result is right shifted by 15 bits. Right shifting 15 bits instead of 16 bits
0022  * is done to remove the extra sign bit formed due to the multiplication.
0023  * When both the 16bit inputs are 0x8000 then the output is saturated to
0024  * 0x7fffffff.
0025  */
0026 s16 qm_muls16(s16 op1, s16 op2)
0027 {
0028     s32 result;
0029     if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000)
0030         result = 0x7fffffff;
0031     else
0032         result = ((s32) (op1) * (s32) (op2));
0033
0034     return (s16) (result >> 15);
0035 }
0036
0037 /*
0038  * Description: This function add two 32 bit numbers and return the 32bit
0039  * result. If the result overflow 32 bits, the output will be saturated to
0040  * 32bits.
0041  */
0042 s32 qm_add32(s32 op1, s32 op2)
0043 {
0044     s32 result;
0045     result = op1 + op2;
0046     if (op1 < 0 && op2 < 0 && result > 0)
0047         result = 0x80000000;
0048     else if (op1 > 0 && op2 > 0 && result < 0)
0049         result = 0x7fffffff;
0050
0051     return result;
0052 }
0053
0054 /*
0055  * Description: This function add two 16 bit numbers and return the 16bit
0056  * result. If the result overflow 16 bits, the output will be saturated to
0057  * 16bits.
0058  */
0059 s16 qm_add16(s16 op1, s16 op2)
0060 {
0061     s16 result;
0062     s32 temp = (s32) op1 + (s32) op2;
0063     if (temp > (s32) 0x7fff)
0064         result = (s16) 0x7fff;
0065     else if (temp < (s32) 0xffff8000)
0066         result = (s16) 0xffff8000;
0067     else
0068         result = (s16) temp;
0069
0070     return result;
0071 }
0072
0073 /*
0074  * Description: This function make 16 bit subtraction and return the 16bit
0075  * result. If the result overflow 16 bits, the output will be saturated to
0076  * 16bits.
0077  */
0078 s16 qm_sub16(s16 op1, s16 op2)
0079 {
0080     s16 result;
0081     s32 temp = (s32) op1 - (s32) op2;
0082     if (temp > (s32) 0x7fff)
0083         result = (s16) 0x7fff;
0084     else if (temp < (s32) 0xffff8000)
0085         result = (s16) 0xffff8000;
0086     else
0087         result = (s16) temp;
0088
0089     return result;
0090 }
0091
0092 /*
0093  * Description: This function make a 32 bit saturated left shift when the
0094  * specified shift is +ve. This function will make a 32 bit right shift when
0095  * the specified shift is -ve. This function return the result after shifting
0096  * operation.
0097  */
0098 s32 qm_shl32(s32 op, int shift)
0099 {
0100     int i;
0101     s32 result;
0102     result = op;
0103     if (shift > 31)
0104         shift = 31;
0105     else if (shift < -31)
0106         shift = -31;
0107     if (shift >= 0) {
0108         for (i = 0; i < shift; i++)
0109             result = qm_add32(result, result);
0110     } else {
0111         result = result >> (-shift);
0112     }
0113
0114     return result;
0115 }
0116
0117 /*
0118  * Description: This function make a 16 bit saturated left shift when the
0119  * specified shift is +ve. This function will make a 16 bit right shift when
0120  * the specified shift is -ve. This function return the result after shifting
0121  * operation.
0122  */
0123 s16 qm_shl16(s16 op, int shift)
0124 {
0125     int i;
0126     s16 result;
0127     result = op;
0128     if (shift > 15)
0129         shift = 15;
0130     else if (shift < -15)
0131         shift = -15;
0132     if (shift > 0) {
0133         for (i = 0; i < shift; i++)
0134             result = qm_add16(result, result);
0135     } else {
0136         result = result >> (-shift);
0137     }
0138
0139     return result;
0140 }
0141
0142 /*
0143  * Description: This function make a 16 bit right shift when shift is +ve.
0144  * This function make a 16 bit saturated left shift when shift is -ve. This
0145  * function return the result of the shift operation.
0146  */
0147 s16 qm_shr16(s16 op, int shift)
0148 {
0149     return qm_shl16(op, -shift);
0150 }
0151
0152 /*
0153  * Description: This function return the number of redundant sign bits in a
0154  * 32 bit number. Example: qm_norm32(0x00000080) = 23
0155  */
0156 s16 qm_norm32(s32 op)
0157 {
0158     u16 u16extraSignBits;
0159     if (op == 0) {
0160         return 31;
0161     } else {
0162         u16extraSignBits = 0;
0163         while ((op >> 31) == (op >> 30)) {
0164             u16extraSignBits++;
0165             op = op << 1;
0166         }
0167     }
0168     return u16extraSignBits;
0169 }
0170
0171 /* This table is log2(1+(i/32)) where i=[0:1:32], in q.15 format */
0172 static const s16 log_table[] = {
0173     0,
0174     1455,
0175     2866,
0176     4236,
0177     5568,
0178     6863,
0179     8124,
0180     9352,
0181     10549,
0182     11716,
0183     12855,
0184     13968,
0185     15055,
0186     16117,
0187     17156,
0188     18173,
0189     19168,
0190     20143,
0191     21098,
0192     22034,
0193     22952,
0194     23852,
0195     24736,
0196     25604,
0197     26455,
0198     27292,
0199     28114,
0200     28922,
0201     29717,
0202     30498,
0203     31267,
0204     32024,
0205     32767
0206 };
0207
0208 #define LOG_TABLE_SIZE 32       /* log_table size */
0209 #define LOG2_LOG_TABLE_SIZE 5   /* log2(log_table size) */
0210 #define Q_LOG_TABLE 15          /* qformat of log_table */
0211 #define LOG10_2         19728   /* log10(2) in q.16 */
0212
0213 /*
0214  * Description:
0215  * This routine takes the input number N and its q format qN and compute
0216  * the log10(N). This routine first normalizes the input no N.  Then N is in
0217  * mag*(2^x) format. mag is any number in the range 2^30-(2^31 - 1).
0218  * Then log2(mag * 2^x) = log2(mag) + x is computed. From that
0219  * log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
0220  * This routine looks the log2 value in the table considering
0221  * LOG2_LOG_TABLE_SIZE+1 MSBs. As the MSB is always 1, only next
0222  * LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. Next 16 MSBs are used
0223  * for interpolation.
0224  * Inputs:
0225  * N - number to which log10 has to be found.
0226  * qN - q format of N
0227  * log10N - address where log10(N) will be written.
0228  * qLog10N - address where log10N qformat will be written.
0229  * Note/Problem:
0230  * For accurate results input should be in normalized or near normalized form.
0231  */
0232 void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
0233 {
0234     s16 s16norm, s16tableIndex, s16errorApproximation;
0235     u16 u16offset;
0236     s32 s32log;
0237
0238     /* normalize the N. */
0239     s16norm = qm_norm32(N);
0240     N = N << s16norm;
0241
0242     /* The qformat of N after normalization.
0243      * -30 is added to treat the no as between 1.0 to 2.0
0244      * i.e. after adding the -30 to the qformat the decimal point will be
0245      * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e.
0246      * at the right side of 30th bit.
0247      */
0248     qN = qN + s16norm - 30;
0249
0250     /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the
0251      * MSB */
0252     s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE)));
0253
0254     /* remove the MSB. the MSB is always 1 after normalization. */
0255     s16tableIndex =
0256         s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
0257
0258     /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
0259     N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1);
0260
0261     /* take the offset as the 16 MSBS after table index.
0262      */
0263     u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16)));
0264
0265     /* look the log value in the table. */
0266     s32log = log_table[s16tableIndex];      /* q.15 format */
0267
0268     /* interpolate using the offset. q.15 format. */
0269     s16errorApproximation = (s16) qm_mulu16(u16offset,
0270                 (u16) (log_table[s16tableIndex + 1] -
0271                        log_table[s16tableIndex]));
0272
0273      /* q.15 format */
0274     s32log = qm_add16((s16) s32log, s16errorApproximation);
0275
0276     /* adjust for the qformat of the N as
0277      * log2(mag * 2^x) = log2(mag) + x
0278      */
0279     s32log = qm_add32(s32log, ((s32) -qN) << 15);   /* q.15 format */
0280
0281     /* normalize the result. */
0282     s16norm = qm_norm32(s32log);
0283
0284     /* bring all the important bits into lower 16 bits */
0285     /* q.15+s16norm-16 format */
0286     s32log = qm_shl32(s32log, s16norm - 16);
0287
0288     /* compute the log10(N) by multiplying log2(N) with log10(2).
0289      * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
0290      * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16)
0291      */
0292     *log10N = qm_muls16((s16) s32log, (s16) LOG10_2);
0293
0294     /* write the q format of the result. */
0295     *qLog10N = 15 + s16norm - 16 + 1;
0296
0297     return;
0298 }