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