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 */ 24u16 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 */ 37s16 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 */ 53s32 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 */ 70s16 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 */ 89s16 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 */ 109s32 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 */ 134s16 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 */ 158s16 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 */ 167s16 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:31], in q.15 format */ 183static 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}; 217 218#define LOG_TABLE_SIZE 32 /* log_table size */ 219#define LOG2_LOG_TABLE_SIZE 5 /* log2(log_table size) */ 220#define Q_LOG_TABLE 15 /* qformat of log_table */ 221#define LOG10_2 19728 /* log10(2) in q.16 */ 222 223/* 224 * Description: 225 * This routine takes the input number N and its q format qN and compute 226 * the log10(N). This routine first normalizes the input no N. Then N is in 227 * mag*(2^x) format. mag is any number in the range 2^30-(2^31 - 1). 228 * Then log2(mag * 2^x) = log2(mag) + x is computed. From that 229 * log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed. 230 * This routine looks the log2 value in the table considering 231 * LOG2_LOG_TABLE_SIZE+1 MSBs. As the MSB is always 1, only next 232 * LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. Next 16 MSBs are used 233 * for interpolation. 234 * Inputs: 235 * N - number to which log10 has to be found. 236 * qN - q format of N 237 * log10N - address where log10(N) will be written. 238 * qLog10N - address where log10N qformat will be written. 239 * Note/Problem: 240 * For accurate results input should be in normalized or near normalized form. 241 */ 242void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N) 243{ 244 s16 s16norm, s16tableIndex, s16errorApproximation; 245 u16 u16offset; 246 s32 s32log; 247 248 /* normalize the N. */ 249 s16norm = qm_norm32(N); 250 N = N << s16norm; 251 252 /* The qformat of N after normalization. 253 * -30 is added to treat the no as between 1.0 to 2.0 254 * i.e. after adding the -30 to the qformat the decimal point will be 255 * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e. 256 * at the right side of 30th bit. 257 */ 258 qN = qN + s16norm - 30; 259 260 /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the 261 * MSB */ 262 s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE))); 263 264 /* remove the MSB. the MSB is always 1 after normalization. */ 265 s16tableIndex = 266 s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1); 267 268 /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */ 269 N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1); 270 271 /* take the offset as the 16 MSBS after table index. 272 */ 273 u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16))); 274 275 /* look the log value in the table. */ 276 s32log = log_table[s16tableIndex]; /* q.15 format */ 277 278 /* interpolate using the offset. q.15 format. */ 279 s16errorApproximation = (s16) qm_mulu16(u16offset, 280 (u16) (log_table[s16tableIndex + 1] - 281 log_table[s16tableIndex])); 282 283 /* q.15 format */ 284 s32log = qm_add16((s16) s32log, s16errorApproximation); 285 286 /* adjust for the qformat of the N as 287 * log2(mag * 2^x) = log2(mag) + x 288 */ 289 s32log = qm_add32(s32log, ((s32) -qN) << 15); /* q.15 format */ 290 291 /* normalize the result. */ 292 s16norm = qm_norm32(s32log); 293 294 /* bring all the important bits into lower 16 bits */ 295 /* q.15+s16norm-16 format */ 296 s32log = qm_shl32(s32log, s16norm - 16); 297 298 /* compute the log10(N) by multiplying log2(N) with log10(2). 299 * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2) 300 * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16) 301 */ 302 *log10N = qm_muls16((s16) s32log, (s16) LOG10_2); 303 304 /* write the q format of the result. */ 305 *qLog10N = 15 + s16norm - 16 + 1; 306 307 return; 308} 309