Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : /*====================================================================================
34 : EVS Codec 3GPP TS26.443 Nov 04, 2021. Version 12.14.0 / 13.10.0 / 14.6.0 / 15.4.0 / 16.3.0
35 : ====================================================================================*/
36 :
37 : #include <assert.h>
38 : #include <stdint.h>
39 : #include "options.h"
40 : #ifdef DEBUGGING
41 : #include "debug.h"
42 : #endif
43 : #include "basop_util.h"
44 : #include "rom_com.h"
45 : #include "basop_settings.h"
46 : #include "basop_mpy.h"
47 : #include "stl.h"
48 : #include "cnst.h"
49 :
50 : #define WMC_TOOL_SKIP
51 :
52 : extern const Word32 SqrtTable[32];
53 : extern const Word16 SqrtDiffTable[32];
54 :
55 : extern const Word32 ISqrtTable[32];
56 : extern const Word16 ISqrtDiffTable[32];
57 :
58 : extern const Word32 InvTable[32];
59 : extern const Word16 InvDiffTable[32];
60 :
61 :
62 136830 : Word32 BASOP_Util_Log2(
63 : Word32 x )
64 : {
65 : Word32 exp;
66 : Word16 exp_e;
67 : Word16 nIn;
68 : Word16 accuSqr;
69 : Word32 accuRes;
70 :
71 136830 : assert( x >= 0 );
72 :
73 136830 : if ( x == 0 )
74 : {
75 0 : return ( (Word32) MIN_32 );
76 : }
77 :
78 : /* normalize input, calculate integer part */
79 136830 : exp_e = norm_l( x );
80 136830 : x = L_shl( x, exp_e );
81 136830 : exp = L_deposit_l( exp_e );
82 :
83 : /* calculate (1-normalized_input) */
84 136830 : nIn = extract_h( L_sub( MAX_32, x ) );
85 :
86 : /* approximate ln() for fractional part (nIn *c0 + nIn^2*c1 + nIn^3*c2 + ... + nIn^8 *c7) */
87 :
88 : /* iteration 1, no need for accumulation */
89 136830 : accuRes = L_mult( nIn, ldCoeff[0] ); /* nIn^i * coeff[0] */
90 136830 : accuSqr = mult( nIn, nIn ); /* nIn^2, nIn^3 .... */
91 :
92 : /* iteration 2 */
93 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[1] ); /* nIn^i * coeff[1] */
94 136830 : accuSqr = mult( accuSqr, nIn ); /* nIn^2, nIn^3 .... */
95 :
96 : /* iteration 3 */
97 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[2] ); /* nIn^i * coeff[2] */
98 136830 : accuSqr = mult( accuSqr, nIn ); /* nIn^2, nIn^3 .... */
99 :
100 : /* iteration 4 */
101 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[3] ); /* nIn^i * coeff[3] */
102 136830 : accuSqr = mult( accuSqr, nIn ); /* nIn^2, nIn^3 .... */
103 :
104 : /* iteration 5 */
105 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[4] ); /* nIn^i * coeff[4] */
106 136830 : accuSqr = mult( accuSqr, nIn ); /* nIn^2, nIn^3 .... */
107 :
108 : /* iteration 6 */
109 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[5] ); /* nIn^i * coeff[5] */
110 136830 : accuSqr = mult( accuSqr, nIn ); /* nIn^2, nIn^3 .... */
111 :
112 : /* iteration 7, no need to calculate accuSqr any more */
113 136830 : accuRes = L_mac( accuRes, accuSqr, ldCoeff[6] ); /* nIn^i * coeff[6] */
114 :
115 : /* ld(fractional part) = ln(fractional part)/ln(2), 1/ln(2) = (1 + 0.44269504) */
116 136830 : accuRes = L_mac0( L_shr( accuRes, 1 ), extract_h( accuRes ), 14506 );
117 :
118 136830 : accuRes = L_shr( accuRes, LD_DATA_SCALE - 1 ); /* fractional part/LD_DATA_SCALE */
119 136830 : exp = L_shl( exp, ( 31 - LD_DATA_SCALE ) ); /* integer part/LD_DATA_SCALE */
120 136830 : accuRes = L_sub( accuRes, exp ); /* result = integer part + fractional part */
121 :
122 136830 : return ( accuRes );
123 : }
124 :
125 :
126 270783 : Word32 BASOP_Util_InvLog2(
127 : Word32 x )
128 : {
129 : Word16 frac;
130 : Word16 exp;
131 : Word32 retVal;
132 : UWord32 index3;
133 : UWord32 index2;
134 : UWord32 index1;
135 : UWord32 lookup3f;
136 : UWord32 lookup12;
137 : UWord32 lookup;
138 :
139 270783 : if ( x < FL2WORD32( -31.0 / 64.0 ) )
140 : {
141 0 : return 0;
142 : }
143 270783 : test();
144 270783 : if ( ( L_sub( x, FL2WORD32( 31.0 / 64.0 ) ) >= 0 ) || ( x == 0 ) )
145 : {
146 0 : return 0x7FFFFFFF;
147 : }
148 :
149 270783 : frac = extract_l( L_and( x, 0x3FF ) );
150 :
151 270783 : index3 = L_and( L_shr( x, 10 ), 0x1F );
152 270783 : index2 = L_and( L_shr( x, 15 ), 0x1F );
153 270783 : index1 = L_and( L_shr( x, 20 ), 0x1F );
154 :
155 270783 : exp = extract_l( L_shr( x, 25 ) );
156 270783 : if ( x > 0 )
157 : {
158 0 : exp = sub( 31, exp );
159 : }
160 270783 : if ( x < 0 )
161 : {
162 270783 : exp = negate( exp );
163 : }
164 :
165 270783 : lookup3f = L_add( exp2x_tab_long[index3], L_shr( Mpy_32_16( 0x0016302F, frac ), 1 ) );
166 270783 : lookup12 = Mpy_32_32( exp2_tab_long[index1], exp2w_tab_long[index2] );
167 270783 : lookup = Mpy_32_32( lookup12, lookup3f );
168 :
169 270783 : retVal = L_shr( lookup, sub( exp, 3 ) );
170 :
171 270783 : return retVal;
172 : }
173 :
174 :
175 160626 : Word16 BASOP_Util_Add_MantExp /*!< Exponent of result */
176 : ( Word16 a_m, /*!< Mantissa of 1st operand a */
177 : Word16 a_e, /*!< Exponent of 1st operand a */
178 : Word16 b_m, /*!< Mantissa of 2nd operand b */
179 : Word16 b_e, /*!< Exponent of 2nd operand b */
180 : Word16 *ptrSum_m ) /*!< Mantissa of result */
181 : {
182 : Word32 L_lm, L_hm;
183 : Word16 shift;
184 :
185 : /* Compare exponents: the difference is limited to +/- 15
186 : The Word16 mantissa of the operand with higher exponent is moved into the low
187 : part of a Word32 and shifted left by the exponent difference. Then, the
188 : unshifted mantissa of the operand with the lower exponent is added to the lower
189 : 16 bits. The addition result is normalized and the upper Word16 of the result represents
190 : the mantissa to return. The returned exponent takes into account all shift operations
191 : including the final 16-bit extraction.
192 : Note: The resulting mantissa may be inaccurate in the case, where the mantissa of the operand
193 : with higher exponent is not really left-aligned, while the mantissa of the operand with
194 : lower exponent is so. If in such a case, the difference in exponents is more than 15,
195 : an inaccuracy is introduced.
196 : Example:
197 : A: a_e = 20, a_m = 0x0001
198 : B: b_e = 0, b_m = 0x4000
199 : correct: A+B=1*2^20+1*2^14=0x0010.0000+0x0000.4000=0x0010.4000=0x4100*2^6
200 : previously: A+B=1*2^20+1*2^14=0x0001+0x0000=0x0001*2^20
201 : this version: A+B=1*2^20+1*2^14=0x0000.8000+0x0000.4000=0x6000*2^6
202 : */
203 :
204 160626 : shift = sub( a_e, b_e );
205 160626 : if ( shift >= 0 )
206 92211 : shift = s_min( 15, shift );
207 :
208 160626 : if ( shift < 0 )
209 68415 : shift = s_max( -15, shift );
210 160626 : a_e = s_max( a_e, b_e );
211 160626 : L_hm = L_deposit_l( a_m ); /* mantissa belonging to higher exponent */
212 160626 : L_lm = L_deposit_l( a_m ); /* mantissa belonging to lower exponent */
213 160626 : if ( shift >= 0 )
214 92211 : L_lm = L_deposit_l( b_m );
215 160626 : if ( shift < 0 )
216 68415 : L_hm = L_deposit_l( b_m );
217 :
218 160626 : if ( shift > 0 )
219 56429 : shift = negate( shift );
220 :
221 160626 : L_hm = L_shr( L_hm, shift ); /* shift left due to negative shift parameter */
222 160626 : a_e = add( a_e, shift );
223 160626 : L_hm = L_add( L_hm, L_lm );
224 160626 : shift = norm_l( L_hm );
225 160626 : L_hm = L_shl( L_hm, shift );
226 160626 : *ptrSum_m = extract_h( L_hm );
227 160626 : move16();
228 :
229 160626 : a_e = sub( a_e, shift );
230 160626 : if ( L_hm )
231 145701 : a_e = add( a_e, 16 );
232 :
233 160626 : return ( a_e );
234 : }
235 :
236 :
237 : /* local function for Sqrt16 */
238 68415 : static Word16 Sqrt16_common(
239 : Word16 m,
240 : Word16 e )
241 : {
242 : Word16 index, frac;
243 : #ifdef BASOP_NOGLOB
244 : Flag Overflow;
245 : #endif /* BASOP_NOGLOB */
246 :
247 68415 : assert( ( m >= 0x4000 ) || ( m == 0 ) );
248 :
249 : /* get table index (upper 6 bits minus 32) */
250 : /* index = (m >> 9) - 32; */
251 68415 : index = mac_r( -32768 - ( 32 << 16 ), m, 1 << 6 );
252 :
253 : /* get fractional part for interpolation (lower 9 bits) */
254 68415 : frac = s_and( m, 0x1FF ); /* Q9 */
255 :
256 : /* interpolate */
257 68415 : if ( m != 0 )
258 : {
259 : BASOP_SATURATE_WARNING_OFF;
260 : #ifndef BASOP_NOGLOB
261 : m = mac_r( SqrtTable[index], SqrtDiffTable[index], frac );
262 : #else /* BASOP_NOGLOB */
263 68415 : m = mac_ro( SqrtTable[index], SqrtDiffTable[index], frac, &Overflow );
264 : #endif /* BASOP_NOGLOB */
265 : BASOP_SATURATE_WARNING_ON;
266 : }
267 :
268 : /* handle odd exponents */
269 68415 : if ( s_and( e, 1 ) != 0 )
270 22979 : m = mult_r( m, 0x5a82 );
271 :
272 68415 : return m;
273 : }
274 :
275 :
276 68415 : Word16 Sqrt16( /*!< output mantissa */
277 : Word16 mantissa, /*!< input mantissa */
278 : Word16 *exponent /*!< pointer to exponent */
279 : )
280 : {
281 : Word16 preShift, e;
282 :
283 68415 : assert( mantissa >= 0 );
284 :
285 : /* normalize */
286 68415 : preShift = norm_s( mantissa );
287 :
288 68415 : e = sub( *exponent, preShift );
289 68415 : mantissa = shl( mantissa, preShift );
290 :
291 : /* calc mantissa */
292 68415 : mantissa = Sqrt16_common( mantissa, e );
293 :
294 : /* e = (e + 1) >> 1 */
295 68415 : *exponent = mult_r( e, 1 << 14 );
296 68415 : move16();
297 :
298 68415 : return mantissa;
299 : }
300 :
301 14462267 : Word16 Inv16( /*!< output mantissa */
302 : Word16 mantissa, /*!< input mantissa */
303 : Word16 *exponent /*!< pointer to exponent */
304 : )
305 : {
306 : Word16 index, frac;
307 : Word16 preShift;
308 : Word16 m, e;
309 :
310 14462267 : assert( mantissa != 0 );
311 :
312 : /* absolute */
313 : BASOP_SATURATE_WARNING_OFF;
314 14462267 : m = abs_s( mantissa );
315 : BASOP_SATURATE_WARNING_ON;
316 :
317 : /* normalize */
318 14462267 : preShift = norm_s( m );
319 :
320 14462267 : e = sub( *exponent, preShift );
321 14462267 : m = shl( m, preShift );
322 :
323 : /* get table index (upper 6 bits minus 32) */
324 : /* index = (m >> 9) - 32; */
325 14462267 : index = mac_r( -32768 - ( 32 << 16 ), m, 1 << 6 );
326 :
327 : /* get fractional part for interpolation (lower 9 bits) */
328 14462267 : frac = shl( s_and( m, 0x1FF ), 1 ); /* Q10 */
329 :
330 : /* interpolate */
331 14462267 : m = msu_r( InvTable[index], InvDiffTable[index], frac );
332 :
333 : /* restore sign */
334 14462267 : if ( mantissa < 0 )
335 0 : m = negate( m );
336 :
337 : /* e = 1 - e */
338 14462267 : *exponent = sub( 1, e );
339 14462267 : move16();
340 :
341 14462267 : return m;
342 : }
343 :
344 :
345 8757120 : void BASOP_Util_Sqrt_InvSqrt_MantExp(
346 : Word16 mantissa, /*!< mantissa */
347 : Word16 exponent, /*!< expoinent */
348 : Word16 *sqrt_mant, /*!< Pointer to sqrt mantissa */
349 : Word16 *sqrt_exp, /*!< Pointer to sqrt exponent */
350 : Word16 *isqrt_mant, /*!< Pointer to 1/sqrt mantissa */
351 : Word16 *isqrt_exp /*!< Pointer to 1/sqrt exponent */
352 : )
353 : {
354 : Word16 index, frac;
355 : Word16 preShift;
356 : Word16 m, mi, e_odd;
357 :
358 8757120 : assert( mantissa > 0 );
359 :
360 : /* normalize */
361 8757120 : preShift = norm_s( mantissa );
362 :
363 8757120 : exponent = sub( exponent, preShift );
364 8757120 : mantissa = shl( mantissa, preShift );
365 :
366 : /* get table index (upper 6 bits minus 32) */
367 : /* index = (m >> 9) - 32; */
368 8757120 : index = mac_r( -32768 - ( 32 << 16 ), mantissa, 1 << 6 );
369 :
370 : /* get fractional part for interpolation (lower 9 bits) */
371 8757120 : frac = s_and( mantissa, 0x1FF ); /* Q9 */
372 :
373 : /* interpolate */
374 : BASOP_SATURATE_WARNING_OFF;
375 8757120 : m = mac_r( SqrtTable[index], SqrtDiffTable[index], frac );
376 8757120 : mi = msu_r( ISqrtTable[index], ISqrtDiffTable[index], frac );
377 : BASOP_SATURATE_WARNING_ON;
378 :
379 : /* handle even/odd exponents */
380 8757120 : e_odd = s_and( exponent, 1 );
381 8757120 : if ( e_odd != 0 )
382 4481703 : m = mult_r( m, 0x5a82 );
383 8757120 : if ( e_odd == 0 )
384 4275417 : mi = mult_r( mi, 0x5a82 );
385 :
386 : /* e = (e + 1) >> 1 */
387 8757120 : *sqrt_exp = mult_r( exponent, 1 << 14 );
388 8757120 : move16();
389 :
390 : /* e = (2 - e) >> 1 */
391 8757120 : *isqrt_exp = msu_r( 1L << 15, exponent, 1 << 14 );
392 8757120 : move16();
393 :
394 : /* Write result */
395 8757120 : *sqrt_mant = m;
396 8757120 : move16();
397 8757120 : *isqrt_mant = mi;
398 8757120 : move16();
399 8757120 : }
400 :
401 : /********************************************************************/
402 : /*!
403 : \brief Calculates the scalefactor needed to normalize input array
404 :
405 : The scalefactor needed to normalize the Word32 input array is returned <br>
406 : If the input array contains only '0', a scalefactor 0 is returned <br>
407 : Scaling factor is determined wrt a normalized target x: 1073741824 <= x <= 2147483647 for positive x <br>
408 : and -2147483648 <= x <= -1073741824 for negative x
409 : */
410 :
411 : /*! r: measured headroom in range [0..31], 0 if all x[i] == 0 */
412 68415 : Word16 getScaleFactor32(
413 : const Word32 *x, /* i : array containing 32-bit data */
414 : const Word16 len_x ) /* i : length of the array to scan */
415 : {
416 : Word16 i, i_min, i_max;
417 : Word32 x_min, x_max;
418 :
419 68415 : x_max = L_add( 0, 0 );
420 68415 : x_min = L_add( 0, 0 );
421 43142655 : FOR( i = 0; i < len_x; i++ )
422 : {
423 43074240 : if ( x[i] >= 0 )
424 43074240 : x_max = L_max( x_max, x[i] );
425 43074240 : if ( x[i] < 0 )
426 0 : x_min = L_min( x_min, x[i] );
427 : }
428 :
429 68415 : i_max = 0x20;
430 68415 : move16();
431 68415 : i_min = 0x20;
432 68415 : move16();
433 :
434 68415 : if ( x_max != 0 )
435 68415 : i_max = norm_l( x_max );
436 :
437 68415 : if ( x_min != 0 )
438 0 : i_min = norm_l( x_min );
439 :
440 68415 : i = s_and( s_min( i_max, i_min ), 0x1F );
441 :
442 68415 : return i;
443 : }
444 :
445 :
446 254826 : Word16 BASOP_Util_Divide1616_Scale(
447 : Word16 x,
448 : Word16 y,
449 : Word16 *s )
450 : {
451 : Word16 z;
452 : Word16 sx;
453 : Word16 sy;
454 : Word16 sign;
455 :
456 : /* assert (x >= (Word16)0); */
457 254826 : assert( y != (Word16) 0 );
458 :
459 254826 : sign = 0;
460 254826 : move16();
461 :
462 254826 : IF( x < 0 )
463 : {
464 0 : x = negate( x );
465 0 : sign = s_xor( sign, 1 );
466 : }
467 :
468 254826 : IF( y < 0 )
469 : {
470 0 : y = negate( y );
471 0 : sign = s_xor( sign, 1 );
472 : }
473 :
474 254826 : IF( x == (Word16) 0 )
475 : {
476 7299 : move16();
477 7299 : *s = 0;
478 :
479 7299 : return ( (Word16) 0 );
480 : }
481 :
482 247527 : sx = norm_s( x );
483 247527 : x = shl( x, sx );
484 247527 : x = shr( x, 1 );
485 247527 : move16();
486 247527 : *s = sub( 1, sx );
487 :
488 247527 : sy = norm_s( y );
489 247527 : y = shl( y, sy );
490 247527 : move16();
491 247527 : *s = add( *s, sy );
492 :
493 247527 : z = div_s( x, y );
494 :
495 247527 : if ( sign != 0 )
496 : {
497 0 : z = negate( z );
498 : }
499 :
500 247527 : return z;
501 : }
502 :
503 :
504 0 : void set_val_Word16(
505 : Word16 X[],
506 : const Word16 val,
507 : Word16 n )
508 : {
509 : Word16 i;
510 :
511 :
512 0 : FOR( i = 0; i < n; i++ )
513 : {
514 0 : X[i] = val;
515 0 : move16();
516 : }
517 :
518 :
519 0 : return;
520 : }
521 :
522 136 : void set_val_Word32(
523 : Word32 X[],
524 : const Word32 val,
525 : Word16 n )
526 : {
527 : Word16 i;
528 :
529 :
530 3248 : FOR( i = 0; i < n; i++ )
531 : {
532 3112 : X[i] = val;
533 3112 : move32();
534 : }
535 :
536 :
537 136 : return;
538 : }
539 :
540 134912 : Word16 mult0(
541 : Word16 x,
542 : Word16 y )
543 : {
544 134912 : return extract_l( L_mult0( x, y ) );
545 : }
546 :
547 :
548 : #define SINETAB SineTable512_fx
549 : #define LD 9
550 :
551 : /*
552 : * Calculates coarse lookup values for sine/cosine and residual angle.
553 : * \param x angle in radians with exponent = 2 or as radix 2 with exponent = 0.
554 : * \param scale shall always be 2
555 : * \param sine pointer to where the sine lookup value is stored into
556 : * \param cosine pointer to where the cosine lookup value is stored into
557 : * \param flag_radix2 flag indicating radix 2 angle if non-zero.
558 : */
559 1094640 : static Word16 fixp_sin_cos_residual_16(
560 : Word16 x,
561 : const Word16 scale,
562 : Word16 *sine,
563 : Word16 *cosine,
564 : Word8 flag_radix2 )
565 : {
566 : Word16 residual;
567 : Word16 s;
568 : Word16 ssign;
569 : Word16 csign;
570 1094640 : Word16 tmp, cl = 0, sl = 0;
571 1094640 : const Word16 shift = 15 - LD - 1 - scale;
572 :
573 1094640 : if ( flag_radix2 == 0 )
574 : {
575 0 : x = mult_r( x, FL2WORD16( 1.0 / EVS_PI ) );
576 : }
577 1094640 : s = shr( x, shift );
578 :
579 1094640 : residual = s_and( x, ( 1 << shift ) - 1 );
580 : /* We assume "2+scale" is a constant */
581 1094640 : residual = shl( residual, 2 + scale );
582 1094640 : residual = mult_r( residual, FL2WORD16( EVS_PI / 4.0 ) );
583 :
584 : /* Sine sign symmetry */
585 1094640 : ssign = s_and( s, ( 1 << LD ) << 1 );
586 :
587 : /* Cosine sign symmetry */
588 1094640 : csign = s_and( add( s, ( 1 << LD ) ), ( 1 << LD ) << 1 );
589 :
590 : /* Modulo EVS_PI */
591 1094640 : s = s_and( s, ( 2 << LD ) - 1 );
592 :
593 : /* EVS_PI/2 symmetry */
594 1094640 : s = s_min( s, sub( 2 << LD, s ) );
595 :
596 : {
597 1094640 : tmp = s_min( sub( 1 << LD, s ), s );
598 1094640 : s = sub( tmp, s );
599 :
600 1094640 : if ( !s )
601 : {
602 515960 : move16();
603 515960 : sl = SINETAB[tmp].v.im;
604 : }
605 1094640 : if ( !s )
606 : {
607 515960 : move16();
608 515960 : cl = SINETAB[tmp].v.re;
609 : }
610 1094640 : if ( s )
611 : {
612 578680 : move16();
613 578680 : sl = SINETAB[tmp].v.re;
614 : }
615 1094640 : if ( s )
616 : {
617 578680 : move16();
618 578680 : cl = SINETAB[tmp].v.im;
619 : }
620 :
621 1094640 : if ( ssign )
622 : {
623 0 : sl = negate( sl );
624 : }
625 1094640 : if ( csign )
626 : {
627 523015 : cl = negate( cl );
628 : }
629 :
630 1094640 : move16();
631 1094640 : move16();
632 1094640 : *sine = sl;
633 1094640 : *cosine = cl;
634 : }
635 :
636 1094640 : return residual;
637 : }
638 :
639 :
640 1094640 : Word16 getCosWord16R2(
641 : Word16 theta )
642 : {
643 : Word16 result, residual, sine, cosine;
644 :
645 1094640 : residual = fixp_sin_cos_residual_16( theta, 1, &sine, &cosine, 1 );
646 : /* This negation prevents the subsequent addition from overflow */
647 : /* The negation cannot overflow, sine is in range [0x0..0x7FFF] */
648 : BASOP_SATURATE_WARNING_OFF
649 1094640 : sine = negate( sine );
650 1094640 : result = msu_r( L_mult( sine, residual ), cosine, -32768 );
651 : BASOP_SATURATE_WARNING_ON
652 :
653 1094640 : return result;
654 : }
655 :
656 :
657 0 : Word16 idiv1616U(
658 : Word16 x,
659 : Word16 y )
660 : {
661 : Word16 s;
662 : Word16 tmp;
663 :
664 : /* make y > x */
665 0 : s = add( sub( norm_s( y ), norm_s( x ) ), 1 );
666 0 : s = s_max( s, 0 );
667 :
668 : BASOP_SATURATE_WARNING_OFF
669 0 : y = shl( y, s );
670 : BASOP_SATURATE_WARNING_ON
671 :
672 : /* divide and shift */
673 0 : tmp = div_s( x, y );
674 0 : y = shr( tmp, sub( 15, s ) );
675 :
676 0 : return y;
677 : }
678 :
679 136830 : Word32 BASOP_util_Pow2(
680 : const Word32 exp_m,
681 : const Word16 exp_e,
682 : Word16 *result_e )
683 : {
684 : static const Word16 pow2Coeff[8] = {
685 : FL2WORD16( 0.693147180559945309417232121458177 ), /* ln(2)^1 /1! */
686 : FL2WORD16( 0.240226506959100712333551263163332 ), /* ln(2)^2 /2! */
687 : FL2WORD16( 0.0555041086648215799531422637686218 ), /* ln(2)^3 /3! */
688 : FL2WORD16( 0.00961812910762847716197907157365887 ), /* ln(2)^4 /4! */
689 : FL2WORD16( 0.00133335581464284434234122219879962 ), /* ln(2)^5 /5! */
690 : FL2WORD16( 1.54035303933816099544370973327423e-4 ), /* ln(2)^6 /6! */
691 : FL2WORD16( 1.52527338040598402800254390120096e-5 ), /* ln(2)^7 /7! */
692 : FL2WORD16( 1.32154867901443094884037582282884e-6 ) /* ln(2)^8 /8! */
693 : };
694 :
695 136830 : Word32 frac_part = 0, tmp_frac, result_m;
696 136830 : Word16 int_part = 0;
697 :
698 136830 : IF( exp_e > 0 )
699 : {
700 : /* "+ 1" compensates L_shr(,1) of the polynomial evaluation at the loop end. */
701 :
702 131374 : int_part = add( 1, extract_l( L_shr( exp_m, sub( 31, exp_e ) ) ) );
703 131374 : frac_part = L_lshl( exp_m, exp_e );
704 131374 : frac_part = L_and( 0x7FFFFFFF, frac_part );
705 : }
706 136830 : if ( exp_e <= 0 )
707 5456 : frac_part = L_shl( exp_m, exp_e );
708 136830 : if ( exp_e <= 0 )
709 : {
710 5456 : int_part = 1;
711 5456 : move16();
712 : }
713 :
714 : /* Best accuracy is around 0, so try to get there with the fractional part. */
715 136830 : IF( ( tmp_frac = L_sub( frac_part, FL2WORD32( 0.5 ) ) ) >= 0 )
716 : {
717 63100 : int_part = add( int_part, 1 );
718 63100 : frac_part = L_sub( tmp_frac, FL2WORD32( 0.5 ) );
719 : }
720 73730 : ELSE IF( ( tmp_frac = L_add( frac_part, FL2WORD32( 0.5 ) ) ) < 0 )
721 : {
722 0 : int_part = sub( int_part, 1 );
723 0 : frac_part = L_add( tmp_frac, FL2WORD32( 0.5 ) );
724 : }
725 :
726 : /* Evaluate taylor polynomial which approximates 2^x */
727 : {
728 : Word32 p;
729 : Word16 i;
730 :
731 :
732 : /* First taylor series coefficient a_0 = 1.0, scaled by 0.5 due to L_shr(,1). */
733 136830 : result_m = L_add( FL2WORD32( 1.0 / 2.0 ), L_shr( Mpy_32_16( frac_part, pow2Coeff[0] ), 1 ) );
734 136830 : p = Mpy_32_32( frac_part, frac_part );
735 957810 : FOR( i = 1; i < 7; i++ )
736 : {
737 : /* next taylor series term: a_i * x^i, x=0 */
738 820980 : result_m = L_add( result_m, L_shr( Mpy_32_16( p, pow2Coeff[i] ), 1 ) );
739 820980 : p = Mpy_32_32( p, frac_part );
740 : }
741 136830 : result_m = L_add( result_m, L_shr( Mpy_32_16( p, pow2Coeff[i] ), 1 ) );
742 : }
743 136830 : *result_e = int_part;
744 136830 : move16();
745 :
746 136830 : return result_m;
747 : }
748 :
749 : /*! r: result of division x/y, not normalized */
750 13848 : Word16 BASOP_Util_Divide3216_Scale(
751 : Word32 x, /* i : numerator, signed */
752 : Word16 y, /* i : denominator, signed */
753 : Word16 *s ) /* o : scaling, 0, if x==0 */
754 : {
755 : Word16 z;
756 : Word16 sx;
757 : Word16 sy;
758 : Word16 sign;
759 :
760 : /*assert (x > (Word32)0);
761 : assert (y >= (Word16)0);*/
762 :
763 : /* check, if numerator equals zero, return zero then */
764 13848 : IF( x == (Word32) 0 )
765 : {
766 3201 : *s = 0;
767 3201 : move16();
768 :
769 3201 : return ( (Word16) 0 );
770 : }
771 :
772 10647 : sign = s_xor( extract_h( x ), y ); /* just to exor the sign bits */
773 : BASOP_SATURATE_WARNING_OFF
774 10647 : x = L_abs( x );
775 10647 : y = abs_s( y );
776 : BASOP_SATURATE_WARNING_ON
777 10647 : sx = sub( norm_l( x ), 1 );
778 10647 : x = L_shl( x, sx );
779 10647 : sy = norm_s( y );
780 10647 : y = shl( y, sy );
781 10647 : *s = sub( sy, sx );
782 10647 : move16();
783 :
784 10647 : z = div_s( round_fx( x ), y );
785 :
786 10647 : if ( sign < 0 ) /* if sign bits differ, negate the result */
787 : {
788 1299 : z = negate( z );
789 : }
790 :
791 10647 : return z;
792 : }
793 :
794 :
795 : static const Word16 table_pow2[32] = {
796 : 16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911,
797 : 20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726,
798 : 25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706,
799 : 31379, 32066
800 : };
801 : /* table of table_pow2[i+1] - table_pow2[i] */
802 : static const Word16 table_pow2_diff_x32[32] = {
803 : 11488, 11712, 12000, 12256, 12512, 12800, 13056, 13376, 13664, 13952,
804 : 14240, 14560, 14912, 15200, 15552, 15872, 16256, 16576, 16960, 17344,
805 : 17696, 18080, 18496, 18880, 19328, 19712, 20192, 20576, 21056, 21536,
806 : 21984, 22432
807 : };
808 :
809 693145 : Word32 Pow2( /* o Q0 : result (range: 0<=val<=0x7fffffff) */
810 : Word16 exponant, /* i Q0 : Integer part. (range: 0<=val<=30) */
811 : Word16 fraction /* i Q15 : Fractional part. (range: 0.0<=val<1.0) */
812 : )
813 : {
814 : Word16 exp, i, a;
815 : Word32 L_x;
816 :
817 693145 : i = mac_r( -32768, fraction, 32 ); /* Extract b10-b16 of fraction */
818 693145 : a = s_and( fraction, 0x3ff ); /* Extract b0-b9 of fraction */
819 :
820 693145 : L_x = L_deposit_h( table_pow2[i] ); /* table[i] << 16 */
821 693145 : L_x = L_mac( L_x, table_pow2_diff_x32[i], a ); /* L_x -= diff*a*2 */
822 :
823 693145 : exp = sub( 30, exponant );
824 :
825 693145 : L_x = L_shr_r( L_x, exp );
826 :
827 693145 : return L_x;
828 : }
829 :
830 : /*************************************************************************
831 : *
832 : * FUNCTION: Log2_norm()
833 : *
834 : * PURPOSE: Computes log2(L_x, exp), where L_x is positive and
835 : * normalized, and exp is the normalisation exponent
836 : * If L_x is negative or zero, the result is 0.
837 : *
838 : * DESCRIPTION:
839 : * The function Log2(L_x) is approximated by a table and linear
840 : * interpolation. The following steps are used to compute Log2(L_x)
841 : *
842 : * 1- exponent = 30-norm_exponent
843 : * 2- i = bit25-b31 of L_x; 32<=i<=63 (because of normalization).
844 : * 3- a = bit10-b24
845 : * 4- i -=32
846 : * 5- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2
847 : *
848 : *************************************************************************/
849 :
850 : static const Word32 L_table[32] = {
851 : -32768L, 95322112L, 187793408L, 277577728L,
852 : 364871680L, 449740800L, 532381696L, 612859904L,
853 : 691306496L, 767787008L, 842432512L, 915308544L,
854 : 986546176L, 1056210944L, 1124302848L, 1190887424L,
855 : 1256095744L, 1319993344L, 1382580224L, 1443921920L,
856 : 1504083968L, 1563131904L, 1621000192L, 1677885440L,
857 : 1733722112L, 1788510208L, 1842380800L, 1895399424L,
858 : 1947435008L, 1998618624L, 2049015808L, 2098626560L
859 : };
860 :
861 : static const Word16 table_diff[32] = {
862 : 1455, 1411, 1370, 1332, 1295, 1261, 1228, 1197,
863 : 1167, 1139, 1112, 1087, 1063, 1039, 1016, 995,
864 : 975, 955, 936, 918, 901, 883, 868, 852,
865 : 836, 822, 809, 794, 781, 769, 757, 744
866 : };
867 :
868 11325 : Word16 Log2_norm_lc( /* o : Fractional part of Log2. (range: 0<=val<1) */
869 : Word32 L_x /* i : input value (normalized) */
870 : )
871 : {
872 : Word16 i, a;
873 : Word16 y;
874 :
875 :
876 11325 : L_x = L_shr( L_x, 9 );
877 11325 : a = extract_l( L_x ); /* Extract b10-b24 of fraction */
878 11325 : a = lshr( a, 1 );
879 :
880 11325 : i = mac_r( L_x, -32 * 2 - 1, 16384 ); /* Extract b25-b31 minus 32 */
881 :
882 11325 : y = mac_r( L_table[i], table_diff[i], a ); /* table[i] << 16 - diff*a*2 */
883 :
884 :
885 11325 : return y;
886 : }
887 :
888 :
889 68415 : Word32 BASOP_Util_fPow(
890 : Word32 base_m,
891 : Word16 base_e,
892 : Word32 exp_m,
893 : Word16 exp_e,
894 : Word16 *result_e )
895 : {
896 :
897 : Word16 ans_lg2_e, base_lg2_e;
898 : Word32 base_lg2_m, ans_lg2_m, result_m;
899 : Word16 shift;
900 :
901 68415 : test();
902 68415 : IF( ( base_m == 0 ) && ( exp_m != 0 ) )
903 : {
904 0 : *result_e = 0;
905 0 : move16();
906 0 : return 0;
907 : }
908 : /* Calc log2 of base */
909 68415 : shift = norm_l( base_m );
910 68415 : base_m = L_shl( base_m, shift );
911 68415 : base_e = sub( base_e, shift );
912 68415 : base_lg2_m = BASOP_Util_Log2( base_m );
913 :
914 : /* shift: max left shift such that neither base_e or base_lg2_m saturate. */
915 68415 : shift = sub( s_min( norm_s( base_e ), WORD16_BITS - 1 - LD_DATA_SCALE ), 1 );
916 : /* Compensate shift into exponent of result. */
917 68415 : base_lg2_e = sub( WORD16_BITS - 1, shift );
918 68415 : base_lg2_m = L_add( L_shr( base_lg2_m, sub( WORD16_BITS - 1 - LD_DATA_SCALE, shift ) ), L_deposit_h( shl( base_e, shift ) ) );
919 :
920 : /* Prepare exp */
921 68415 : shift = norm_l( exp_m );
922 68415 : exp_m = L_shl( exp_m, shift );
923 68415 : exp_e = sub( exp_e, shift );
924 :
925 : /* Calc base pow exp */
926 68415 : ans_lg2_m = Mpy_32_32( base_lg2_m, exp_m );
927 68415 : ans_lg2_e = add( exp_e, base_lg2_e );
928 :
929 : /* Calc antilog */
930 68415 : result_m = BASOP_util_Pow2( ans_lg2_m, ans_lg2_e, result_e );
931 :
932 68415 : return result_m;
933 : }
934 :
935 92490 : Word32 BASOP_Util_Add_Mant32Exp /* o : normalized result mantissa */
936 : ( Word32 a_m, /* i : Mantissa of 1st operand a */
937 : Word16 a_e, /* i : Exponent of 1st operand a */
938 : Word32 b_m, /* i : Mantissa of 2nd operand b */
939 : Word16 b_e, /* i : Exponent of 2nd operand b */
940 : Word16 *ptr_e ) /* o : exponent of result */
941 : {
942 : Word32 L_tmp;
943 : Word16 shift;
944 :
945 : /* Compare exponents: the difference is limited to +/- 30
946 : The Word32 mantissa of the operand with lower exponent is shifted right by the exponent difference.
947 : Then, the unshifted mantissa of the operand with the higher exponent is added. The addition result
948 : is normalized and the result represents the mantissa to return. The returned exponent takes into
949 : account all shift operations.
950 : */
951 :
952 92490 : if ( !a_m )
953 3405 : a_e = add( b_e, 0 );
954 :
955 92490 : if ( !b_m )
956 3321 : b_e = add( a_e, 0 );
957 :
958 92490 : shift = sub( a_e, b_e );
959 92490 : shift = s_max( -31, shift );
960 92490 : shift = s_min( 31, shift );
961 92490 : if ( shift < 0 )
962 : {
963 : /* exponent of b is greater than exponent of a, shr a_m */
964 74647 : a_m = L_shl( a_m, shift );
965 : }
966 92490 : if ( shift > 0 )
967 : {
968 : /* exponent of a is greater than exponent of b */
969 14393 : b_m = L_shr( b_m, shift );
970 : }
971 92490 : a_e = add( s_max( a_e, b_e ), 1 );
972 92490 : L_tmp = L_add( L_shr( a_m, 1 ), L_shr( b_m, 1 ) );
973 92490 : shift = norm_l( L_tmp );
974 92490 : if ( shift )
975 88902 : L_tmp = L_shl( L_tmp, shift );
976 92490 : if ( L_tmp == 0 )
977 3321 : a_e = add( 0, 0 );
978 92490 : if ( L_tmp != 0 )
979 89169 : a_e = sub( a_e, shift );
980 92490 : *ptr_e = a_e;
981 :
982 92490 : return ( L_tmp );
983 : }
984 :
985 : static const Word32 L_table_isqrt[48] = {
986 : 2147418112L, 2083389440L, 2024669184L, 1970667520L,
987 : 1920794624L, 1874460672L, 1831403520L, 1791098880L,
988 : 1753415680L, 1717960704L, 1684602880L, 1653145600L,
989 : 1623326720L, 1595080704L, 1568276480L, 1542782976L,
990 : 1518469120L, 1495334912L, 1473183744L, 1451950080L,
991 : 1431633920L, 1412169728L, 1393491968L, 1375469568L,
992 : 1358168064L, 1341521920L, 1325465600L, 1309933568L,
993 : 1294991360L, 1280507904L, 1266548736L, 1252982784L,
994 : 1239875584L, 1227161600L, 1214775296L, 1202847744L,
995 : 1191182336L, 1179910144L, 1168965632L, 1158283264L,
996 : 1147863040L, 1137770496L, 1127940096L, 1118306304L,
997 : 1108934656L, 1099825152L, 1090912256L, 1082261504L
998 : };
999 : /* table of table_isqrt[i] - table_isqrt[i+1] */
1000 : static const Word16 table_isqrt_diff[48] = {
1001 : 977, 896, 824, 761, 707, 657, 615, 575,
1002 : 541, 509, 480, 455, 431, 409, 389, 371,
1003 : 353, 338, 324, 310, 297, 285, 275, 264,
1004 : 254, 245, 237, 228, 221, 213, 207, 200,
1005 : 194, 189, 182, 178, 172, 167, 163, 159,
1006 : 154, 150, 147, 143, 139, 136, 132, 130
1007 : };
1008 :
1009 : static const Word16 shift[] = { 9, 10 };
1010 :
1011 3112 : Word32 Isqrt_lc1(
1012 : Word32 frac, /* i : Q31: normalized value (1.0 < frac <= 0.5) */
1013 : Word16 *exp /* i/o: exponent (value = frac x 2^exponent) */
1014 : )
1015 : {
1016 : Word16 i, a;
1017 : Word32 L_tmp;
1018 :
1019 3112 : IF( frac <= (Word32) 0 )
1020 : {
1021 0 : *exp = 0;
1022 0 : move16();
1023 0 : return 0x7fffffff; /*0x7fffffff*/
1024 : }
1025 :
1026 : /* If exponant odd -> shift right by 10 (otherwise 9) */
1027 3112 : L_tmp = L_shr( frac, shift[s_and( *exp, 1 )] );
1028 :
1029 : /* 1) -16384 to shift left and change sign */
1030 : /* 2) 32768 to Add 1 to Exponent like it was divided by 2 */
1031 : /* 3) We let the mac_r add another 0.5 because it imitates */
1032 : /* the behavior of shr on negative number that should */
1033 : /* not be rounded towards negative infinity. */
1034 : /* It replaces: */
1035 : /* *exp = negate(shr(sub(*exp, 1), 1)); move16(); */
1036 3112 : *exp = mac_r( 32768, *exp, -16384 );
1037 3112 : move16();
1038 :
1039 3112 : a = extract_l( L_tmp ); /* Extract b10-b24 */
1040 3112 : a = lshr( a, 1 );
1041 :
1042 3112 : i = mac_r( L_tmp, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
1043 :
1044 3112 : L_tmp = L_msu( L_table_isqrt[i], table_isqrt_diff[i], a ); /* table[i] << 16 - diff*a*2 */
1045 :
1046 3112 : return L_tmp;
1047 : }
1048 :
1049 : static const Word16 sqrt_table[49] = {
1050 : 16384, 16888, 17378, 17854, 18318, 18770, 19212,
1051 : 19644, 20066, 20480, 20886, 21283, 21674, 22058,
1052 : 22435, 22806, 23170, 23530, 23884, 24232, 24576,
1053 : 24915, 25249, 25580, 25905, 26227, 26545, 26859,
1054 : 27170, 27477, 27780, 28081, 28378, 28672, 28963,
1055 : 29251, 29537, 29819, 30099, 30377, 30652, 30924,
1056 : 31194, 31462, 31727, 31991, 32252, 32511, 32767
1057 : };
1058 :
1059 : /*! r: output value, Q31 */
1060 20250 : Word32 Sqrt_l(
1061 : Word32 L_x, /* i : input value, Q31 */
1062 : Word16 *exp /* o : right shift to be applied to result, Q1 */
1063 : )
1064 : {
1065 : /*
1066 : y = sqrt(x)
1067 :
1068 : x = f * 2^-e, 0.5 <= f < 1 (normalization)
1069 :
1070 : y = sqrt(f) * 2^(-e/2)
1071 :
1072 : a) e = 2k --> y = sqrt(f) * 2^-k (k = e div 2,
1073 : 0.707 <= sqrt(f) < 1)
1074 : b) e = 2k+1 --> y = sqrt(f/2) * 2^-k (k = e div 2,
1075 : 0.5 <= sqrt(f/2) < 0.707)
1076 : */
1077 :
1078 : Word16 e, i, a, tmp;
1079 : Word32 L_y;
1080 :
1081 20250 : if ( L_x <= 0 )
1082 : {
1083 0 : *exp = 0;
1084 0 : move16();
1085 0 : return L_deposit_l( 0 );
1086 : }
1087 :
1088 : #if 0 /* original version creates an overflow warning */
1089 : e = s_and(norm_l(L_x), 0xFFFE); /* get next lower EVEN norm. exp */
1090 : #else
1091 20250 : e = s_and( norm_l( L_x ), 0x7FFE ); /* get next lower EVEN norm. exp */
1092 : #endif
1093 20250 : L_x = L_shl( L_x, e ); /* L_x is normalized to [0.25..1) */
1094 20250 : *exp = e;
1095 20250 : move16(); /* return 2*exponent (or Q1) */
1096 :
1097 20250 : L_x = L_shr( L_x, 9 );
1098 20250 : a = extract_l( L_x ); /* Extract b10-b24 */
1099 20250 : a = lshr( a, 1 );
1100 :
1101 20250 : i = mac_r( L_x, -16 * 2 - 1, 16384 ); /* Extract b25-b31 minus 16 */
1102 :
1103 20250 : L_y = L_deposit_h( sqrt_table[i] ); /* table[i] << 16 */
1104 20250 : tmp = sub( sqrt_table[i], sqrt_table[i + 1] ); /* table[i] - table[i+1]) */
1105 20250 : L_y = L_msu( L_y, tmp, a ); /* L_y -= tmp*a*2 */
1106 :
1107 : /* L_y = L_shr (L_y, *exp); */ /* denormalization done by caller */
1108 :
1109 20250 : return ( L_y );
1110 : }
1111 :
1112 : #undef WMC_TOOL_SKIP
|
