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 */ 13u16 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 */ 26s16 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 */ 42s32 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 */ 59s16 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 */ 78s16 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 */ 98s32 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 */ 123s16 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 */ 147s16 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 */ 156s16 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 */ 172static 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 */ 232void 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} 299