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