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 5966083 : 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 5966083 : exp = norm_l( L_x );
48 5966083 : L_x = L_shl( L_x, exp ); /* L_x is normalized */
49 5966083 : exp = sub( 31, exp );
50 :
51 5966083 : L_x = Isqrt_lc( L_x, &exp );
52 :
53 5966083 : L_y = L_shl( L_x, exp ); /* denormalization */
54 :
55 5966083 : 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 11829646 : 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 11829646 : IF( frac <= (Word32) 0 )
87 : {
88 5801 : *exp = 0;
89 5801 : move16();
90 5801 : return 0x7fffffff; /*0x7fffffff*/
91 : }
92 :
93 : /* If exponant odd -> shift right by 10 (otherwise 9) */
94 11823845 : 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 11823845 : *exp = mac_r( 32768, *exp, -16384 );
104 11823845 : move16();
105 :
106 11823845 : a = extract_l( L_tmp ); /* Extract b10-b24 */
107 11823845 : a = lshr( a, 1 );
108 :
109 11823845 : i = mac_r( L_tmp, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
110 :
111 11823845 : L_tmp = L_msu( L_table_isqrt[i], table_isqrt_diff[i], a ); /* table[i] << 16 - diff*a*2 */
112 :
113 11823845 : 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 7668069 : 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 7668069 : i = mac_r( -32768, fraction, 32 ); /* Extract b10-b16 of fraction */
145 7668069 : a = s_and( fraction, 0x3ff ); /* Extract b0-b9 of fraction */
146 :
147 7668069 : L_x = L_deposit_h( table_pow2[i] ); /* table[i] << 16 */
148 7668069 : L_x = L_mac( L_x, table_pow2_diff_x32[i], a ); /* L_x -= diff*a*2 */
149 :
150 7668069 : exp = sub( 30, exponant );
151 :
152 7668069 : L_x = L_shr_r( L_x, exp );
153 :
154 :
155 7668069 : 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 2194215 : 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 2194215 : Flag Overflow_ignored = 0;
183 :
184 2194215 : L_sum = L_mac_o( 1, x[0], y[0], &Overflow_ignored );
185 173705799 : FOR( i = 1; i < lg; i++ )
186 : {
187 171511584 : 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 2194215 : sft = norm_l( L_sum );
193 2194215 : L_sum = L_shl( L_sum, sft );
194 :
195 2194215 : *exp = sub( 30, sft );
196 2194215 : move16(); /* exponent = 0..30 */
197 :
198 2194215 : return L_sum;
199 : }
200 :
201 2194215 : 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 2194215 : 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 315 : 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 : #ifndef ISSUE_1836_replace_overflow_libcom
232 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
233 : Flag Overflow = 0;
234 : #endif
235 : #endif
236 :
237 :
238 315 : L_sum = 0; /* just to avoid superflous compiler warning about uninitialized use of L_sum */
239 :
240 315 : IF( expi == 0 )
241 : {
242 : #ifdef ISSUE_1836_replace_overflow_libcom
243 102 : L_sum = L_mac_sat( 1, x[0], x[0] );
244 20000 : FOR( i = 1; i < lg; i++ )
245 : {
246 19898 : L_sum = L_mac_sat( L_sum, x[i], x[i] );
247 : }
248 : #else
249 : L_sum = L_mac_o( 1, x[0], x[0], &Overflow );
250 : FOR( i = 1; i < lg; i++ )
251 : {
252 : L_sum = L_mac_o( L_sum, x[i], x[i], &Overflow );
253 : }
254 : #endif
255 : }
256 315 : IF( expi < 0 )
257 : {
258 0 : sft = lshl( -32768 /* 0x8000 */, expi );
259 0 : tmp = mult_r( x[0], sft );
260 0 : L_sum = L_mac( 1, tmp, tmp );
261 0 : FOR( i = 1; i < lg; i++ )
262 : {
263 0 : tmp = mult_r( x[i], sft );
264 : #ifdef ISSUE_1836_replace_overflow_libcom
265 0 : L_sum = L_mac_sat( L_sum, tmp, tmp );
266 : #else
267 : L_sum = L_mac_o( L_sum, tmp, tmp, &Overflow );
268 : #endif
269 : }
270 : }
271 315 : IF( expi > 0 )
272 : {
273 : #ifdef ISSUE_1836_replace_overflow_libcom
274 213 : tmp = shl_sat( x[0], expi );
275 213 : L_sum = L_mac_sat( 1, tmp, tmp );
276 34016 : FOR( i = 1; i < lg; i++ )
277 : {
278 33803 : tmp = shl_sat( x[i], expi );
279 33803 : L_sum = L_mac_sat( L_sum, tmp, tmp );
280 : }
281 : #else
282 : tmp = shl_o( x[0], expi, &Overflow );
283 : L_sum = L_mac_o( 1, tmp, tmp, &Overflow );
284 : FOR( i = 1; i < lg; i++ )
285 : {
286 : tmp = shl_o( x[i], expi, &Overflow );
287 : L_sum = L_mac_o( L_sum, tmp, tmp, &Overflow );
288 : }
289 : #endif
290 : }
291 :
292 : /* Normalize acc in Q31 */
293 :
294 315 : sft = norm_l( L_sum );
295 315 : L_sum = L_shl( L_sum, sft );
296 :
297 315 : *exp = sub( 30, sft );
298 315 : move16(); /* exponent = 0..30 */
299 :
300 :
301 315 : return L_sum;
302 : }
303 :
304 81009 : Word32 Sqrt_l( /* o : output value, Q31 */
305 : Word32 L_x, /* i : input value, Q31 */
306 : Word16 *exp /* o : right shift to be applied to result, Q1 */
307 : )
308 : {
309 : /*
310 : y = sqrt(x)
311 :
312 : x = f * 2^-e, 0.5 <= f < 1 (normalization)
313 :
314 : y = sqrt(f) * 2^(-e/2)
315 :
316 : a) e = 2k --> y = sqrt(f) * 2^-k (k = e div 2,
317 : 0.707 <= sqrt(f) < 1)
318 : b) e = 2k+1 --> y = sqrt(f/2) * 2^-k (k = e div 2,
319 : 0.5 <= sqrt(f/2) < 0.707)
320 : */
321 :
322 : Word16 e, i, a, tmp;
323 : Word32 L_y;
324 :
325 81009 : if ( L_x <= 0 )
326 : {
327 123 : *exp = 0;
328 123 : move16();
329 123 : return L_deposit_l( 0 );
330 : }
331 :
332 80886 : e = s_and( norm_l( L_x ), 0x7FFE ); /* get next lower EVEN norm. exp */
333 80886 : L_x = L_shl( L_x, e ); /* L_x is normalized to [0.25..1) */
334 80886 : *exp = e;
335 80886 : move16(); /* return 2*exponent (or Q1) */
336 :
337 80886 : L_x = L_shr( L_x, 9 );
338 80886 : a = extract_l( L_x ); /* Extract b10-b24 */
339 80886 : a = lshr( a, 1 );
340 :
341 80886 : i = mac_r( L_x, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
342 :
343 80886 : L_y = L_deposit_h( sqrt_table[i] ); /* table[i] << 16 */
344 80886 : tmp = sub( sqrt_table[i], sqrt_table[i + 1] ); /* table[i] - table[i+1]) */
345 80886 : L_y = L_msu( L_y, tmp, a ); /* L_y -= tmp*a*2 */
346 :
347 : /* L_y = L_shr (L_y, *exp); */ /* denormalization done by caller */
348 :
349 80886 : return ( L_y );
350 : }
351 :
352 : /*---------------------------------------------------------------------------*
353 : * L_Frac_sqrtQ31
354 : *
355 : * Calculate square root from fractional values (Q31 -> Q31)
356 : * Uses 32 bit internal representation for precision
357 : *---------------------------------------------------------------------------*/
358 4543762 : Word32 L_Frac_sqrtQ31( /* o : Square root if input */
359 : const Word32 x /* i : Input */
360 : )
361 : {
362 : Word32 log2_work;
363 : Word16 log2_int, log2_frac;
364 :
365 4543762 : test();
366 4543762 : if ( x > 0 )
367 : {
368 4465945 : log2_int = norm_l( x );
369 4465945 : log2_frac = Log2_norm_lc( L_shl( x, log2_int ) );
370 :
371 4465945 : log2_work = L_msu( ( 31 + 30 ) * 65536L / 2, 16384, log2_int );
372 4465945 : log2_work = L_mac0( log2_work, log2_frac, 1 );
373 :
374 4465945 : log2_frac = L_Extract_lc( log2_work, &log2_int );
375 :
376 4465945 : return Pow2( log2_int, log2_frac );
377 : }
378 77817 : return 0;
379 : }
380 :
381 : /*----------------------------------------------------------------------------------*
382 : * Frac_sqrt
383 : *
384 : * Calculate square root from fractional values (Q15 -> Q15)
385 : *----------------------------------------------------------------------------------*/
386 670240 : Word16 Frac_sqrt( /* o : Square root if input */
387 : const Word16 x /* i : Input */
388 : )
389 : {
390 670240 : return round_fx( L_Frac_sqrtQ31( L_deposit_h( x ) ) );
391 : }
392 :
393 :
394 : /*----------------------------------------------------------------------------------*
395 : * i_mult2
396 : *
397 : * Faster Integer Multiplication
398 : *----------------------------------------------------------------------------------*/
399 :
400 10606684 : Word16 i_mult2( Word16 a, Word16 b )
401 : {
402 10606684 : return extract_l( L_mult0( a, b ) );
403 : }
|
