Line data Source code
1 : /*___________________________________________________________________________
2 : | |
3 : | This file contains mathematic operations in fixed point. |
4 : | |
5 : | Isqrt() : inverse square root (16 bits precision). |
6 : | Pow2() : 2^x (16 bits precision). |
7 : | Log2() : log2 (16 bits precision). |
8 : | Dot_product() : scalar product of <x[],y[]> |
9 : | |
10 : | In this file, the values use theses representations: |
11 : | |
12 : | Word32 L_32 : standard signed 32 bits format |
13 : | Word16 hi, lo : L_32 = hi<<16 + lo<<1 (DPF - Double Precision Format) |
14 : | Word32 frac, Word16 exp : L_32 = frac << exp-31 (normalised format) |
15 : | Word16 int, frac : L_32 = int.frac (fractional format) |
16 : |___________________________________________________________________________|
17 : */
18 :
19 : #include "stl.h"
20 : #include "math_op.h"
21 : #include "rom_basic_math.h"
22 :
23 : #include <stdlib.h>
24 : #include <stdio.h>
25 :
26 : /*___________________________________________________________________________
27 : | |
28 : | Function Name : Isqrt |
29 : | |
30 : | Compute 1/sqrt(L_x). |
31 : | if L_x is negative or zero, result is 1 (7fffffff). |
32 : |---------------------------------------------------------------------------|
33 : | Algorithm: |
34 : | |
35 : | 1- Normalization of L_x. |
36 : | 2- call Isqrt_lc(L_x, exponant) |
37 : | 3- L_y = L_x << exponant |
38 : |___________________________________________________________________________|
39 : */
40 6124659 : Word32 Isqrt( /* (o) Q31 : output value (range: 0<=val<1) */
41 : Word32 L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */
42 : )
43 : {
44 : Word16 exp;
45 : Word32 L_y;
46 :
47 6124659 : exp = norm_l( L_x );
48 6124659 : L_x = L_shl( L_x, exp ); /* L_x is normalized */
49 6124659 : exp = sub( 31, exp );
50 :
51 6124659 : L_x = Isqrt_lc( L_x, &exp );
52 :
53 6124659 : L_y = L_shl( L_x, exp ); /* denormalization */
54 :
55 6124659 : return ( L_y );
56 : }
57 :
58 : /*___________________________________________________________________________
59 : | |
60 : | Function Name : Isqrt_lc |
61 : | |
62 : | Compute 1/sqrt(value). |
63 : | if value is negative or zero, result is 1 (frac=7fffffff, exp=0). |
64 : |---------------------------------------------------------------------------|
65 : | Algorithm: |
66 : | |
67 : | The function 1/sqrt(value) is approximated by a table and linear |
68 : | interpolation. |
69 : | |
70 : | 1- If exponant is odd then shift fraction right once. |
71 : | 2- exponant = -((exponant-1)>>1) |
72 : | 3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization. |
73 : | 4- a = bit10-b24 |
74 : | 5- i -=16 |
75 : | 6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2 |
76 : |___________________________________________________________________________|
77 : */
78 42403558 : Word32 Isqrt_lc(
79 : Word32 frac, /* (i) Q31: normalized value (1.0 < frac <= 0.5) */
80 : Word16 *exp /* (i/o) : exponent (value = frac x 2^exponent) */
81 : )
82 : {
83 : Word16 i, a;
84 : Word32 L_tmp;
85 :
86 42403558 : IF( frac <= (Word32) 0 )
87 : {
88 141258 : *exp = 0;
89 141258 : move16();
90 141258 : return 0x7fffffff; /*0x7fffffff*/
91 : }
92 :
93 : /* If exponant odd -> shift right by 10 (otherwise 9) */
94 42262300 : L_tmp = L_shr( frac, shift_Isqrt_lc[s_and( *exp, 1 )] );
95 :
96 : /* 1) -16384 to shift left and change sign */
97 : /* 2) 32768 to Add 1 to Exponent like it was divided by 2 */
98 : /* 3) We let the mac_r add another 0.5 because it imitates */
99 : /* the behavior of shr on negative number that should */
100 : /* not be rounded towards negative infinity. */
101 : /* It replaces: */
102 : /* *exp = negate(shr(sub(*exp, 1), 1)); move16(); */
103 42262300 : *exp = mac_r( 32768, *exp, -16384 );
104 42262300 : move16();
105 :
106 42262300 : a = extract_l( L_tmp ); /* Extract b10-b24 */
107 42262300 : a = lshr( a, 1 );
108 :
109 42262300 : i = mac_r( L_tmp, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
110 :
111 42262300 : L_tmp = L_msu( L_table_isqrt[i], table_isqrt_diff[i], a ); /* table[i] << 16 - diff*a*2 */
112 :
113 42262300 : return L_tmp;
114 : }
115 :
116 : /*___________________________________________________________________________
117 : | |
118 : | Function Name : Pow2() |
119 : | |
120 : | L_x = pow(2.0, exponant.fraction) (exponant = interger part) |
121 : | = pow(2.0, 0.fraction) << exponant |
122 : |---------------------------------------------------------------------------|
123 : | Algorithm: |
124 : | |
125 : | The function Pow2(L_x) is approximated by a table and linear |
126 : | interpolation. |
127 : | |
128 : | 1- i = bit10-b15 of fraction, 0 <= i <= 31 |
129 : | 2- a = bit0-b9 of fraction |
130 : | 3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2 |
131 : | 4- L_x = L_x >> (30-exponant) (with rounding) |
132 : |___________________________________________________________________________|
133 : */
134 :
135 10119814 : Word32 Pow2( /* (o) Q0 : result (range: 0<=val<=0x7fffffff) */
136 : Word16 exponant, /* (i) Q0 : Integer part. (range: 0<=val<=30) */
137 : Word16 fraction /* (i) Q15 : Fractionnal part. (range: 0.0<=val<1.0) */
138 : )
139 : {
140 : Word16 exp, i, a;
141 : Word32 L_x;
142 :
143 :
144 10119814 : i = mac_r( -32768, fraction, 32 ); /* Extract b10-b16 of fraction */
145 10119814 : a = s_and( fraction, 0x3ff ); /* Extract b0-b9 of fraction */
146 :
147 10119814 : L_x = L_deposit_h( table_pow2[i] ); /* table[i] << 16 */
148 10119814 : L_x = L_mac( L_x, table_pow2_diff_x32[i], a ); /* L_x -= diff*a*2 */
149 :
150 10119814 : exp = sub( 30, exponant );
151 :
152 10119814 : L_x = L_shr_r( L_x, exp );
153 :
154 :
155 10119814 : return L_x;
156 : }
157 :
158 : /*___________________________________________________________________________
159 : | |
160 : | Function Name : Dot_product12() |
161 : | |
162 : | Compute scalar product of <x[],y[]> using accumulator. |
163 : | |
164 : | The result is normalized (in Q31) with exponent (0..30). |
165 : |---------------------------------------------------------------------------|
166 : | Algorithm: |
167 : | |
168 : | dot_product = sum(x[i]*y[i]) i=0..N-1 |
169 : |___________________________________________________________________________|
170 : */
171 :
172 11576892 : Word32 Dot_product12_o( /* (o) Q31: normalized result (1 < val <= -1) */
173 : const Word16 x[], /* (i) 12bits: x vector */
174 : const Word16 y[], /* (i) 12bits: y vector */
175 : const Word16 lg, /* (i) : vector length */
176 : Word16 *exp, /* (o) : exponent of result (0..+30) */
177 : Flag *Overflow_out /* o : propagating the Overflow flag to upper level, set to NULL to ignore internal overflows */
178 : )
179 : {
180 : Word16 i, sft;
181 : Word32 L_sum;
182 11576892 : Flag Overflow_ignored = 0;
183 :
184 11576892 : L_sum = L_mac_o( 1, x[0], y[0], &Overflow_ignored );
185 779805382 : FOR( i = 1; i < lg; i++ )
186 : {
187 768228490 : L_sum = L_mac_o( L_sum, x[i], y[i], Overflow_out ? Overflow_out : &Overflow_ignored );
188 : }
189 :
190 : /* Normalize acc in Q31 */
191 :
192 11576892 : sft = norm_l( L_sum );
193 11576892 : L_sum = L_shl( L_sum, sft );
194 :
195 11576892 : *exp = sub( 30, sft );
196 11576892 : move16(); /* exponent = 0..30 */
197 :
198 11576892 : return L_sum;
199 : }
200 :
201 9233137 : Word32 Dot_product12( /* (o) Q31: normalized result (1 < val <= -1) */
202 : const Word16 x[], /* (i) 12bits: x vector */
203 : const Word16 y[], /* (i) 12bits: y vector */
204 : const Word16 lg, /* (i) : vector length */
205 : Word16 *exp /* (o) : exponent of result (0..+30) */
206 : )
207 : {
208 : /* Ignore internal overflows */
209 9233137 : return Dot_product12_o( x, y, lg, exp, NULL );
210 : }
211 :
212 : /*___________________________________________________________________________
213 : | |
214 : | Function Name : Energy_scale() |
215 : | |
216 : | Compute energy of signal (scaling the input if specified) |
217 : | |
218 : | The result is normalized (in Q31) with exponent (0..30). |
219 : |___________________________________________________________________________|
220 : */
221 :
222 6999 : Word32 Energy_scale( /* (o) : Q31: normalized result (1 < val <= -1) */
223 : const Word16 x[], /* (i) : input vector x */
224 : const Word16 lg, /* (i) : vector length */
225 : Word16 expi, /* (i) : exponent of input */
226 : Word16 *exp /* (o) : exponent of result (0..+30) */
227 : )
228 : {
229 : Word16 i, sft, tmp;
230 : Word32 L_sum;
231 :
232 6999 : L_sum = 0; /* just to avoid superflous compiler warning about uninitialized use of L_sum */
233 :
234 6999 : IF( expi == 0 )
235 : {
236 2978 : L_sum = L_mac_sat( 1, x[0], x[0] );
237 223936 : FOR( i = 1; i < lg; i++ )
238 : {
239 220958 : L_sum = L_mac_sat( L_sum, x[i], x[i] );
240 : }
241 : }
242 6999 : IF( expi < 0 )
243 : {
244 3816 : sft = lshl( -32768 /* 0x8000 */, expi );
245 3816 : tmp = mult_r( x[0], sft );
246 3816 : L_sum = L_mac( 1, tmp, tmp );
247 260352 : FOR( i = 1; i < lg; i++ )
248 : {
249 256536 : tmp = mult_r( x[i], sft );
250 256536 : L_sum = L_mac_sat( L_sum, tmp, tmp );
251 : }
252 : }
253 6999 : IF( expi > 0 )
254 : {
255 205 : tmp = shl_sat( x[0], expi );
256 205 : L_sum = L_mac_sat( 1, tmp, tmp );
257 32736 : FOR( i = 1; i < lg; i++ )
258 : {
259 32531 : tmp = shl_sat( x[i], expi );
260 32531 : L_sum = L_mac_sat( L_sum, tmp, tmp );
261 : }
262 : }
263 :
264 : /* Normalize acc in Q31 */
265 :
266 6999 : sft = norm_l( L_sum );
267 6999 : L_sum = L_shl( L_sum, sft );
268 :
269 6999 : *exp = sub( 30, sft );
270 6999 : move16(); /* exponent = 0..30 */
271 :
272 :
273 6999 : return L_sum;
274 : }
275 :
276 149609 : Word32 Sqrt_l( /* o : output value, Q31 */
277 : Word32 L_x, /* i : input value, Q31 */
278 : Word16 *exp /* o : right shift to be applied to result, Q1 */
279 : )
280 : {
281 : /*
282 : y = sqrt(x)
283 :
284 : x = f * 2^-e, 0.5 <= f < 1 (normalization)
285 :
286 : y = sqrt(f) * 2^(-e/2)
287 :
288 : a) e = 2k --> y = sqrt(f) * 2^-k (k = e div 2,
289 : 0.707 <= sqrt(f) < 1)
290 : b) e = 2k+1 --> y = sqrt(f/2) * 2^-k (k = e div 2,
291 : 0.5 <= sqrt(f/2) < 0.707)
292 : */
293 :
294 : Word16 e, i, a, tmp;
295 : Word32 L_y;
296 :
297 149609 : if ( L_x <= 0 )
298 : {
299 114 : *exp = 0;
300 114 : move16();
301 114 : return L_deposit_l( 0 );
302 : }
303 :
304 149495 : e = s_and( norm_l( L_x ), 0x7FFE ); /* get next lower EVEN norm. exp */
305 149495 : L_x = L_shl( L_x, e ); /* L_x is normalized to [0.25..1) */
306 149495 : *exp = e;
307 149495 : move16(); /* return 2*exponent (or Q1) */
308 :
309 149495 : L_x = L_shr( L_x, 9 );
310 149495 : a = extract_l( L_x ); /* Extract b10-b24 */
311 149495 : a = lshr( a, 1 );
312 :
313 149495 : i = mac_r( L_x, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
314 :
315 149495 : L_y = L_deposit_h( sqrt_table[i] ); /* table[i] << 16 */
316 149495 : tmp = sub( sqrt_table[i], sqrt_table[i + 1] ); /* table[i] - table[i+1]) */
317 149495 : L_y = L_msu( L_y, tmp, a ); /* L_y -= tmp*a*2 */
318 :
319 : /* L_y = L_shr (L_y, *exp); */ /* denormalization done by caller */
320 :
321 149495 : return ( L_y );
322 : }
323 :
324 : /*---------------------------------------------------------------------------*
325 : * L_Frac_sqrtQ31
326 : *
327 : * Calculate square root from fractional values (Q31 -> Q31)
328 : * Uses 32 bit internal representation for precision
329 : *---------------------------------------------------------------------------*/
330 4529306 : Word32 L_Frac_sqrtQ31( /* o : Square root if input */
331 : const Word32 x /* i : Input */
332 : )
333 : {
334 : Word32 log2_work;
335 : Word16 log2_int, log2_frac;
336 :
337 4529306 : test();
338 4529306 : if ( x > 0 )
339 : {
340 4451870 : log2_int = norm_l( x );
341 4451870 : log2_frac = Log2_norm_lc( L_shl( x, log2_int ) );
342 :
343 4451870 : log2_work = L_msu( ( 31 + 30 ) * 65536L / 2, 16384, log2_int );
344 4451870 : log2_work = L_mac0( log2_work, log2_frac, 1 );
345 :
346 4451870 : log2_frac = L_Extract_lc( log2_work, &log2_int );
347 :
348 4451870 : return Pow2( log2_int, log2_frac );
349 : }
350 77436 : return 0;
351 : }
352 :
353 : /*----------------------------------------------------------------------------------*
354 : * Frac_sqrt
355 : *
356 : * Calculate square root from fractional values (Q15 -> Q15)
357 : *----------------------------------------------------------------------------------*/
358 669274 : Word16 Frac_sqrt( /* o : Square root if input */
359 : const Word16 x /* i : Input */
360 : )
361 : {
362 669274 : return round_fx( L_Frac_sqrtQ31( L_deposit_h( x ) ) );
363 : }
364 :
365 :
366 : /*----------------------------------------------------------------------------------*
367 : * i_mult2
368 : *
369 : * Faster Integer Multiplication
370 : *----------------------------------------------------------------------------------*/
371 :
372 158173686 : Word16 i_mult2( Word16 a, Word16 b )
373 : {
374 158173686 : return extract_l( L_mult0( a, b ) );
375 : }
|
