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