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 : 3GPP TS26.258 Aug 24, 2023. IVAS Codec Version IVAS-FL-1.0
35 : ====================================================================================*/
36 :
37 : /*====================================================================================
38 : EVS Codec 3GPP TS26.443 Nov 04, 2021. Version 12.14.0 / 13.10.0 / 14.6.0 / 15.4.0 / 16.3.0
39 : ====================================================================================*/
40 :
41 : #include <stdlib.h>
42 : #include <assert.h>
43 : #include <stdint.h>
44 : #include "options.h"
45 : #include "stl.h"
46 : #include <math.h>
47 : #include "cnst.h"
48 : #include "prot_fx.h"
49 : #include "basop_util.h"
50 : #include "basop32.h"
51 : #include "wmc_auto.h"
52 : #include "prot_fx_enc.h"
53 : #include "ivas_prot_fx.h"
54 :
55 : #define INV_BANDS10 3277 /* 1/10 in Q15 */
56 : #define INV_BANDS9 3641 /* 1/9 in Q15 */
57 : #define INV_BANDS3 10923 /* 1/9 in Q15 */
58 : const Word16 b_hp400_fx[3] = { 3660, -7320, 3660 }; /* Q12 (/4) */
59 : const Word16 a_hp400_fx[3] = { 16384, 29280, -14160 };
60 : const Word16 a_hp400_ivas_fx[3] = { 4096, 7320, -3540 }; /*Q12*/
61 :
62 : #define WMC_TOOL_SKIP
63 :
64 : // conversion functions:
65 4794771 : Word32 float_to_fix( float number, Word32 Q )
66 : {
67 4794771 : assert( Q >= 0 );
68 4794771 : if ( number == 1.0f && Q == Q31 )
69 : {
70 0 : return ONE_IN_Q31;
71 : }
72 4794771 : if ( isnan( number ) )
73 : {
74 0 : number = 0;
75 : }
76 4794771 : assert( fabs( number ) < pow( 2, 31 - Q ) );
77 4794771 : Word32 ret = (Word32) ( number * ( (UWord32) 1 << Q ) );
78 4794771 : return ret;
79 : }
80 :
81 0 : float fix_to_float( Word32 number, Word32 Q )
82 : {
83 0 : assert( Q >= 0 );
84 0 : float ret = (float) number / ( (UWord32) 1 << Q );
85 0 : return ret;
86 : }
87 :
88 92160 : Word16 float_to_fix16( float number, Word16 Q )
89 : {
90 92160 : assert( Q >= 0 );
91 92160 : IF( isnan( number ) )
92 0 : return 0;
93 92160 : if ( number == 1.0f && Q == Q15 )
94 0 : return MAX16B;
95 92160 : if ( number == -1.0f && Q == Q15 )
96 0 : return MIN16B;
97 92160 : assert( fabs( number ) < pow( 2, 15 - Q ) );
98 92160 : Word16 ret = (Word16) ( number * ( (UWord16) 1 << Q ) );
99 92160 : return ret;
100 : }
101 :
102 0 : Word16 float_to_fix16_thrld( float number, Word16 Q )
103 : {
104 0 : assert( Q >= 0 );
105 0 : if ( number == 1.0f && Q == Q15 )
106 0 : return MAX16B;
107 0 : float limit = (float) pow( 2, 15 - Q );
108 : /*Add threshold*/
109 0 : if ( number > MAX16B_FLT )
110 : {
111 0 : number = MAX16B_FLT;
112 : }
113 0 : else if ( number < MIN16B_FLT )
114 : {
115 0 : number = MIN16B_FLT;
116 : }
117 0 : assert( number <= limit && number >= -limit );
118 0 : Word16 ret = (Word16) ( number * ( (UWord16) 1 << Q ) );
119 0 : return ret;
120 : }
121 :
122 68942 : float fix16_to_float( Word16 number, Word16 Q )
123 : {
124 68942 : assert( Q >= 0 );
125 68942 : float ret = (float) number / ( (UWord16) 1 << Q );
126 68942 : return ret;
127 : }
128 :
129 : // Float to 32-bit Mantissa and Exponent
130 4214584 : void f2me( float n, Word32 *mantissa, Word16 *expo )
131 : {
132 : Word32 e;
133 4214584 : float mf = (float) frexp( n, &e );
134 4214584 : *expo = (Word16) e;
135 4214584 : *mantissa = float_to_fix( mf, Q31 );
136 4214584 : }
137 :
138 : // 32-bit Mantissa and Exponent to Float
139 0 : float me2f( Word32 m, Word16 expo )
140 : {
141 0 : float mf = fix_to_float( m, Q31 );
142 0 : return (float) ldexp( mf, expo );
143 : }
144 :
145 : // Float buffer to 32-bit mantissa buffer and common exponent.
146 72 : void f2me_buf( const float *x, Word32 *m, Word16 *e, const Word32 n )
147 : {
148 72 : Word16 max_e = -32, tmp_e;
149 : Word32 i;
150 :
151 2107364 : for ( i = 0; i < n; i++ )
152 : {
153 2107292 : f2me( x[i], &m[i], &tmp_e );
154 2107292 : max_e = ( max_e > tmp_e ) ? max_e : tmp_e;
155 : }
156 :
157 2107364 : for ( i = 0; i < n; i++ )
158 : {
159 2107292 : f2me( x[i], &m[i], &tmp_e );
160 2107292 : m[i] = L_shr( m[i], max_e - tmp_e );
161 : }
162 :
163 72 : *e = max_e;
164 72 : }
165 :
166 : // 32-bit Mantissa buffer and exponent into float buffer.
167 0 : void me2f_buf( const Word32 *m, const Word16 e, float *out, const Word32 n )
168 : {
169 0 : for ( int i = 0; i < n; i++ )
170 : {
171 0 : out[i] = me2f( m[i], e );
172 : }
173 0 : }
174 :
175 : // Float to 16-bit Mantissa and Exponent
176 0 : void f2me_16( float n, Word16 *mantissa, Word16 *expo )
177 : {
178 : Word32 e;
179 0 : float mf = (float) frexp( n, &e );
180 0 : *expo = (Word16) e;
181 0 : *mantissa = float_to_fix16( mf, 15 );
182 0 : }
183 :
184 : // 16-bit Mantissa and Exponent to Float
185 0 : float me2f_16( Word16 m, Word16 expo )
186 : {
187 0 : float mf = fix16_to_float( m, 15 );
188 0 : return (float) ldexp( mf, expo );
189 : }
190 :
191 : // Float buffer to 16-bit mantissa buffer and common exponent.
192 0 : void f2me_buf_16( const float *x, Word16 *m, Word16 *e, const Word32 n )
193 : {
194 0 : Word16 max_e = -16, tmp_e;
195 : Word32 i;
196 :
197 0 : for ( i = 0; i < n; i++ )
198 : {
199 0 : f2me_16( x[i], &m[i], &tmp_e );
200 0 : max_e = ( max_e > tmp_e ) ? max_e : tmp_e;
201 : }
202 :
203 0 : for ( i = 0; i < n; i++ )
204 : {
205 0 : f2me_16( x[i], &m[i], &tmp_e );
206 0 : m[i] = shr( m[i], max_e - tmp_e );
207 : }
208 :
209 0 : *e = max_e;
210 0 : }
211 :
212 : // 16-bit Mantissa buffer and exponent into float buffer.
213 0 : void me2f_buf_16( const Word16 *m, const Word16 e, float *out, const Word32 n )
214 : {
215 0 : for ( int i = 0; i < n; i++ )
216 : {
217 0 : out[i] = me2f_16( m[i], e );
218 : }
219 0 : }
220 0 : void f2fix( float *var_flt, Word32 *var_fix, Word32 expo )
221 : {
222 0 : *var_fix = (Word32) ( *var_flt * pow( 2, 31 - expo ) );
223 0 : }
224 :
225 0 : void fix2f( Word32 *var_fix, float *var_flt, Word32 expo )
226 : {
227 0 : float mf = fix_to_float( *var_fix, 31 );
228 0 : *var_flt = (float) ldexp( mf, expo );
229 0 : }
230 :
231 0 : void f2fix_16( float *var_flt, Word16 *var_fix, Word32 expo )
232 : {
233 0 : *var_fix = (Word16) ( *var_flt * pow( 2, 15 - expo ) );
234 0 : }
235 :
236 0 : void fix2f_16( Word16 *var_fix, float *var_flt, Word32 expo )
237 : {
238 0 : float mf = fix16_to_float( *var_fix, 15 );
239 0 : *var_flt = (float) ldexp( mf, expo );
240 0 : }
241 :
242 : #undef WMC_TOOL_SKIP
243 :
244 : /*-------------------------------------------------------------------*
245 : * usdequant_fx()
246 : *
247 : * Uniform scalar de-quantizer routine
248 : *
249 : * Applies de-quantization based on scale and round operations.
250 : *-------------------------------------------------------------------*/
251 276181 : Word16 usdequant_fx( /* Qx*/
252 : const Word16 idx, /* i: quantizer index Q0*/
253 : const Word16 qlow, /* i: lowest codebook entry (index 0) Qx*/
254 : const Word16 delta /* i: quantization step Qx-1*/
255 : )
256 : {
257 : Word16 g;
258 : Word32 L_tmp;
259 :
260 : /*g = idx * delta + qlow;*/
261 276181 : L_tmp = L_deposit_l( qlow ); /*Qx */
262 276181 : L_tmp = L_mac( L_tmp, idx, delta ); /*Qx */
263 276181 : g = round_fx_sat( L_shl_sat( L_tmp, 16 ) ); /*Qx */
264 276181 : return ( g );
265 : }
266 :
267 188430 : Word32 usdequant32_fx( /* Qx*/
268 : const Word16 idx, /* i: quantizer index Q0*/
269 : const Word32 qlow, /* i: lowest codebook entry (index 0) Qx*/
270 : const Word32 delta /* i: quantization step Qx-1*/
271 : )
272 : {
273 : Word32 g;
274 : Word64 L_tmp;
275 :
276 : /*g = idx * delta + qlow;*/
277 188430 : L_tmp = W_deposit32_l( qlow ); /*Qx */
278 188430 : L_tmp = W_mac_32_16( L_tmp, delta, idx );
279 188430 : IF( GE_64( L_tmp, MAX_32 ) )
280 : {
281 42608 : g = MAX_32;
282 42608 : move32();
283 : }
284 : ELSE
285 : {
286 145822 : g = W_extract_l( L_tmp ); /*Qx */
287 : }
288 188430 : return ( g );
289 : }
290 :
291 : /*-------------------------------------------------------------------*
292 : * usquant()
293 : *
294 : * Uniform scalar quantizer according to MMSE criterion
295 : * (nearest neighbour in Euclidean space)
296 : *
297 : * Applies quantization based on scale and round operations.
298 : * Index of the winning codeword and the winning codeword itself are returned.
299 : *-------------------------------------------------------------------*/
300 176638 : Word16 usquant_fx( /* o: index of the winning codeword */
301 : const Word16 x, /* i: scalar value to quantize Qx*/
302 : Word16 *xq, /* o: quantized value Qx*/
303 : const Word16 qlow, /* i: lowest codebook entry (index 0) Qx*/
304 : const Word16 delta, /* i: quantization step Qx-1*/
305 : const Word16 cbsize /* i: codebook size */
306 : )
307 : {
308 : Word16 idx;
309 : Word16 tmp, exp;
310 : Word32 L_tmp;
311 : #ifndef ISSUE_1836_replace_overflow_libcom
312 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
313 : Flag Overflow = 0;
314 : move32();
315 : #endif
316 : #endif
317 :
318 : /* idx = (short)( (x - qlow)/delta + 0.5f); */
319 176638 : exp = norm_s( delta );
320 176638 : tmp = div_s( shl( 1, sub( 14, exp ) ), delta ); /*Q(29-exp-(Qx-1))->Q(30-exp-Qx) */
321 : #ifdef ISSUE_1836_replace_overflow_libcom
322 176638 : L_tmp = L_mult( sub_sat( x, qlow ), tmp ); /*Q(31-exp) */
323 176638 : idx = extract_l( L_shr_r( L_add( L_tmp, shl_sat( 1, sub( 30, exp ) ) ), sub( 31, exp ) ) ); /*Q0 */
324 : #else
325 : L_tmp = L_mult( sub_o( x, qlow, &Overflow ), tmp ); /*Q(31-exp) */
326 : idx = extract_l( L_shr_r( L_add( L_tmp, shl_o( 1, sub( 30, exp ), &Overflow ) ), sub( 31, exp ) ) ); /*Q0 */
327 : #endif
328 :
329 176638 : idx = s_min( idx, sub( cbsize, 1 ) );
330 176638 : idx = s_max( idx, 0 );
331 :
332 : /* *xq = idx*delta + qlow; */
333 176638 : L_tmp = L_deposit_l( qlow ); /*Qx */
334 176638 : L_tmp = L_mac( L_tmp, idx, delta ); /*Qx */
335 : #ifdef ISSUE_1836_replace_overflow_libcom
336 176638 : *xq = round_fx_sat( L_shl_sat( L_tmp, 16 ) ); /*Qx */
337 : #else
338 : *xq = round_fx_o( L_shl_o( L_tmp, 16, &Overflow ), &Overflow ); /*Qx */
339 : #endif
340 176638 : return idx;
341 : }
342 : /*-------------------------------------------------------------------*
343 : * Dot_product:
344 : *
345 : * Compute scalar product of <x[],y[]> using accumulator.
346 : * Performs no normalization, as opposed to Dot_product12()
347 : *-------------------------------------------------------------------*/
348 3228258 : Word32 Dot_product( /* o : Sum */
349 : const Word16 x[], /* i : 12bits: x vector */
350 : const Word16 y[], /* i : 12bits: y vector */
351 : const Word16 lg /* i : vector length */
352 : )
353 : {
354 : Word16 i;
355 : Word32 L_sum;
356 : Word64 L64_sum;
357 :
358 3228258 : L64_sum = 1;
359 3228258 : move64();
360 34256724 : FOR( i = 0; i < lg; i++ )
361 : {
362 31028466 : L64_sum = W_mac_16_16( L64_sum, x[i], y[i] );
363 : }
364 3228258 : L_sum = W_sat_l( L64_sum );
365 3228258 : return L_sum;
366 : }
367 : /*---------------------------------------------------------------------*
368 : * dotp_fx()
369 : *
370 : * Dot product of vector x[] and vector y[]
371 : *---------------------------------------------------------------------*/
372 :
373 6411 : Word32 dotp_fx( /* o : dot product of x[] and y[] */
374 : const Word16 x[], /* i : vector x[] */
375 : const Word16 y[], /* i : vector y[] */
376 : const Word16 n, /* i : vector length */
377 : Word16 *exp /* (o) : exponent of result (0..+30) */
378 : )
379 : {
380 : Word16 sft;
381 : Word32 L_sum;
382 :
383 6411 : assert( *exp == 0 );
384 :
385 6411 : L_sum = L_add( L_shr( Dot_product( x, y, n ), 1 ), 1 );
386 :
387 : /* Normalize acc in Q31 */
388 :
389 6411 : sft = norm_l( L_sum );
390 6411 : L_sum = L_shl( L_sum, sft );
391 :
392 6411 : *exp = sub( 30, sft );
393 6411 : move16(); /* exponent = 0..30 */
394 :
395 6411 : return L_sum;
396 : }
397 :
398 115123 : Word32 sum2_fx( /* o : sum of all squared vector elements Q(2x+1)*/
399 : const Word16 *vec, /* i : input vector Qx*/
400 : const Word16 lvec /* i : length of input vector */
401 : )
402 : {
403 : Word16 i;
404 : Word32 L_tmp;
405 : #ifndef ISSUE_1836_replace_overflow_libcom
406 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
407 : Flag Overflow = 0;
408 : move32();
409 : #endif
410 : #endif
411 115123 : L_tmp = L_deposit_l( 0 );
412 13111091 : FOR( i = 0; i < lvec; i++ )
413 : {
414 : #ifdef ISSUE_1836_replace_overflow_libcom
415 12995968 : L_tmp = L_mac_sat( L_tmp, vec[i], vec[i] ); /*Q(2x+1) */
416 : #else
417 : L_tmp = L_mac_o( L_tmp, vec[i], vec[i], &Overflow ); /*Q(2x+1) */
418 : #endif
419 : }
420 :
421 115123 : return L_tmp;
422 : }
423 :
424 0 : Word64 sum2_fx_no_sat( /* o : sum of all squared vector elements Q(2*Qx)*/
425 : const Word16 *vec, /* i : input vector Qx*/
426 : const Word16 lvec /* i : length of input vector */
427 : )
428 : {
429 : Word16 i;
430 : Word64 sum;
431 :
432 0 : sum = 0;
433 0 : move64();
434 0 : FOR( i = 0; i < lvec; i++ )
435 : {
436 0 : sum = W_mac0_16_16( sum, vec[i], vec[i] ); // 2*Qx
437 : }
438 :
439 0 : return sum;
440 : }
441 :
442 13146 : Word32 sum_32_fx(
443 : const Word32 *vec, /* i : input vector */
444 : const Word16 lvec, /* i : length of input vector */
445 : Word16 *e )
446 : {
447 : Word16 i, shift;
448 13146 : Word64 tmp = 0;
449 13146 : move64();
450 : Word32 ans;
451 :
452 61090 : FOR( i = 0; i < lvec; i++ )
453 : {
454 47944 : tmp = W_add( tmp, vec[i] ); // e
455 : }
456 13146 : shift = W_norm( tmp );
457 13146 : tmp = W_shl( tmp, shift ); // shift + (31 - e)
458 13146 : ans = W_extract_h( tmp ); // shift + (31 - e) - 32
459 13146 : *e = add( sub( *e, shift ), 32 );
460 13146 : move16();
461 :
462 13146 : return ans;
463 : }
464 :
465 70466 : Word32 sum2_fx_mod( /* o : sum of all squared vector elements Q(2x+1 -5)*/
466 : const Word16 *vec, /* i : input vector Qx*/
467 : const Word16 lvec /* i : length of input vector */
468 : )
469 : {
470 : Word16 i;
471 : Word32 L_tmp;
472 : #ifndef ISSUE_1836_replace_overflow_libcom
473 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
474 : Flag Overflow = 0;
475 : move32();
476 : #endif
477 : #endif
478 :
479 70466 : L_tmp = L_deposit_l( 0 );
480 45168706 : FOR( i = 0; i < lvec; i++ )
481 : {
482 : #ifdef ISSUE_1836_replace_overflow_libcom
483 45098240 : L_tmp = L_add_sat( L_tmp, L_shr( L_mult_sat( vec[i], vec[i] ), 9 ) );
484 : #else
485 : L_tmp = L_add_o( L_tmp, L_shr( L_mult_o( vec[i], vec[i], &Overflow ), 9 ), &Overflow );
486 : #endif
487 : }
488 :
489 70466 : return L_tmp;
490 : }
491 : /*-------------------------------------------------------------------*
492 : * Copy:
493 : *
494 : * Copy vector x[] to y[]
495 : *
496 : *-------------------------------------------------------------------*/
497 57229638 : void Copy(
498 : const Word16 x[], /* i : input vector */
499 : Word16 y[], /* o : output vector */
500 : const Word16 L /* i : vector length */
501 : )
502 : {
503 : Word16 i;
504 :
505 57229638 : IF( y < x )
506 : {
507 6371114789 : FOR( i = 0; i < L; i++ )
508 : {
509 6344297700 : y[i] = x[i];
510 6344297700 : move16();
511 : }
512 :
513 : /* Location of x and y may differ depending on platform/memory allocation. Since IF and ELSE has different complexity count, the early return is used instead of ELSE to ensure the same complexity number regardless of x and y memory addresses. */
514 26817089 : return;
515 : }
516 :
517 6854485414 : FOR( i = L - 1; i >= 0; i-- )
518 : {
519 6824072865 : y[i] = x[i];
520 6824072865 : move16();
521 : }
522 :
523 30412549 : return;
524 : }
525 : /*-------------------------------------------------------------------*
526 : * Copy64:
527 : *
528 : * Copy vector x[] to y[] (64 bits)
529 : *-------------------------------------------------------------------*/
530 8214 : void Copy64(
531 : const Word64 x[], /* i : input vector */
532 : Word64 y[], /* o : output vector */
533 : const Word16 L /* i : vector length */
534 : )
535 : {
536 : Word16 i;
537 8214 : IF( y < x )
538 : {
539 0 : FOR( i = 0; i < L; i++ )
540 : {
541 0 : y[i] = x[i];
542 0 : move64();
543 : }
544 :
545 : /* Location of x and y may differ depending on platform/memory allocation. Since IF and ELSE has different complexity count, the early return is used instead of ELSE to ensure the same complexity number regardless of x and y memory addresses. */
546 0 : return;
547 : }
548 :
549 35814 : FOR( i = L - 1; i >= 0; i-- )
550 : {
551 27600 : y[i] = x[i];
552 27600 : move64();
553 : }
554 :
555 8214 : return;
556 : }
557 :
558 464840 : void set64_fx(
559 : Word64 y[], /* i/o: Vector to set */
560 : const Word64 a, /* i : Value to set the vector to */
561 : const Word16 N /* i : Lenght of the vector */
562 : )
563 : {
564 : Word16 i;
565 27666646 : FOR( i = 0; i < N; i++ )
566 : {
567 27201806 : y[i] = a;
568 27201806 : move64();
569 : }
570 :
571 464840 : return;
572 : }
573 :
574 57737 : void Copy_pword(
575 : const PWord16 x[], /* i : input vector */
576 : PWord16 y[], /* o : output vector */
577 : const Word16 L /* i : vector length */
578 : )
579 : {
580 : Word16 i;
581 :
582 57737 : IF( y < x )
583 : {
584 1090540 : FOR( i = 0; i < L; i++ )
585 : {
586 1038224 : y[i].v.im = x[i].v.im;
587 1038224 : y[i].v.re = x[i].v.re;
588 1038224 : move16();
589 1038224 : move16();
590 : }
591 :
592 : /* Location of x and y may differ depending on platform/memory allocation. Since IF and ELSE has different complexity count, the early return is used instead of ELSE to ensure the same complexity number regardless of x and y memory addresses. */
593 52316 : return;
594 : }
595 :
596 331901 : FOR( i = L - 1; i >= 0; i-- )
597 : {
598 326480 : y[i].v.im = x[i].v.im;
599 326480 : y[i].v.re = x[i].v.re;
600 326480 : move16();
601 326480 : move16();
602 : }
603 :
604 5421 : return;
605 : }
606 : /*-------------------------------------------------------------------*
607 : * Copy32:
608 : *
609 : * Copy vector x[] to y[] (32 bits)
610 : *-------------------------------------------------------------------*/
611 217617679 : void Copy32(
612 : const Word32 x[], /* i : input vector */
613 : Word32 y[], /* o : output vector */
614 : const Word16 L /* i : vector length */
615 : )
616 : {
617 : Word16 i;
618 217617679 : IF( y < x )
619 : {
620 27482448068 : FOR( i = 0; i < L; i++ )
621 : {
622 27372618863 : y[i] = x[i];
623 27372618863 : move32();
624 : }
625 :
626 109829205 : return;
627 : }
628 :
629 24950249707 : FOR( i = L - 1; i >= 0; i-- )
630 : {
631 24842461233 : y[i] = x[i];
632 24842461233 : move32();
633 : }
634 : }
635 :
636 128 : void set8_fx(
637 : Word8 y[], /* i/o: Vector to set */
638 : const Word8 a, /* i : Value to set the vector to */
639 : const Word16 N /* i : Lenght of the vector */
640 : )
641 : {
642 : Word16 i;
643 :
644 1536 : FOR( i = 0; i < N; i++ )
645 : {
646 1408 : y[i] = a;
647 1408 : move16();
648 : }
649 :
650 128 : return;
651 : }
652 :
653 : /*-------------------------------------------------------------------*
654 : * set16_fx()
655 : * set32_fx()
656 : *
657 : * Set the vector elements to a value
658 : *-------------------------------------------------------------------*/
659 156042033 : void set16_fx(
660 : Word16 y[], /* i/o: Vector to set */
661 : const Word16 a, /* i : Value to set the vector to */
662 : const Word16 N /* i : Lenght of the vector */
663 : )
664 : {
665 : Word16 i;
666 :
667 15854939901 : FOR( i = 0; i < N; i++ )
668 : {
669 15698897868 : y[i] = a;
670 15698897868 : move16();
671 : }
672 :
673 156042033 : return;
674 : }
675 :
676 522927717 : void set32_fx(
677 : Word32 y[], /* i/o: Vector to set */
678 : const Word32 a, /* i : Value to set the vector to */
679 : const Word16 N /* i : Lenght of the vector */
680 : )
681 : {
682 : Word16 i;
683 :
684 46337696079 : FOR( i = 0; i < N; i++ )
685 : {
686 45814768362 : y[i] = a;
687 45814768362 : move32();
688 : }
689 :
690 522927717 : return;
691 : }
692 : /*-------------------------------------------------------------------*
693 : * Copy_Scale_sig
694 : *
695 : * Up/down scale a 16 bits vector x and move it into y
696 : *-------------------------------------------------------------------*/
697 8355123 : void Copy_Scale_sig(
698 : const Word16 x[], /* i : signal to scale input Qx */
699 : Word16 y[], /* o : scaled signal output Qx */
700 : const Word16 lg, /* i : size of x[] Q0 */
701 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
702 : )
703 : {
704 : Word16 i;
705 : Word16 tmp;
706 : #ifndef ISSUE_1836_replace_overflow_libcom
707 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
708 : Flag Overflow = 0;
709 : move32();
710 : #endif
711 : #endif
712 8355123 : IF( exp0 == 0 )
713 : {
714 1504123000 : FOR( i = 0; i < lg; i++ )
715 : {
716 1502339246 : y[i] = x[i];
717 1502339246 : move16();
718 : }
719 1783754 : return;
720 : }
721 6571369 : IF( exp0 < 0 )
722 : {
723 4560075 : tmp = shl( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
724 2589830421 : FOR( i = 0; i < lg; i++ )
725 : {
726 2585270346 : y[i] = msu_r( 0, x[i], tmp );
727 2585270346 : move16();
728 : }
729 4560075 : return;
730 : }
731 1346905593 : FOR( i = 0; i < lg; i++ )
732 : {
733 : #ifdef ISSUE_1836_replace_overflow_libcom
734 1344894299 : y[i] = shl_sat( x[i], exp0 );
735 : #else
736 : y[i] = shl_o( x[i], exp0, &Overflow );
737 : #endif
738 1344894299 : move16(); /* saturation can occur here */
739 : }
740 : }
741 : /*-------------------------------------------------------------------*
742 : * Copy_Scale_sig
743 : *
744 : * Up/down scale a 16 bits vector x and move it into y
745 : *-------------------------------------------------------------------*/
746 2542235 : void Copy_Scale_sig_16_32_DEPREC(
747 : const Word16 x[], /* i : signal to scale input Qx */
748 : Word32 y[], /* o : scaled signal output Qx */
749 : const Word16 lg, /* i : size of x[] Q0 */
750 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
751 : )
752 : {
753 : Word16 i;
754 : Word16 tmp;
755 : #ifndef ISSUE_1836_replace_overflow_libcom
756 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
757 : Flag Overflow = 0;
758 : move32();
759 : #endif
760 : #endif
761 :
762 :
763 2542235 : IF( exp0 == 0 )
764 : {
765 1054867907 : FOR( i = 0; i < lg; i++ )
766 : {
767 1053752900 : y[i] = L_deposit_l( x[i] );
768 1053752900 : move32();
769 : }
770 1115007 : return;
771 : }
772 1427228 : IF( exp0 < 0 )
773 : {
774 : /*Should not happen */
775 43253059 : FOR( i = 0; i < lg; i++ )
776 : {
777 : #ifdef ISSUE_1836_replace_overflow_libcom
778 43103232 : y[i] = L_deposit_l( shl_sat( x[i], exp0 ) );
779 : #else
780 : y[i] = L_deposit_l( shl_o( x[i], exp0, &Overflow ) );
781 : #endif
782 43103232 : move32();
783 : }
784 149827 : return;
785 : }
786 : #ifdef DEBUGGING
787 : if ( exp0 >= 16 )
788 : {
789 : printf( "Issue in Copy_Scale_sig_16_32_DEPREC\n" );
790 : }
791 : #else
792 1277401 : assert( exp0 < 16 );
793 : #endif
794 : #ifdef ISSUE_1836_replace_overflow_libcom
795 1277401 : tmp = shl_sat( 1, exp0 );
796 : #else
797 : tmp = shl_o( 1, exp0, &Overflow );
798 : #endif
799 354895701 : FOR( i = 0; i < lg; i++ )
800 : {
801 353618300 : y[i] = L_mult0( x[i], tmp );
802 353618300 : move32(); /* saturation can occur here */
803 : }
804 : }
805 :
806 11457880 : void Copy_Scale_sig_16_32_no_sat(
807 : const Word16 x[], /* i : signal to scale input Qx */
808 : Word32 y[], /* o : scaled signal output Qx */
809 : const Word16 lg, /* i : size of x[] Q0 */
810 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
811 : )
812 : {
813 : Word16 i;
814 : Word32 L_tmp;
815 : #ifndef ISSUE_1836_replace_overflow_libcom
816 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
817 : Flag Overflow = 0;
818 : move32();
819 : #endif
820 : #endif
821 :
822 :
823 11457880 : IF( exp0 == 0 )
824 : {
825 9440625 : FOR( i = 0; i < lg; i++ )
826 : {
827 9417145 : y[i] = L_deposit_l( x[i] );
828 9417145 : move32();
829 : }
830 23480 : return;
831 : }
832 11434400 : IF( exp0 < 0 )
833 : {
834 : /*Should not happen */
835 268037422 : FOR( i = 0; i < lg; i++ )
836 : {
837 : #ifdef ISSUE_1836_replace_overflow_libcom
838 267507575 : y[i] = L_deposit_l( shl_sat( x[i], exp0 ) );
839 : #else
840 : y[i] = L_deposit_l( shl_o( x[i], exp0, &Overflow ) );
841 : #endif
842 267507575 : move32();
843 : }
844 529847 : return;
845 : }
846 : #ifdef ISSUE_1836_replace_overflow_libcom
847 10904553 : L_tmp = L_shl_sat( 1, exp0 - 1 );
848 : #else
849 : L_tmp = L_shl_o( 1, exp0 - 1, &Overflow );
850 : #endif
851 :
852 10904553 : IF( L_tmp >= 0x7FFF )
853 : {
854 26207142 : FOR( i = 0; i < lg; i++ )
855 : {
856 : // y[i] = L_mult0(x[i], L_tmp);
857 25575851 : y[i] = W_extract_l( W_mult_32_16( L_tmp, x[i] ) );
858 25575851 : move32(); /* Overflow can occur here */
859 : }
860 631291 : return;
861 : }
862 : // ELSE
863 : {
864 10273262 : Word16 tmp = extract_l( L_tmp );
865 5190937218 : FOR( i = 0; i < lg; i++ )
866 : {
867 5180663956 : y[i] = L_mult( x[i], tmp );
868 5180663956 : move32();
869 : }
870 : }
871 : }
872 :
873 6311568 : void Copy_Scale_sig_32_16(
874 : const Word32 x[], /* i : signal to scale input Qx */
875 : Word16 y[], /* o : scaled signal output Qx */
876 : const Word16 lg, /* i : size of x[] Q0 */
877 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
878 : )
879 : {
880 : Word16 i;
881 : Word16 tmp;
882 : #ifndef ISSUE_1836_replace_overflow_libcom
883 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
884 : Flag Overflow = 0;
885 : move32();
886 : #endif
887 : #endif
888 :
889 6311568 : tmp = add( 16, exp0 );
890 6311568 : IF( tmp != 0 )
891 : {
892 4093910814 : FOR( i = 0; i < lg; i++ )
893 : {
894 : #ifdef ISSUE_1836_replace_overflow_libcom
895 4087612434 : y[i] = round_fx_sat( L_shl_sat( x[i], tmp ) );
896 : #else
897 : y[i] = round_fx_o( L_shl_o( x[i], tmp, &Overflow ), &Overflow );
898 : #endif
899 4087612434 : move16();
900 : }
901 : }
902 : ELSE
903 : {
904 12209152 : FOR( i = 0; i < lg; i++ )
905 : {
906 : #ifdef ISSUE_1836_replace_overflow_libcom
907 12195964 : y[i] = round_fx_sat( x[i] );
908 : #else
909 : y[i] = round_fx_o( x[i], &Overflow );
910 : #endif
911 12195964 : move16();
912 : }
913 : }
914 6311568 : }
915 :
916 : /*-------------------------------------------------------------------*
917 : * Scale_sig32
918 : *
919 : * Up/down scale a 32 bits vector
920 : *-------------------------------------------------------------------*/
921 97098624 : void Scale_sig32(
922 : Word32 x[], /* i/o: signal to scale Qx */
923 : const Word16 lg, /* i : size of x[] Q0 */
924 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
925 : )
926 : {
927 : Word16 i;
928 : #ifndef ISSUE_1836_replace_overflow_libcom
929 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
930 : Flag Overflow = 0;
931 : move32();
932 : #endif
933 : #endif
934 97098624 : IF( 0 == exp0 )
935 : {
936 27134319 : return;
937 : }
938 :
939 23458939410 : FOR( i = 0; i < lg; i++ )
940 : {
941 : #ifdef ISSUE_1836_replace_overflow_libcom
942 23388975105 : x[i] = L_shl_sat( x[i], exp0 );
943 : #else
944 : x[i] = L_shl_o( x[i], exp0, &Overflow );
945 : #endif
946 23388975105 : move32(); /* saturation can occur here */
947 : }
948 : }
949 :
950 : /*------------------------------------------------------------------*
951 : * function Random_Fill
952 : *
953 : * Signed 16 bits random generator.
954 : * (Avoids Store of Seed to Memory for 'n' Random Values and
955 : * Combines Scaling Operation.)
956 : *------------------------------------------------------------------*/
957 60697 : void Random_Fill(
958 : Word16 *seed, /* i/o: random seed */
959 : Word16 n, /* i : number of values */
960 : Word16 *y, /* o : output values */
961 : Word16 scaling /* i : scaling of values */
962 : )
963 : {
964 : Word16 i;
965 : Word16 local_seed;
966 :
967 60697 : local_seed = *seed;
968 60697 : move16();
969 5158873 : FOR( i = 0; i < n; i++ )
970 : {
971 5098176 : local_seed = extract_l( L_mac0( 13849L, local_seed, 31821 ) );
972 5098176 : *y++ = shr( local_seed, scaling );
973 5098176 : move16();
974 : }
975 60697 : *seed = local_seed;
976 60697 : move16();
977 60697 : }
978 : /*-------------------------------------------------------------------*
979 : * Scale_sig
980 : * Up/down scale a 16 bits vector
981 : *-------------------------------------------------------------------*/
982 35471681 : void Scale_sig(
983 : Word16 x[], /* i/o: signal to scale Qx */
984 : const Word16 lg, /* i : size of x[] Q0 */
985 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
986 : )
987 : {
988 : Word16 i;
989 : Word16 tmp;
990 :
991 35471681 : IF( exp0 > 0 )
992 : {
993 6095890022 : FOR( i = 0; i < lg; i++ )
994 : {
995 6088678200 : x[i] = shl_sat( x[i], exp0 );
996 6088678200 : move16(); /* saturation can occur here */
997 : }
998 7211822 : return;
999 : }
1000 28259859 : IF( exp0 < 0 )
1001 : {
1002 9212469 : tmp = shl_sat( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
1003 5416121045 : FOR( i = 0; i < lg; i++ )
1004 : {
1005 5406908576 : x[i] = msu_r_sat( 0, x[i], tmp );
1006 5406908576 : move16(); /* msu instead of mac because factor is negative */
1007 : }
1008 9212469 : return;
1009 : }
1010 : }
1011 :
1012 : /*-------------------------------------------------------------------*
1013 : * scale_sig
1014 : * Up/down scale a 16 bits vector
1015 : *-------------------------------------------------------------------*/
1016 252898 : void scale_sig(
1017 : Word16 x[], /* i/o: signal to scale Qx */
1018 : const Word16 lg, /* i : size of x[] Q0 */
1019 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx exp */
1020 : )
1021 : {
1022 : Word16 i;
1023 :
1024 252898 : IF( exp0 != 0 )
1025 : {
1026 39384576 : FOR( i = 0; i < lg; i++ )
1027 : {
1028 39259990 : x[i] = shl( x[i], exp0 );
1029 39259990 : move16();
1030 : }
1031 : }
1032 252898 : }
1033 :
1034 : /*---------------------------------------------------------------------*
1035 : * mean_fx()
1036 : *
1037 : *---------------------------------------------------------------------*/
1038 18535 : Word16 mean_fx( /* o : mean of vector */
1039 : const Word16 *vec_fx, /* i : input vector */
1040 : const Word16 lvec_fx /* i : length of input vector */
1041 : )
1042 : {
1043 : Word16 tmp;
1044 : // PMT("TBV : this function could be written differently to minimize the risk of saturation");
1045 18535 : tmp = sum16_fx( vec_fx, lvec_fx );
1046 18535 : tmp = mult_r( tmp, div_s( 1, lvec_fx ) );
1047 :
1048 18535 : return tmp;
1049 : }
1050 :
1051 0 : Word16 mean_no_sat_fx( /* o : mean of vector Qx */
1052 : const Word16 *vec_fx, /* i : input vector Qx */
1053 : const Word16 lvec_fx /* i : length of input vector */
1054 : )
1055 : {
1056 : Word16 i;
1057 0 : Word32 L_tmp = 0;
1058 0 : move32();
1059 0 : FOR( i = 0; i < lvec_fx; ++i )
1060 : {
1061 0 : L_tmp = L_add( L_tmp, vec_fx[i] );
1062 : }
1063 0 : L_tmp = Mpy_32_16_1( L_tmp, div_s( 1, lvec_fx ) ); /* Qx */
1064 :
1065 0 : return extract_l( L_tmp );
1066 : }
1067 0 : Word32 mean_no_sat_Word32_fx( /* o : mean of vector Qx */
1068 : const Word32 *vec_fx, /* i : input vector Qx */
1069 : const Word16 lvec_fx, /* i : length of input vector */
1070 : const Word16 gb )
1071 : {
1072 : Word16 i;
1073 0 : Word32 L_tmp = 0;
1074 0 : move32();
1075 0 : FOR( i = 0; i < lvec_fx; ++i )
1076 : {
1077 0 : L_tmp = L_add( L_tmp, L_shr( vec_fx[i], gb ) );
1078 : }
1079 0 : L_tmp = Mpy_32_16_1( L_tmp, div_s( 1, lvec_fx ) ); /* Qx-gb */
1080 :
1081 0 : return L_tmp;
1082 : }
1083 : /*-------------------------------------------------------------------*
1084 : * Vr_add
1085 : *
1086 : * Add two Word16 vectors together integer by integer
1087 : *-------------------------------------------------------------------*/
1088 855698 : void Vr_add(
1089 : const Word16 *in1, /* i : Input vector 1 */
1090 : const Word16 *in2, /* i : Input vector 2 */
1091 : Word16 *out, /* o : Output vector that contains vector 1 + vector 2 */
1092 : Word16 Len /* i : Vector lenght */
1093 : )
1094 : {
1095 : Word16 i;
1096 :
1097 37111250 : FOR( i = 0; i < Len; i++ )
1098 : {
1099 36255552 : out[i] = add_sat( in1[i], in2[i] );
1100 36255552 : move16();
1101 : }
1102 855698 : }
1103 :
1104 349424 : void sort_fx(
1105 : Word16 *r, /* i/o: Vector to be sorted in place */
1106 : Word16 lo, /* i : Low limit of sorting range */
1107 : Word16 up /* I : High limit of sorting range */
1108 : )
1109 : {
1110 : Word16 i, j, i1;
1111 : Word16 tempr;
1112 :
1113 5585534 : FOR( i = sub( up, 1 ); i >= lo; i-- )
1114 : {
1115 5236110 : i1 = add( i, 1 );
1116 5236110 : tempr = r[i];
1117 5236110 : move16();
1118 5236110 : move16(); /*supplementary move for the j-1 PTR initialization*/
1119 5241958 : FOR( j = i1; j <= up; j++ )
1120 : {
1121 5241410 : IF( LE_16( tempr, r[j] ) )
1122 : {
1123 5235562 : BREAK;
1124 : }
1125 :
1126 5848 : r[j - 1] = r[j];
1127 5848 : move16();
1128 : }
1129 5236110 : r[j - 1] = tempr;
1130 5236110 : move16();
1131 : }
1132 349424 : }
1133 :
1134 0 : void sort_32_fx(
1135 : Word32 *r, /* i/o: Vector to be sorted in place */
1136 : const Word16 lo, /* i : Low limit of sorting range */
1137 : const Word16 up /* I : High limit of sorting range */
1138 : )
1139 : {
1140 : Word16 i, j;
1141 : Word32 tempr;
1142 0 : FOR( i = sub( up, 1 ); i >= lo; i-- )
1143 : {
1144 0 : tempr = r[i];
1145 0 : move32();
1146 0 : FOR( j = add( i, 1 ); j <= up; j++ )
1147 : {
1148 0 : IF( LE_32( tempr, r[j] ) )
1149 : {
1150 0 : BREAK;
1151 : }
1152 0 : r[j - 1] = r[j];
1153 0 : move32();
1154 : }
1155 :
1156 0 : r[j - 1] = tempr;
1157 0 : move32();
1158 : }
1159 :
1160 0 : return;
1161 : }
1162 :
1163 307894 : Word16 minimum_fx( /* o : index of the minimum value in the input vector */
1164 : const Word16 *vec_fx, /* i : input vector */
1165 : const Word16 lvec_fx, /* i : length of input vector */
1166 : Word16 *min_fx /* o : minimum value in the input vector */
1167 : )
1168 : {
1169 : Word16 j, ind;
1170 : Word16 tmp;
1171 307894 : ind = 0;
1172 307894 : move16();
1173 307894 : tmp = vec_fx[0];
1174 307894 : move16();
1175 :
1176 2905900 : FOR( j = 1; j < lvec_fx; j++ )
1177 : {
1178 2598006 : if ( LT_16( vec_fx[j], tmp ) )
1179 : {
1180 489137 : ind = j;
1181 489137 : move16();
1182 : /*tmp = vec_fx[j]; move16(); */
1183 : }
1184 2598006 : tmp = s_min( tmp, vec_fx[j] );
1185 : }
1186 :
1187 307894 : *min_fx = tmp;
1188 307894 : move16();
1189 :
1190 307894 : return ind;
1191 : }
1192 :
1193 1804863 : Word16 maximum_fx( /* o : index of the maximum value in the input vector */
1194 : const Word16 *vec_fx, /* i : input vector */
1195 : const Word16 lvec_fx, /* i : length of input vector */
1196 : Word16 *max_fx /* o : maximum value in the input vector */
1197 : )
1198 : {
1199 : Word16 j, ind;
1200 : Word16 tmp;
1201 1804863 : ind = 0;
1202 1804863 : move16();
1203 1804863 : tmp = vec_fx[0];
1204 1804863 : move16();
1205 :
1206 66487281 : FOR( j = 1; j < lvec_fx; j++ )
1207 : {
1208 64682418 : if ( GT_16( vec_fx[j], tmp ) )
1209 : {
1210 1926867 : ind = j;
1211 1926867 : move16();
1212 : }
1213 64682418 : tmp = s_max( tmp, vec_fx[j] );
1214 : }
1215 1804863 : *max_fx = tmp;
1216 1804863 : move16();
1217 :
1218 1804863 : return ind;
1219 : }
1220 :
1221 0 : Word16 maximum_exp_fx( /* o : index of the maximum value in the input vector */
1222 : const Word16 *vec_fx, /* i : input vector */
1223 : const Word16 *exp_vec, /* i : exponents of input vector */
1224 : const Word16 lvec_fx /* i : length of input vector */
1225 : )
1226 : {
1227 : Word16 j, ind;
1228 : Word16 tmp, exp;
1229 0 : ind = 0;
1230 0 : move16();
1231 0 : tmp = vec_fx[0];
1232 0 : move16();
1233 0 : exp = exp_vec[0];
1234 0 : move16();
1235 :
1236 0 : FOR( j = 1; j < lvec_fx; j++ )
1237 : {
1238 0 : IF( LT_16( tmp, shr_sat( vec_fx[j], sub( exp, exp_vec[j] ) ) ) )
1239 : {
1240 0 : ind = j;
1241 0 : move16();
1242 0 : tmp = vec_fx[j];
1243 0 : move16();
1244 0 : exp = exp_vec[j];
1245 0 : move16();
1246 : }
1247 : }
1248 :
1249 0 : return ind;
1250 : }
1251 :
1252 : /*---------------------------------------------------------------------*
1253 : * maximum_abs_16_fx()
1254 : *
1255 : * Find index and value of the absolute maximum in a vector
1256 : *---------------------------------------------------------------------*/
1257 :
1258 1403 : Word16 maximum_abs_16_fx( /* o : index of the maximum abs value in the input vector */
1259 : const Word16 *vec, /* i : input vector */
1260 : const Word16 lvec, /* i : length of input vector */
1261 : Word16 *max_val /* o : maximum value in the input vector */
1262 : )
1263 : {
1264 : Word16 j, ind;
1265 : Word16 tmp;
1266 1403 : ind = 0;
1267 1403 : move16();
1268 1403 : tmp = abs_s( vec[0] );
1269 :
1270 530192 : FOR( j = 1; j < lvec; j++ )
1271 : {
1272 528789 : if ( GT_16( abs_s( vec[j] ), tmp ) )
1273 : {
1274 2843 : ind = j;
1275 2843 : move16();
1276 : }
1277 528789 : tmp = s_max( tmp, abs_s( vec[j] ) );
1278 : }
1279 1403 : *max_val = tmp;
1280 1403 : move16();
1281 :
1282 1403 : return ind;
1283 : }
1284 :
1285 : /*---------------------------------------------------------------------*
1286 : * minimum_abs32_fx()
1287 : *
1288 : * Find index and value of the absolute minimum in a vector
1289 : *---------------------------------------------------------------------*/
1290 0 : Word16 minimum_abs32_fx( /* o : index of the minimum value in the input vector */
1291 : const Word32 *vec_fx, /* i : input vector */
1292 : const Word16 lvec_fx, /* i : length of input vector */
1293 : Word32 *min_fx /* o : minimum value in the input vector */
1294 : )
1295 : {
1296 : Word16 j, ind;
1297 : Word32 tmp;
1298 0 : ind = 0;
1299 0 : move16();
1300 0 : tmp = vec_fx[0];
1301 0 : move16();
1302 :
1303 0 : FOR( j = 1; j < lvec_fx; j++ )
1304 : {
1305 0 : IF( LT_32( L_abs( vec_fx[j] ), tmp ) )
1306 : {
1307 0 : ind = j;
1308 0 : move16();
1309 : /*tmp = vec_fx[j]; move16(); */
1310 : }
1311 0 : tmp = L_min( tmp, L_abs( vec_fx[j] ) );
1312 : }
1313 :
1314 0 : *min_fx = tmp;
1315 0 : move16();
1316 :
1317 0 : return ind;
1318 : }
1319 :
1320 : /*---------------------------------------------------------------------*
1321 : * minimum_32_fx()
1322 : *
1323 : * Find index and value of the minimum in a vector
1324 : *---------------------------------------------------------------------*/
1325 :
1326 2 : Word16 minimum_32_fx( /* o : index of the minimum value in the input vector */
1327 : const Word32 *vec_fx, /* i : input vector */
1328 : const Word16 lvec_fx, /* i : length of input vector */
1329 : Word32 *min_fx /* o : minimum value in the input vector */
1330 : )
1331 : {
1332 : Word16 j, ind;
1333 : Word32 tmp;
1334 2 : ind = 0;
1335 2 : move16();
1336 2 : tmp = vec_fx[0];
1337 2 : move16();
1338 :
1339 8 : FOR( j = 1; j < lvec_fx; j++ )
1340 : {
1341 6 : if ( LT_32( vec_fx[j], tmp ) )
1342 : {
1343 0 : ind = j;
1344 0 : move16();
1345 : /*tmp = vec_fx[j]; move32(); */
1346 : }
1347 6 : tmp = L_min( tmp, vec_fx[j] );
1348 : }
1349 2 : if ( min_fx != NULL )
1350 : {
1351 2 : *min_fx = tmp;
1352 : }
1353 2 : move32();
1354 :
1355 2 : return ind;
1356 : }
1357 :
1358 : /*---------------------------------------------------------------------*
1359 : * maximum_32_fx()
1360 : *
1361 : * Find index and value of the maximum in a vector
1362 : *---------------------------------------------------------------------*/
1363 :
1364 480194 : Word16 maximum_32_fx( /* o : index of the maximum value in the input vector */
1365 : const Word32 *vec, /* i : input vector */
1366 : const Word16 lvec, /* i : length of input vector */
1367 : Word32 *max_val /* o : maximum value in the input vector */
1368 : )
1369 : {
1370 : Word16 j, ind;
1371 : Word32 tmp;
1372 480194 : ind = 0;
1373 480194 : move16();
1374 480194 : tmp = vec[0];
1375 480194 : move16();
1376 :
1377 8115492 : FOR( j = 1; j < lvec; j++ )
1378 : {
1379 7635298 : IF( GT_32( vec[j], tmp ) )
1380 : {
1381 362744 : ind = j;
1382 362744 : move16();
1383 : }
1384 7635298 : tmp = L_max( tmp, vec[j] );
1385 : }
1386 480194 : if ( max_val != NULL )
1387 : {
1388 480194 : *max_val = tmp;
1389 : }
1390 480194 : move32();
1391 :
1392 480194 : return ind;
1393 : }
1394 :
1395 2191440 : Word16 maximum_abs_32_fx( /* o : index of the maximum abs value in the input vector */
1396 : const Word32 *vec, /* i : input vector */
1397 : const Word16 lvec, /* i : length of input vector */
1398 : Word32 *max_val /* o : maximum value in the input vector */
1399 : )
1400 : {
1401 : Word16 j, ind;
1402 : Word32 tmp;
1403 2191440 : ind = 0;
1404 2191440 : move16();
1405 2191440 : tmp = L_abs( vec[0] );
1406 :
1407 613761202 : FOR( j = 1; j < lvec; j++ )
1408 : {
1409 611569762 : if ( GT_32( L_abs( vec[j] ), tmp ) )
1410 : {
1411 3341042 : ind = j;
1412 3341042 : move16();
1413 : }
1414 611569762 : tmp = L_max( tmp, L_abs( vec[j] ) );
1415 : }
1416 2191440 : *max_val = tmp;
1417 2191440 : move32();
1418 :
1419 2191440 : return ind;
1420 : }
1421 :
1422 :
1423 : /*----------------------------------------------------------------
1424 : *Function:
1425 : *Finds number of shifts to normalize a 16-bit array variable.
1426 : *Return value
1427 : *Number of shifts
1428 : *----------------------------------------------------------------*/
1429 26931 : Word16 Exp16Array(
1430 : const Word16 n, /* (i): Array size */
1431 : const Word16 *sx /* (i): Data array */
1432 : )
1433 : {
1434 : Word16 k;
1435 : Word16 exp;
1436 : Word16 sMax;
1437 : Word16 sAbs;
1438 :
1439 26931 : sMax = abs_s( sx[0] );
1440 26931 : move16();
1441 :
1442 1801856 : FOR( k = 1; k < n; k++ )
1443 : {
1444 1774925 : sAbs = abs_s( sx[k] );
1445 1774925 : sMax = s_max( sMax, sAbs );
1446 : }
1447 :
1448 26931 : exp = norm_s( sMax );
1449 26931 : return exp;
1450 : }
1451 :
1452 0 : Word16 Exp32Array(
1453 : const Word16 n, /* (i): Array size */
1454 : const Word32 *sx /* (i): Data array */
1455 : )
1456 : {
1457 : Word16 k;
1458 : Word16 exp;
1459 : Word32 L_Max;
1460 : Word32 L_Abs;
1461 :
1462 0 : L_Max = L_abs( sx[0] );
1463 0 : FOR( k = 1; k < n; k++ )
1464 : {
1465 0 : L_Abs = L_abs( sx[k] );
1466 0 : L_Max = L_max( L_Max, L_Abs );
1467 : }
1468 0 : exp = norm_l( L_Max );
1469 :
1470 0 : if ( L_Max == 0 )
1471 : {
1472 0 : exp = 31;
1473 0 : move16();
1474 : }
1475 0 : return exp;
1476 : }
1477 :
1478 4754 : Word32 sum16_32_fx( /* o : sum of all vector elements Qx*/
1479 : const Word16 *vec, /* i : input vector Qx*/
1480 : const Word16 lvec /* i : length of input vector */
1481 : )
1482 : {
1483 : Word16 i;
1484 : Word32 tmp, L_var;
1485 4754 : tmp = 0;
1486 4754 : move16();
1487 24788 : FOR( i = 0; i < lvec; i++ )
1488 : {
1489 20034 : L_var = L_deposit_l( vec[i] );
1490 20034 : tmp = L_add( tmp, L_var ); /*Qx */
1491 : }
1492 :
1493 4754 : return tmp;
1494 : }
1495 :
1496 1803 : Word32 sum32_sat( /* o : sum of all vector elements Qx*/
1497 : const Word32 *vec, /* i : input vector Qx*/
1498 : const Word16 lvec /* i : length of input vector */
1499 : )
1500 : {
1501 : Word16 i;
1502 : Word32 tmp;
1503 1803 : tmp = 0;
1504 1803 : move16();
1505 117195 : FOR( i = 0; i < lvec; i++ )
1506 : {
1507 115392 : tmp = L_add_sat( tmp, vec[i] ); /*Qx */
1508 : }
1509 :
1510 1803 : return tmp;
1511 : }
1512 :
1513 0 : Word32 var_fx_32( /* o: variance of vector Qx+16*/
1514 : const Word16 *x, /* i: input vector Qx*/
1515 : const Word16 Qx,
1516 : const Word16 len /* i: length of inputvector */
1517 : )
1518 : {
1519 : Word16 m;
1520 : Word32 v;
1521 : Word16 i;
1522 : Word16 tmp, exp, inv_len;
1523 : Word32 L_tmp;
1524 :
1525 :
1526 0 : L_tmp = L_add( x[0], 0 );
1527 0 : FOR( i = 1; i < len; i++ )
1528 : {
1529 0 : L_tmp = L_add( L_tmp, x[i] ); /*Qx */
1530 : }
1531 0 : exp = norm_s( len );
1532 0 : inv_len = div_s( shl( 1, sub( 14, exp ) ), len ); /*Q(29-exp) */
1533 0 : L_tmp = Mult_32_16( L_tmp, inv_len ); /*Q(14-exp+Qx) */
1534 0 : m = round_fx( L_shl( L_tmp, add( exp, 2 ) ) ); /*Qx */
1535 :
1536 0 : v = L_deposit_l( 0 );
1537 0 : FOR( i = 0; i < len; i++ )
1538 : {
1539 0 : tmp = sub( x[i], m ); /*Qx */
1540 0 : v = L_mac0_sat( v, tmp, tmp ); /*(Qx+Qx) */
1541 : }
1542 0 : L_tmp = Mult_32_16( v, inv_len ); /*Q(14-exp+Qx+Qx) */
1543 0 : v = L_shl( L_tmp, add( exp, sub( 2, Qx ) ) ); /*Qx+16 */
1544 :
1545 0 : return v;
1546 : }
1547 :
1548 0 : Word32 var_fx_32in_32out( /* o: variance of vector Qx*/
1549 : const Word32 *x, /* i: input vector Qx*/
1550 : Word16 *Qx, /*i/o:Q for input/output */
1551 : const Word16 len, /* i: length of inputvector */
1552 : const Word16 gb )
1553 : {
1554 : Word16 i;
1555 : Word16 shift;
1556 : Word32 L_tmp1, L_tmp;
1557 : Word64 W_temp;
1558 :
1559 0 : L_tmp = mean_no_sat_Word32_fx( x, len, gb ); /*Qx-gb */
1560 0 : W_temp = 0;
1561 0 : FOR( i = 0; i < len; i++ )
1562 : {
1563 0 : L_tmp1 = L_sub( L_shr( x[i], gb ), L_tmp ); /*Qx-gb */
1564 0 : W_temp = W_add( W_temp, W_mult0_32_32( L_tmp1, L_tmp1 ) ); /*Qx-gb +Qx-gb*/
1565 : }
1566 0 : shift = W_norm( W_temp );
1567 0 : L_tmp = Mult_32_16( W_extract_h( W_shl( W_temp, shift ) ), div_s( 1, len ) ); /*Q2*(Qx-gb)+shift -32 */
1568 :
1569 0 : *Qx = sub( add( shl( sub( *Qx, gb ), 1 ), shift ), 32 );
1570 0 : move16();
1571 0 : return L_tmp;
1572 : }
1573 : /*-------------------------------------------------------------------*
1574 : * conv()
1575 : *
1576 : * Convolution between vectors x[] and h[] written to y[]
1577 : * All vectors are of length L. Only the first L samples of the
1578 : * convolution are considered.
1579 : *-------------------------------------------------------------------*/
1580 :
1581 0 : Flag conv_fx(
1582 : const Word16 x[], /* i : input vector Q_new*/
1583 : const Word16 h[], /* i : impulse response (or second input vector) Q(15)*/
1584 : Word16 y[], /* o : output vetor (result of convolution) 12 bits*/
1585 : const Word16 L /* i : vector size */
1586 : )
1587 : {
1588 :
1589 : Word16 i, n;
1590 : Word32 L_sum;
1591 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
1592 0 : Flag Overflow = 0;
1593 0 : move32();
1594 : #endif
1595 0 : y[0] = mult_r( x[0], h[0] );
1596 0 : move16();
1597 0 : FOR( n = 1; n < L; n++ )
1598 : {
1599 0 : L_sum = L_mult( x[0], h[n] );
1600 0 : FOR( i = 1; i < n; i++ )
1601 : {
1602 0 : L_sum = L_mac_o( L_sum, x[i], h[n - i], &Overflow );
1603 : }
1604 0 : y[n] = mac_ro( L_sum, x[i], h[0], &Overflow );
1605 0 : move16();
1606 : }
1607 0 : return Overflow;
1608 : }
1609 :
1610 : /*-------------------------------------------------------------------*
1611 : * conv_fx_32()
1612 : *
1613 : * Convolution between vectors x[] and h[] written to y[]
1614 : * All vectors are of length L. Only the first L samples of the
1615 : * convolution are considered.
1616 : *-------------------------------------------------------------------*/
1617 2474 : Flag conv_fx_32(
1618 : const Word16 x[], /* i : input vector Q_new*/
1619 : const Word16 h[], /* i : impulse response (or second input vector) Q(15)*/
1620 : Word32 y[], /* o : output vetor (result of convolution) 12 bits*/
1621 : const Word16 L /* i : vector size */
1622 : )
1623 : {
1624 :
1625 : Word16 i, n;
1626 : Word32 L_sum;
1627 2474 : Flag Overflow = 0;
1628 2474 : y[0] = L_mult( x[0], h[0] );
1629 2474 : move32();
1630 44532 : FOR( n = 1; n < L; n++ )
1631 : {
1632 42058 : L_sum = L_mult( x[0], h[n] );
1633 378522 : FOR( i = 1; i < n; i++ )
1634 : {
1635 336464 : L_sum = L_mac_o( L_sum, x[i], h[n - i], &Overflow );
1636 : }
1637 42058 : y[n] = L_mac_o( L_sum, x[i], h[0], &Overflow );
1638 42058 : move32();
1639 : }
1640 2474 : return Overflow;
1641 : }
1642 0 : Word16 var_fx( /* o: variance of vector Qx*/
1643 : const Word16 *x, /* i: input vector Qx*/
1644 : const Word16 Qx,
1645 : const Word16 len /* i: length of inputvector */
1646 : )
1647 : {
1648 : Word16 m;
1649 : Word32 v;
1650 : Word16 v_16;
1651 : Word16 i;
1652 : Word16 tmp, exp, inv_len;
1653 : Word32 L_tmp;
1654 :
1655 0 : L_tmp = x[0];
1656 0 : move32();
1657 0 : FOR( i = 1; i < len; i++ )
1658 : {
1659 0 : L_tmp = L_add( L_tmp, x[i] ); /*Qx */
1660 : }
1661 0 : exp = norm_s( len );
1662 0 : inv_len = div_s( shl( 1, sub( 14, exp ) ), len ); /*Q(29-exp) */
1663 0 : L_tmp = Mult_32_16( L_tmp, inv_len ); /*Q(14-exp+Qx) */
1664 0 : m = round_fx( L_shl( L_tmp, add( exp, 2 ) ) ); /*Qx */
1665 :
1666 0 : v = L_deposit_l( 0 );
1667 0 : FOR( i = 0; i < len; i++ )
1668 : {
1669 0 : tmp = sub_sat( x[i], m ); /*Qx */
1670 0 : v = L_mac0_sat( v, tmp, tmp ); /*(Qx+Qx) */
1671 : }
1672 0 : L_tmp = Mult_32_16( v, inv_len ); /*Q(14-exp+Qx+Qx) */
1673 0 : v_16 = round_fx_sat( L_shl_sat( L_tmp, add( exp, sub( 2, Qx ) ) ) ); /*Qx */
1674 :
1675 0 : return v_16;
1676 : }
1677 :
1678 : /*---------------------------------------------------------------------*
1679 : * std_fx()
1680 : *
1681 : * Calculate the standard deviation of a vector
1682 : *---------------------------------------------------------------------*/
1683 :
1684 0 : Word16 std_fx( /* o: standard deviation */
1685 : const Word16 x[], /* i: input vector */
1686 : const Word16 len /* i: length of the input vector */
1687 : )
1688 : {
1689 : Word16 i;
1690 : Word32 L_tmp;
1691 : Word16 exp1, exp2, tmp;
1692 : Word32 stdev;
1693 :
1694 0 : stdev = 0;
1695 0 : move16();
1696 0 : FOR( i = 0; i < len; i++ )
1697 : {
1698 0 : L_tmp = L_mult( x[i], x[i] ); /*29 */
1699 0 : stdev = L_add( stdev, L_shr( L_tmp, 3 ) ); /*26 */
1700 : }
1701 :
1702 0 : IF( stdev != 0 )
1703 : {
1704 0 : exp1 = norm_l( stdev );
1705 0 : tmp = div_s( 16384, extract_h( L_shl( stdev, exp1 ) ) ); /*15 + 14 - (26 + exp1 - 16) */
1706 0 : L_tmp = L_mult( tmp, len ); /*15 + 14 - (26 + exp1 - 16) + 1 */
1707 0 : exp2 = norm_l( L_tmp );
1708 0 : exp1 = add( sub( exp1, exp2 ), 11 );
1709 0 : stdev = Isqrt_lc( L_shl( L_tmp, exp2 ), &exp1 ); /*31-exp1 */
1710 0 : stdev = L_shl( stdev, sub( exp1, 1 ) ); /*30 */
1711 : }
1712 :
1713 :
1714 0 : return extract_h( stdev );
1715 : }
1716 :
1717 0 : Word32 dot_product_mat_fx( /* o : the dot product x'*A*x */
1718 : const Word16 *x, /* i : vector x Q15 */
1719 : const Word32 *A, /* i : matrix A Q0*/
1720 : const Word16 m /* i : vector & matrix size */
1721 :
1722 : )
1723 : {
1724 : Word16 i, j;
1725 : Word64 tmp_sum_64;
1726 : Word32 suma, tmp_sum;
1727 : const Word32 *pt_A;
1728 : const Word16 *pt_x;
1729 :
1730 0 : pt_A = A;
1731 0 : suma = L_deposit_l( 0 );
1732 :
1733 0 : FOR( i = 0; i < m; i++ )
1734 : {
1735 0 : tmp_sum_64 = 0;
1736 0 : move64();
1737 0 : pt_x = x;
1738 0 : FOR( j = 0; j < m; j++ )
1739 : {
1740 0 : tmp_sum_64 = W_mac_32_16( tmp_sum_64, *pt_A, *pt_x ); /*Q0 */
1741 0 : pt_A++;
1742 0 : pt_x++;
1743 : }
1744 0 : tmp_sum = W_sat_m( tmp_sum_64 );
1745 0 : suma = Madd_32_16( suma, tmp_sum, x[i] ); /*Q0 */
1746 : }
1747 0 : return suma;
1748 : }
1749 : /*-------------------------------------------------------------------*
1750 : * Vr_subt
1751 : *
1752 : * Subtract two Word16 vectors integer by integer
1753 : *-------------------------------------------------------------------*/
1754 81212 : void Vr_subt(
1755 : const Word16 x1[], /* i : Input vector 1 */
1756 : const Word16 x2[], /* i : Input vector 2 */
1757 : Word16 y[], /* o : Output vector that contains vector 1 - vector 2 */
1758 : Word16 N /* i : Vector lenght */
1759 : )
1760 : {
1761 : Word16 i;
1762 :
1763 1420780 : FOR( i = 0; i < N; i++ )
1764 : {
1765 1339568 : y[i] = sub_sat( x1[i], x2[i] );
1766 1339568 : move16();
1767 : }
1768 81212 : }
1769 : /*-------------------------------------------------------------------*
1770 : * vquant()
1771 : *
1772 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
1773 : *
1774 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
1775 : * Index of the winning codevector and the winning vector itself are returned.
1776 : *-------------------------------------------------------------------*/
1777 0 : Word16 vquant_ivas_fx( /* o: index of the winning codevector */
1778 : Word32 x[], /* i: vector to quantize Q25 */
1779 : const Word32 x_mean[], /* i: vector mean to subtract (0 if none) Q25 */
1780 : Word32 xq[], /* o: quantized vector Q25 */
1781 : const Word32 cb[], /* i: codebook Q25 */
1782 : const Word16 dim, /* i: dimension of codebook vectors */
1783 : const Word16 cbsize /* i: codebook size */
1784 : )
1785 : {
1786 : Word16 c, d, idx, j;
1787 : Word32 L_dist, L_tmp, L_mindist;
1788 :
1789 0 : idx = 0;
1790 0 : move16();
1791 0 : L_mindist = MAX_32;
1792 0 : move32();
1793 0 : IF( x_mean != 0 )
1794 : {
1795 0 : FOR( d = 0; d < dim; d++ )
1796 : {
1797 : /*x[d] -= x_mean[d]; */
1798 0 : x[d] = L_sub( x[d], x_mean[d] );
1799 0 : move32(); /*Qx */
1800 : }
1801 : }
1802 0 : j = 0;
1803 0 : move16();
1804 0 : FOR( c = 0; c < cbsize; c++ )
1805 : {
1806 0 : L_dist = 0;
1807 0 : move32();
1808 :
1809 0 : FOR( d = 0; d < dim; d++ )
1810 : {
1811 : /*tmp = x[d] - cb[j++];*/
1812 0 : L_tmp = L_sub( x[d], cb[j++] ); // Q25
1813 0 : L_dist = L_add( L_dist, Mpy_32_32( L_tmp, L_tmp ) ); // (Q25, Q25) -> Q19
1814 : }
1815 0 : if ( LT_32( L_dist, L_mindist ) ) // Q19
1816 : {
1817 0 : idx = c;
1818 0 : move16();
1819 : }
1820 0 : L_mindist = L_min( L_mindist, L_dist ); // Q19
1821 : }
1822 0 : IF( xq == 0 )
1823 : {
1824 0 : return idx;
1825 : }
1826 :
1827 : /*j = idx*dim;*/
1828 0 : j = i_mult2( idx, dim );
1829 0 : FOR( d = 0; d < dim; d++ )
1830 : {
1831 0 : xq[d] = cb[j++]; // Q25
1832 0 : move32();
1833 : }
1834 0 : IF( x_mean != 0 )
1835 : {
1836 0 : FOR( d = 0; d < dim; d++ )
1837 : {
1838 : /*xq[d] += x_mean[d]; */
1839 0 : xq[d] = L_add( xq[d], x_mean[d] ); // Q25
1840 0 : move32();
1841 : }
1842 : }
1843 :
1844 0 : return idx;
1845 : }
1846 :
1847 0 : Word16 vquant_fx( /* o: index of the winning codevector */
1848 : Word16 x[], /* i: vector to quantize Q13 */
1849 : const Word16 x_mean[], /* i: vector mean to subtract (0 if none) Q13 */
1850 : Word16 xq[], /* o: quantized vector Q13 */
1851 : const Word16 cb[], /* i: codebook Q13 */
1852 : const Word16 dim, /* i: dimension of codebook vectors */
1853 : const Word16 cbsize /* i: codebook size */
1854 : )
1855 : {
1856 : Word16 tmp;
1857 : Word16 c, d, idx, j;
1858 : Word32 L_dist, /*L_tmp,*/ L_mindist;
1859 :
1860 0 : idx = 0;
1861 0 : move16();
1862 0 : L_mindist = MAX_32;
1863 0 : move32();
1864 0 : IF( x_mean != 0 )
1865 : {
1866 0 : FOR( d = 0; d < dim; d++ )
1867 : {
1868 : /*x[d] -= x_mean[d]; */
1869 0 : x[d] = sub( x[d], x_mean[d] );
1870 0 : move16(); /*Qx */
1871 : }
1872 : }
1873 0 : j = 0;
1874 0 : move16();
1875 0 : FOR( c = 0; c < cbsize; c++ )
1876 : {
1877 0 : L_dist = 0;
1878 0 : move32();
1879 :
1880 0 : FOR( d = 0; d < dim; d++ )
1881 : {
1882 : /*tmp = x[d] - cb[j++];*/
1883 0 : tmp = sub( x[d], cb[j++] ); /*Qx */
1884 0 : L_dist = L_mac0( L_dist, tmp, tmp );
1885 : }
1886 0 : if ( LT_32( L_dist, L_mindist ) )
1887 : {
1888 0 : idx = c;
1889 0 : move16();
1890 : }
1891 0 : L_mindist = L_min( L_mindist, L_dist );
1892 : }
1893 0 : IF( xq == 0 )
1894 : {
1895 0 : return idx;
1896 : }
1897 :
1898 : /*j = idx*dim;*/
1899 0 : j = i_mult2( idx, dim );
1900 0 : FOR( d = 0; d < dim; d++ )
1901 : {
1902 0 : xq[d] = cb[j++];
1903 0 : move16();
1904 : }
1905 0 : IF( x_mean != 0 )
1906 : {
1907 0 : FOR( d = 0; d < dim; d++ )
1908 : {
1909 : /*xq[d] += x_mean[d]; */
1910 0 : xq[d] = add( xq[d], x_mean[d] );
1911 0 : move16();
1912 : }
1913 : }
1914 :
1915 0 : return idx;
1916 : }
1917 :
1918 : /*-------------------------------------------------------------------*
1919 : * w_vquant_fx()
1920 : *
1921 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
1922 : *
1923 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
1924 : * Weights are put on the error for each vector element.
1925 : * Index of the winning codevector and the winning vector itself are returned.
1926 : *-------------------------------------------------------------------*/
1927 0 : Word16 w_vquant_fx(
1928 : Word16 x[], /* i: vector to quantize in Q10 */
1929 : Word16 Qx,
1930 : const Word16 weights[], /* i: error weights in Q0 */
1931 : Word16 xq[], /* o: quantized vector in Q15 */
1932 : const Word16 cb[], /* i: codebook in Q15 */
1933 : const Word16 cbsize, /* i: codebook size */
1934 : const Word16 rev_vect /* i: reverse codebook vectors */
1935 : )
1936 : {
1937 : Word16 tmp;
1938 : Word16 c, idx, j;
1939 : Word32 dist, minDist;
1940 : #ifndef ISSUE_1836_replace_overflow_libcom
1941 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
1942 : Flag Overflow = 0;
1943 : move32();
1944 : #endif
1945 : #endif
1946 :
1947 :
1948 0 : idx = 0;
1949 0 : move16();
1950 0 : minDist = 0x7fffffffL;
1951 0 : move32();
1952 0 : Qx = sub( 15, Qx );
1953 :
1954 0 : j = 0;
1955 0 : move16();
1956 0 : IF( rev_vect )
1957 : {
1958 0 : FOR( c = 0; c < cbsize; c++ )
1959 : {
1960 0 : dist = L_deposit_l( 0 );
1961 :
1962 : #ifdef ISSUE_1836_replace_overflow_libcom
1963 0 : tmp = sub_sat( x[3], shr( cb[j++], Qx ) );
1964 0 : if ( weights[3] != 0 )
1965 : {
1966 0 : dist = L_mac0_sat( dist, tmp, tmp );
1967 : }
1968 0 : tmp = sub_sat( x[2], shr( cb[j++], Qx ) );
1969 0 : if ( weights[2] != 0 )
1970 : {
1971 0 : dist = L_mac0_sat( dist, tmp, tmp );
1972 : }
1973 0 : tmp = sub_sat( x[1], shr( cb[j++], Qx ) );
1974 0 : if ( weights[1] != 0 )
1975 : {
1976 0 : dist = L_mac0_sat( dist, tmp, tmp );
1977 : }
1978 0 : tmp = sub_sat( x[0], shr( cb[j++], Qx ) );
1979 0 : if ( weights[0] != 0 )
1980 : {
1981 0 : dist = L_mac0_sat( dist, tmp, tmp );
1982 : }
1983 : #else
1984 : tmp = sub_o( x[3], shr( cb[j++], Qx ), &Overflow );
1985 : if ( weights[3] != 0 )
1986 : {
1987 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
1988 : }
1989 : tmp = sub_o( x[2], shr( cb[j++], Qx ), &Overflow );
1990 : if ( weights[2] != 0 )
1991 : {
1992 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
1993 : }
1994 : tmp = sub_o( x[1], shr( cb[j++], Qx ), &Overflow );
1995 : if ( weights[1] != 0 )
1996 : {
1997 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
1998 : }
1999 : tmp = sub_o( x[0], shr( cb[j++], Qx ), &Overflow );
2000 : if ( weights[0] != 0 )
2001 : {
2002 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
2003 : }
2004 : #endif
2005 0 : if ( LT_32( dist, minDist ) )
2006 : {
2007 0 : idx = c;
2008 0 : move16();
2009 : }
2010 0 : minDist = L_min( minDist, dist );
2011 : }
2012 :
2013 0 : IF( xq == 0 )
2014 : {
2015 0 : return idx;
2016 : }
2017 :
2018 0 : j = shl( idx, 2 );
2019 0 : xq[3] = cb[j++];
2020 0 : move16(); /* in Q15 */
2021 0 : xq[2] = cb[j++];
2022 0 : move16(); /* in Q15 */
2023 0 : xq[1] = cb[j++];
2024 0 : move16(); /* in Q15 */
2025 0 : xq[0] = cb[j++];
2026 0 : move16(); /* in Q15 */
2027 : }
2028 : ELSE
2029 : {
2030 0 : FOR( c = 0; c < cbsize; c++ )
2031 : {
2032 0 : dist = L_deposit_l( 0 );
2033 :
2034 : #ifdef ISSUE_1836_replace_overflow_libcom
2035 0 : tmp = sub_sat( x[0], shr( cb[j++], Qx ) );
2036 0 : if ( weights[0] != 0 )
2037 : {
2038 0 : dist = L_mac0_sat( dist, tmp, tmp );
2039 : }
2040 0 : tmp = sub_sat( x[1], shr( cb[j++], Qx ) );
2041 0 : if ( weights[1] != 0 )
2042 : {
2043 0 : dist = L_mac0_sat( dist, tmp, tmp );
2044 : }
2045 0 : tmp = sub_sat( x[2], shr( cb[j++], Qx ) );
2046 0 : if ( weights[2] != 0 )
2047 : {
2048 0 : dist = L_mac0_sat( dist, tmp, tmp );
2049 : }
2050 0 : tmp = sub_sat( x[3], shr( cb[j++], Qx ) );
2051 0 : if ( weights[3] != 0 )
2052 : {
2053 0 : dist = L_mac0_sat( dist, tmp, tmp );
2054 : }
2055 : #else
2056 : tmp = sub_o( x[0], shr( cb[j++], Qx ), &Overflow );
2057 : if ( weights[0] != 0 )
2058 : {
2059 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
2060 : }
2061 : tmp = sub_o( x[1], shr( cb[j++], Qx ), &Overflow );
2062 : if ( weights[1] != 0 )
2063 : {
2064 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
2065 : }
2066 : tmp = sub_o( x[2], shr( cb[j++], Qx ), &Overflow );
2067 : if ( weights[2] != 0 )
2068 : {
2069 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
2070 : }
2071 : tmp = sub_o( x[3], shr( cb[j++], Qx ), &Overflow );
2072 : if ( weights[3] != 0 )
2073 : {
2074 : dist = L_mac0_o( dist, tmp, tmp, &Overflow );
2075 : }
2076 : #endif
2077 0 : if ( LT_32( dist, minDist ) )
2078 : {
2079 0 : idx = c;
2080 0 : move16();
2081 : }
2082 0 : minDist = L_min( minDist, dist );
2083 : }
2084 :
2085 0 : IF( xq == 0 )
2086 : {
2087 0 : return idx;
2088 : }
2089 :
2090 0 : j = shl( idx, 2 );
2091 0 : xq[0] = cb[j++];
2092 0 : move16(); /* in Q15 */
2093 0 : xq[1] = cb[j++];
2094 0 : move16(); /* in Q15 */
2095 0 : xq[2] = cb[j++];
2096 0 : move16(); /* in Q15 */
2097 0 : xq[3] = cb[j++];
2098 0 : move16(); /* in Q15 */
2099 : }
2100 :
2101 0 : return idx;
2102 : }
2103 : /*-------------------------------------------------------------------*
2104 : * Emaximum:
2105 : *
2106 : * Find index of a maximum energy in a vector
2107 : *-------------------------------------------------------------------*/
2108 91377 : Word16 emaximum_fx( /* o : return index with max energy value in vector Q0 */
2109 : const Word16 Qvec, /* i : Q of input vector Q0 */
2110 : const Word16 *vec, /* i : input vector Qx */
2111 : const Word16 lvec, /* i : length of input vector Q0 */
2112 : Word32 *ener_max /* o : maximum energy value Q0 */
2113 : )
2114 : {
2115 : Word16 j, ind;
2116 : Word32 L_tmp, L_tmp1;
2117 : Word32 emax;
2118 :
2119 91377 : emax = L_mult0( vec[0], vec[0] );
2120 91377 : ind = 0;
2121 91377 : move16();
2122 :
2123 8475877 : FOR( j = 1; j < lvec; j++ )
2124 : {
2125 8384500 : L_tmp = L_mult0( vec[j], vec[j] );
2126 8384500 : L_tmp1 = L_sub( L_tmp, emax );
2127 8384500 : if ( L_tmp1 > 0 )
2128 : {
2129 664416 : ind = j;
2130 664416 : move16();
2131 : }
2132 8384500 : emax = L_max( emax, L_tmp );
2133 : }
2134 :
2135 91377 : *ener_max = L_shr_sat( emax, add( Qvec, Qvec ) );
2136 91377 : move32();
2137 :
2138 91377 : return ind;
2139 : }
2140 :
2141 0 : Word16 emaximum_32fx( /* o : return index with max energy value in vector Q0 */
2142 : const Word16 Qvec, /* i : Q of input vector Q0 */
2143 : const Word32 *vec, /* i : input vector Qx */
2144 : const Word16 lvec, /* i : length of input vector Q0 */
2145 : Word32 *ener_max /* o : maximum energy value Q0 */
2146 : )
2147 : {
2148 : Word16 j, ind;
2149 : Word64 W_tmp, W_tmp1;
2150 : Word64 emax;
2151 :
2152 0 : emax = W_mult0_32_32( vec[0], vec[0] );
2153 0 : ind = 0;
2154 0 : move16();
2155 :
2156 0 : FOR( j = 1; j < lvec; j++ )
2157 : {
2158 0 : W_tmp = W_mult0_32_32( vec[j], vec[j] );
2159 0 : W_tmp1 = W_sub( W_tmp, emax );
2160 0 : if ( W_tmp1 > 0 )
2161 : {
2162 0 : ind = j;
2163 0 : move16();
2164 : }
2165 0 : if ( LE_64( emax, W_tmp ) )
2166 : {
2167 0 : emax = W_tmp;
2168 0 : move64();
2169 : }
2170 : }
2171 :
2172 0 : *ener_max = W_extract_l( W_shr( emax, add( Qvec, Qvec ) ) );
2173 0 : move32();
2174 :
2175 0 : return ind;
2176 : }
2177 :
2178 : /*-------------------------------------------------------------------*
2179 : * mean32:
2180 : *
2181 : * Find the mean of a 32 bits vector
2182 : *-------------------------------------------------------------------*/
2183 0 : Word32 Mean32( /* o : mean of the elements of the vector */
2184 : const Word32 in[], /* i : input vector */
2185 : const Word16 L /* i : length of input vector */
2186 : )
2187 : {
2188 : Word32 Ltmp;
2189 : Word16 inv_L;
2190 :
2191 0 : inv_L = INV_BANDS9;
2192 0 : move16();
2193 0 : if ( EQ_16( L, 10 ) )
2194 : {
2195 0 : inv_L = INV_BANDS10;
2196 0 : move16();
2197 : }
2198 :
2199 0 : Ltmp = sum32_fx( in, L );
2200 :
2201 0 : Ltmp = Mult_32_16( Ltmp, inv_L );
2202 :
2203 0 : return Ltmp;
2204 : }
2205 128477 : Word32 sum32_fx( /* o : sum of all vector elements Qx*/
2206 : const Word32 *vec, /* i : input vector Qx*/
2207 : const Word16 lvec /* i : length of input vector */
2208 : )
2209 : {
2210 : Word16 i;
2211 : Word32 tmp;
2212 :
2213 128477 : tmp = L_deposit_l( 0 );
2214 6666765 : FOR( i = 0; i < lvec; i++ )
2215 : {
2216 6538288 : tmp = L_add_sat( tmp, vec[i] ); /*Qx */
2217 : }
2218 :
2219 128477 : return tmp;
2220 : }
2221 707605 : Word16 sum16_fx( /* o : sum of all vector elements Qx*/
2222 : const Word16 *vec, /* i : input vector Qx*/
2223 : const Word16 lvec /* i : length of input vector */
2224 : )
2225 : {
2226 : Word16 i;
2227 : Word16 tmp;
2228 707605 : tmp = 0;
2229 707605 : move16();
2230 5476190 : FOR( i = 0; i < lvec; i++ )
2231 : {
2232 4768585 : tmp = add_sat( tmp, vec[i] ); /*Qx */
2233 : }
2234 :
2235 707605 : return tmp;
2236 : }
2237 : /*------------------------------------------------------------------*
2238 : * function Random
2239 : *
2240 : * Signed 16 bits random generator.
2241 : *------------------------------------------------------------------*/
2242 618204462 : Word16 Random( /* o : output random value */
2243 : Word16 *seed /* i/o: random seed */
2244 : )
2245 : {
2246 618204462 : *seed = extract_l( L_mac0( 13849L, *seed, 31821 ) );
2247 618204462 : move16();
2248 618204462 : return *seed;
2249 : }
2250 :
2251 123036 : Word16 own_random2_fx( Word16 seed )
2252 : {
2253 123036 : return extract_l( L_mac0( 13849, seed, 31821 ) );
2254 : }
2255 :
2256 : /*---------------------------------------------------------------------
2257 : * sign_fx()
2258 : *
2259 : *---------------------------------------------------------------------*/
2260 :
2261 : /*! r: sign of x (+1/-1) */
2262 0 : Word16 sign_fx(
2263 : const Word32 x /* i : input value of x */
2264 : )
2265 : {
2266 0 : IF( LT_32( x, 0 ) )
2267 : {
2268 0 : return -1;
2269 : }
2270 : ELSE
2271 : {
2272 0 : return 1;
2273 : }
2274 : }
2275 :
2276 0 : Word16 sign16_fx(
2277 : const Word16 x /* i : input value of x */
2278 : )
2279 : {
2280 0 : IF( LT_16( x, 0 ) )
2281 : {
2282 0 : return -1;
2283 : }
2284 : ELSE
2285 : {
2286 0 : return 1;
2287 : }
2288 : }
2289 :
2290 : /*------------------------------------------------------------------*
2291 : * function Div_32_optmz
2292 : *
2293 : * Performs 32 bits interger division
2294 : *------------------------------------------------------------------*/
2295 3675473 : static Word32 Div_32_optmz( Word32 L_num, Word16 denom_hi )
2296 : {
2297 : Word16 approx, hi, lo, n_hi, n_lo;
2298 : Word32 L_32;
2299 :
2300 : /* First approximation: 1 / L_denom = 1/denom_hi */
2301 :
2302 3675473 : approx = div_s( (Word16) 0x3fff, denom_hi );
2303 :
2304 : /* 1/L_denom = approx * (2.0 - L_denom * approx) */
2305 :
2306 3675473 : L_32 = L_msu( (Word32) 0x7fffffffL, denom_hi, approx );
2307 :
2308 3675473 : lo = L_Extract_lc( L_32, &hi );
2309 3675473 : L_32 = Mpy_32_16( hi, lo, approx );
2310 :
2311 : /* L_num * (1/L_denom) */
2312 :
2313 3675473 : lo = L_Extract_lc( L_32, &hi );
2314 3675473 : n_lo = L_Extract_lc( L_num, &n_hi );
2315 3675473 : L_32 = Mpy_32( n_hi, n_lo, hi, lo );
2316 :
2317 3675473 : return ( L_32 );
2318 : }
2319 : /*------------------------------------------------------------------*
2320 : * function iDiv_and_mod_32
2321 : *
2322 : * return the quotient and the modulo 32 bits numerator divided by a 16 bit denominator
2323 : * The denominator might be right shifted by 1
2324 : *------------------------------------------------------------------*/
2325 3675473 : void iDiv_and_mod_32(
2326 : const Word32 Numer, /* i : 32 bits numerator */
2327 : const Word16 Denom, /* i : 16 bits denominator */
2328 : Word32 *Int_quotient, /* o : integer result of the division (int)(num/den) */
2329 : Word32 *Int_mod, /* o : modulo result of the division num-((int)(num/den)*den)*/
2330 : const Word16 rshift /* i : 0 if no right shift / 1 if the denom is right shifted by 1 */
2331 : )
2332 : {
2333 : Word32 Quotient;
2334 : Word16 expA, expB;
2335 : Word32 N, TEMP;
2336 : Word16 D;
2337 :
2338 : /* Normalize 'Numer' & 'Denom' */
2339 : /* Use Temporary to Preserve the Original Value */
2340 3675473 : expA = norm_l( Numer );
2341 3675473 : N = L_shl( Numer, expA );
2342 3675473 : expB = norm_s( Denom );
2343 3675473 : D = shl( Denom, expB );
2344 :
2345 : /* Need to shift right 'Numer' by 1? */
2346 3675473 : if ( L_mac( N, D, -32768L ) >= 0 )
2347 : {
2348 2020724 : expA = sub( expA, 1 );
2349 : }
2350 3675473 : N = L_shl( Numer, expA );
2351 :
2352 : /* Perform Approximation of the Division
2353 : * Since 'lo' part is '0' AND 'denom' is supposed to be constant in the targeted usage
2354 : * one could import the Div32 code and pre-calc the div_s and eliminate all calcs
2355 : * with 'lo' to save some complexity */
2356 :
2357 3675473 : Quotient = Div_32_optmz( N, D ); /* takes 36 clocks */
2358 : /* Bring Back to Q0 (minus 2 because we removed the left shift by 2 in the Div32_optmz) */
2359 3675473 : IF( rshift )
2360 : {
2361 0 : Quotient = L_shr( Quotient, add( 15 - 2, sub( expA, sub( expB, 1 ) ) ) );
2362 : }
2363 : ELSE
2364 : {
2365 3675473 : Quotient = L_shr( Quotient, add( 15 - 2, sub( expA, expB ) ) );
2366 : }
2367 :
2368 : /* Cross Check (do Quotient x Divisor)
2369 : * The Quotient is unsigned but we cannot just use extract_h because
2370 : * extract_l on the low part will get the sign of the bit #15.
2371 : * In a 32 bits value, what is in bits 0-15 is un unsigned 16 bits value.
2372 : * So, we shift left by 1, extract the hi part and mult it by 32768 (hence the L_shl by 16-1.
2373 : * Then we take 15 bits left (mask the others) and multiply by Denom.
2374 : * Technically this could overflow. But since we are mutiplying to get
2375 : * back to the Numer value that fitted in a 32 bits, doing Divisor x Quotient
2376 : * must necessarily fit in a 32 bits
2377 : * It is assumed that all are positive values. If not, one could
2378 : * check the sign of the numer and denom, turn them into abs values
2379 : * and restore the sign after*/
2380 3675473 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
2381 3675473 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
2382 :
2383 :
2384 : /* Here we test to see if the previous "Quotient x Divisor" (or TEMP) is too small
2385 : * that is the "Numer" minus TEMP is bigger or Equal to the Divisor.
2386 : * If it is then the quotient is too small. We need to increase it by 1.
2387 : * That is caused by our Div32 fractionnal division that can be off by 1
2388 : * sometimes.
2389 : * In some cases, when the divisor is very small (like 10 or something)
2390 : * the quotient could be off by more than 1 and we would need a loop
2391 : * to check again. That is not the case here with the current divisor */
2392 3675473 : IF( rshift )
2393 : {
2394 0 : TEMP = L_shl( TEMP, 1 );
2395 0 : WHILE( L_msu0( L_sub( Numer, TEMP ), 2, Denom ) >= 0 )
2396 : {
2397 0 : Quotient = L_add( Quotient, 1 );
2398 0 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
2399 0 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
2400 0 : TEMP = L_shl( TEMP, 1 );
2401 : }
2402 : }
2403 : ELSE{
2404 6007118 : WHILE( L_msu0( L_sub( Numer, TEMP ), 1, Denom ) >= 0 ){
2405 2331645 : Quotient = L_add( Quotient, 1 );
2406 2331645 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
2407 2331645 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
2408 : }
2409 : }
2410 3675473 : *Int_quotient = Quotient;
2411 3675473 : move32();
2412 3675473 : IF( L_msu0( L_sub( Numer, TEMP ), 1, Denom ) == 0 )
2413 : {
2414 0 : *Int_mod = 0L;
2415 0 : move32();
2416 : }
2417 : ELSE
2418 : {
2419 3675473 : *Int_mod = L_sub( Numer, TEMP );
2420 3675473 : move32();
2421 : }
2422 3675473 : }
2423 :
2424 : /*===================================================================*/
2425 : /* FUNCTION : pz_filter_sp_fx () */
2426 : /*-------------------------------------------------------------------*/
2427 : /* PURPOSE : Generic pole-zero filter routine, with single */
2428 : /* precision memory */
2429 : /*-------------------------------------------------------------------*/
2430 : /* INPUT ARGUMENTS : */
2431 : /* */
2432 : /* _ (Word16 []) b : zero filter coefficients (Qc). */
2433 : /* _ (Word16 []) a : pole filter coefficients (Qc), a(0)=1 in Qc */
2434 : /* _ (Word16 []) x : input signal (Qn). */
2435 : /* _ (Word16) PNR : NR filter order */
2436 : /* _ (Word16) PDR : DR filter order */
2437 : /* _ (Word16) N : number of input samples. */
2438 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
2439 : /*-------------------------------------------------------------------*/
2440 : /* OUTPUT ARGUMENTS : */
2441 : /* */
2442 : /* _ (Word16 []) y : output signal (Qn) */
2443 : /*-------------------------------------------------------------------*/
2444 : /* INPUT/OUTPUT ARGUMENTS : */
2445 : /* */
2446 : /* _ (Word16 []) buf : filter memory (Qn-Qa). */
2447 : /*-------------------------------------------------------------------*/
2448 : /* RETURN ARGUMENTS : _ None. */
2449 : /*===================================================================*/
2450 :
2451 0 : void pz_filter_sp_fx(
2452 : const Word16 b[],
2453 : const Word16 a[],
2454 : Word16 x[],
2455 : Word16 y[],
2456 : Word16 buf[],
2457 : Word16 PNR,
2458 : Word16 PDR,
2459 : Word16 N,
2460 : Word16 Qa )
2461 : {
2462 : Word16 i, j;
2463 : Word16 s;
2464 : Word16 s_mem;
2465 : Word32 Ltemp1;
2466 : Word32 Lacc;
2467 0 : s = negate( Qa );
2468 0 : s = add( s, s ); /* s=-2Qa*/
2469 0 : s = add( s, 1 );
2470 0 : FOR( i = 0; i < N; i++ )
2471 : {
2472 0 : Lacc = L_deposit_h( x[i] ); /* Lacc in Q(16+Qn)*/
2473 0 : Lacc = L_shl( Lacc, s ); /* Lacc=x[i] in Q(16+Qn-2Qa+1)*/
2474 0 : FOR( j = PDR - 1; j >= 0; j-- )
2475 0 : Lacc = L_msu_sat( Lacc, buf[j], a[j + 1] ); /*Q(16+Qn-2Qa+1)*/
2476 :
2477 :
2478 0 : Lacc = L_shr( Lacc, 1 );
2479 0 : Ltemp1 = L_add_sat( L_shl_sat( Lacc, Qa ), 0x08000 );
2480 0 : s_mem = extract_h( Ltemp1 );
2481 :
2482 0 : Lacc = L_deposit_l( 0 );
2483 0 : FOR( j = PNR - 1; j >= 0; j-- )
2484 0 : Lacc = L_mac_sat( Lacc, buf[j], b[j + 1] );
2485 0 : Lacc = L_mac_sat( Lacc, s_mem, b[0] );
2486 : /* Lacc in Q(1+Qc+Qn-Qa)*/
2487 :
2488 0 : FOR( j = s_max( PDR, PNR ) - 1; j > 0; j-- )
2489 : {
2490 : /* Update filter memory */
2491 0 : buf[j] = buf[j - 1];
2492 0 : move16();
2493 : }
2494 0 : buf[0] = s_mem;
2495 0 : move16();
2496 :
2497 0 : Ltemp1 = L_add_sat( L_shr_sat( Lacc, s ), 0x08000 ); /* Ltemp1 in Qc+Qa+Qn=Q(Qn) */
2498 0 : y[i] = extract_h( Ltemp1 ); /* y[i] in Qn */
2499 0 : move16();
2500 : }
2501 0 : }
2502 :
2503 :
2504 33076 : Word32 root_a_fx(
2505 : Word32 a,
2506 : Word16 Q_a,
2507 : Word16 *exp_out )
2508 : {
2509 : Word16 exp, tmp;
2510 : Word32 L_tmp;
2511 :
2512 33076 : IF( a <= 0 )
2513 : {
2514 366 : *exp_out = 0;
2515 366 : move16();
2516 366 : return 0;
2517 : }
2518 :
2519 32710 : exp = norm_l( a );
2520 32710 : tmp = extract_h( L_shl( a, exp ) );
2521 32710 : exp = sub( exp, sub( 30, Q_a ) );
2522 32710 : tmp = div_s( 16384, tmp );
2523 32710 : L_tmp = L_deposit_h( tmp );
2524 32710 : L_tmp = Isqrt_lc( L_tmp, &exp );
2525 :
2526 32710 : *exp_out = exp;
2527 32710 : move16();
2528 :
2529 32710 : return L_tmp;
2530 : }
2531 :
2532 :
2533 592092 : Word32 root_a_over_b_fx(
2534 : Word32 a, /* Q(Q_a) */
2535 : Word16 Q_a,
2536 : Word32 b, /* Q(Q_b) */
2537 : Word16 Q_b,
2538 : Word16 *exp_out )
2539 : {
2540 : Word16 tmp, num, den, scale;
2541 : Word16 exp, exp_num, exp_den;
2542 : Word32 L_tmp;
2543 : #ifndef ISSUE_1836_replace_overflow_libcom
2544 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
2545 : Flag Overflow = 0;
2546 : move32();
2547 : #endif
2548 : #endif
2549 592092 : test();
2550 592092 : IF( ( a <= 0 ) || ( b <= 0 ) )
2551 : {
2552 1717 : *exp_out = 0;
2553 1717 : move16();
2554 1717 : return 0;
2555 : }
2556 :
2557 590375 : exp_num = norm_l( b );
2558 : #ifdef ISSUE_1836_replace_overflow_libcom
2559 590375 : num = round_fx_sat( L_shl_sat( b, exp_num ) );
2560 : #else
2561 : num = round_fx_o( L_shl_o( b, exp_num, &Overflow ), &Overflow );
2562 : #endif
2563 590375 : exp_num = sub( sub( 30, exp_num ), Q_b );
2564 :
2565 590375 : exp_den = norm_l( a );
2566 : #ifdef ISSUE_1836_replace_overflow_libcom
2567 590375 : den = round_fx_sat( L_shl_sat( a, exp_den ) );
2568 : #else
2569 : den = round_fx_o( L_shl_o( a, exp_den, &Overflow ), &Overflow );
2570 : #endif
2571 590375 : exp_den = sub( sub( 30, exp_den ), Q_a );
2572 :
2573 590375 : scale = shr( sub( den, num ), 15 );
2574 : #ifdef ISSUE_1836_replace_overflow_libcom
2575 590375 : num = shl_sat( num, scale );
2576 : #else
2577 : num = shl_o( num, scale, &Overflow );
2578 : #endif
2579 590375 : exp_num = sub( exp_num, scale );
2580 :
2581 590375 : tmp = div_s( num, den );
2582 590375 : exp = sub( exp_num, exp_den );
2583 :
2584 590375 : L_tmp = L_deposit_h( tmp );
2585 590375 : L_tmp = Isqrt_lc( L_tmp, &exp );
2586 :
2587 590375 : *exp_out = exp;
2588 590375 : move16();
2589 :
2590 590375 : return L_tmp;
2591 : }
2592 :
2593 0 : Word32 root_a_over_b_ivas_fx(
2594 : Word32 a, /* Q(Q_a) */
2595 : Word16 Q_a,
2596 : Word32 b, /* Q(Q_b) */
2597 : Word16 Q_b,
2598 : Word16 *q_out )
2599 : {
2600 : Word16 shift_a, shift_b, shift;
2601 : Word32 mod_a, mod_b, one_in_Q_a, one_in_Q_b, half_in_Q_a, half_in_Q_b;
2602 : Word32 a_sqr, b_sqr, p0, p1, p2, approx;
2603 : Word16 exp;
2604 :
2605 0 : test();
2606 0 : IF( ( a <= 0 ) || ( b <= 0 ) )
2607 : {
2608 0 : *q_out = 0;
2609 0 : move16();
2610 0 : return 0;
2611 : }
2612 :
2613 0 : one_in_Q_a = L_shl( 1, Q_a ); // 1.0f in Q_a
2614 0 : one_in_Q_b = L_shl( 1, Q_b ); // 1.0f in Q_b
2615 0 : half_in_Q_a = L_shr( one_in_Q_a, 1 ); // 0.5f in Q_a
2616 0 : half_in_Q_b = L_shr( one_in_Q_b, 1 ); // 0.5f in Q_b
2617 :
2618 0 : a = L_add( a, one_in_Q_a );
2619 0 : b = L_add( b, one_in_Q_b );
2620 :
2621 : /* This next piece of code implements a "norm" function */
2622 : /* and returns the shift needed to scale "a" to have a */
2623 : /* 1 in the (MSB-1) position. This is equivalent to */
2624 : /* giving a value between 0.5 & 1.0. */
2625 :
2626 0 : mod_a = a;
2627 0 : move32();
2628 :
2629 0 : shift_a = 0;
2630 0 : move16();
2631 0 : WHILE( GT_32( mod_a, one_in_Q_a ) )
2632 : {
2633 0 : mod_a = L_shr( mod_a, 1 );
2634 0 : shift_a = sub( shift_a, 1 );
2635 : }
2636 :
2637 0 : WHILE( LT_32( mod_a, half_in_Q_a ) )
2638 : {
2639 0 : mod_a = L_shl( mod_a, 1 );
2640 0 : shift_a = add( shift_a, 1 );
2641 : }
2642 :
2643 0 : shift_a = s_and( shift_a, -2 );
2644 0 : mod_a = L_shl( a, shift_a ); // Q_a
2645 :
2646 : /* This next piece of code implements a "norm" function */
2647 : /* and returns the shift needed to scale "b" to have a */
2648 : /* 1 in the (MSB-1) position. This is equivalent to */
2649 : /* giving a value between 0.5 & 1.0. */
2650 0 : mod_b = b;
2651 0 : move32();
2652 :
2653 0 : shift_b = 0;
2654 0 : move16();
2655 0 : WHILE( GT_32( mod_b, one_in_Q_b ) )
2656 : {
2657 0 : mod_b = L_shr( mod_b, 1 );
2658 0 : shift_b = sub( shift_b, 1 );
2659 : }
2660 :
2661 0 : WHILE( LT_32( mod_b, half_in_Q_b ) )
2662 : {
2663 0 : mod_b = L_shl( mod_b, 1 );
2664 0 : shift_b = add( shift_b, 1 );
2665 : }
2666 :
2667 0 : shift_b = s_and( shift_b, -2 );
2668 0 : mod_b = L_shl( b, shift_b ); // Q_b
2669 :
2670 0 : shift = shr( sub( shift_b, shift_a ), 1 );
2671 :
2672 0 : a_sqr = W_extract_h( W_shl( W_mult0_32_32( mod_a, mod_a ), sub( 32, Q_a ) ) ); // Q_a
2673 0 : b_sqr = W_extract_h( W_shl( W_mult0_32_32( mod_b, mod_b ), sub( 32, Q_b ) ) ); // Q_b
2674 :
2675 0 : p2 = L_shl( -408505077 /* -0.7609f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
2676 0 : p1 = L_shl( 1444612250 /* 2.6908f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
2677 0 : p0 = L_shl( 385258566 /* 0.7176f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
2678 :
2679 0 : p2 = Madd_32_32( Madd_32_32( p2, 501759554 /* 0.9346f in Q29*/, mod_b ), -252060893 /* -0.4695f in Q29 */, b_sqr ); // Q_b-2
2680 0 : p1 = Madd_32_32( Madd_32_32( p1, -1774680487 /* -3.3056f in Q29 */, mod_b ), 891635211 /* 1.6608f in Q29 */, b_sqr ); // Q_b-2
2681 0 : p0 = Madd_32_32( Madd_32_32( p0, -473251709 /* -0.8815f in Q29 */, mod_b ), 237780127 /* 0.4429f in Q29 */, b_sqr ); // Q_b-2
2682 :
2683 : /* approx = p0 + p1 * mod_a + p2 * mod_a * mod_a; */
2684 0 : approx = Madd_32_32( Mpy_32_32( p1, mod_a ), p2, a_sqr ); // Q_a+Q_b-33
2685 0 : approx = L_add( approx, L_shl( p0, sub( Q_a, 31 ) ) ); // Q_a+Q_b-33
2686 :
2687 0 : exp = sub( norm_l( approx ), 1 );
2688 0 : approx = L_shl( approx, exp ); // // Q_a+Q_b-33+exp
2689 :
2690 0 : *q_out = sub( add( sub( add( Q_a, Q_b ), 33 ), exp ), shift );
2691 0 : move16();
2692 :
2693 0 : return approx;
2694 : }
2695 :
2696 : /*===================================================================*/
2697 : /* FUNCTION : fir_fx () */
2698 : /*-------------------------------------------------------------------*/
2699 : /* PURPOSE : Generic FIR filter routine */
2700 : /*-------------------------------------------------------------------*/
2701 : /* INPUT ARGUMENTS : */
2702 : /* */
2703 : /* _ (Word16 []) b : filter coefficients (Qc). */
2704 : /* _ (Word16 []) x : input signal (Qn). */
2705 : /* _ (Word16) P : filter order. */
2706 : /* _ (Word16) N : number of input samples. */
2707 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
2708 : /*-------------------------------------------------------------------*/
2709 : /* OUTPUT ARGUMENTS : */
2710 : /* */
2711 : /* _ (Word16 []) y : output signal (Qn) */
2712 : /*-------------------------------------------------------------------*/
2713 : /* INPUT/OUTPUT ARGUMENTS : */
2714 : /* */
2715 : /* _ : None */
2716 : /*-------------------------------------------------------------------*/
2717 : /* RETURN ARGUMENTS : _ None. */
2718 : /*===================================================================*/
2719 112628 : void fir_fx( const Word16 x[], /* i : input vector Qx*/
2720 : const Word16 h[], /* i : impulse response of the FIR filter Q12*/
2721 : Word16 y[], /* o : output vector (result of filtering) Qx*/
2722 : Word16 mem[], /* i/o: memory of the input signal (L samples) Qx*/
2723 : const Word16 L, /* i : input vector size */
2724 : const Word16 K, /* i : order of the FIR filter (K+1 coefs.) */
2725 : const Word16 upd /* i : 1 = update the memory, 0 = not */
2726 : ,
2727 : Word16 shift /* i : difference between Q15 and scaling of h[] */
2728 : )
2729 : {
2730 :
2731 : Word16 buf_in[L_FRAME32k + L_FILT_MAX];
2732 : Word16 i, j;
2733 : Word32 s;
2734 : #ifndef ISSUE_1836_replace_overflow_libcom
2735 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
2736 : Flag Overflow = 0;
2737 : move32();
2738 : #endif
2739 : #endif
2740 : /* prepare the input buffer (copy and update memory) */
2741 112628 : Copy( mem, buf_in, K );
2742 112628 : Copy( x, buf_in + K, L );
2743 112628 : IF( upd )
2744 : {
2745 25 : Copy( buf_in + L, mem, K );
2746 : }
2747 :
2748 : /* do the filtering */
2749 34599428 : FOR( i = 0; i < L; i++ )
2750 : {
2751 : #ifdef ISSUE_1836_replace_overflow_libcom
2752 34486800 : s = L_mult_sat( buf_in[K + i], h[0] );
2753 : #else
2754 : s = L_mult_o( buf_in[K + i], h[0], &Overflow );
2755 : #endif
2756 :
2757 171875760 : FOR( j = 1; j <= K; j++ )
2758 : {
2759 : #ifdef ISSUE_1836_replace_overflow_libcom
2760 137388960 : s = L_mac_sat( s, h[j], buf_in[K + i - j] );
2761 : #else
2762 : s = L_mac_o( s, h[j], buf_in[K + i - j], &Overflow );
2763 : #endif
2764 : }
2765 : #ifdef ISSUE_1836_replace_overflow_libcom
2766 34486800 : s = L_shl_sat( s, shift );
2767 34486800 : y[i] = round_fx_sat( s ); /*Qx */
2768 : #else
2769 : s = L_shl_o( s, shift, &Overflow );
2770 : y[i] = round_fx_o( s, &Overflow ); /*Qx */
2771 : #endif
2772 34486800 : move16();
2773 : }
2774 112628 : }
2775 :
2776 : /*-------------------------------------------------------------------*
2777 : * v_add_32()
2778 : *
2779 : * Addition of two vectors sample by sample
2780 : *-------------------------------------------------------------------*/
2781 :
2782 4901854 : void v_add_32(
2783 : const Word32 x1[], /* i : Input vector 1 */
2784 : const Word32 x2[], /* i : Input vector 2 */
2785 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
2786 : const Word16 N /* i : Vector length */
2787 : )
2788 : {
2789 : Word16 i;
2790 :
2791 910913555 : FOR( i = 0; i < N; i++ )
2792 : {
2793 906011701 : y[i] = L_add( x1[i], x2[i] );
2794 906011701 : move32();
2795 : }
2796 :
2797 4901854 : return;
2798 : }
2799 :
2800 51422 : void v_shr_32(
2801 : Word32 x1[], /* i : Input vector 1 */
2802 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
2803 : const Word16 N, /* i : Vector length */
2804 : Word16 shift /*shift value*/
2805 : )
2806 : {
2807 : Word16 i;
2808 :
2809 15904712 : FOR( i = 0; i < N; i++ )
2810 : {
2811 15853290 : y[i] = L_shr( x1[i], shift );
2812 15853290 : move32();
2813 : }
2814 :
2815 51422 : return;
2816 : }
2817 :
2818 :
2819 : /*-------------------------------------------------------------------*
2820 : * v_sub_32()
2821 : *
2822 : * Subtraction of two vectors sample by sample
2823 : *-------------------------------------------------------------------*/
2824 :
2825 2511085 : void v_sub_32(
2826 : const Word32 x1[], /* i : Input vector 1 */
2827 : const Word32 x2[], /* i : Input vector 2 */
2828 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
2829 : const Word16 N /* i : Vector length */
2830 : )
2831 : {
2832 : Word16 i;
2833 :
2834 205293052 : FOR( i = 0; i < N; i++ )
2835 : {
2836 202781967 : y[i] = L_sub( x1[i], x2[i] );
2837 202781967 : move32();
2838 : }
2839 :
2840 2511085 : return;
2841 : }
2842 :
2843 :
2844 : /*-------------------------------------------------------------------*
2845 : * v_add_16()
2846 : *
2847 : * Addition of two vectors sample by sample
2848 : *-------------------------------------------------------------------*/
2849 :
2850 62682 : void v_add_16(
2851 : const Word16 x1[], /* i : Input vector 1 */
2852 : const Word16 x2[], /* i : Input vector 2 */
2853 : Word16 y[], /* o : Output vector that contains vector 1 + vector 2 */
2854 : const Word16 N /* i : Vector length */
2855 : )
2856 : {
2857 : Word16 i;
2858 :
2859 4902890 : FOR( i = 0; i < N; i++ )
2860 : {
2861 4840208 : y[i] = add_sat( x1[i], x2[i] );
2862 4840208 : move16();
2863 : }
2864 :
2865 62682 : return;
2866 : }
2867 :
2868 :
2869 : /*-------------------------------------------------------------------*
2870 : * v_sub_16()
2871 : *
2872 : * Subtraction of two vectors sample by sample
2873 : *-------------------------------------------------------------------*/
2874 :
2875 96944 : void v_sub_16(
2876 : const Word16 x1[], /* i : Input vector 1 */
2877 : const Word16 x2[], /* i : Input vector 2 */
2878 : Word16 y[], /* o : Output vector that contains vector 1 - vector 2 */
2879 : const Word16 N /* i : Vector length */
2880 : )
2881 : {
2882 : Word16 i;
2883 :
2884 346267 : FOR( i = 0; i < N; i++ )
2885 : {
2886 249323 : y[i] = sub_sat( x1[i], x2[i] );
2887 249323 : move16();
2888 : }
2889 :
2890 96944 : return;
2891 : }
2892 :
2893 :
2894 : /*--------------------------------------------------------------------------------*/
2895 : /* squant_fx() */
2896 : /*--------------------------------------------------------------------------------*/
2897 0 : Word16 squant_fx( /* o: index of the winning codeword */
2898 : const Word16 x, /* i: scalar value to quantize */
2899 : Word16 *xq, /* o: quantized value */
2900 : const Word16 cb[], /* i: codebook */
2901 : const Word16 cbsize /* i: codebook size */
2902 : )
2903 : {
2904 : Word16 tmp;
2905 : Word16 c, idx;
2906 : Word32 L_mindist, L_dist;
2907 : #ifndef ISSUE_1836_replace_overflow_libcom
2908 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
2909 : Flag Overflow = 0;
2910 : #endif
2911 : #endif
2912 :
2913 0 : idx = 0;
2914 0 : move16();
2915 0 : L_mindist = MAX_32;
2916 0 : move32();
2917 :
2918 0 : FOR( c = 0; c < cbsize; c++ )
2919 : {
2920 0 : L_dist = L_deposit_l( 0 );
2921 : #ifdef ISSUE_1836_replace_overflow_libcom
2922 0 : tmp = sub_sat( x, cb[c] );
2923 : #else
2924 : tmp = sub_o( x, cb[c], &Overflow );
2925 : #endif
2926 :
2927 : /*dist += tmp*tmp; */
2928 : #ifdef ISSUE_1836_replace_overflow_libcom
2929 0 : L_dist = L_mac_sat( L_dist, tmp, tmp );
2930 : #else
2931 : L_dist = L_mac_o( L_dist, tmp, tmp, &Overflow );
2932 : #endif
2933 :
2934 0 : if ( LT_32( L_dist, L_mindist ) )
2935 : {
2936 0 : idx = c;
2937 0 : move16();
2938 : }
2939 0 : L_mindist = L_min( L_mindist, L_dist );
2940 : }
2941 :
2942 0 : *xq = cb[idx];
2943 0 : move16();
2944 :
2945 0 : return idx;
2946 : }
2947 :
2948 : /*! r: index of the winning codeword */
2949 0 : Word16 squant_int_fx(
2950 : UWord8 x, /* i : scalar value to quantize */
2951 : UWord8 *xq, /* o : quantized value */
2952 : const UWord8 *cb, /* i : codebook */
2953 : const Word16 cbsize /* i : codebook size */
2954 : )
2955 : {
2956 : Word16 i, idx;
2957 : Word32 mindist, d;
2958 :
2959 0 : idx = 0;
2960 0 : move16();
2961 0 : mindist = 10000000; // Q0
2962 0 : move32();
2963 0 : FOR( i = 0; i < cbsize; i++ )
2964 : {
2965 0 : d = L_mult0( sub( x, cb[i] ), sub( x, cb[i] ) );
2966 0 : IF( LT_32( d, mindist ) )
2967 : {
2968 0 : mindist = d;
2969 0 : move32();
2970 0 : idx = i;
2971 0 : move16();
2972 : }
2973 : }
2974 0 : *xq = cb[idx];
2975 0 : move16();
2976 :
2977 0 : return idx;
2978 : }
2979 :
2980 : /*===================================================================*/
2981 : /* FUNCTION : pz_filter_dp_fx () */
2982 : /*-------------------------------------------------------------------*/
2983 : /* PURPOSE : Generic pole-zero filter routine, with double */
2984 : /* precision memory, transposed direct form II */
2985 : /*-------------------------------------------------------------------*/
2986 : /* INPUT ARGUMENTS : */
2987 : /* */
2988 : /* _ (Word16 []) b : zero filter coefficients (Qc). */
2989 : /* _ (Word16 []) a : pole filter coefficients (Qc), a(0)=1 */
2990 : /* _ (Word16 []) x : input signal (Qx). */
2991 : /* _ (Word16) P : filter order. */
2992 : /* _ (Word16) N : number of input samples. */
2993 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
2994 : /*-------------------------------------------------------------------*/
2995 : /* OUTPUT ARGUMENTS : */
2996 : /* */
2997 : /* _ (Word16 []) y : output signal (Qx) */
2998 : /*-------------------------------------------------------------------*/
2999 : /* INPUT/OUTPUT ARGUMENTS : */
3000 : /* */
3001 : /* _ (Word32 []) buf : filter memory (Qx+Qc) */
3002 : /*-------------------------------------------------------------------*/
3003 : /* RETURN ARGUMENTS : _ None. */
3004 : /*===================================================================*/
3005 :
3006 0 : void pz_filter_dp_fx(
3007 : const Word16 b[],
3008 : const Word16 a[],
3009 : Word16 x[],
3010 : Word16 y[],
3011 : Word32 buf[],
3012 : Word16 PNR,
3013 : Word16 PDR,
3014 : Word16 N,
3015 : Word16 Qa )
3016 : {
3017 : Word16 i, j;
3018 : Word16 s;
3019 : Word32 s_mem;
3020 : Word32 Ltemp1;
3021 : Word32 Lacc;
3022 :
3023 0 : s = negate( Qa );
3024 0 : s = add( s, s ); /* s=-2Qa */
3025 0 : s = add( s, 1 );
3026 0 : FOR( i = 0; i < N; i++ )
3027 : {
3028 0 : Lacc = L_deposit_h( x[i] ); /* Lacc in Q(16+Qn)*/
3029 0 : Lacc = L_shl( Lacc, s ); /* Lacc=x[i] in Q(16+Qn-2Qa+1)*/
3030 0 : FOR( j = PDR - 1; j >= 0; j-- )
3031 0 : Lacc = Msub_32_16( Lacc, buf[j], a[j + 1] ); /*Q(16+Qn-2Qa+1)*/
3032 :
3033 0 : s_mem = L_shl_sat( Lacc, sub( Qa, 1 ) ); /*Qn-Qa+16=Qn+Qc*/
3034 0 : Lacc = L_deposit_l( 0 );
3035 0 : FOR( j = PNR - 1; j >= 0; j-- )
3036 0 : Lacc = Madd_32_16( Lacc, buf[j], b[j + 1] );
3037 0 : Lacc = Madd_32_16( Lacc, s_mem, b[0] );
3038 : /* Lacc in Q(1+Qc+Qn-Qa) */
3039 :
3040 0 : FOR( j = s_max( PDR, PNR ) - 1; j > 0; j-- )
3041 : {
3042 : /* Update filter memory */
3043 0 : buf[j] = buf[j - 1];
3044 0 : move16();
3045 : }
3046 0 : buf[0] = s_mem;
3047 0 : move16();
3048 :
3049 0 : Ltemp1 = L_shr_sat( Lacc, s ); /* Ltemp1 in Qc+Qa+Qn=Q(16+Qn) */
3050 0 : y[i] = extract_h( Ltemp1 ); /* y[i] in Qn */
3051 0 : move16();
3052 : }
3053 0 : }
3054 :
3055 : /*-------------------------------------------------------------------*
3056 : * Copy_Scale_sig
3057 : *
3058 : * Up/down scale a 16 bits vector x and move it into y
3059 : *-------------------------------------------------------------------*/
3060 4538384 : void Copy_Scale_sig32(
3061 : const Word32 x[], /* i : signal to scale input Qx */
3062 : Word32 y[], /* o : scaled signal output Qx */
3063 : const Word16 lg, /* i : size of x[] Q0 */
3064 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
3065 : )
3066 : {
3067 : Word16 i;
3068 : Word32 L_tmp;
3069 4538384 : Word16 tmp = exp0;
3070 : #ifndef ISSUE_1836_replace_overflow_libcom
3071 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
3072 : Flag Overflow = 0;
3073 : move32();
3074 : #endif
3075 : #endif
3076 4538384 : IF( exp0 == 0 )
3077 : {
3078 15116712 : FOR( i = 0; i < lg; i++ )
3079 : {
3080 14889824 : y[i] = x[i];
3081 14889824 : move32();
3082 : }
3083 226888 : return;
3084 : }
3085 4311496 : IF( exp0 < 0 )
3086 : {
3087 395654722 : FOR( i = 0; i < lg; i++ )
3088 : {
3089 : #ifdef ISSUE_1836_replace_overflow_libcom
3090 392456235 : y[i] = L_shl_sat( x[i], tmp );
3091 : #else
3092 : y[i] = L_shl_o( x[i], tmp, &Overflow );
3093 : #endif
3094 392456235 : move16();
3095 : }
3096 3198487 : return;
3097 : }
3098 : #ifdef ISSUE_1836_replace_overflow_libcom
3099 1113009 : L_tmp = L_shl_sat( 1, exp0 - 1 );
3100 : #else
3101 : L_tmp = L_shl_o( 1, exp0 - 1, &Overflow );
3102 : #endif
3103 124218346 : FOR( i = 0; i < lg; i++ )
3104 : {
3105 123105337 : y[i] = W_extract_l( W_mult_32_32( L_tmp, x[i] ) );
3106 123105337 : move32(); /* saturation can occur here */
3107 : }
3108 : }
3109 :
3110 :
3111 : /*-------------------------------------------------------------------*
3112 : * Copy_Scale_sig32_16
3113 : *
3114 : * Up/down scale a 32 bits vector and round to 16 bits vector
3115 : *-------------------------------------------------------------------*/
3116 928420 : void Copy_Scale_sig32_16(
3117 : const Word32 *src, /* i : signal to scale Qx */
3118 : Word16 *dst, /* o : scaled signal Qx */
3119 : Word16 len, /* i : size of x[] Q0 */
3120 : Word16 exp0 ) /* i : exponent: x = round(x << exp) Qx ?exp */
3121 : {
3122 : Word16 i;
3123 : Word32 L_temp;
3124 :
3125 928420 : IF( exp0 == 0 )
3126 : {
3127 53407605 : FOR( i = 0; i < len; i++ )
3128 : {
3129 53208768 : *dst++ = round_fx_sat( *src++ );
3130 53208768 : move16();
3131 : }
3132 198837 : return;
3133 : }
3134 :
3135 60957197 : FOR( i = 0; i < len; i++ )
3136 : {
3137 60227614 : L_temp = L_shl_sat( *src++, exp0 );
3138 :
3139 60227614 : *dst++ = round_fx_sat( L_temp );
3140 60227614 : move16();
3141 : }
3142 : }
3143 :
3144 : /*-------------------------------------------------------------------*
3145 : * v_multc_att()
3146 : *
3147 : * Attenuation of a vector,, attenuation factor in Q15
3148 : *-------------------------------------------------------------------*/
3149 :
3150 0 : void v_multc_att(
3151 : const Word16 x[], /* i : Input vector Qx */
3152 : const Word16 att, /* i : Constant Q15, <= MAX_16 */
3153 : Word16 y[], /* o : Output vector that contains att*x */
3154 : const Word16 N /* i : Vector length */
3155 : )
3156 : {
3157 : Word16 i;
3158 0 : IF( LT_16( att, 32767 ) )
3159 : {
3160 0 : FOR( i = 0; i < N; i++ )
3161 : {
3162 0 : y[i] = mult_r( x[i], att );
3163 0 : move16();
3164 : }
3165 : }
3166 0 : } /*-------------------------------------------------------------------*
3167 : * v_multc_att32()
3168 : *
3169 : * Attenuation of a vector,, attenuation factor in Q15
3170 : *-------------------------------------------------------------------*/
3171 :
3172 0 : void v_multc_att32(
3173 : const Word32 x[], /* i : Input vector Qx */
3174 : const Word16 att, /* i : Constant Q15, <= MAX_16 */
3175 : Word32 y[], /* o : Output vector that contains att*x */
3176 : const Word16 N /* i : Vector length */
3177 : )
3178 : {
3179 : Word16 i;
3180 0 : IF( LT_16( att, 32767 ) )
3181 : {
3182 0 : FOR( i = 0; i < N; i++ )
3183 : {
3184 0 : y[i] = Mpy_32_16_r( x[i], att );
3185 0 : move32();
3186 : }
3187 : }
3188 0 : }
3189 :
3190 : /*-------------------------------------------------------------------*
3191 : * v_multc_att3232()
3192 : *
3193 : * Attenuation of a vector, attenuation factor in Q31
3194 : *-------------------------------------------------------------------*/
3195 :
3196 0 : void v_multc_att3232(
3197 : const Word32 x[], /* i : Input vector Qx */
3198 : const Word32 att, /* i : Constant Q32, <= MAX_32 */
3199 : Word32 y[], /* o : Output vector that contains att*x */
3200 : const Word16 N /* i : Vector length */
3201 : )
3202 : {
3203 : Word16 i;
3204 0 : IF( LE_32( att, MAX_32 ) )
3205 : {
3206 0 : FOR( i = 0; i < N; i++ )
3207 : {
3208 0 : y[i] = Mpy_32_32_r( x[i], att );
3209 0 : move32();
3210 : }
3211 : }
3212 0 : }
3213 :
3214 : /*-------------------------------------------------------------------*
3215 : * v_L_mult_1616()
3216 : *
3217 : * Multiplication of two vectors, Output in Word32
3218 : *-------------------------------------------------------------------*/
3219 :
3220 0 : void v_L_mult_1616(
3221 : const Word16 x1[], /* i : Input vector 1 */
3222 : const Word16 x2[], /* i : Input vector 2 */
3223 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
3224 : const Word16 N /* i : Vector length */
3225 : )
3226 : {
3227 : Word16 i;
3228 :
3229 0 : for ( i = 0; i < N; i++ )
3230 : {
3231 0 : y[i] = L_mult( x1[i], x2[i] );
3232 0 : move32();
3233 : }
3234 :
3235 0 : return;
3236 : }
3237 :
3238 : /*-------------------------------------------------------------------*
3239 : * v_L_mult_3216()
3240 : *
3241 : * Multiplication of two vectors, Output in Word32
3242 : *-------------------------------------------------------------------*/
3243 :
3244 90653 : void v_L_mult_3216(
3245 : const Word32 x1[], /* i : Input vector 1 */
3246 : const Word16 x2[], /* i : Input vector 2 */
3247 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
3248 : const Word16 N /* i : Vector length */
3249 : )
3250 : {
3251 : Word16 i;
3252 :
3253 52558237 : for ( i = 0; i < N; i++ )
3254 : {
3255 52467584 : y[i] = Mpy_32_16_1( x1[i], x2[i] );
3256 52467584 : move32();
3257 : }
3258 :
3259 90653 : return;
3260 : }
3261 :
3262 : /*-------------------------------------------------------------------*
3263 : * add_vec()
3264 : *
3265 : * Addition of two vectors sample by sample
3266 : *-------------------------------------------------------------------*/
3267 :
3268 2036 : void add_vec_fx(
3269 : const Word16 x1[], /* i : Input vector 1 */
3270 : const Word16 Qx1, /* i : SCaling of input 1 */
3271 : const Word16 x2[], /* i : Input vector 2 */
3272 : const Word16 Qx2, /* i : SCaling of input 1 */
3273 : Word16 y[], /* o : Output vector that contains vector 1 + vector 2 */
3274 : const Word16 Qy, /* i : SCaling of output 1 */
3275 : const Word16 N /* i : Vector lenght */
3276 : )
3277 : {
3278 : Word16 i, Qyx1, Qyx2;
3279 : #ifndef ISSUE_1836_replace_overflow_libcom
3280 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
3281 : Flag Overflow = 0;
3282 : move32();
3283 : #endif
3284 : #endif
3285 2036 : Qyx1 = sub( Qx1, Qy );
3286 2036 : Qyx2 = sub( Qx2, Qy );
3287 2036 : IF( Qyx1 == 0 )
3288 : {
3289 1719476 : FOR( i = 0; i < N; i++ )
3290 : {
3291 : #ifdef ISSUE_1836_replace_overflow_libcom
3292 1717440 : y[i] = add_sat( x1[i], shr_r_sat( x2[i], Qyx2 ) );
3293 : #else
3294 : y[i] = add_o( x1[i], shr_r_sat( x2[i], Qyx2 ), &Overflow );
3295 : #endif
3296 1717440 : move16();
3297 : }
3298 : }
3299 : ELSE
3300 : {
3301 0 : FOR( i = 0; i < N; i++ )
3302 : {
3303 : #ifdef ISSUE_1836_replace_overflow_libcom
3304 0 : y[i] = add_sat( shr_r_sat( x1[i], Qyx1 ), shr_r_sat( x2[i], Qyx2 ) ); //!!sat //!!sat
3305 : #else
3306 : y[i] = add_o( shr_r_sat( x1[i], Qyx1 ), shr_r_sat( x2[i], Qyx2 ), &Overflow );
3307 : #endif
3308 0 : move16();
3309 : }
3310 : }
3311 2036 : return;
3312 : }
3313 :
3314 : /*-------------------------------------------------------------------*
3315 : * Add_flt32_flt32
3316 : *
3317 : * Add two Pseudo Float Value that are 32 Bits Mantisa and 16 Bits Exp
3318 : *-------------------------------------------------------------------*/
3319 5352 : Word32 Add_flt32_flt32( /* o: Result (Normalized) */
3320 : Word32 a, /* i: 1st Value */
3321 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
3322 : Word32 b, /* i: 2nd Value */
3323 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
3324 : Word16 *exp_out /* o: Exponent of Result */
3325 : )
3326 : {
3327 : Word16 temp, temp2;
3328 : Word32 L_temp;
3329 :
3330 : /* Subract 1 to further divide by 2 to avoid overflow on L_add */
3331 5352 : temp = sub( s_min( exp_a, exp_b ), 1 );
3332 :
3333 : /* Put both to same exponent */
3334 5352 : exp_a = sub( exp_a, temp );
3335 5352 : a = L_shr( a, exp_a );
3336 5352 : exp_b = sub( exp_b, temp );
3337 5352 : b = L_shr( b, exp_b );
3338 :
3339 : /* add them together */
3340 5352 : L_temp = L_add( a, b );
3341 5352 : temp2 = norm_l( L_temp );
3342 :
3343 5352 : *exp_out = add( temp, temp2 );
3344 5352 : move16();
3345 :
3346 5352 : return L_shl( L_temp, temp2 );
3347 : }
3348 :
3349 : /*-------------------------------------------------------------------*
3350 : * Mul_flt32_Q15
3351 : *
3352 : * Multiply one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
3353 : * with a Q15 value
3354 : *-------------------------------------------------------------------*/
3355 0 : Word32 Mul_flt32_Q15( /* o: Result (Normalized) */
3356 : Word32 value, /* i: Pseudo_float Value */
3357 : Word16 *exp_v, /*i/o: Exponent of Value (Q of Value) */
3358 : Word16 frac /* i: Q15 value */
3359 : )
3360 : {
3361 : Word16 temp;
3362 : Word32 L_temp;
3363 :
3364 0 : L_temp = Mult_32_16( value, frac );
3365 0 : temp = norm_l( L_temp );
3366 :
3367 0 : *exp_v = add( temp, *exp_v );
3368 0 : move16();
3369 :
3370 0 : return L_shl( L_temp, temp );
3371 : }
3372 :
3373 : /*-------------------------------------------------------------------*
3374 : * Div_flt32_flt32
3375 : *
3376 : * Divide one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
3377 : * by another one
3378 : *-------------------------------------------------------------------*/
3379 2 : Word32 Div_flt32_flt32( /* o: Result (Normalized) */
3380 : Word32 a, /* i: 1st Value */
3381 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
3382 : Word32 b, /* i: 2nd Value */
3383 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
3384 : Word16 *exp_out /* o: Exponent of Result */
3385 : )
3386 : {
3387 : Word16 temp, temp2;
3388 : Word32 L_temp;
3389 :
3390 2 : temp = div_s( 16384, round_fx( b ) );
3391 2 : L_temp = Mult_32_16( a, temp );
3392 2 : temp2 = sub( 31 - 1, exp_b );
3393 2 : temp2 = add( temp2, exp_a );
3394 :
3395 2 : temp = norm_l( L_temp );
3396 :
3397 2 : *exp_out = add( temp, temp2 );
3398 2 : move16();
3399 :
3400 2 : return L_shl( L_temp, temp );
3401 : }
3402 :
3403 :
3404 : /*-------------------------------------------------------------------*
3405 : * Calc_Energy_Autoscaled
3406 : *
3407 : * Calculate Energy with overflow protection
3408 : *-------------------------------------------------------------------*/
3409 2775366 : Word32 Calc_Energy_Autoscaled( /* o: Result (Energy) */
3410 : const Word16 *signal, /* i: Signal */
3411 : Word16 signal_exp, /* i: Exponent of Signal (Q of Signal) */
3412 : Word16 len, /* i: Frame Length */
3413 : Word16 *energy_exp /* o: Exponent of Energy (Q of Energy) */
3414 : )
3415 : {
3416 : Word16 temp, temp2;
3417 : Word32 L_temp, L_Energy;
3418 : Word16 i, j;
3419 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
3420 2775366 : Flag Overflow = 0;
3421 2775366 : move32();
3422 : #endif
3423 :
3424 2775366 : Overflow = 0;
3425 2775366 : move16();
3426 :
3427 2775366 : temp2 = 0;
3428 2775366 : move16();
3429 2775366 : L_Energy = L_deposit_l( 1 );
3430 2775366 : j = s_and( 7, len );
3431 2775366 : FOR( i = 0; i < j; i++ )
3432 : {
3433 : /* divide by 2 so energy will be divided by 4 */
3434 : #ifdef ISSUE_1836_replace_overflow_libcom
3435 0 : temp = mult_r( *signal++, 16384 );
3436 : #else
3437 : temp = mult_ro( *signal++, 16384, &Overflow );
3438 : #endif
3439 0 : L_Energy = L_mac0_o( L_Energy, temp, temp, &Overflow );
3440 : }
3441 8974376 : FOR( i = j; i < len; i += 8 ) /* Process 8 Samples at a time */
3442 : {
3443 : /* divide by 2 so energy will be divided by 4 */
3444 6199010 : temp = mult_ro( *signal++, 16384, &Overflow );
3445 6199010 : L_temp = L_mult0( temp, temp );
3446 49592080 : FOR( j = 1; j < 8; j++ )
3447 : {
3448 : #ifdef ISSUE_1836_replace_overflow_libcom
3449 43393070 : temp = mult_r( *signal++, 16384 );
3450 : #else
3451 : temp = mult_ro( *signal++, 16384, &Overflow );
3452 : #endif
3453 43393070 : L_temp = L_mac0_o( L_temp, temp, temp, &Overflow );
3454 : }
3455 : /*Overfloe will never happen because temp2 is always positive*/
3456 6199010 : L_temp = L_shr( L_temp, temp2 );
3457 : /* Here we try the addition just to check if we can sum
3458 : the energy of the small (8 Iterations) loop with the
3459 : total energy calculated so far without an overflow.
3460 : The result is discarded. If there is an overflow both
3461 : energies are div by 2. Otherwise, nothing is done.
3462 : After the 'IF', the sum is done again and will always
3463 : be without an overflow. */
3464 6199010 : L_add_o( L_Energy, L_temp, &Overflow );
3465 6199010 : IF( Overflow != 0 )
3466 : {
3467 47 : L_Energy = L_shr( L_Energy, 1 );
3468 47 : L_temp = L_shr( L_temp, 1 );
3469 47 : temp2 = add( temp2, 1 );
3470 47 : Overflow = 0;
3471 47 : move16();
3472 : }
3473 6199010 : L_Energy = L_add_o( L_Energy, L_temp, &Overflow );
3474 : }
3475 : /* Calc Final Exponent (sub 2 because of the mult_r by 16384 that already divs the ener by 4) */
3476 2775366 : temp2 = sub( sub( shl( signal_exp, 1 ), temp2 ), 2 );
3477 :
3478 2775366 : *energy_exp = temp2;
3479 2775366 : move16();
3480 :
3481 2775366 : return L_Energy;
3482 : }
3483 :
3484 3584128 : Word16 Find_Max_Norm16( const Word16 *src, Word16 len )
3485 : {
3486 : Word16 i;
3487 : Word16 max16;
3488 :
3489 : /* it starts at '0' and not '1' like in Find_Max_Norm32() */
3490 : /* and that is necessary. */
3491 3584128 : max16 = 0;
3492 3584128 : move16();
3493 :
3494 2275061410 : FOR( i = 0; i < len; i++ )
3495 : {
3496 2271477282 : max16 = s_max( max16, abs_s( *src++ ) );
3497 : }
3498 :
3499 3584128 : return norm_s( max16 );
3500 : }
3501 :
3502 3553385 : Word16 Find_Max_Norm32( const Word32 *src, Word16 len )
3503 : {
3504 : Word16 i;
3505 : Word32 max32;
3506 :
3507 3553385 : max32 = L_deposit_l( 1 );
3508 :
3509 2840854324 : FOR( i = 0; i < len; i++ )
3510 : {
3511 2837300939 : max32 = L_max( max32, L_abs( *src++ ) );
3512 : }
3513 :
3514 3553385 : return norm_l( max32 );
3515 : }
3516 :
3517 : /*-------------------------------------------------------------------*
3518 : * Sqrt_Ratio32
3519 : *
3520 : * Calculate Sqrt of Val1/Val2
3521 : *-------------------------------------------------------------------*/
3522 1091 : Word32 Sqrt_Ratio32( /* o: Result in Q31 */
3523 : Word32 L_val1, /* i: Mantisa of Val1 */
3524 : Word16 exp1, /* i: Exp of Val1 (>0: Val was Left Shifted, <0:Right Shifted) */
3525 : Word32 L_val2, /* i: Mantisa of Val2 */
3526 : Word16 exp2, /* i: Exp of Val2 (same as exp1) */
3527 : Word16 *exp /* o: Exp of Result (# of 'L_shl' Req to get to Final Value) */
3528 : )
3529 : {
3530 : Word16 temp;
3531 :
3532 : /* Normalize Energy #1 */
3533 1091 : temp = norm_l( L_val1 );
3534 1091 : L_val1 = L_shl( L_val1, temp );
3535 : /* Adjust Exponent of Energy #1 */
3536 1091 : exp1 = add( exp1, temp );
3537 :
3538 : /* Normalize Energy #2 */
3539 1091 : temp = norm_l( L_val2 );
3540 1091 : L_val2 = L_shl( L_val2, temp );
3541 : /* Adjust Exponent of Energy #1 */
3542 1091 : exp2 = add( exp2, temp );
3543 :
3544 : /* Prepare for Inverse */
3545 1091 : temp = round_fx_sat( L_val1 );
3546 1091 : temp = div_s( 16384, temp );
3547 : /* Mult Now */
3548 1091 : L_val2 = Mult_32_16( L_val2, temp );
3549 1091 : exp1 = add( sub( exp2, exp1 ), 15 * 2 );
3550 :
3551 : /* Here Result of ('L_val2' / 2^'exp2') / ('L_val1' / 2^'exp1') is */
3552 : /* 'L_val2' / 2^'exp1' */
3553 : /* Which is val2/val1 instead of val1/val2 because we will use Inverted Square Root */
3554 : /* Normalize before Square Root */
3555 1091 : temp = norm_l( L_val2 );
3556 1091 : L_val2 = L_shl( L_val2, temp );
3557 1091 : exp1 = add( temp, exp1 );
3558 : /* Do Sqrt */
3559 1091 : temp = sub( 31, exp1 );
3560 1091 : L_val1 = Isqrt_lc( L_val2, &temp );
3561 :
3562 1091 : *exp = temp;
3563 1091 : move16();
3564 :
3565 1091 : return L_val1;
3566 : }
3567 :
3568 0 : Word16 Invert16( /* result in Q'15 + 'exp' */
3569 : Word16 val,
3570 : Word16 *exp )
3571 : {
3572 : Word16 temp;
3573 :
3574 : /* prevent 0 input */
3575 0 : val = s_max( val, 1 );
3576 : /* Normalize Value */
3577 0 : temp = norm_s( val );
3578 0 : val = shl( val, temp );
3579 :
3580 0 : *exp = sub( sub( 15 - 1, *exp ), temp );
3581 0 : move16(); /* -1 because of 0x4000 is 1.0 in Q14 (and not Q15) */
3582 :
3583 0 : temp = div_s( 0x4000, val );
3584 :
3585 0 : return temp;
3586 : }
3587 :
3588 0 : Word16 find_rem( Word16 n, Word16 m, Word16 *r )
3589 : {
3590 : Word16 i, q1, q2, qd;
3591 : Word32 Ltemp2;
3592 : Word32 Lacc;
3593 :
3594 0 : test();
3595 0 : test();
3596 0 : IF( n <= 0 || m <= 0 || n < m )
3597 : {
3598 0 : *r = n;
3599 0 : move16();
3600 0 : return ( 0 );
3601 : }
3602 :
3603 0 : q1 = norm_s( n );
3604 0 : q1 = sub( q1, 1 );
3605 0 : Lacc = L_deposit_h( shl( n, q1 ) );
3606 0 : qd = sub( q1, 1 );
3607 0 : q2 = norm_s( m );
3608 0 : q2 = sub( q2, 1 );
3609 0 : Ltemp2 = L_deposit_h( shl( m, q2 ) );
3610 0 : qd = sub( q2, qd );
3611 0 : q2 = add( q2, 1 );
3612 :
3613 0 : FOR( i = 0; i < qd; i++ )
3614 : {
3615 0 : Lacc = L_sub( Lacc, Ltemp2 );
3616 0 : IF( Lacc >= 0 )
3617 : {
3618 0 : Lacc = L_add( L_shl( Lacc, 1 ), 1 );
3619 : }
3620 : ELSE
3621 : {
3622 0 : Lacc = L_add( Lacc, Ltemp2 );
3623 0 : Lacc = L_shl( Lacc, 1 );
3624 : }
3625 : }
3626 0 : q1 = extract_l( Lacc );
3627 0 : Ltemp2 = L_shr( Lacc, q2 );
3628 0 : *r = extract_h( Ltemp2 );
3629 0 : move16();
3630 0 : return ( q1 );
3631 : }
3632 :
3633 :
3634 0 : Word32 find_remd( Word32 n, Word32 m, Word32 *r )
3635 : {
3636 : Word16 i, q1, q2, qd;
3637 : Word32 Ltemp2, qo;
3638 : Word32 Lacc;
3639 :
3640 0 : test();
3641 0 : test();
3642 0 : IF( n <= 0 || m <= 0 || n < m )
3643 : {
3644 0 : *r = n;
3645 0 : move16();
3646 0 : return ( 0 );
3647 : }
3648 :
3649 0 : q1 = norm_l( n );
3650 0 : q1 = sub( q1, 1 );
3651 0 : Lacc = L_shl( n, q1 );
3652 0 : qd = sub( q1, 1 );
3653 0 : q2 = norm_l( m );
3654 0 : q2 = sub( q2, 1 );
3655 0 : Ltemp2 = L_shl( m, q2 );
3656 0 : qd = sub( q2, qd );
3657 0 : q2 = add( q2, 1 );
3658 0 : qo = 0;
3659 0 : move16();
3660 :
3661 0 : FOR( i = 0; i < qd; i++ )
3662 : {
3663 0 : Lacc = L_sub( Lacc, Ltemp2 );
3664 0 : qo = L_shl( qo, 1 );
3665 0 : IF( Lacc >= 0 )
3666 : {
3667 0 : Lacc = L_shl( Lacc, 1 );
3668 0 : qo = L_add( qo, 1 );
3669 : }
3670 : ELSE
3671 : {
3672 0 : Lacc = L_add( Lacc, Ltemp2 );
3673 0 : Lacc = L_shl( Lacc, 1 );
3674 : }
3675 : }
3676 0 : *r = L_shr( Lacc, q2 );
3677 0 : move16();
3678 0 : return ( qo );
3679 : }
3680 :
3681 0 : Word16 rint_new_fx(
3682 : Word32 x /*Q16 */
3683 : )
3684 : {
3685 : Word16 a;
3686 : Word32 L_tmp;
3687 : Word16 frac, tmp;
3688 :
3689 : /* middle value point test */
3690 0 : frac = lshr( extract_l( x ), 1 ); /*Q15 */
3691 0 : tmp = sub( frac, 0x4000 );
3692 :
3693 0 : IF( !tmp )
3694 : {
3695 0 : a = add( extract_h( x ), 1 );
3696 :
3697 0 : IF( s_and( a, 1 ) == 0 )
3698 : {
3699 0 : return a;
3700 : }
3701 0 : IF( s_and( a, 1 ) != 0 )
3702 : {
3703 0 : return extract_h( x );
3704 : }
3705 0 : return extract_h( x );
3706 : }
3707 : ELSE
3708 : {
3709 0 : L_tmp = L_add( x, 32768 ); /*Q16 */
3710 0 : return extract_h( L_tmp );
3711 : }
3712 : }
3713 :
3714 :
3715 : /*===================================================================*/
3716 : /* FUNCTION : erb_diff_search_fx () */
3717 : /*-------------------------------------------------------------------*/
3718 : /* PURPOSE : erb amplitude VQ search for QPPP */
3719 : /*-------------------------------------------------------------------*/
3720 : /* INPUT ARGUMENTS : */
3721 : /* _ (Word16 []) prev_erb : Previous erb amplitude, Q13 */
3722 : /* _ (Word16 []) curr_erb : Current erb amplitude, Q13 */
3723 : /* _ (Word16 []) dif_erb: erb differential, Q13 */
3724 : /* _ (Word16 []) pow_spec : LPC power spectrum, Q7 */
3725 : /* _ (Word16 [][]) cb_fx : differential erb codebook, Q13 */
3726 : /* _ (Word16) cb_size : codebook size */
3727 : /* _ (Word16) cb_dim : codebook dimension */
3728 : /* _ (Word16) offset : index to current segment of erb array */
3729 : /* for quantization */
3730 : /*-------------------------------------------------------------------*/
3731 : /* OUTPUT ARGUMENTS : */
3732 : /* _ None */
3733 : /*-------------------------------------------------------------------*/
3734 : /* INPUT/OUTPUT ARGUMENTS : */
3735 : /* _ None */
3736 : /*-------------------------------------------------------------------*/
3737 : /* RETURN ARGUMENTS : */
3738 : /* _ (Word16) index: best codebook index */
3739 : /*-------------------------------------------------------------------*/
3740 : /* CALLED FROM : TX */
3741 : /*===================================================================*/
3742 :
3743 0 : Word16 erb_diff_search_fx( Word16 *prev_erb, const Word16 *curr_erb, Word16 *dif_erb, Word16 *pow_spec, const Word16 *cb_fx, Word16 cb_size, Word16 cb_dim, Word16 offset )
3744 : {
3745 : Word16 i, j, mmseindex;
3746 : Word16 dh, dl;
3747 : Word32 mmse;
3748 : Word32 Ltemp1;
3749 : Word32 Lacc;
3750 :
3751 0 : mmse = EVS_LW_MAX;
3752 0 : move32();
3753 0 : mmseindex = -1;
3754 0 : move16();
3755 0 : FOR( j = 0; j < cb_size; j++ )
3756 : {
3757 :
3758 0 : Lacc = L_deposit_l( 0 );
3759 0 : FOR( i = 0; i < cb_dim; i++ )
3760 : {
3761 0 : IF( add_sat( cb_fx[j * cb_dim + i], prev_erb[i + offset] ) < 0 )
3762 : {
3763 0 : Ltemp1 = L_mult( curr_erb[i + offset], curr_erb[i + offset] ); /* Q27 */
3764 0 : dh = extract_h( Ltemp1 );
3765 0 : dl = extract_l( Ltemp1 );
3766 0 : IF( dl < 0 )
3767 : {
3768 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3769 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
3770 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3771 : }
3772 : ELSE
3773 : {
3774 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl );
3775 : }
3776 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) );
3777 : }
3778 : ELSE
3779 : {
3780 0 : dh = sub_sat( dif_erb[i + offset], cb_fx[j * cb_dim + i] ); /* Q13 */
3781 0 : Ltemp1 = L_mult_sat( dh, dh ); /* Q27 */
3782 0 : dh = extract_h( Ltemp1 );
3783 0 : dl = extract_l( Ltemp1 );
3784 :
3785 0 : IF( dl < 0 )
3786 : {
3787 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3788 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
3789 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3790 : }
3791 : ELSE
3792 : {
3793 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl ); /* Q33 */
3794 : }
3795 :
3796 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) ); /* Q18 */
3797 : }
3798 :
3799 0 : IF( LT_16( cb_fx[j * cb_dim + i], dif_erb[i + offset] ) )
3800 : {
3801 0 : dh = extract_h( Ltemp1 );
3802 0 : dl = extract_l( Ltemp1 );
3803 0 : IF( dl < 0 )
3804 : {
3805 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3806 0 : Ltemp1 = Mult_32_16( Ltemp1, 29491 );
3807 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3808 : }
3809 : ELSE
3810 : {
3811 0 : Ltemp1 = (Word32) L_mult0( 29491, dl ); /* 29491=0.9 in Q15 */
3812 : }
3813 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( dh, 29491 ) );
3814 : }
3815 0 : Lacc = L_add( Lacc, Ltemp1 ); /* Q18 */
3816 : }
3817 :
3818 0 : IF( LT_32( Lacc, mmse ) )
3819 : {
3820 0 : mmse = L_add( Lacc, 0 );
3821 0 : mmseindex = j;
3822 0 : move16();
3823 : }
3824 : }
3825 :
3826 0 : return ( mmseindex );
3827 : }
3828 491688 : void Acelp_dec_total_exc(
3829 : Word16 *exc_fx, /* i/o: adapt. excitation exc */
3830 : Word16 *exc2_fx, /* i/o: adapt. excitation/total exc */
3831 : const Word16 gain_code16, /* i : Gain code Q0 */
3832 : const Word16 gain_pit_fx, /* i ; Pitch gain in Q14 */
3833 : const Word16 i_subfr, /* i ; subfr */
3834 : const Word16 *code_fx, /* i : code in Q9 */
3835 : const Word16 L_subfr /* i : Subframne lenght */
3836 : )
3837 : {
3838 : Word16 i;
3839 : Word32 L_tmp;
3840 :
3841 31959720 : FOR( i = 0; i < L_subfr; i++ )
3842 : {
3843 31468032 : L_tmp = L_shl_sat( L_mult( gain_pit_fx, exc_fx[i + i_subfr] ), 1 ); /*Q16+Q_exc*/
3844 31468032 : exc2_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
3845 31468032 : L_tmp = L_add_sat( L_tmp, L_shl_sat( L_mult( gain_code16, code_fx[i] ), 6 ) ); /*Q16+Q_exc*/
3846 31468032 : exc_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
3847 31468032 : move16();
3848 31468032 : move16();
3849 : }
3850 491688 : }
3851 :
3852 : /*-------------------------------------------------------------------*
3853 : * UL_inverse
3854 : *
3855 : * Calculate inverse of UL_val. Output in Q_exp.
3856 : *-------------------------------------------------------------------*/
3857 306303 : UWord32 UL_inverse( const UWord32 UL_val, Word16 *exp )
3858 : {
3859 : UWord32 UL_tmp;
3860 :
3861 306303 : *exp = norm_ul( UL_val );
3862 306303 : move16();
3863 306303 : UL_tmp = UL_lshl( UL_val, *exp ); /* Q32 */
3864 :
3865 306303 : *exp = add( 32, sub( 31, *exp ) );
3866 306303 : move16();
3867 :
3868 306303 : return UL_div( 0x80000000, UL_tmp );
3869 : }
3870 :
3871 : /*-------------------------------------------------------------------*
3872 : * UL_div
3873 : *
3874 : * Calculate UL_num/UL_den. UL_num assumed to be Q31, UL_den assumed
3875 : * to be Q32, then result is in Q32.
3876 : *-------------------------------------------------------------------*/
3877 612606 : UWord32 UL_div( const UWord32 UL_num, const UWord32 UL_den )
3878 : {
3879 : UWord32 UL_e, UL_Q;
3880 : UWord32 UL_msb, UL_lsb;
3881 : Word16 i;
3882 :
3883 612606 : UL_e = UL_subNsD( 0xffffffff, UL_den );
3884 612606 : UL_Q = UL_num;
3885 612606 : move32();
3886 :
3887 3675636 : FOR( i = 0; i < 5; i++ )
3888 : {
3889 3063030 : Mpy_32_32_uu( UL_Q, UL_e, &UL_msb, &UL_lsb ); /*31+32-32=31 */
3890 3063030 : UL_Q = UL_addNsD( UL_Q, UL_msb );
3891 3063030 : Mpy_32_32_uu( UL_e, UL_e, &UL_e, &UL_lsb ); /*32+32-32=32 */
3892 : }
3893 :
3894 612606 : return UL_Q;
3895 : }
3896 :
3897 : /*-----------------------------------------------------------------------------
3898 : * ratio()
3899 : *
3900 : * Divide the numerator by the denominator.
3901 : *----------------------------------------------------------------------------*/
3902 6849384 : Word16 ratio( const Word32 numer, const Word32 denom, Word16 *expo )
3903 : {
3904 : Word16 expNumer, expDenom;
3905 : Word16 manNumer, manDenom;
3906 : Word16 quotient;
3907 :
3908 6849384 : expDenom = norm_l( denom ); /* exponent */
3909 6849384 : manDenom = extract_h( L_shl( denom, expDenom ) ); /* mantissa */
3910 6849384 : expNumer = norm_l( numer ); /* exponent */
3911 6849384 : manNumer = extract_h( L_shl( numer, expNumer ) ); /* mantissa */
3912 6849384 : manNumer = shr( manNumer, 1 ); /* Ensure the numerator < the denominator */
3913 6849384 : quotient = div_s( manNumer, manDenom ); /* in Q14 */
3914 :
3915 6849384 : *expo = sub( expNumer, expDenom );
3916 6849384 : move16();
3917 6849384 : return quotient; /* Q14 */
3918 : }
3919 :
3920 : /*-----------------------------------------------------------------------*
3921 : * Function hp400_12k8() *
3922 : * *
3923 : * 2nd order Cheb2 high pass filter with cut off frequency at 400 Hz. *
3924 : * Optimized for fixed-point to get the following frequency response : *
3925 : * *
3926 : * frequency : 0Hz 100Hz 200Hz 300Hz 400Hz 630Hz 1.5kHz 3kHz *
3927 : * dB loss : -infdB -30dB -20dB -10dB -3dB +6dB +1dB 0dB *
3928 : * *
3929 : * Algorithm : *
3930 : * *
3931 : * y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] *
3932 : * + a[1]*y[i-1] + a[2]*y[i-2]; *
3933 : * *
3934 : * short b[3] = {3660, -7320, 3660}; in Q12 *
3935 : * short a[3] = {4096, 7320, -3540}; in Q12 *
3936 : * *
3937 : * float --> b[3] = {0.893554687, -1.787109375, 0.893554687}; *
3938 : * a[3] = {1.000000000, 1.787109375, -0.864257812}; *
3939 : *-----------------------------------------------------------------------*/
3940 0 : void hp400_12k8_fx(
3941 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
3942 : const Word16 lg, /* i : lenght of signal */
3943 : Word16 mem[] /* i/o: filter memory [6] */
3944 : )
3945 : {
3946 : Word16 i;
3947 : Word16 y1_hi, y1_lo;
3948 : Word32 L_tmp, L_tmp2, L_tmp3;
3949 :
3950 0 : y1_hi = mem[2];
3951 0 : move16();
3952 0 : y1_lo = mem[3];
3953 0 : move16();
3954 :
3955 0 : L_tmp3 = L_mac( 16384L, mem[1], a_hp400_fx[2] ); /* rounding to maximize precision */
3956 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
3957 0 : L_tmp3 = L_shr( L_tmp3, 15 );
3958 0 : L_tmp2 = L_mac( L_tmp3, mem[0], a_hp400_fx[2] );
3959 0 : L_tmp2 = L_mac( L_tmp2, mem[5], b_hp400_fx[2] );
3960 0 : L_tmp2 = L_mac( L_tmp2, mem[4], b_hp400_fx[1] );
3961 0 : L_tmp3 = L_mult( mem[4], b_hp400_fx[2] );
3962 :
3963 0 : mem[5] = signal[lg - 2];
3964 0 : move16();
3965 0 : FOR( i = 1; i < lg; i++ )
3966 : {
3967 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
3968 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
3969 :
3970 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
3971 0 : L_tmp = L_mac( L_tmp, *signal, b_hp400_fx[0] );
3972 :
3973 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
3974 :
3975 0 : L_tmp2 = L_mac( L_tmp3, y1_hi, a_hp400_fx[2] );
3976 0 : L_tmp2 = L_mac( L_tmp2, *signal, b_hp400_fx[1] );
3977 0 : L_tmp3 = L_mac( 16384L, y1_lo, a_hp400_fx[2] ); /* rounding to maximize precision */
3978 :
3979 0 : y1_lo = L_Extract_lc( L_tmp, &y1_hi );
3980 :
3981 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
3982 0 : L_tmp3 = L_shr( L_tmp3, 15 );
3983 :
3984 0 : L_tmp2 = L_add( L_tmp3, L_tmp2 );
3985 :
3986 0 : L_tmp3 = L_mult( *signal, b_hp400_fx[2] );
3987 :
3988 : /* signal is divided by 16 to avoid overflow in energy computation */
3989 0 : *signal++ = round_fx( L_tmp );
3990 0 : move16();
3991 : }
3992 :
3993 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
3994 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
3995 :
3996 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
3997 :
3998 0 : mem[4] = *signal;
3999 0 : move16();
4000 0 : L_tmp = L_mac( L_tmp, mem[4], b_hp400_fx[0] );
4001 :
4002 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
4003 :
4004 0 : mem[0] = y1_hi;
4005 0 : move16();
4006 0 : mem[1] = y1_lo;
4007 0 : move16();
4008 0 : L_Extract( L_tmp, &mem[2], &mem[3] );
4009 :
4010 : /* signal is divided by 16 to avoid overflow in energy computation */
4011 0 : *signal++ = round_fx( L_tmp );
4012 0 : move16();
4013 :
4014 0 : return;
4015 : }
4016 :
4017 59556 : void hp400_12k8_ivas_fx(
4018 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
4019 : const Word16 lg, /* i : lenght of signal */
4020 : Word16 mem[] /* i/o: filter memory [6] */
4021 : )
4022 : {
4023 : Word16 i;
4024 : Word16 x0, x1, x2;
4025 : Word32 L_tmp, yy1, y2;
4026 :
4027 59556 : yy1 = L_Comp( mem[2], mem[3] ); /* Q_syn + 13 */
4028 59556 : y2 = L_Comp( mem[0], mem[1] ); /* Q_syn + 13 */
4029 59556 : x0 = mem[4]; /* Q_syn */
4030 59556 : move16();
4031 59556 : x1 = mem[5]; /* Q_syn */
4032 59556 : move16();
4033 :
4034 3871140 : FOR( i = 0; i < lg; i++ )
4035 : {
4036 3811584 : x2 = x1; /* Q_syn */
4037 3811584 : move16();
4038 3811584 : x1 = x0; /* Q_syn */
4039 3811584 : move16();
4040 3811584 : x0 = signal[i]; /* Q_syn */
4041 3811584 : move16();
4042 :
4043 3811584 : L_tmp = Mpy_32_16_1( yy1, a_hp400_ivas_fx[1] ); /*yy1 * a_hp400[1]*/ /* Qx(Q_of_yy1) + 10 ---->( (Q_syn+13) + 12 - 15)*/
4044 3811584 : L_tmp = Madd_32_16( L_tmp, y2, a_hp400_ivas_fx[2] ); /*y2 * a_hp400[2]*/ /* Qx + 10 ---->( (Q_syn+13) + 12 - 15)*/
4045 3811584 : L_tmp = L_shl( L_tmp, 3 ); /* shifting by 3 to maintain same Q (Q_syn+13) */
4046 :
4047 3811584 : L_tmp = L_mac( L_tmp, x0, b_hp400_fx[0] ); /* Q_syn + 13 */
4048 3811584 : L_tmp = L_mac( L_tmp, x1, b_hp400_fx[1] ); /* Q_syn + 13 */
4049 3811584 : L_tmp = L_mac( L_tmp, x2, b_hp400_fx[2] ); /* Q_syn + 13 */
4050 :
4051 3811584 : y2 = yy1; /* Q_syn + 13 */
4052 3811584 : move32();
4053 3811584 : yy1 = L_tmp; /* Q_syn + 13 */
4054 3811584 : move32();
4055 :
4056 3811584 : signal[i] = round_fx( L_tmp ); /* Q_syn - 3 */
4057 3811584 : move16();
4058 : }
4059 :
4060 59556 : L_Extract( yy1, &mem[2], &mem[3] );
4061 59556 : L_Extract( y2, &mem[0], &mem[1] );
4062 59556 : mem[4] = x0; /* Q_syn */
4063 59556 : mem[5] = x1; /* Q_syn */
4064 59556 : move16();
4065 59556 : move16();
4066 59556 : return;
4067 : }
4068 :
4069 0 : Word16 dot_prod_satcontr( const Word16 *x, const Word16 *y, Word16 qx, Word16 qy, Word16 *qo, Word16 len )
4070 : {
4071 : Word16 tmp_tab_x[L_FRAME16k];
4072 : Word16 tmp_tab_y[L_FRAME16k];
4073 : Word16 shift, q, ener, i;
4074 : Word32 L_tmp;
4075 : Word16 *pt1, *pt2;
4076 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
4077 0 : Flag Overflow = 0;
4078 0 : move32();
4079 : #endif
4080 :
4081 :
4082 0 : Copy( x, tmp_tab_x, len ); /* OPTIMIZE !!!!! the copy into local table is not necessary */
4083 0 : Copy( y, tmp_tab_y, len ); /* could be reworked to do a 1st iteration with the original x[] and y[] */
4084 : /* then check if there is an overflow and do a more complex 2nd, 3rd, ... processing */
4085 0 : shift = 0;
4086 0 : move16();
4087 : BASOP_SATURATE_WARNING_OFF_EVS
4088 : DO
4089 : {
4090 0 : Overflow = 0;
4091 0 : move16();
4092 0 : L_tmp = L_shl_o( 1, s_max( sub( add( add( qx, qy ), 7 ), shift ), 0 ), &Overflow );
4093 0 : pt1 = tmp_tab_x;
4094 0 : pt2 = tmp_tab_y;
4095 0 : FOR( i = 0; i < len; i++ )
4096 : {
4097 0 : L_tmp = L_mac0_o( L_tmp, *pt1++, *pt2++, &Overflow ); /*Q(qx+qy-shift) */
4098 : }
4099 :
4100 0 : IF( Overflow != 0 )
4101 : {
4102 0 : Scale_sig( tmp_tab_x, len, -2 );
4103 0 : Scale_sig( tmp_tab_y, len, -2 );
4104 0 : shift = add( shift, 4 );
4105 : }
4106 : }
4107 0 : WHILE( Overflow != 0 );
4108 : BASOP_SATURATE_WARNING_ON_EVS
4109 :
4110 0 : q = norm_l( L_tmp );
4111 0 : L_tmp = L_shl( L_tmp, q ); /*Q(qx+qy-shift+q) */
4112 0 : ener = extract_h( L_tmp ); /*Q(qx+qy-shift+q-16) */
4113 0 : q = add( q, add( qx, qy ) );
4114 0 : *qo = sub( q, add( shift, 16 ) );
4115 0 : move16();
4116 0 : return ener;
4117 : }
4118 :
4119 :
4120 : /*
4121 : * E_UTIL_f_convolve
4122 : *
4123 : * Parameters:
4124 : * x I: input vector <14bits
4125 : * h I: impulse response (or second input vector) (1Q14)
4126 : * y O: output vetor (result of convolution)
4127 : *
4128 : * Function:
4129 : * Perform the convolution between two vectors x[] and h[] and
4130 : * write the result in the vector y[]. All vectors are of length L.
4131 : * Only the first L samples of the convolution are considered.
4132 : * Vector size = L_SUBFR
4133 : *
4134 : * Returns:
4135 : * void
4136 : */
4137 0 : void E_UTIL_f_convolve( const Word16 x[], const Word16 h[], Word16 y[], const Word16 size )
4138 : {
4139 : Word16 i, n;
4140 : Word32 L_sum;
4141 : Word64 L64_sum;
4142 :
4143 0 : FOR( n = 0; n < size; n++ )
4144 : {
4145 0 : L64_sum = 0;
4146 0 : move64();
4147 0 : FOR( i = 0; i < n; i++ )
4148 : {
4149 0 : L64_sum = W_mac_16_16( L64_sum, x[i], h[n - i] );
4150 : }
4151 0 : L_sum = W_sat_l( L64_sum );
4152 0 : y[n] = mac_r( L_sum, x[i], h[0] );
4153 0 : move16();
4154 : }
4155 0 : return;
4156 : }
4157 :
4158 : /*-----------------------------------------------------------------------------
4159 : * floating_point_add:
4160 : *
4161 : * Add two floating point numbers: x <- x + y.
4162 : *----------------------------------------------------------------------------*/
4163 21121 : void floating_point_add(
4164 : Word32 *mx, /* io: mantissa of the addend Q31 */
4165 : Word16 *ex, /* io: exponent of the addend Q0 */
4166 : const Word32 my, /* i: mantissa of the adder Q31 */
4167 : const Word16 ey /* i: exponent of the adder Q0 */
4168 : )
4169 : {
4170 : Word32 accX, accY;
4171 : Word16 align, expo;
4172 : /* NB: This function will not work properly if the mantissa is zero and the exponent is not 32.
4173 : It is up to the caller function to avoid this condition. */
4174 : /* Ensure 1 bit headroom before addition. */
4175 21121 : accX = L_shr( *mx, 1 );
4176 21121 : accY = L_shr( my, 1 );
4177 : /* First, align the Q-points of the two operands. Then, add. */
4178 21121 : align = sub( *ex, ey );
4179 :
4180 21121 : IF( align < 0 )
4181 : {
4182 16697 : accX = L_add( accX, L_shl( accY, align ) );
4183 : }
4184 : ELSE
4185 : {
4186 4424 : accX = L_add( accY, L_shr( accX, align ) );
4187 4424 : *ex = ey;
4188 4424 : move16();
4189 : }
4190 : /* Normalize the result and update the mantissa and exponent. */
4191 21121 : expo = norm_l( accX );
4192 21121 : *mx = L_shl( accX, expo );
4193 21121 : *ex = sub( add( *ex, expo ), 1 ); /* Subtract 1 due to 1-bit down-shift above ensuring 1 bit headroom before addition. */
4194 21121 : move32();
4195 21121 : move16();
4196 21121 : return;
4197 : }
4198 :
4199 : /*-------------------------------------------------------------------*
4200 : * delay_signal_fx()
4201 : *
4202 : * Delay buffer by defined number of samples
4203 : *-------------------------------------------------------------------*/
4204 :
4205 940034 : void delay_signal_fx(
4206 : Word16 x[], /* i/o: signal to be delayed */
4207 : const Word16 len, /* i : length of the input signal */
4208 : Word16 mem[], /* i/o: synchronization memory */
4209 : const Word16 delay /* i : delay in samples */
4210 : )
4211 : {
4212 : Word16 tmp_buffer[L_FRAME48k];
4213 :
4214 940034 : Copy( mem, tmp_buffer, delay );
4215 940034 : Copy( x + sub( len, delay ), mem, delay );
4216 940034 : Copy( x, x + delay, sub( len, delay ) );
4217 940034 : Copy( tmp_buffer, x, delay );
4218 :
4219 940034 : return;
4220 : }
4221 :
4222 2454926 : void delay_signal32_fx(
4223 : Word32 x[], /* i/o: signal to be delayed */
4224 : const Word16 len, /* i : length of the input signal */
4225 : Word32 mem[], /* i/o: synchronization memory */
4226 : const Word16 delay /* i : delay in samples */
4227 : )
4228 : {
4229 : Word32 tmp_buffer[L_FRAME48k];
4230 :
4231 2454926 : Copy32( mem, tmp_buffer, delay );
4232 2454926 : Copy32( x + sub( len, delay ), mem, delay );
4233 2454926 : Copy32( x, x + delay, sub( len, delay ) );
4234 2454926 : Copy32( tmp_buffer, x, delay );
4235 :
4236 2454926 : return;
4237 : }
4238 :
4239 54184 : void delay_signal_q_adj_fx(
4240 : Word32 x[], /* i/o: signal to be delayed */
4241 : const Word16 len, /* i : length of the input signal */
4242 : Word32 mem[], /* i/o: synchronization memory */
4243 : const Word16 delay, /* i : delay in samples */
4244 : const Word16 q_x,
4245 : const Word16 q_mem )
4246 : {
4247 :
4248 : Word32 tmp_buffer[L_FRAME48k];
4249 :
4250 54184 : Copy32( mem, tmp_buffer, delay );
4251 54184 : Copy32( x + sub( len, delay ), mem, delay );
4252 54184 : Copy32( x, x + delay, sub( len, delay ) );
4253 :
4254 :
4255 54184 : IF( EQ_16( q_x, q_mem ) )
4256 : {
4257 46208 : Copy32( tmp_buffer, x, delay );
4258 : }
4259 : ELSE
4260 : {
4261 7976 : v_shr( tmp_buffer, sub( q_mem, q_x ), x, delay );
4262 : }
4263 :
4264 54184 : return;
4265 : }
4266 :
4267 24118300 : void v_add_fx(
4268 : const Word32 x1[], /* i : Input vector 1 */
4269 : const Word32 x2[], /* i : Input vector 2 */
4270 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
4271 : const Word16 N /* i : Vector length */
4272 : )
4273 : {
4274 : Word16 i;
4275 :
4276 2800023470 : FOR( i = 0; i < N; i++ )
4277 : {
4278 2775905170 : y[i] = L_add_sat( x1[i], x2[i] );
4279 2775905170 : move32();
4280 : }
4281 :
4282 24118300 : return;
4283 : }
4284 915 : Word16 floor_log_2( Word32 num )
4285 : {
4286 :
4287 915 : IF( num == 0 )
4288 : {
4289 0 : return 0;
4290 : }
4291 :
4292 915 : return ( sub( 30, norm_l( num ) ) );
4293 : }
4294 :
4295 3577750 : void v_shr(
4296 : const Word32 x[], /* i : Input vector */
4297 : const Word16 shift, /* i : Constant */
4298 : Word32 y[], /* o : Output vector that contains x >> shift */
4299 : const Word16 N /* i : Vector length */
4300 : )
4301 : {
4302 : Word16 i;
4303 :
4304 636115997 : FOR( i = 0; i < N; i++ )
4305 : {
4306 632538247 : y[i] = L_shr( x[i], shift );
4307 632538247 : move32();
4308 : }
4309 :
4310 3577750 : return;
4311 : }
4312 :
4313 0 : void v_shr_16(
4314 : const Word16 x[], /* i : Input vector */
4315 : const Word16 shift, /* i : Constant */
4316 : Word16 y[], /* o : Output vector that contains x >> shift */
4317 : const Word16 N /* i : Vector length */
4318 : )
4319 : {
4320 : Word16 i;
4321 :
4322 0 : FOR( i = 0; i < N; i++ )
4323 : {
4324 0 : y[i] = shr( x[i], shift );
4325 0 : move16();
4326 : }
4327 :
4328 0 : return;
4329 : }
4330 :
4331 : /*---------------------------------------------------------------------*
4332 : * lin_interp_fx()
4333 : *
4334 : * Linearly maps x from source range <x1, x2> to the target range <y1, y2>
4335 : *---------------------------------------------------------------------*/
4336 :
4337 : /*! r: mapped output value */
4338 728 : Word16 lin_interp_fx(
4339 : const Word16 x, /* i : Q15 the value to be mapped */
4340 : const Word16 x1, /* i : Q15 source range interval: low end */
4341 : const Word16 y1, /* i : Q15 source range interval: high end */
4342 : const Word16 x2, /* i : Q15 target range interval: low */
4343 : const Word16 y2, /* i : Q15 target range interval: high */
4344 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
4345 : )
4346 : {
4347 728 : IF( sub( x2, x1 ) == 0 )
4348 : {
4349 0 : return y1;
4350 : }
4351 728 : ELSE IF( flag_sat )
4352 : {
4353 728 : IF( GE_16( x, s_max( x1, x2 ) ) )
4354 : {
4355 0 : IF( GT_16( x1, x2 ) )
4356 : {
4357 0 : return y1;
4358 : }
4359 : ELSE
4360 : {
4361 0 : return y2;
4362 : }
4363 : }
4364 728 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
4365 : {
4366 16 : IF( LT_16( x1, x2 ) )
4367 : {
4368 16 : return y1;
4369 : }
4370 : ELSE
4371 : {
4372 0 : return y2;
4373 : }
4374 : }
4375 : }
4376 :
4377 712 : return add_sat( y1, mult( sub( x, x1 ), div_s( sub( y2, y1 ), sub( x2, x1 ) ) ) );
4378 : }
4379 :
4380 :
4381 0 : Word16 lin_interp_ivas_fx(
4382 : const Word16 x, /* i : Q15 the value to be mapped */
4383 : const Word16 x1, /* i : Q15 source range interval: low end */
4384 : const Word16 y1, /* i : Q15 source range interval: high end */
4385 : const Word16 x2, /* i : Q15 target range interval: low */
4386 : const Word16 y2, /* i : Q15 target range interval: high */
4387 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
4388 : )
4389 : {
4390 0 : IF( EQ_16( sub( x2, x1 ), 0 ) )
4391 : {
4392 0 : return y1;
4393 : }
4394 0 : ELSE IF( flag_sat )
4395 : {
4396 0 : IF( GE_16( x, s_max( x1, x2 ) ) )
4397 : {
4398 0 : return GT_16( x1, x2 ) ? y1 : y2;
4399 : }
4400 0 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
4401 : {
4402 0 : return LT_16( x1, x2 ) ? y1 : y2;
4403 : }
4404 : }
4405 : Word16 div_res_e;
4406 0 : Word16 div_res = BASOP_Util_Divide1616_Scale( sub( y2, y1 ), sub( x2, x1 ), &div_res_e );
4407 : // div_res = shl( div_res, div_res_e );
4408 0 : return add_sat( y1, round_fx( L_shl( L_mult( sub( x, x1 ), div_res ), div_res_e ) ) );
4409 : }
4410 :
4411 : /*---------------------------------------------------------------------
4412 : * sign_l()
4413 : *
4414 : *---------------------------------------------------------------------*/
4415 :
4416 : /*! r: sign of x (+1/-1) */
4417 56280 : Word32 sign_l(
4418 : const Word32 x /* i : input value of x */
4419 : )
4420 : {
4421 56280 : IF( x < 0 )
4422 : {
4423 26038 : return MIN_32;
4424 : }
4425 : ELSE
4426 : {
4427 30242 : return MAX_32;
4428 : }
4429 : }
4430 :
4431 143 : void v_mult16_fixed(
4432 : const Word16 x1[], /* i : Input vector 1 */
4433 : const Word16 x2[], /* i : Input vector 2 */
4434 : Word16 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4435 : const Word16 N /* i : Vector length */
4436 : )
4437 : {
4438 : Word16 i;
4439 :
4440 10413 : FOR( i = 0; i < N; i++ )
4441 : {
4442 10270 : y[i] = mult_r( x1[i], x2[i] );
4443 10270 : move16();
4444 : }
4445 :
4446 143 : return;
4447 : }
4448 :
4449 : /*---------------------------------------------------------------------*
4450 : * set_zero_fx()
4451 : *
4452 : * Set a vector vec[] of dimension lvec to zero
4453 : *---------------------------------------------------------------------*/
4454 :
4455 142561364 : void set_zero_fx(
4456 : Word32 *vec, /* o : input vector */
4457 : const Word16 lvec /* i : length of the vector */
4458 : )
4459 : {
4460 : Word16 i;
4461 :
4462 17361839134 : FOR( i = 0; i < lvec; i++ )
4463 : {
4464 17219277770 : *vec++ = 0;
4465 17219277770 : move32();
4466 : }
4467 :
4468 142561364 : return;
4469 : }
4470 9059 : void set_zero2_fx(
4471 : Word32 *vec, /* o : input vector */
4472 : const Word32 lvec /* i : length of the vector */
4473 : )
4474 : {
4475 : Word32 i;
4476 :
4477 34153899 : FOR( i = 0; i < lvec; i++ )
4478 : {
4479 34144840 : *vec++ = 0;
4480 34144840 : move32();
4481 : }
4482 :
4483 9059 : return;
4484 : }
4485 :
4486 6193 : void set16_zero_fx(
4487 : Word16 *vec, /* o : input vector */
4488 : const Word16 lvec /* i : length of the vector */
4489 : )
4490 : {
4491 : Word16 i;
4492 :
4493 676153 : FOR( i = 0; i < lvec; i++ )
4494 : {
4495 669960 : *vec++ = 0;
4496 669960 : move16();
4497 : }
4498 :
4499 6193 : return;
4500 : }
4501 :
4502 1532725 : UWord32 mvl2s_r(
4503 : const Word32 x[], /* i : input vector */
4504 : const Word16 q_x,
4505 : Word16 y[], /* o : output vector */
4506 : const Word16 n /* i : vector size */
4507 : )
4508 : {
4509 : Word16 i;
4510 : Word32 temp;
4511 1532725 : UWord32 noClipping = 0;
4512 1532725 : move32();
4513 :
4514 1532725 : IF( n <= 0 )
4515 : {
4516 : /* cannot transfer vectors with size 0 */
4517 174 : return 0;
4518 : }
4519 :
4520 1532551 : IF( (void *) y <= (const void *) x )
4521 : {
4522 0 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
4523 0 : FOR( i = 0; i < n; i++ )
4524 : {
4525 0 : temp = L_add( x[i], tempd );
4526 0 : temp = L_shr( temp, q_x );
4527 :
4528 0 : IF( GT_32( temp, MAX16B ) )
4529 : {
4530 0 : temp = MAX16B;
4531 0 : move32();
4532 0 : noClipping = L_add( (Word32) noClipping, 1 );
4533 : }
4534 0 : ELSE IF( LT_32( temp, MIN16B ) )
4535 : {
4536 0 : temp = MIN16B;
4537 0 : move32();
4538 0 : noClipping = L_add( (Word32) noClipping, 1 );
4539 : }
4540 :
4541 0 : y[i] = extract_l( temp );
4542 0 : move16();
4543 : }
4544 : }
4545 : ELSE
4546 : {
4547 1532551 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
4548 1227499631 : FOR( i = n - 1; i >= 0; i-- )
4549 : {
4550 1225967080 : temp = L_add( x[i], tempd );
4551 1225967080 : temp = L_shr( temp, q_x );
4552 :
4553 1225967080 : IF( GT_32( temp, MAX16B ) )
4554 : {
4555 270 : temp = MAX16B;
4556 270 : move32();
4557 270 : noClipping = L_add( (Word32) noClipping, 1 );
4558 : }
4559 1225966810 : ELSE IF( LT_32( temp, MIN16B ) )
4560 : {
4561 344 : temp = MIN16B;
4562 344 : move32();
4563 344 : noClipping = L_add( (Word32) noClipping, 1 );
4564 : }
4565 :
4566 1225967080 : y[i] = extract_l( temp );
4567 1225967080 : move16();
4568 : }
4569 : }
4570 :
4571 1532551 : return noClipping;
4572 : }
4573 :
4574 0 : Word32 dotp_me_fx(
4575 : const Word32 x[], /* i : vector x[] */
4576 : const Word32 y[], /* i : vector y[] */
4577 : const Word16 n, /* i : vector length */
4578 : Word16 exp_x,
4579 : Word16 exp_y,
4580 : Word16 *exp_suma )
4581 : {
4582 : Word16 i;
4583 : Word32 suma;
4584 : Word32 mul;
4585 0 : Word16 mul_exp = add( exp_x, exp_y );
4586 0 : suma = Mpy_32_32( x[0], y[0] );
4587 0 : *exp_suma = mul_exp;
4588 0 : FOR( i = 1; i < n; i++ )
4589 : {
4590 0 : mul = Mpy_32_32( x[i], y[i] );
4591 0 : suma = BASOP_Util_Add_Mant32Exp( suma, *exp_suma, mul, mul_exp, exp_suma ); // exp_x+exp_A
4592 : }
4593 :
4594 0 : return suma;
4595 : }
4596 :
4597 0 : Word32 dotp_fixed_guarded(
4598 : const Word32 x[], /* i : vector x[] */
4599 : const Word32 y[], /* i : vector y[] */
4600 : const Word16 n /* i : vector length */
4601 : )
4602 : {
4603 : Word16 i;
4604 : Word32 suma;
4605 0 : Word16 guarded_bits = find_guarded_bits_fx( n );
4606 0 : suma = L_shr( Mpy_32_32( x[0], y[0] ), guarded_bits );
4607 :
4608 0 : FOR( i = 1; i < n; i++ )
4609 : {
4610 0 : suma = L_add( suma, L_shr( Mpy_32_32( x[i], y[i] ), guarded_bits ) );
4611 : }
4612 :
4613 0 : return suma;
4614 : }
4615 :
4616 :
4617 0 : Word32 dotp_fixed_ivas_fx(
4618 : const Word32 x[], /* i : vector x[] */
4619 : Word16 x_e,
4620 : const Word32 y[], /* i : vector y[] */
4621 : Word16 y_e,
4622 : const Word16 n, /* i : vector length */
4623 : Word16 *out_e )
4624 : {
4625 : Word16 i, exp;
4626 0 : Word32 suma = 0;
4627 :
4628 0 : exp = 31;
4629 0 : move16();
4630 :
4631 0 : FOR( i = 0; i < n; i++ )
4632 : {
4633 0 : suma = BASOP_Util_Add_Mant32Exp( suma, exp, Mpy_32_32( x[i], y[i] ), x_e + y_e, &exp );
4634 : }
4635 :
4636 0 : *out_e = exp;
4637 0 : move16();
4638 0 : return suma;
4639 : }
4640 :
4641 : /*-------------------------------------------------------------------*
4642 : * v_mult()
4643 : *
4644 : * Multiplication of two vectors
4645 : *-------------------------------------------------------------------*/
4646 :
4647 :
4648 14162715 : void v_mult_fixed(
4649 : const Word32 x1[], /* i : Input vector 1 */
4650 : const Word32 x2[], /* i : Input vector 2 */
4651 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4652 : const Word16 N /* i : Vector length */
4653 : )
4654 : {
4655 : Word16 i;
4656 :
4657 554894968 : FOR( i = 0; i < N; i++ )
4658 : {
4659 540732253 : y[i] = Mpy_32_32( x1[i], x2[i] );
4660 540732253 : move32();
4661 : }
4662 :
4663 14162715 : return;
4664 : }
4665 :
4666 : /*-------------------------------------------------------------------*
4667 : * anint_fixed()
4668 : *
4669 : * Round to the nearest integer.
4670 : *-------------------------------------------------------------------*/
4671 9400 : Word32 anint_fixed( Word32 x, Word16 exp )
4672 : {
4673 9400 : IF( x == 0 )
4674 : {
4675 856 : return 0;
4676 : }
4677 8544 : IF( x >= 0 )
4678 : {
4679 8544 : return L_add( x, L_shl( 1, sub( exp, 1 ) ) );
4680 : }
4681 : ELSE
4682 : {
4683 0 : return L_sub( x, L_shl( 1, sub( exp, 1 ) ) );
4684 : }
4685 : }
4686 :
4687 : /*-------------------------------------------------------------------*
4688 : * ceil_fixed()
4689 : *
4690 : * Ceil to the next multiple of (1 << exp).
4691 : *-------------------------------------------------------------------*/
4692 25503 : Word32 ceil_fixed( Word32 x, Word16 exp )
4693 : {
4694 : Word32 step;
4695 : // step = x / L_shl( 1, exp );
4696 25503 : step = L_shr( x, exp );
4697 25503 : IF( ( x % L_shl( 1, exp ) ) > 0 )
4698 : {
4699 15376 : step = L_add( step, 1 );
4700 : }
4701 25503 : return L_shl( step, exp );
4702 : }
4703 :
4704 783 : void sort_l(
4705 : Word32 *x, /* i/o: Vector to be sorted */
4706 : Word16 len /* i/o: vector length */
4707 : )
4708 : {
4709 : Word16 i, j;
4710 : Word32 tempr;
4711 :
4712 7133 : FOR( i = len - 2; i >= 0; i-- )
4713 : {
4714 6350 : tempr = x[i];
4715 6350 : move32();
4716 23195 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
4717 : {
4718 16845 : x[j - 1] = x[j];
4719 16845 : move32();
4720 : }
4721 6350 : x[j - 1] = tempr;
4722 6350 : move32();
4723 : }
4724 :
4725 783 : return;
4726 : }
4727 :
4728 : /*-------------------------------------------------------------------*
4729 : * v_add_fixed()
4730 : *
4731 : * Subtraction of two vectors sample by sample
4732 : *-------------------------------------------------------------------*/
4733 :
4734 4623239 : void v_add_fixed(
4735 : const Word32 x1[], /* i : Input vector 1 */
4736 : const Word32 x2[], /* i : Input vector 2 */
4737 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
4738 : const Word16 N, /* i : Vector length */
4739 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
4740 : )
4741 : {
4742 : Word16 i;
4743 :
4744 146524009 : FOR( i = 0; i < N; i++ )
4745 : {
4746 141900770 : y[i] = L_add( L_shr( x1[i], hdrm ), L_shr( x2[i], hdrm ) );
4747 141900770 : move32();
4748 : }
4749 :
4750 4623239 : return;
4751 : }
4752 :
4753 471874554 : void v_add_fixed_no_hdrm(
4754 : const Word32 x1[], /* i : Input vector 1 */
4755 : const Word32 x2[], /* i : Input vector 2 */
4756 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
4757 : const Word16 N /* i : Vector length */
4758 : )
4759 : {
4760 : Word16 i;
4761 :
4762 2587716980 : FOR( i = 0; i < N; i++ )
4763 : {
4764 2115842426 : y[i] = L_add( x1[i], x2[i] );
4765 2115842426 : move32();
4766 : }
4767 :
4768 471874554 : return;
4769 : }
4770 :
4771 8862603 : void v_add_fixed_me(
4772 : const Word32 x1[], /* i : Input vector 1 */
4773 : const Word16 x1_e, /* i : Exponent for input vector 1 */
4774 : const Word32 x2[], /* i : Input vector 2 */
4775 : const Word16 x2_e, /* i : Exponent for input vector 2 */
4776 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
4777 : Word16 *y_e, /* i : Exponent for output vector */
4778 : const Word16 N, /* i : Vector length */
4779 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
4780 : )
4781 : {
4782 : Word16 i;
4783 8862603 : Word16 x1_shift = sub( s_max( x1_e, x2_e ), x1_e );
4784 8862603 : Word16 x2_shift = sub( s_max( x1_e, x2_e ), x2_e );
4785 :
4786 138828619 : FOR( i = 0; i < N; i++ )
4787 : {
4788 129966016 : y[i] = L_add( L_shr( x1[i], hdrm + x1_shift ), L_shr( x2[i], hdrm + x2_shift ) );
4789 129966016 : move32();
4790 : }
4791 :
4792 8862603 : *y_e = add( s_max( x1_e, x2_e ), hdrm );
4793 8862603 : move16();
4794 :
4795 8862603 : return;
4796 : }
4797 :
4798 39522955 : Word16 find_guarded_bits_fx( Word32 n )
4799 : {
4800 : // return n <= 1 ? 0 : n <= 2 ? 1
4801 : // : n <= 4 ? 2
4802 : // : n <= 8 ? 3
4803 : // : n <= 16 ? 4
4804 : // : n <= 32 ? 5
4805 : // : n <= 64 ? 6
4806 : // : n <= 128 ? 7
4807 : // : n <= 256 ? 8
4808 : // : n <= 512 ? 9
4809 : // : n <= 1024 ? 10
4810 : // : n <= 2048 ? 11
4811 : // : n <= 4096 ? 12
4812 : // : n <= 8192 ? 13
4813 : // : n <= 16384 ? 14
4814 : // : 15;
4815 : /*Word16 val = 0;
4816 : move32();
4817 : test();
4818 : WHILE( GT_32( n, L_shl( 1, val ) ) && LT_32( val, 16 ) )
4819 : {
4820 : val = add( val, 1 );
4821 : }*/
4822 39522955 : IF( LE_32( n, 1 ) )
4823 : {
4824 219910 : return 0;
4825 : }
4826 : ELSE
4827 : {
4828 :
4829 39303045 : return sub( 31, norm_l( L_sub( n, 1 ) ) );
4830 : }
4831 : }
4832 :
4833 577507370 : Word16 L_norm_arr( const Word32 *arr, Word16 size )
4834 : {
4835 577507370 : Word16 q = 31;
4836 577507370 : move16();
4837 9503461315 : FOR( Word16 i = 0; i < size; i++ )
4838 : {
4839 : Word16 q_tst;
4840 :
4841 8925953945 : q_tst = norm_l( arr[i] );
4842 8925953945 : if ( arr[i] != 0 )
4843 : {
4844 6128934228 : q = s_min( q, q_tst );
4845 : }
4846 : }
4847 :
4848 577507370 : return q;
4849 : }
4850 :
4851 8412303 : Word16 norm_arr( Word16 *arr, Word16 size )
4852 : {
4853 8412303 : Word16 q = 15;
4854 8412303 : Word16 exp = 0;
4855 8412303 : move16();
4856 8412303 : move16();
4857 3702108983 : FOR( Word16 i = 0; i < size; i++ )
4858 : {
4859 3693696680 : if ( arr[i] != 0 )
4860 : {
4861 2564459191 : exp = norm_s( arr[i] );
4862 : }
4863 3693696680 : if ( arr[i] != 0 )
4864 : {
4865 2564459191 : q = s_min( q, exp );
4866 : }
4867 : }
4868 8412303 : return q;
4869 : }
4870 :
4871 635836 : Word16 W_norm_arr( Word64 *arr, Word16 size )
4872 : {
4873 635836 : Word16 q = 63;
4874 635836 : Word16 exp = 0;
4875 635836 : move16();
4876 635836 : move16();
4877 343264828 : FOR( Word16 i = 0; i < size; i++ )
4878 : {
4879 342628992 : if ( arr[i] != 0 )
4880 : {
4881 315946415 : exp = W_norm( arr[i] );
4882 : }
4883 342628992 : if ( arr[i] != 0 )
4884 : {
4885 315946415 : q = s_min( q, exp );
4886 : }
4887 : }
4888 635836 : return q;
4889 : }
4890 :
4891 271967070 : Word16 get_min_scalefactor( Word32 x, Word32 y )
4892 : {
4893 : Word16 scf_y;
4894 271967070 : Word16 scf = Q31;
4895 271967070 : move16();
4896 :
4897 271967070 : test();
4898 271967070 : if ( x == 0 && y == 0 )
4899 : {
4900 20102724 : scf = 0;
4901 20102724 : move16();
4902 : }
4903 :
4904 271967070 : if ( x != 0 )
4905 : {
4906 251808878 : scf = norm_l( x );
4907 : }
4908 :
4909 271967070 : scf_y = norm_l( y );
4910 271967070 : if ( y != 0 )
4911 : {
4912 194160583 : scf = s_min( scf_y, scf );
4913 : }
4914 :
4915 271967070 : return scf;
4916 : }
4917 :
4918 :
4919 358716951 : Flag is_zero_arr( Word32 *arr, Word16 size )
4920 : {
4921 649752955 : FOR( Word16 i = 0; i < size; i++ )
4922 586664243 : IF( arr[i] != 0 )
4923 : {
4924 295628239 : return 0;
4925 : }
4926 :
4927 63088712 : return 1;
4928 : }
4929 :
4930 767111 : Flag is_zero_arr16( Word16 *arr, Word16 size )
4931 : {
4932 224751917 : FOR( Word16 i = 0; i < size; i++ )
4933 224594167 : IF( arr[i] != 0 )
4934 : {
4935 609361 : return 0;
4936 : }
4937 :
4938 157750 : return 1;
4939 : }
4940 :
4941 0 : Flag is_zero_arr64( Word64 *arr, Word16 size )
4942 : {
4943 0 : FOR( Word16 i = 0; i < size; i++ )
4944 : {
4945 0 : IF( arr[i] != 0 )
4946 : {
4947 0 : return 0;
4948 : }
4949 : }
4950 0 : return 1;
4951 : }
4952 :
4953 0 : void Scale_sig64(
4954 : Word64 x[], /* i/o: signal to scale Qx */
4955 : Word16 len, /* i : size of x[] Q0 */
4956 : Word16 exp /* i : exponent: x = round(x << exp) Qx exp */
4957 : )
4958 : {
4959 : Word16 i;
4960 0 : assert( exp <= 63 && exp >= -63 );
4961 0 : IF( exp == 0 )
4962 : {
4963 0 : return;
4964 : }
4965 :
4966 0 : FOR( i = 0; i < len; i++ )
4967 : {
4968 : /* saturation can occur here */
4969 0 : x[i] = W_shl( x[i], exp );
4970 0 : move64();
4971 : }
4972 : }
|
