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 3030967 : Word16 norm_ul( UWord32 UL_var1 )
245 : {
246 : Word16 var_out;
247 :
248 3030967 : if ( UL_var1 == 0 )
249 : {
250 14 : var_out = 0;
251 : }
252 : else
253 : {
254 35465128 : for ( var_out = 0; UL_var1 < (UWord32) 0x80000000U; var_out++ )
255 : {
256 32434175 : UL_var1 <<= 1;
257 : }
258 : }
259 : BASOP_CHECK();
260 :
261 3030967 : 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 1246396 : 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 1246396 : Flag Overflow = 0;
333 1246396 : move32();
334 : #endif
335 :
336 : /* idx = (short)( (x - qlow)/delta + 0.5f); */
337 1246396 : exp = norm_s( delta );
338 1246396 : tmp = div_s( shl( 1, sub( 14, exp ) ), delta ); /*Q(29-exp-(Qx-1))->Q(30-exp-Qx) */
339 1246396 : L_tmp = L_mult( sub_o( x, qlow, &Overflow ), tmp ); /*Q(31-exp) */
340 1246396 : idx = extract_l( L_shr_r( L_add( L_tmp, shl_o( 1, sub( 30, exp ), &Overflow ) ), sub( 31, exp ) ) ); /*Q0 */
341 :
342 1246396 : idx = s_min( idx, sub( cbsize, 1 ) );
343 1246396 : idx = s_max( idx, 0 );
344 :
345 : /* *xq = idx*delta + qlow; */
346 1246396 : L_tmp = L_deposit_l( qlow ); /*Qx */
347 1246396 : L_tmp = L_mac( L_tmp, idx, delta ); /*Qx */
348 1246396 : *xq = round_fx_o( L_shl_o( L_tmp, 16, &Overflow ), &Overflow ); /*Qx */
349 1246396 : 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 50552658 : 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 50552658 : L64_sum = 1;
368 50552658 : move64();
369 3111835881 : FOR( i = 0; i < lg; i++ )
370 : {
371 3061283223 : L64_sum = W_mac_16_16( L64_sum, x[i], y[i] );
372 : }
373 50552658 : L_sum = W_sat_l( L64_sum );
374 50552658 : return L_sum;
375 : }
376 : /*---------------------------------------------------------------------*
377 : * dotp_fx()
378 : *
379 : * Dot product of vector x[] and vector y[]
380 : *---------------------------------------------------------------------*/
381 :
382 1367859 : 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 1367859 : assert( *exp == 0 );
393 :
394 1367859 : L_sum = L_add( L_shr( Dot_product( x, y, n ), 1 ), 1 );
395 :
396 : /* Normalize acc in Q31 */
397 :
398 1367859 : sft = norm_l( L_sum );
399 1367859 : L_sum = L_shl( L_sum, sft );
400 :
401 1367859 : *exp = sub( 30, sft );
402 1367859 : move16(); /* exponent = 0..30 */
403 :
404 1367859 : return L_sum;
405 : }
406 :
407 391573 : 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 391573 : Flag Overflow = 0;
416 391573 : move32();
417 : #endif
418 391573 : L_tmp = L_deposit_l( 0 );
419 30362693 : FOR( i = 0; i < lvec; i++ )
420 : {
421 29971120 : L_tmp = L_mac_o( L_tmp, vec[i], vec[i], &Overflow ); /*Q(2x+1) */
422 : }
423 :
424 391573 : return L_tmp;
425 : }
426 :
427 131343 : 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 131343 : sum = 0;
436 131343 : move64();
437 56171023 : FOR( i = 0; i < lvec; i++ )
438 : {
439 56039680 : sum = W_mac0_16_16( sum, vec[i], vec[i] ); // 2*Qx
440 : }
441 :
442 131343 : 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 172392183 : 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 172392183 : IF( y < x )
503 : {
504 17055596761 : FOR( i = 0; i < L; i++ )
505 : {
506 16967754226 : y[i] = x[i];
507 16967754226 : 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 87842535 : return;
512 : }
513 :
514 16600437778 : FOR( i = L - 1; i >= 0; i-- )
515 : {
516 16515888130 : y[i] = x[i];
517 16515888130 : move16();
518 : }
519 :
520 84549648 : 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 21837279 : 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 498495843 : FOR( i = 0; i < N; i++ )
563 : {
564 476658564 : y[i] = a;
565 476658564 : move64();
566 : }
567 :
568 21837279 : return;
569 : }
570 :
571 137878 : 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 137878 : IF( y < x )
580 : {
581 2691802 : FOR( i = 0; i < L; i++ )
582 : {
583 2565636 : y[i].v.im = x[i].v.im;
584 2565636 : y[i].v.re = x[i].v.re;
585 2565636 : move16();
586 2565636 : 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 126166 : 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 284666191 : 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 284666191 : IF( y < x )
616 : {
617 33316893162 : FOR( i = 0; i < L; i++ )
618 : {
619 33179275968 : y[i] = x[i];
620 33179275968 : move32();
621 : }
622 :
623 137617194 : return;
624 : }
625 :
626 30463639768 : FOR( i = L - 1; i >= 0; i-- )
627 : {
628 30316590771 : y[i] = x[i];
629 30316590771 : move32();
630 : }
631 : }
632 :
633 85079 : 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 47245629 : FOR( i = 0; i < N; i++ )
642 : {
643 47160550 : y[i] = a;
644 47160550 : move16();
645 : }
646 :
647 85079 : return;
648 : }
649 :
650 : /*-------------------------------------------------------------------*
651 : * set16_fx()
652 : * set32_fx()
653 : *
654 : * Set the vector elements to a value
655 : *-------------------------------------------------------------------*/
656 258035164 : 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 33483474267 : FOR( i = 0; i < N; i++ )
665 : {
666 33225439103 : y[i] = a;
667 33225439103 : move16();
668 : }
669 :
670 258035164 : return;
671 : }
672 :
673 631686362 : 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 65296340986 : FOR( i = 0; i < N; i++ )
682 : {
683 64664654624 : y[i] = a;
684 64664654624 : move32();
685 : }
686 :
687 631686362 : return;
688 : }
689 : /*-------------------------------------------------------------------*
690 : * Copy_Scale_sig
691 : *
692 : * Up/down scale a 16 bits vector x and move it into y
693 : *-------------------------------------------------------------------*/
694 19564398 : 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 19564398 : Flag Overflow = 0;
705 19564398 : move32();
706 : #endif
707 19564398 : IF( exp0 == 0 )
708 : {
709 1888191626 : FOR( i = 0; i < lg; i++ )
710 : {
711 1884635905 : y[i] = x[i];
712 1884635905 : move16();
713 : }
714 15818485 : return;
715 : }
716 16008677 : IF( exp0 < 0 )
717 : {
718 12262764 : tmp = shl( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
719 5297848971 : FOR( i = 0; i < lg; i++ )
720 : {
721 5285586207 : y[i] = msu_r( 0, x[i], tmp );
722 5285586207 : move16();
723 : }
724 12262764 : return;
725 : }
726 1927471485 : FOR( i = 0; i < lg; i++ )
727 : {
728 1923725572 : y[i] = shl_o( x[i], exp0, &Overflow );
729 1923725572 : 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 3576042 : 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 3576042 : Flag Overflow = 0;
748 3576042 : move32();
749 : #endif
750 :
751 :
752 3576042 : IF( exp0 == 0 )
753 : {
754 1045077014 : FOR( i = 0; i < lg; i++ )
755 : {
756 1042949911 : y[i] = L_deposit_l( x[i] );
757 1042949911 : move32();
758 : }
759 2303141 : 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 16416955 : 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 16416955 : Flag Overflow = 0;
798 16416955 : move32();
799 : #endif
800 :
801 :
802 16416955 : IF( exp0 == 0 )
803 : {
804 554675850 : FOR( i = 0; i < lg; i++ )
805 : {
806 552522208 : y[i] = L_deposit_l( x[i] );
807 552522208 : move32();
808 : }
809 5332173 : return;
810 : }
811 14263313 : IF( exp0 < 0 )
812 : {
813 : /*Should not happen */
814 269155244 : FOR( i = 0; i < lg; i++ )
815 : {
816 268505190 : y[i] = L_deposit_l( shl_o( x[i], exp0, &Overflow ) );
817 268505190 : move32();
818 : }
819 650054 : return;
820 : }
821 13613259 : L_tmp = L_shl_o( 1, exp0 - 1, &Overflow );
822 :
823 13613259 : IF( L_tmp >= 0x7FFF )
824 : {
825 1494589021 : FOR( i = 0; i < lg; i++ )
826 : {
827 : // y[i] = L_mult0(x[i], L_tmp);
828 1492060544 : y[i] = W_extract_l( W_mult_32_16( L_tmp, x[i] ) );
829 1492060544 : move32(); /* Overflow can occur here */
830 : }
831 2528477 : return;
832 : }
833 : // ELSE
834 : {
835 11084782 : Word16 tmp = extract_l( L_tmp );
836 5140767148 : FOR( i = 0; i < lg; i++ )
837 : {
838 5129682366 : y[i] = L_mult( x[i], tmp );
839 5129682366 : move32();
840 : }
841 : }
842 : }
843 :
844 9964395 : 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 9964395 : Flag Overflow = 0;
855 9964395 : move32();
856 : #endif
857 :
858 9964395 : tmp = add( 16, exp0 );
859 9964395 : IF( tmp != 0 )
860 : {
861 6518550915 : FOR( i = 0; i < lg; i++ )
862 : {
863 6508931830 : y[i] = round_fx_o( L_shl_o( x[i], tmp, &Overflow ), &Overflow );
864 6508931830 : move16();
865 : }
866 : }
867 : ELSE
868 : {
869 104736754 : FOR( i = 0; i < lg; i++ )
870 : {
871 104391444 : y[i] = round_fx_o( x[i], &Overflow );
872 104391444 : move16();
873 : }
874 : }
875 9964395 : }
876 :
877 : /*-------------------------------------------------------------------*
878 : * Scale_sig32
879 : *
880 : * Up/down scale a 32 bits vector
881 : *-------------------------------------------------------------------*/
882 107156108 : 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 107156108 : Flag Overflow = 0;
891 107156108 : move32();
892 : #endif
893 107156108 : IF( 0 == exp0 )
894 : {
895 27783904 : return;
896 : }
897 :
898 28922409661 : FOR( i = 0; i < lg; i++ )
899 : {
900 28843037457 : x[i] = L_shl_o( x[i], exp0, &Overflow );
901 28843037457 : 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 87095643 : 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 87095643 : IF( exp0 > 0 )
947 : {
948 9581345348 : FOR( i = 0; i < lg; i++ )
949 : {
950 9562116155 : x[i] = shl_sat( x[i], exp0 );
951 9562116155 : move16(); /* saturation can occur here */
952 : }
953 19229193 : return;
954 : }
955 67866450 : IF( exp0 < 0 )
956 : {
957 35394653 : tmp = shl_sat( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
958 14344230009 : FOR( i = 0; i < lg; i++ )
959 : {
960 14308835356 : x[i] = msu_r_sat( 0, x[i], tmp );
961 14308835356 : move16(); /* msu instead of mac because factor is negative */
962 : }
963 35394653 : return;
964 : }
965 : }
966 :
967 : /*-------------------------------------------------------------------*
968 : * scale_sig
969 : * Up/down scale a 16 bits vector
970 : *-------------------------------------------------------------------*/
971 8991810 : 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 8991810 : IF( exp0 != 0 )
980 : {
981 2303554734 : FOR( i = 0; i < lg; i++ )
982 : {
983 2297734601 : x[i] = shl( x[i], exp0 );
984 2297734601 : move16();
985 : }
986 : }
987 8991810 : }
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 3927050 : 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 100574794 : FOR( i = 0; i < Len; i++ )
1053 : {
1054 96647744 : out[i] = add_sat( in1[i], in2[i] );
1055 96647744 : move16();
1056 : }
1057 3927050 : }
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 21315336 : FOR( j = i1; j <= up; j++ )
1075 : {
1076 21314301 : IF( LE_16( tempr, r[j] ) )
1077 : {
1078 21292883 : BREAK;
1079 : }
1080 :
1081 21418 : r[j - 1] = r[j];
1082 21418 : 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 2667602 : 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 2667602 : ind = 0;
1127 2667602 : move16();
1128 2667602 : tmp = vec_fx[0];
1129 2667602 : move16();
1130 :
1131 74697816 : FOR( j = 1; j < lvec_fx; j++ )
1132 : {
1133 72030214 : if ( LT_16( vec_fx[j], tmp ) )
1134 : {
1135 1361164 : ind = j;
1136 1361164 : move16();
1137 : /*tmp = vec_fx[j]; move16(); */
1138 : }
1139 72030214 : tmp = s_min( tmp, vec_fx[j] );
1140 : }
1141 :
1142 2667602 : *min_fx = tmp;
1143 2667602 : move16();
1144 :
1145 2667602 : return ind;
1146 : }
1147 :
1148 26845050 : 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 26845050 : ind = 0;
1157 26845050 : move16();
1158 26845050 : tmp = vec_fx[0];
1159 26845050 : move16();
1160 :
1161 175257996 : FOR( j = 1; j < lvec_fx; j++ )
1162 : {
1163 148412946 : if ( GT_16( vec_fx[j], tmp ) )
1164 : {
1165 25979987 : ind = j;
1166 25979987 : move16();
1167 : }
1168 148412946 : tmp = s_max( tmp, vec_fx[j] );
1169 : }
1170 26845050 : *max_fx = tmp;
1171 26845050 : move16();
1172 :
1173 26845050 : return ind;
1174 : }
1175 :
1176 24098730 : 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 24098730 : ind = 0;
1185 24098730 : move16();
1186 24098730 : tmp = vec_fx[0];
1187 24098730 : move16();
1188 24098730 : exp = exp_vec[0];
1189 24098730 : move16();
1190 :
1191 685786389 : FOR( j = 1; j < lvec_fx; j++ )
1192 : {
1193 661687659 : IF( LT_16( tmp, shr_sat( vec_fx[j], sub( exp, exp_vec[j] ) ) ) )
1194 : {
1195 118542740 : ind = j;
1196 118542740 : move16();
1197 118542740 : tmp = vec_fx[j];
1198 118542740 : move16();
1199 118542740 : exp = exp_vec[j];
1200 118542740 : move16();
1201 : }
1202 : }
1203 :
1204 24098730 : 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 868286 : 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 868286 : ind = 0;
1222 868286 : move16();
1223 868286 : tmp = abs_s( vec[0] );
1224 :
1225 104535400 : FOR( j = 1; j < lvec; j++ )
1226 : {
1227 103667114 : if ( GT_16( abs_s( vec[j] ), tmp ) )
1228 : {
1229 4312675 : ind = j;
1230 4312675 : move16();
1231 : }
1232 103667114 : tmp = s_max( tmp, abs_s( vec[j] ) );
1233 : }
1234 868286 : *max_val = tmp;
1235 868286 : move16();
1236 :
1237 868286 : 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 239126 : ind = j;
1299 239126 : 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 5558882 : 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 5558882 : ind = 0;
1328 5558882 : move16();
1329 5558882 : tmp = vec[0];
1330 5558882 : move16();
1331 :
1332 66623923 : FOR( j = 1; j < lvec; j++ )
1333 : {
1334 61065041 : IF( GT_32( vec[j], tmp ) )
1335 : {
1336 5393752 : ind = j;
1337 5393752 : move16();
1338 : }
1339 61065041 : tmp = L_max( tmp, vec[j] );
1340 : }
1341 5558882 : if ( max_val != NULL )
1342 : {
1343 5544965 : *max_val = tmp;
1344 : }
1345 5558882 : move32();
1346 :
1347 5558882 : return ind;
1348 : }
1349 :
1350 4729965 : 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 4729965 : ind = 0;
1359 4729965 : move16();
1360 4729965 : tmp = L_abs( vec[0] );
1361 :
1362 1744209988 : FOR( j = 1; j < lvec; j++ )
1363 : {
1364 1739480023 : if ( GT_32( L_abs( vec[j] ), tmp ) )
1365 : {
1366 18769763 : ind = j;
1367 18769763 : move16();
1368 : }
1369 1739480023 : tmp = L_max( tmp, L_abs( vec[j] ) );
1370 : }
1371 4729965 : *max_val = tmp;
1372 4729965 : move32();
1373 :
1374 4729965 : 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 92141 : 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 92141 : sMax = abs_s( sx[0] );
1395 92141 : move16();
1396 :
1397 5975296 : FOR( k = 1; k < n; k++ )
1398 : {
1399 5883155 : sAbs = abs_s( sx[k] );
1400 5883155 : sMax = s_max( sMax, sAbs );
1401 : }
1402 :
1403 92141 : exp = norm_s( sMax );
1404 92141 : 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 16114 : 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 16114 : tmp = 0;
1441 16114 : move16();
1442 1703964 : FOR( i = 0; i < lvec; i++ )
1443 : {
1444 1687850 : L_var = L_deposit_l( vec[i] );
1445 1687850 : tmp = L_add( tmp, L_var ); /*Qx */
1446 : }
1447 :
1448 16114 : 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 1780601 : 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 1780601 : Flag Overflow = 0;
1548 1780601 : move32();
1549 : #endif
1550 1780601 : y[0] = mult_r( x[0], h[0] );
1551 1780601 : move16();
1552 117651840 : FOR( n = 1; n < L; n++ )
1553 : {
1554 115871239 : L_sum = L_mult( x[0], h[n] );
1555 4145800000 : FOR( i = 1; i < n; i++ )
1556 : {
1557 4029928761 : L_sum = L_mac_o( L_sum, x[i], h[n - i], &Overflow );
1558 : }
1559 115871239 : y[n] = mac_ro( L_sum, x[i], h[0], &Overflow );
1560 115871239 : move16();
1561 : }
1562 1780601 : 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 240412 : 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 240412 : idx = 0;
1816 240412 : move16();
1817 240412 : L_mindist = MAX_32;
1818 240412 : move32();
1819 240412 : IF( x_mean != 0 )
1820 : {
1821 478473 : FOR( d = 0; d < dim; d++ )
1822 : {
1823 : /*x[d] -= x_mean[d]; */
1824 354857 : x[d] = sub( x[d], x_mean[d] );
1825 354857 : move16(); /*Qx */
1826 : }
1827 : }
1828 240412 : j = 0;
1829 240412 : move16();
1830 8854660 : FOR( c = 0; c < cbsize; c++ )
1831 : {
1832 8614248 : L_dist = 0;
1833 8614248 : move32();
1834 :
1835 37572440 : FOR( d = 0; d < dim; d++ )
1836 : {
1837 : /*tmp = x[d] - cb[j++];*/
1838 28958192 : tmp = sub( x[d], cb[j++] ); /*Qx */
1839 28958192 : L_dist = L_mac0( L_dist, tmp, tmp );
1840 : }
1841 8614248 : if ( LT_32( L_dist, L_mindist ) )
1842 : {
1843 940608 : idx = c;
1844 940608 : move16();
1845 : }
1846 8614248 : L_mindist = L_min( L_mindist, L_dist );
1847 : }
1848 240412 : IF( xq == 0 )
1849 : {
1850 0 : return idx;
1851 : }
1852 :
1853 : /*j = idx*dim;*/
1854 240412 : j = i_mult2( idx, dim );
1855 1062453 : FOR( d = 0; d < dim; d++ )
1856 : {
1857 822041 : xq[d] = cb[j++];
1858 822041 : move16();
1859 : }
1860 240412 : IF( x_mean != 0 )
1861 : {
1862 478473 : FOR( d = 0; d < dim; d++ )
1863 : {
1864 : /*xq[d] += x_mean[d]; */
1865 354857 : xq[d] = add( xq[d], x_mean[d] );
1866 354857 : move16();
1867 : }
1868 : }
1869 :
1870 240412 : 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 196015 : idx = c;
1987 196015 : 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 20179471 : 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 20179471 : emax = L_mult0( vec[0], vec[0] );
2027 20179471 : ind = 0;
2028 20179471 : move16();
2029 :
2030 653123602 : FOR( j = 1; j < lvec; j++ )
2031 : {
2032 632944131 : L_tmp = L_mult0( vec[j], vec[j] );
2033 632944131 : L_tmp1 = L_sub( L_tmp, emax );
2034 632944131 : if ( L_tmp1 > 0 )
2035 : {
2036 79793695 : ind = j;
2037 79793695 : move16();
2038 : }
2039 632944131 : emax = L_max( emax, L_tmp );
2040 : }
2041 :
2042 20179471 : *ener_max = L_shr_sat( emax, add( Qvec, Qvec ) );
2043 20179471 : move32();
2044 :
2045 20179471 : return ind;
2046 : }
2047 :
2048 33875 : 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 33875 : emax = W_mult0_32_32( vec[0], vec[0] );
2060 33875 : ind = 0;
2061 33875 : move16();
2062 :
2063 3237543 : FOR( j = 1; j < lvec; j++ )
2064 : {
2065 3203668 : W_tmp = W_mult0_32_32( vec[j], vec[j] );
2066 3203668 : W_tmp1 = W_sub( W_tmp, emax );
2067 3203668 : if ( W_tmp1 > 0 )
2068 : {
2069 262810 : ind = j;
2070 262810 : move16();
2071 : }
2072 3203668 : if ( LE_64( emax, W_tmp ) )
2073 : {
2074 265463 : emax = W_tmp;
2075 265463 : move64();
2076 : }
2077 : }
2078 :
2079 33875 : *ener_max = W_extract_l( W_shr( emax, add( Qvec, Qvec ) ) );
2080 33875 : move32();
2081 :
2082 33875 : 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 185279 : 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 185279 : tmp = L_deposit_l( 0 );
2121 6959310 : FOR( i = 0; i < lvec; i++ )
2122 : {
2123 6774031 : tmp = L_add_sat( tmp, vec[i] ); /*Qx */
2124 : }
2125 :
2126 185279 : return tmp;
2127 : }
2128 1138628 : 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 1138628 : tmp = 0;
2136 1138628 : move16();
2137 8289984 : FOR( i = 0; i < lvec; i++ )
2138 : {
2139 7151356 : tmp = add_sat( tmp, vec[i] ); /*Qx */
2140 : }
2141 :
2142 1138628 : return tmp;
2143 : }
2144 : /*------------------------------------------------------------------*
2145 : * function Random
2146 : *
2147 : * Signed 16 bits random generator.
2148 : *------------------------------------------------------------------*/
2149 640322681 : Word16 Random( /* o : output random value */
2150 : Word16 *seed /* i/o: random seed */
2151 : )
2152 : {
2153 640322681 : *seed = extract_l( L_mac0( 13849L, *seed, 31821 ) );
2154 640322681 : move16();
2155 640322681 : 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 206631 : 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 206631 : IF( a <= 0 )
2420 : {
2421 2780 : *exp_out = 0;
2422 2780 : move16();
2423 2780 : return 0;
2424 : }
2425 :
2426 203851 : exp = norm_l( a );
2427 203851 : tmp = extract_h( L_shl( a, exp ) );
2428 203851 : exp = sub( exp, sub( 30, Q_a ) );
2429 203851 : tmp = div_s( 16384, tmp );
2430 203851 : L_tmp = L_deposit_h( tmp );
2431 203851 : L_tmp = Isqrt_lc( L_tmp, &exp );
2432 :
2433 203851 : *exp_out = exp;
2434 203851 : move16();
2435 :
2436 203851 : return L_tmp;
2437 : }
2438 :
2439 :
2440 1915369 : 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 1915369 : Flag Overflow = 0;
2452 1915369 : move32();
2453 : #endif
2454 1915369 : test();
2455 1915369 : IF( ( a <= 0 ) || ( b <= 0 ) )
2456 : {
2457 10459 : *exp_out = 0;
2458 10459 : move16();
2459 10459 : return 0;
2460 : }
2461 :
2462 1904910 : exp_num = norm_l( b );
2463 1904910 : num = round_fx_o( L_shl_o( b, exp_num, &Overflow ), &Overflow );
2464 1904910 : exp_num = sub( sub( 30, exp_num ), Q_b );
2465 :
2466 1904910 : exp_den = norm_l( a );
2467 1904910 : den = round_fx_o( L_shl_o( a, exp_den, &Overflow ), &Overflow );
2468 1904910 : exp_den = sub( sub( 30, exp_den ), Q_a );
2469 :
2470 1904910 : scale = shr( sub( den, num ), 15 );
2471 1904910 : num = shl_o( num, scale, &Overflow );
2472 1904910 : exp_num = sub( exp_num, scale );
2473 :
2474 1904910 : tmp = div_s( num, den );
2475 1904910 : exp = sub( exp_num, exp_den );
2476 :
2477 1904910 : L_tmp = L_deposit_h( tmp );
2478 1904910 : L_tmp = Isqrt_lc( L_tmp, &exp );
2479 :
2480 1904910 : *exp_out = exp;
2481 1904910 : move16();
2482 :
2483 1904910 : 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 250182 : 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 250182 : Flag Overflow = 0;
2629 250182 : move32();
2630 : #endif
2631 : /* prepare the input buffer (copy and update memory) */
2632 250182 : Copy( mem, buf_in, K );
2633 250182 : Copy( x, buf_in + K, L );
2634 250182 : IF( upd )
2635 : {
2636 154 : Copy( buf_in + L, mem, K );
2637 : }
2638 :
2639 : /* do the filtering */
2640 77041558 : FOR( i = 0; i < L; i++ )
2641 : {
2642 76791376 : s = L_mult_o( buf_in[K + i], h[0], &Overflow );
2643 :
2644 413365664 : FOR( j = 1; j <= K; j++ )
2645 : {
2646 336574288 : s = L_mac_o( s, h[j], buf_in[K + i - j], &Overflow );
2647 : }
2648 76791376 : s = L_shl_o( s, shift, &Overflow );
2649 76791376 : y[i] = round_fx_o( s, &Overflow ); /*Qx */
2650 76791376 : move16();
2651 : }
2652 250182 : }
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 235833 : 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 6801615 : FOR( i = 0; i < N; i++ )
2763 : {
2764 6565782 : y[i] = sub_sat( x1[i], x2[i] );
2765 6565782 : move16();
2766 : }
2767 :
2768 235833 : return;
2769 : }
2770 :
2771 :
2772 : /*--------------------------------------------------------------------------------*/
2773 : /* squant_fx() */
2774 : /*--------------------------------------------------------------------------------*/
2775 5183342 : 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 5183342 : Flag Overflow = 0;
2787 : #endif
2788 :
2789 5183342 : idx = 0;
2790 5183342 : move16();
2791 5183342 : L_mindist = MAX_32;
2792 5183342 : move32();
2793 :
2794 25764283 : FOR( c = 0; c < cbsize; c++ )
2795 : {
2796 20580941 : L_dist = L_deposit_l( 0 );
2797 20580941 : tmp = sub_o( x, cb[c], &Overflow );
2798 :
2799 : /*dist += tmp*tmp; */
2800 20580941 : L_dist = L_mac_o( L_dist, tmp, tmp, &Overflow );
2801 :
2802 20580941 : if ( LT_32( L_dist, L_mindist ) )
2803 : {
2804 11722060 : idx = c;
2805 11722060 : move16();
2806 : }
2807 20580941 : L_mindist = L_min( L_mindist, L_dist );
2808 : }
2809 :
2810 5183342 : *xq = cb[idx];
2811 5183342 : move16();
2812 :
2813 5183342 : 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 8209954 : 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 8209954 : Word16 tmp = exp0;
2938 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
2939 8209954 : Flag Overflow = 0;
2940 8209954 : move32();
2941 : #endif
2942 8209954 : IF( exp0 == 0 )
2943 : {
2944 964621127 : FOR( i = 0; i < lg; i++ )
2945 : {
2946 963178525 : y[i] = x[i];
2947 963178525 : move32();
2948 : }
2949 5971926 : return;
2950 : }
2951 6767352 : IF( exp0 < 0 )
2952 : {
2953 509881238 : FOR( i = 0; i < lg; i++ )
2954 : {
2955 505351914 : y[i] = L_shl_o( x[i], tmp, &Overflow );
2956 505351914 : move16();
2957 : }
2958 4529324 : return;
2959 : }
2960 2238028 : L_tmp = L_shl_o( 1, exp0 - 1, &Overflow );
2961 296298475 : FOR( i = 0; i < lg; i++ )
2962 : {
2963 294060447 : y[i] = W_extract_l( W_mult_32_32( L_tmp, x[i] ) );
2964 294060447 : 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 11656414 : 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 11656414 : IF( exp0 == 0 )
2984 : {
2985 195804651 : FOR( i = 0; i < len; i++ )
2986 : {
2987 191401334 : *dst++ = round_fx_sat( *src++ );
2988 191401334 : move16();
2989 : }
2990 4403317 : return;
2991 : }
2992 :
2993 4061366674 : FOR( i = 0; i < len; i++ )
2994 : {
2995 4054113577 : L_temp = L_shl_sat( *src++, exp0 );
2996 :
2997 4054113577 : *dst++ = round_fx_sat( L_temp );
2998 4054113577 : 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 : #ifdef ISSUE_1770_replace_shr_ro
3148 1732480 : y[i] = add_o( x1[i], shr_r_sat( x2[i], Qyx2 ), &Overflow );
3149 : #else
3150 : y[i] = add_o( x1[i], shr_ro( x2[i], Qyx2, &Overflow ), &Overflow );
3151 : #endif
3152 1732480 : move16();
3153 : }
3154 : }
3155 : ELSE
3156 : {
3157 0 : FOR( i = 0; i < N; i++ )
3158 : {
3159 : #ifdef ISSUE_1770_replace_shr_ro
3160 0 : y[i] = add_o( shr_r_sat( x1[i], Qyx1 ), shr_r_sat( x2[i], Qyx2 ), &Overflow );
3161 : #else
3162 : y[i] = add_o( shr_ro( x1[i], Qyx1, &Overflow ), shr_ro( x2[i], Qyx2, &Overflow ), &Overflow );
3163 : #endif
3164 0 : move16();
3165 : }
3166 : }
3167 2056 : return;
3168 : }
3169 :
3170 : /*-------------------------------------------------------------------*
3171 : * Add_flt32_flt32
3172 : *
3173 : * Add two Pseudo Float Value that are 32 Bits Mantisa and 16 Bits Exp
3174 : *-------------------------------------------------------------------*/
3175 5352 : Word32 Add_flt32_flt32( /* o: Result (Normalized) */
3176 : Word32 a, /* i: 1st Value */
3177 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
3178 : Word32 b, /* i: 2nd Value */
3179 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
3180 : Word16 *exp_out /* o: Exponent of Result */
3181 : )
3182 : {
3183 : Word16 temp, temp2;
3184 : Word32 L_temp;
3185 :
3186 : /* Subract 1 to further divide by 2 to avoid overflow on L_add */
3187 5352 : temp = sub( s_min( exp_a, exp_b ), 1 );
3188 :
3189 : /* Put both to same exponent */
3190 5352 : exp_a = sub( exp_a, temp );
3191 5352 : a = L_shr( a, exp_a );
3192 5352 : exp_b = sub( exp_b, temp );
3193 5352 : b = L_shr( b, exp_b );
3194 :
3195 : /* add them together */
3196 5352 : L_temp = L_add( a, b );
3197 5352 : temp2 = norm_l( L_temp );
3198 :
3199 5352 : *exp_out = add( temp, temp2 );
3200 5352 : move16();
3201 :
3202 5352 : return L_shl( L_temp, temp2 );
3203 : }
3204 :
3205 : /*-------------------------------------------------------------------*
3206 : * Mul_flt32_Q15
3207 : *
3208 : * Multiply one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
3209 : * with a Q15 value
3210 : *-------------------------------------------------------------------*/
3211 0 : Word32 Mul_flt32_Q15( /* o: Result (Normalized) */
3212 : Word32 value, /* i: Pseudo_float Value */
3213 : Word16 *exp_v, /*i/o: Exponent of Value (Q of Value) */
3214 : Word16 frac /* i: Q15 value */
3215 : )
3216 : {
3217 : Word16 temp;
3218 : Word32 L_temp;
3219 :
3220 0 : L_temp = Mult_32_16( value, frac );
3221 0 : temp = norm_l( L_temp );
3222 :
3223 0 : *exp_v = add( temp, *exp_v );
3224 0 : move16();
3225 :
3226 0 : return L_shl( L_temp, temp );
3227 : }
3228 :
3229 : /*-------------------------------------------------------------------*
3230 : * Div_flt32_flt32
3231 : *
3232 : * Divide one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
3233 : * by another one
3234 : *-------------------------------------------------------------------*/
3235 2 : Word32 Div_flt32_flt32( /* o: Result (Normalized) */
3236 : Word32 a, /* i: 1st Value */
3237 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
3238 : Word32 b, /* i: 2nd Value */
3239 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
3240 : Word16 *exp_out /* o: Exponent of Result */
3241 : )
3242 : {
3243 : Word16 temp, temp2;
3244 : Word32 L_temp;
3245 :
3246 2 : temp = div_s( 16384, round_fx( b ) );
3247 2 : L_temp = Mult_32_16( a, temp );
3248 2 : temp2 = sub( 31 - 1, exp_b );
3249 2 : temp2 = add( temp2, exp_a );
3250 :
3251 2 : temp = norm_l( L_temp );
3252 :
3253 2 : *exp_out = add( temp, temp2 );
3254 2 : move16();
3255 :
3256 2 : return L_shl( L_temp, temp );
3257 : }
3258 :
3259 :
3260 : /*-------------------------------------------------------------------*
3261 : * Calc_Energy_Autoscaled
3262 : *
3263 : * Calculate Energy with overflow protection
3264 : *-------------------------------------------------------------------*/
3265 3489927 : Word32 Calc_Energy_Autoscaled( /* o: Result (Energy) */
3266 : const Word16 *signal, /* i: Signal */
3267 : Word16 signal_exp, /* i: Exponent of Signal (Q of Signal) */
3268 : Word16 len, /* i: Frame Length */
3269 : Word16 *energy_exp /* o: Exponent of Energy (Q of Energy) */
3270 : )
3271 : {
3272 : Word16 temp, temp2;
3273 : Word32 L_temp, L_Energy;
3274 : Word16 i, j;
3275 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
3276 3489927 : Flag Overflow = 0;
3277 3489927 : move32();
3278 : #endif
3279 :
3280 3489927 : Overflow = 0;
3281 3489927 : move16();
3282 :
3283 3489927 : temp2 = 0;
3284 3489927 : move16();
3285 3489927 : L_Energy = L_deposit_l( 1 );
3286 3489927 : j = s_and( 7, len );
3287 3501767 : FOR( i = 0; i < j; i++ )
3288 : {
3289 : /* divide by 2 so energy will be divided by 4 */
3290 11840 : temp = mult_ro( *signal++, 16384, &Overflow );
3291 11840 : L_Energy = L_mac0_o( L_Energy, temp, temp, &Overflow );
3292 : }
3293 11546173 : FOR( i = j; i < len; i += 8 ) /* Process 8 Samples at a time */
3294 : {
3295 : /* divide by 2 so energy will be divided by 4 */
3296 8056246 : temp = mult_ro( *signal++, 16384, &Overflow );
3297 8056246 : L_temp = L_mult0( temp, temp );
3298 64449968 : FOR( j = 1; j < 8; j++ )
3299 : {
3300 56393722 : temp = mult_ro( *signal++, 16384, &Overflow );
3301 56393722 : L_temp = L_mac0_o( L_temp, temp, temp, &Overflow );
3302 : }
3303 :
3304 8056246 : L_temp = L_shr_o( L_temp, temp2, &Overflow );
3305 : /* Here we try the addition just to check if we can sum
3306 : the energy of the small (8 Iterations) loop with the
3307 : total energy calculated so far without an overflow.
3308 : The result is discarded. If there is an overflow both
3309 : energies are div by 2. Otherwise, nothing is done.
3310 : After the 'IF', the sum is done again and will always
3311 : be without an overflow. */
3312 8056246 : L_add_o( L_Energy, L_temp, &Overflow );
3313 8056246 : IF( Overflow != 0 )
3314 : {
3315 49 : L_Energy = L_shr( L_Energy, 1 );
3316 49 : L_temp = L_shr( L_temp, 1 );
3317 49 : temp2 = add( temp2, 1 );
3318 49 : Overflow = 0;
3319 49 : move16();
3320 : }
3321 8056246 : L_Energy = L_add_o( L_Energy, L_temp, &Overflow );
3322 : }
3323 : /* Calc Final Exponent (sub 2 because of the mult_r by 16384 that already divs the ener by 4) */
3324 3489927 : temp2 = sub( sub( shl( signal_exp, 1 ), temp2 ), 2 );
3325 :
3326 3489927 : *energy_exp = temp2;
3327 3489927 : move16();
3328 :
3329 3489927 : return L_Energy;
3330 : }
3331 :
3332 4040857 : Word16 Find_Max_Norm16( const Word16 *src, Word16 len )
3333 : {
3334 : Word16 i;
3335 : Word16 max16;
3336 :
3337 : /* it starts at '0' and not '1' like in Find_Max_Norm32() */
3338 : /* and that is necessary. */
3339 4040857 : max16 = 0;
3340 4040857 : move16();
3341 :
3342 2371323312 : FOR( i = 0; i < len; i++ )
3343 : {
3344 2367282455 : max16 = s_max( max16, abs_s( *src++ ) );
3345 : }
3346 :
3347 4040857 : return norm_s( max16 );
3348 : }
3349 :
3350 3888322 : Word16 Find_Max_Norm32( const Word32 *src, Word16 len )
3351 : {
3352 : Word16 i;
3353 : Word32 max32;
3354 :
3355 3888322 : max32 = L_deposit_l( 1 );
3356 :
3357 2995377638 : FOR( i = 0; i < len; i++ )
3358 : {
3359 2991489316 : max32 = L_max( max32, L_abs( *src++ ) );
3360 : }
3361 :
3362 3888322 : return norm_l( max32 );
3363 : }
3364 :
3365 : /*-------------------------------------------------------------------*
3366 : * Sqrt_Ratio32
3367 : *
3368 : * Calculate Sqrt of Val1/Val2
3369 : *-------------------------------------------------------------------*/
3370 5521 : Word32 Sqrt_Ratio32( /* o: Result in Q31 */
3371 : Word32 L_val1, /* i: Mantisa of Val1 */
3372 : Word16 exp1, /* i: Exp of Val1 (>0: Val was Left Shifted, <0:Right Shifted) */
3373 : Word32 L_val2, /* i: Mantisa of Val2 */
3374 : Word16 exp2, /* i: Exp of Val2 (same as exp1) */
3375 : Word16 *exp /* o: Exp of Result (# of 'L_shl' Req to get to Final Value) */
3376 : )
3377 : {
3378 : Word16 temp;
3379 :
3380 : /* Normalize Energy #1 */
3381 5521 : temp = norm_l( L_val1 );
3382 5521 : L_val1 = L_shl( L_val1, temp );
3383 : /* Adjust Exponent of Energy #1 */
3384 5521 : exp1 = add( exp1, temp );
3385 :
3386 : /* Normalize Energy #2 */
3387 5521 : temp = norm_l( L_val2 );
3388 5521 : L_val2 = L_shl( L_val2, temp );
3389 : /* Adjust Exponent of Energy #1 */
3390 5521 : exp2 = add( exp2, temp );
3391 :
3392 : /* Prepare for Inverse */
3393 5521 : temp = round_fx_sat( L_val1 );
3394 5521 : temp = div_s( 16384, temp );
3395 : /* Mult Now */
3396 5521 : L_val2 = Mult_32_16( L_val2, temp );
3397 5521 : exp1 = add( sub( exp2, exp1 ), 15 * 2 );
3398 :
3399 : /* Here Result of ('L_val2' / 2^'exp2') / ('L_val1' / 2^'exp1') is */
3400 : /* 'L_val2' / 2^'exp1' */
3401 : /* Which is val2/val1 instead of val1/val2 because we will use Inverted Square Root */
3402 : /* Normalize before Square Root */
3403 5521 : temp = norm_l( L_val2 );
3404 5521 : L_val2 = L_shl( L_val2, temp );
3405 5521 : exp1 = add( temp, exp1 );
3406 : /* Do Sqrt */
3407 5521 : temp = sub( 31, exp1 );
3408 5521 : L_val1 = Isqrt_lc( L_val2, &temp );
3409 :
3410 5521 : *exp = temp;
3411 5521 : move16();
3412 :
3413 5521 : return L_val1;
3414 : }
3415 :
3416 4430 : Word16 Invert16( /* result in Q'15 + 'exp' */
3417 : Word16 val,
3418 : Word16 *exp )
3419 : {
3420 : Word16 temp;
3421 :
3422 : /* prevent 0 input */
3423 4430 : val = s_max( val, 1 );
3424 : /* Normalize Value */
3425 4430 : temp = norm_s( val );
3426 4430 : val = shl( val, temp );
3427 :
3428 4430 : *exp = sub( sub( 15 - 1, *exp ), temp );
3429 4430 : move16(); /* -1 because of 0x4000 is 1.0 in Q14 (and not Q15) */
3430 :
3431 4430 : temp = div_s( 0x4000, val );
3432 :
3433 4430 : return temp;
3434 : }
3435 :
3436 0 : Word16 find_rem( Word16 n, Word16 m, Word16 *r )
3437 : {
3438 : Word16 i, q1, q2, qd;
3439 : Word32 Ltemp2;
3440 : Word32 Lacc;
3441 :
3442 0 : test();
3443 0 : test();
3444 0 : IF( n <= 0 || m <= 0 || n < m )
3445 : {
3446 0 : *r = n;
3447 0 : move16();
3448 0 : return ( 0 );
3449 : }
3450 :
3451 0 : q1 = norm_s( n );
3452 0 : q1 = sub( q1, 1 );
3453 0 : Lacc = L_deposit_h( shl( n, q1 ) );
3454 0 : qd = sub( q1, 1 );
3455 0 : q2 = norm_s( m );
3456 0 : q2 = sub( q2, 1 );
3457 0 : Ltemp2 = L_deposit_h( shl( m, q2 ) );
3458 0 : qd = sub( q2, qd );
3459 0 : q2 = add( q2, 1 );
3460 :
3461 0 : FOR( i = 0; i < qd; i++ )
3462 : {
3463 0 : Lacc = L_sub( Lacc, Ltemp2 );
3464 0 : IF( Lacc >= 0 )
3465 : {
3466 0 : Lacc = L_add( L_shl( Lacc, 1 ), 1 );
3467 : }
3468 : ELSE
3469 : {
3470 0 : Lacc = L_add( Lacc, Ltemp2 );
3471 0 : Lacc = L_shl( Lacc, 1 );
3472 : }
3473 : }
3474 0 : q1 = extract_l( Lacc );
3475 0 : Ltemp2 = L_shr( Lacc, q2 );
3476 0 : *r = extract_h( Ltemp2 );
3477 0 : move16();
3478 0 : return ( q1 );
3479 : }
3480 :
3481 :
3482 0 : Word32 find_remd( Word32 n, Word32 m, Word32 *r )
3483 : {
3484 : Word16 i, q1, q2, qd;
3485 : Word32 Ltemp2, qo;
3486 : Word32 Lacc;
3487 :
3488 0 : test();
3489 0 : test();
3490 0 : IF( n <= 0 || m <= 0 || n < m )
3491 : {
3492 0 : *r = n;
3493 0 : move16();
3494 0 : return ( 0 );
3495 : }
3496 :
3497 0 : q1 = norm_l( n );
3498 0 : q1 = sub( q1, 1 );
3499 0 : Lacc = L_shl( n, q1 );
3500 0 : qd = sub( q1, 1 );
3501 0 : q2 = norm_l( m );
3502 0 : q2 = sub( q2, 1 );
3503 0 : Ltemp2 = L_shl( m, q2 );
3504 0 : qd = sub( q2, qd );
3505 0 : q2 = add( q2, 1 );
3506 0 : qo = 0;
3507 0 : move16();
3508 :
3509 0 : FOR( i = 0; i < qd; i++ )
3510 : {
3511 0 : Lacc = L_sub( Lacc, Ltemp2 );
3512 0 : qo = L_shl( qo, 1 );
3513 0 : IF( Lacc >= 0 )
3514 : {
3515 0 : Lacc = L_shl( Lacc, 1 );
3516 0 : qo = L_add( qo, 1 );
3517 : }
3518 : ELSE
3519 : {
3520 0 : Lacc = L_add( Lacc, Ltemp2 );
3521 0 : Lacc = L_shl( Lacc, 1 );
3522 : }
3523 : }
3524 0 : *r = L_shr( Lacc, q2 );
3525 0 : move16();
3526 0 : return ( qo );
3527 : }
3528 :
3529 30442 : Word16 rint_new_fx(
3530 : Word32 x /*Q16 */
3531 : )
3532 : {
3533 : Word16 a;
3534 : Word32 L_tmp;
3535 : Word16 frac, tmp;
3536 :
3537 : /* middle value point test */
3538 30442 : frac = lshr( extract_l( x ), 1 ); /*Q15 */
3539 30442 : tmp = sub( frac, 0x4000 );
3540 :
3541 30442 : IF( !tmp )
3542 : {
3543 0 : a = add( extract_h( x ), 1 );
3544 :
3545 0 : IF( s_and( a, 1 ) == 0 )
3546 : {
3547 0 : return a;
3548 : }
3549 0 : IF( s_and( a, 1 ) != 0 )
3550 : {
3551 0 : return extract_h( x );
3552 : }
3553 0 : return extract_h( x );
3554 : }
3555 : ELSE
3556 : {
3557 30442 : L_tmp = L_add( x, 32768 ); /*Q16 */
3558 30442 : return extract_h( L_tmp );
3559 : }
3560 : }
3561 :
3562 :
3563 : /*===================================================================*/
3564 : /* FUNCTION : erb_diff_search_fx () */
3565 : /*-------------------------------------------------------------------*/
3566 : /* PURPOSE : erb amplitude VQ search for QPPP */
3567 : /*-------------------------------------------------------------------*/
3568 : /* INPUT ARGUMENTS : */
3569 : /* _ (Word16 []) prev_erb : Previous erb amplitude, Q13 */
3570 : /* _ (Word16 []) curr_erb : Current erb amplitude, Q13 */
3571 : /* _ (Word16 []) dif_erb: erb differential, Q13 */
3572 : /* _ (Word16 []) pow_spec : LPC power spectrum, Q7 */
3573 : /* _ (Word16 [][]) cb_fx : differential erb codebook, Q13 */
3574 : /* _ (Word16) cb_size : codebook size */
3575 : /* _ (Word16) cb_dim : codebook dimension */
3576 : /* _ (Word16) offset : index to current segment of erb array */
3577 : /* for quantization */
3578 : /*-------------------------------------------------------------------*/
3579 : /* OUTPUT ARGUMENTS : */
3580 : /* _ None */
3581 : /*-------------------------------------------------------------------*/
3582 : /* INPUT/OUTPUT ARGUMENTS : */
3583 : /* _ None */
3584 : /*-------------------------------------------------------------------*/
3585 : /* RETURN ARGUMENTS : */
3586 : /* _ (Word16) index: best codebook index */
3587 : /*-------------------------------------------------------------------*/
3588 : /* CALLED FROM : TX */
3589 : /*===================================================================*/
3590 :
3591 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 )
3592 : {
3593 : Word16 i, j, mmseindex;
3594 : Word16 dh, dl;
3595 : Word32 mmse;
3596 : Word32 Ltemp1;
3597 : Word32 Lacc;
3598 :
3599 0 : mmse = EVS_LW_MAX;
3600 0 : move32();
3601 0 : mmseindex = -1;
3602 0 : move16();
3603 0 : FOR( j = 0; j < cb_size; j++ )
3604 : {
3605 :
3606 0 : Lacc = L_deposit_l( 0 );
3607 0 : FOR( i = 0; i < cb_dim; i++ )
3608 : {
3609 0 : IF( add_sat( cb_fx[j * cb_dim + i], prev_erb[i + offset] ) < 0 )
3610 : {
3611 0 : Ltemp1 = L_mult( curr_erb[i + offset], curr_erb[i + offset] ); /* Q27 */
3612 0 : dh = extract_h( Ltemp1 );
3613 0 : dl = extract_l( Ltemp1 );
3614 0 : IF( dl < 0 )
3615 : {
3616 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3617 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
3618 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3619 : }
3620 : ELSE
3621 : {
3622 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl );
3623 : }
3624 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) );
3625 : }
3626 : ELSE
3627 : {
3628 0 : dh = sub_sat( dif_erb[i + offset], cb_fx[j * cb_dim + i] ); /* Q13 */
3629 0 : Ltemp1 = L_mult_sat( dh, dh ); /* Q27 */
3630 0 : dh = extract_h( Ltemp1 );
3631 0 : dl = extract_l( Ltemp1 );
3632 :
3633 0 : IF( dl < 0 )
3634 : {
3635 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3636 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
3637 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3638 : }
3639 : ELSE
3640 : {
3641 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl ); /* Q33 */
3642 : }
3643 :
3644 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) ); /* Q18 */
3645 : }
3646 :
3647 0 : IF( LT_16( cb_fx[j * cb_dim + i], dif_erb[i + offset] ) )
3648 : {
3649 0 : dh = extract_h( Ltemp1 );
3650 0 : dl = extract_l( Ltemp1 );
3651 0 : IF( dl < 0 )
3652 : {
3653 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
3654 0 : Ltemp1 = Mult_32_16( Ltemp1, 29491 );
3655 0 : Ltemp1 = L_shl( Ltemp1, 1 );
3656 : }
3657 : ELSE
3658 : {
3659 0 : Ltemp1 = (Word32) L_mult0( 29491, dl ); /* 29491=0.9 in Q15 */
3660 : }
3661 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( dh, 29491 ) );
3662 : }
3663 0 : Lacc = L_add( Lacc, Ltemp1 ); /* Q18 */
3664 : }
3665 :
3666 0 : IF( LT_32( Lacc, mmse ) )
3667 : {
3668 0 : mmse = L_add( Lacc, 0 );
3669 0 : mmseindex = j;
3670 0 : move16();
3671 : }
3672 : }
3673 :
3674 0 : return ( mmseindex );
3675 : }
3676 482187 : void Acelp_dec_total_exc(
3677 : Word16 *exc_fx, /* i/o: adapt. excitation exc */
3678 : Word16 *exc2_fx, /* i/o: adapt. excitation/total exc */
3679 : const Word16 gain_code16, /* i : Gain code Q0 */
3680 : const Word16 gain_pit_fx, /* i ; Pitch gain in Q14 */
3681 : const Word16 i_subfr, /* i ; subfr */
3682 : const Word16 *code_fx, /* i : code in Q9 */
3683 : const Word16 L_subfr /* i : Subframne lenght */
3684 : )
3685 : {
3686 : Word16 i;
3687 : Word32 L_tmp;
3688 :
3689 31342155 : FOR( i = 0; i < L_subfr; i++ )
3690 : {
3691 30859968 : L_tmp = L_shl_sat( L_mult( gain_pit_fx, exc_fx[i + i_subfr] ), 1 ); /*Q16+Q_exc*/
3692 30859968 : exc2_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
3693 30859968 : L_tmp = L_add_sat( L_tmp, L_shl_sat( L_mult( gain_code16, code_fx[i] ), 6 ) ); /*Q16+Q_exc*/
3694 30859968 : exc_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
3695 30859968 : move16();
3696 30859968 : move16();
3697 : }
3698 482187 : }
3699 :
3700 : /*-------------------------------------------------------------------*
3701 : * UL_inverse
3702 : *
3703 : * Calculate inverse of UL_val. Output in Q_exp.
3704 : *-------------------------------------------------------------------*/
3705 627682 : UWord32 UL_inverse( const UWord32 UL_val, Word16 *exp )
3706 : {
3707 : UWord32 UL_tmp;
3708 :
3709 627682 : *exp = norm_ul( UL_val );
3710 627682 : move16();
3711 627682 : UL_tmp = UL_lshl( UL_val, *exp ); /* Q32 */
3712 :
3713 627682 : *exp = add( 32, sub( 31, *exp ) );
3714 627682 : move16();
3715 :
3716 627682 : return UL_div( 0x80000000, UL_tmp );
3717 : }
3718 :
3719 : /*-------------------------------------------------------------------*
3720 : * UL_div
3721 : *
3722 : * Calculate UL_num/UL_den. UL_num assumed to be Q31, UL_den assumed
3723 : * to be Q32, then result is in Q32.
3724 : *-------------------------------------------------------------------*/
3725 931451 : UWord32 UL_div( const UWord32 UL_num, const UWord32 UL_den )
3726 : {
3727 : UWord32 UL_e, UL_Q;
3728 : UWord32 UL_msb, UL_lsb;
3729 : Word16 i;
3730 :
3731 931451 : UL_e = UL_subNsD( 0xffffffff, UL_den );
3732 931451 : UL_Q = UL_num;
3733 931451 : move32();
3734 :
3735 5588706 : FOR( i = 0; i < 5; i++ )
3736 : {
3737 4657255 : Mpy_32_32_uu( UL_Q, UL_e, &UL_msb, &UL_lsb ); /*31+32-32=31 */
3738 4657255 : UL_Q = UL_addNsD( UL_Q, UL_msb );
3739 4657255 : Mpy_32_32_uu( UL_e, UL_e, &UL_e, &UL_lsb ); /*32+32-32=32 */
3740 : }
3741 :
3742 931451 : return UL_Q;
3743 : }
3744 :
3745 : /*-----------------------------------------------------------------------------
3746 : * ratio()
3747 : *
3748 : * Divide the numerator by the denominator.
3749 : *----------------------------------------------------------------------------*/
3750 7113693 : Word16 ratio( const Word32 numer, const Word32 denom, Word16 *expo )
3751 : {
3752 : Word16 expNumer, expDenom;
3753 : Word16 manNumer, manDenom;
3754 : Word16 quotient;
3755 :
3756 7113693 : expDenom = norm_l( denom ); /* exponent */
3757 7113693 : manDenom = extract_h( L_shl( denom, expDenom ) ); /* mantissa */
3758 7113693 : expNumer = norm_l( numer ); /* exponent */
3759 7113693 : manNumer = extract_h( L_shl( numer, expNumer ) ); /* mantissa */
3760 7113693 : manNumer = shr( manNumer, 1 ); /* Ensure the numerator < the denominator */
3761 7113693 : quotient = div_s( manNumer, manDenom ); /* in Q14 */
3762 :
3763 7113693 : *expo = sub( expNumer, expDenom );
3764 7113693 : move16();
3765 7113693 : return quotient; /* Q14 */
3766 : }
3767 :
3768 : /*-----------------------------------------------------------------------*
3769 : * Function hp400_12k8() *
3770 : * *
3771 : * 2nd order Cheb2 high pass filter with cut off frequency at 400 Hz. *
3772 : * Optimized for fixed-point to get the following frequency response : *
3773 : * *
3774 : * frequency : 0Hz 100Hz 200Hz 300Hz 400Hz 630Hz 1.5kHz 3kHz *
3775 : * dB loss : -infdB -30dB -20dB -10dB -3dB +6dB +1dB 0dB *
3776 : * *
3777 : * Algorithm : *
3778 : * *
3779 : * y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] *
3780 : * + a[1]*y[i-1] + a[2]*y[i-2]; *
3781 : * *
3782 : * short b[3] = {3660, -7320, 3660}; in Q12 *
3783 : * short a[3] = {4096, 7320, -3540}; in Q12 *
3784 : * *
3785 : * float --> b[3] = {0.893554687, -1.787109375, 0.893554687}; *
3786 : * a[3] = {1.000000000, 1.787109375, -0.864257812}; *
3787 : *-----------------------------------------------------------------------*/
3788 0 : void hp400_12k8_fx(
3789 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
3790 : const Word16 lg, /* i : lenght of signal */
3791 : Word16 mem[] /* i/o: filter memory [6] */
3792 : )
3793 : {
3794 : Word16 i;
3795 : Word16 y1_hi, y1_lo;
3796 : Word32 L_tmp, L_tmp2, L_tmp3;
3797 :
3798 0 : y1_hi = mem[2];
3799 0 : move16();
3800 0 : y1_lo = mem[3];
3801 0 : move16();
3802 :
3803 0 : L_tmp3 = L_mac( 16384L, mem[1], a_hp400_fx[2] ); /* rounding to maximize precision */
3804 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
3805 0 : L_tmp3 = L_shr( L_tmp3, 15 );
3806 0 : L_tmp2 = L_mac( L_tmp3, mem[0], a_hp400_fx[2] );
3807 0 : L_tmp2 = L_mac( L_tmp2, mem[5], b_hp400_fx[2] );
3808 0 : L_tmp2 = L_mac( L_tmp2, mem[4], b_hp400_fx[1] );
3809 0 : L_tmp3 = L_mult( mem[4], b_hp400_fx[2] );
3810 :
3811 0 : mem[5] = signal[lg - 2];
3812 0 : move16();
3813 0 : FOR( i = 1; i < lg; i++ )
3814 : {
3815 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
3816 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
3817 :
3818 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
3819 0 : L_tmp = L_mac( L_tmp, *signal, b_hp400_fx[0] );
3820 :
3821 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
3822 :
3823 0 : L_tmp2 = L_mac( L_tmp3, y1_hi, a_hp400_fx[2] );
3824 0 : L_tmp2 = L_mac( L_tmp2, *signal, b_hp400_fx[1] );
3825 0 : L_tmp3 = L_mac( 16384L, y1_lo, a_hp400_fx[2] ); /* rounding to maximize precision */
3826 :
3827 0 : y1_lo = L_Extract_lc( L_tmp, &y1_hi );
3828 :
3829 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
3830 0 : L_tmp3 = L_shr( L_tmp3, 15 );
3831 :
3832 0 : L_tmp2 = L_add( L_tmp3, L_tmp2 );
3833 :
3834 0 : L_tmp3 = L_mult( *signal, b_hp400_fx[2] );
3835 :
3836 : /* signal is divided by 16 to avoid overflow in energy computation */
3837 0 : *signal++ = round_fx( L_tmp );
3838 0 : move16();
3839 : }
3840 :
3841 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
3842 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
3843 :
3844 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
3845 :
3846 0 : mem[4] = *signal;
3847 0 : move16();
3848 0 : L_tmp = L_mac( L_tmp, mem[4], b_hp400_fx[0] );
3849 :
3850 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
3851 :
3852 0 : mem[0] = y1_hi;
3853 0 : move16();
3854 0 : mem[1] = y1_lo;
3855 0 : move16();
3856 0 : L_Extract( L_tmp, &mem[2], &mem[3] );
3857 :
3858 : /* signal is divided by 16 to avoid overflow in energy computation */
3859 0 : *signal++ = round_fx( L_tmp );
3860 0 : move16();
3861 :
3862 0 : return;
3863 : }
3864 :
3865 59804 : void hp400_12k8_ivas_fx(
3866 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
3867 : const Word16 lg, /* i : lenght of signal */
3868 : Word16 mem[] /* i/o: filter memory [6] */
3869 : )
3870 : {
3871 : Word16 i;
3872 : Word16 x0, x1, x2;
3873 : Word32 L_tmp, yy1, y2;
3874 :
3875 59804 : yy1 = L_Comp( mem[2], mem[3] ); /* Q_syn + 13 */
3876 59804 : y2 = L_Comp( mem[0], mem[1] ); /* Q_syn + 13 */
3877 59804 : x0 = mem[4]; /* Q_syn */
3878 59804 : move16();
3879 59804 : x1 = mem[5]; /* Q_syn */
3880 59804 : move16();
3881 :
3882 3887260 : FOR( i = 0; i < lg; i++ )
3883 : {
3884 3827456 : x2 = x1; /* Q_syn */
3885 3827456 : move16();
3886 3827456 : x1 = x0; /* Q_syn */
3887 3827456 : move16();
3888 3827456 : x0 = signal[i]; /* Q_syn */
3889 3827456 : move16();
3890 :
3891 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)*/
3892 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)*/
3893 3827456 : L_tmp = L_shl( L_tmp, 3 ); /* shifting by 3 to maintain same Q (Q_syn+13) */
3894 :
3895 3827456 : L_tmp = L_mac( L_tmp, x0, b_hp400_fx[0] ); /* Q_syn + 13 */
3896 3827456 : L_tmp = L_mac( L_tmp, x1, b_hp400_fx[1] ); /* Q_syn + 13 */
3897 3827456 : L_tmp = L_mac( L_tmp, x2, b_hp400_fx[2] ); /* Q_syn + 13 */
3898 :
3899 3827456 : y2 = yy1; /* Q_syn + 13 */
3900 3827456 : move32();
3901 3827456 : yy1 = L_tmp; /* Q_syn + 13 */
3902 3827456 : move32();
3903 :
3904 3827456 : signal[i] = round_fx( L_tmp ); /* Q_syn - 3 */
3905 3827456 : move16();
3906 : }
3907 :
3908 59804 : L_Extract( yy1, &mem[2], &mem[3] );
3909 59804 : L_Extract( y2, &mem[0], &mem[1] );
3910 59804 : mem[4] = x0; /* Q_syn */
3911 59804 : mem[5] = x1; /* Q_syn */
3912 59804 : move16();
3913 59804 : move16();
3914 59804 : return;
3915 : }
3916 :
3917 0 : Word16 dot_prod_satcontr( const Word16 *x, const Word16 *y, Word16 qx, Word16 qy, Word16 *qo, Word16 len )
3918 : {
3919 : Word16 tmp_tab_x[L_FRAME16k];
3920 : Word16 tmp_tab_y[L_FRAME16k];
3921 : Word16 shift, q, ener, i;
3922 : Word32 L_tmp;
3923 : Word16 *pt1, *pt2;
3924 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
3925 0 : Flag Overflow = 0;
3926 0 : move32();
3927 : #endif
3928 :
3929 :
3930 0 : Copy( x, tmp_tab_x, len ); /* OPTIMIZE !!!!! the copy into local table is not necessary */
3931 0 : Copy( y, tmp_tab_y, len ); /* could be reworked to do a 1st iteration with the original x[] and y[] */
3932 : /* then check if there is an overflow and do a more complex 2nd, 3rd, ... processing */
3933 0 : shift = 0;
3934 0 : move16();
3935 : BASOP_SATURATE_WARNING_OFF_EVS
3936 : DO
3937 : {
3938 0 : Overflow = 0;
3939 0 : move16();
3940 0 : L_tmp = L_shl_o( 1, s_max( sub( add( add( qx, qy ), 7 ), shift ), 0 ), &Overflow );
3941 0 : pt1 = tmp_tab_x;
3942 0 : pt2 = tmp_tab_y;
3943 0 : FOR( i = 0; i < len; i++ )
3944 : {
3945 0 : L_tmp = L_mac0_o( L_tmp, *pt1++, *pt2++, &Overflow ); /*Q(qx+qy-shift) */
3946 : }
3947 :
3948 0 : IF( Overflow != 0 )
3949 : {
3950 0 : Scale_sig( tmp_tab_x, len, -2 );
3951 0 : Scale_sig( tmp_tab_y, len, -2 );
3952 0 : shift = add( shift, 4 );
3953 : }
3954 : }
3955 0 : WHILE( Overflow != 0 );
3956 : BASOP_SATURATE_WARNING_ON_EVS
3957 :
3958 0 : q = norm_l( L_tmp );
3959 0 : L_tmp = L_shl( L_tmp, q ); /*Q(qx+qy-shift+q) */
3960 0 : ener = extract_h( L_tmp ); /*Q(qx+qy-shift+q-16) */
3961 0 : q = add( q, add( qx, qy ) );
3962 0 : *qo = sub( q, add( shift, 16 ) );
3963 0 : move16();
3964 0 : return ener;
3965 : }
3966 :
3967 :
3968 : /*
3969 : * E_UTIL_f_convolve
3970 : *
3971 : * Parameters:
3972 : * x I: input vector <14bits
3973 : * h I: impulse response (or second input vector) (1Q14)
3974 : * y O: output vetor (result of convolution)
3975 : *
3976 : * Function:
3977 : * Perform the convolution between two vectors x[] and h[] and
3978 : * write the result in the vector y[]. All vectors are of length L.
3979 : * Only the first L samples of the convolution are considered.
3980 : * Vector size = L_SUBFR
3981 : *
3982 : * Returns:
3983 : * void
3984 : */
3985 15542 : void E_UTIL_f_convolve( const Word16 x[], const Word16 h[], Word16 y[], const Word16 size )
3986 : {
3987 : Word16 i, n;
3988 : Word32 L_sum;
3989 : Word64 L64_sum;
3990 :
3991 1010230 : FOR( n = 0; n < size; n++ )
3992 : {
3993 994688 : L64_sum = 0;
3994 994688 : move64();
3995 32327360 : FOR( i = 0; i < n; i++ )
3996 : {
3997 31332672 : L64_sum = W_mac_16_16( L64_sum, x[i], h[n - i] );
3998 : }
3999 994688 : L_sum = W_sat_l( L64_sum );
4000 994688 : y[n] = mac_r( L_sum, x[i], h[0] );
4001 994688 : move16();
4002 : }
4003 15542 : return;
4004 : }
4005 :
4006 : /*-----------------------------------------------------------------------------
4007 : * floating_point_add:
4008 : *
4009 : * Add two floating point numbers: x <- x + y.
4010 : *----------------------------------------------------------------------------*/
4011 23248568 : void floating_point_add(
4012 : Word32 *mx, /* io: mantissa of the addend Q31 */
4013 : Word16 *ex, /* io: exponent of the addend Q0 */
4014 : const Word32 my, /* i: mantissa of the adder Q31 */
4015 : const Word16 ey /* i: exponent of the adder Q0 */
4016 : )
4017 : {
4018 : Word32 accX, accY;
4019 : Word16 align, expo;
4020 : /* NB: This function will not work properly if the mantissa is zero and the exponent is not 32.
4021 : It is up to the caller function to avoid this condition. */
4022 : /* Ensure 1 bit headroom before addition. */
4023 23248568 : accX = L_shr( *mx, 1 );
4024 23248568 : accY = L_shr( my, 1 );
4025 : /* First, align the Q-points of the two operands. Then, add. */
4026 23248568 : align = sub( *ex, ey );
4027 :
4028 23248568 : IF( align < 0 )
4029 : {
4030 22334983 : accX = L_add( accX, L_shl( accY, align ) );
4031 : }
4032 : ELSE
4033 : {
4034 913585 : accX = L_add( accY, L_shr( accX, align ) );
4035 913585 : *ex = ey;
4036 913585 : move16();
4037 : }
4038 : /* Normalize the result and update the mantissa and exponent. */
4039 23248568 : expo = norm_l( accX );
4040 23248568 : *mx = L_shl( accX, expo );
4041 23248568 : *ex = sub( add( *ex, expo ), 1 ); /* Subtract 1 due to 1-bit down-shift above ensuring 1 bit headroom before addition. */
4042 23248568 : move32();
4043 23248568 : move16();
4044 23248568 : return;
4045 : }
4046 :
4047 : /*-------------------------------------------------------------------*
4048 : * delay_signal_fx()
4049 : *
4050 : * Delay buffer by defined number of samples
4051 : *-------------------------------------------------------------------*/
4052 :
4053 906708 : void delay_signal_fx(
4054 : Word16 x[], /* i/o: signal to be delayed */
4055 : const Word16 len, /* i : length of the input signal */
4056 : Word16 mem[], /* i/o: synchronization memory */
4057 : const Word16 delay /* i : delay in samples */
4058 : )
4059 : {
4060 : Word16 tmp_buffer[L_FRAME48k];
4061 :
4062 906708 : Copy( mem, tmp_buffer, delay );
4063 906708 : Copy( x + sub( len, delay ), mem, delay );
4064 906708 : Copy( x, x + delay, sub( len, delay ) );
4065 906708 : Copy( tmp_buffer, x, delay );
4066 :
4067 906708 : return;
4068 : }
4069 :
4070 2370509 : void delay_signal32_fx(
4071 : Word32 x[], /* i/o: signal to be delayed */
4072 : const Word16 len, /* i : length of the input signal */
4073 : Word32 mem[], /* i/o: synchronization memory */
4074 : const Word16 delay /* i : delay in samples */
4075 : )
4076 : {
4077 : Word32 tmp_buffer[L_FRAME48k];
4078 :
4079 2370509 : Copy32( mem, tmp_buffer, delay );
4080 2370509 : Copy32( x + sub( len, delay ), mem, delay );
4081 2370509 : Copy32( x, x + delay, sub( len, delay ) );
4082 2370509 : Copy32( tmp_buffer, x, delay );
4083 :
4084 2370509 : return;
4085 : }
4086 :
4087 53744 : void delay_signal_q_adj_fx(
4088 : Word32 x[], /* i/o: signal to be delayed */
4089 : const Word16 len, /* i : length of the input signal */
4090 : Word32 mem[], /* i/o: synchronization memory */
4091 : const Word16 delay, /* i : delay in samples */
4092 : const Word16 q_x,
4093 : const Word16 q_mem )
4094 : {
4095 :
4096 : Word32 tmp_buffer[L_FRAME48k];
4097 :
4098 53744 : Copy32( mem, tmp_buffer, delay );
4099 53744 : Copy32( x + sub( len, delay ), mem, delay );
4100 53744 : Copy32( x, x + delay, sub( len, delay ) );
4101 :
4102 :
4103 53744 : IF( EQ_16( q_x, q_mem ) )
4104 : {
4105 45812 : Copy32( tmp_buffer, x, delay );
4106 : }
4107 : ELSE
4108 : {
4109 7932 : v_shr( tmp_buffer, sub( q_mem, q_x ), x, delay );
4110 : }
4111 :
4112 53744 : return;
4113 : }
4114 :
4115 24040594 : void v_add_fx(
4116 : const Word32 x1[], /* i : Input vector 1 */
4117 : const Word32 x2[], /* i : Input vector 2 */
4118 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
4119 : const Word16 N /* i : Vector length */
4120 : )
4121 : {
4122 : Word16 i;
4123 :
4124 2938345192 : FOR( i = 0; i < N; i++ )
4125 : {
4126 2914304598 : y[i] = L_add_sat( x1[i], x2[i] );
4127 2914304598 : move32();
4128 : }
4129 :
4130 24040594 : return;
4131 : }
4132 3508 : Word16 floor_log_2( Word32 num )
4133 : {
4134 :
4135 3508 : IF( num == 0 )
4136 : {
4137 0 : return 0;
4138 : }
4139 :
4140 3508 : return ( sub( 30, norm_l( num ) ) );
4141 : }
4142 :
4143 3423311 : void v_shr(
4144 : const Word32 x[], /* i : Input vector */
4145 : const Word16 shift, /* i : Constant */
4146 : Word32 y[], /* o : Output vector that contains x >> shift */
4147 : const Word16 N /* i : Vector length */
4148 : )
4149 : {
4150 : Word16 i;
4151 :
4152 609269709 : FOR( i = 0; i < N; i++ )
4153 : {
4154 605846398 : y[i] = L_shr( x[i], shift );
4155 605846398 : move32();
4156 : }
4157 :
4158 3423311 : return;
4159 : }
4160 :
4161 0 : void v_shr_16(
4162 : const Word16 x[], /* i : Input vector */
4163 : const Word16 shift, /* i : Constant */
4164 : Word16 y[], /* o : Output vector that contains x >> shift */
4165 : const Word16 N /* i : Vector length */
4166 : )
4167 : {
4168 : Word16 i;
4169 :
4170 0 : FOR( i = 0; i < N; i++ )
4171 : {
4172 0 : y[i] = shr( x[i], shift );
4173 0 : move16();
4174 : }
4175 :
4176 0 : return;
4177 : }
4178 :
4179 : /*---------------------------------------------------------------------*
4180 : * lin_interp_fx()
4181 : *
4182 : * Linearly maps x from source range <x1, x2> to the target range <y1, y2>
4183 : *---------------------------------------------------------------------*/
4184 :
4185 : /*! r: mapped output value */
4186 722 : Word16 lin_interp_fx(
4187 : const Word16 x, /* i : Q15 the value to be mapped */
4188 : const Word16 x1, /* i : Q15 source range interval: low end */
4189 : const Word16 y1, /* i : Q15 source range interval: high end */
4190 : const Word16 x2, /* i : Q15 target range interval: low */
4191 : const Word16 y2, /* i : Q15 target range interval: high */
4192 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
4193 : )
4194 : {
4195 722 : IF( sub( x2, x1 ) == 0 )
4196 : {
4197 0 : return y1;
4198 : }
4199 722 : ELSE IF( flag_sat )
4200 : {
4201 722 : IF( GE_16( x, s_max( x1, x2 ) ) )
4202 : {
4203 0 : IF( GT_16( x1, x2 ) )
4204 : {
4205 0 : return y1;
4206 : }
4207 : ELSE
4208 : {
4209 0 : return y2;
4210 : }
4211 : }
4212 722 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
4213 : {
4214 16 : IF( LT_16( x1, x2 ) )
4215 : {
4216 16 : return y1;
4217 : }
4218 : ELSE
4219 : {
4220 0 : return y2;
4221 : }
4222 : }
4223 : }
4224 :
4225 706 : return add_sat( y1, mult( sub( x, x1 ), div_s( sub( y2, y1 ), sub( x2, x1 ) ) ) );
4226 : }
4227 :
4228 :
4229 164705 : Word16 lin_interp_ivas_fx(
4230 : const Word16 x, /* i : Q15 the value to be mapped */
4231 : const Word16 x1, /* i : Q15 source range interval: low end */
4232 : const Word16 y1, /* i : Q15 source range interval: high end */
4233 : const Word16 x2, /* i : Q15 target range interval: low */
4234 : const Word16 y2, /* i : Q15 target range interval: high */
4235 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
4236 : )
4237 : {
4238 164705 : IF( EQ_16( sub( x2, x1 ), 0 ) )
4239 : {
4240 0 : return y1;
4241 : }
4242 164705 : ELSE IF( flag_sat )
4243 : {
4244 161907 : IF( GE_16( x, s_max( x1, x2 ) ) )
4245 : {
4246 776 : return GT_16( x1, x2 ) ? y1 : y2;
4247 : }
4248 161131 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
4249 : {
4250 124361 : return LT_16( x1, x2 ) ? y1 : y2;
4251 : }
4252 : }
4253 : Word16 div_res_e;
4254 39568 : Word16 div_res = BASOP_Util_Divide1616_Scale( sub( y2, y1 ), sub( x2, x1 ), &div_res_e );
4255 : // div_res = shl( div_res, div_res_e );
4256 39568 : return add_sat( y1, round_fx( L_shl( L_mult( sub( x, x1 ), div_res ), div_res_e ) ) );
4257 : }
4258 :
4259 : /*---------------------------------------------------------------------
4260 : * sign_l()
4261 : *
4262 : *---------------------------------------------------------------------*/
4263 :
4264 : /*! r: sign of x (+1/-1) */
4265 56280 : Word32 sign_l(
4266 : const Word32 x /* i : input value of x */
4267 : )
4268 : {
4269 56280 : IF( x < 0 )
4270 : {
4271 26026 : return MIN_32;
4272 : }
4273 : ELSE
4274 : {
4275 30254 : return MAX_32;
4276 : }
4277 : }
4278 :
4279 143 : void v_mult16_fixed(
4280 : const Word16 x1[], /* i : Input vector 1 */
4281 : const Word16 x2[], /* i : Input vector 2 */
4282 : Word16 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4283 : const Word16 N /* i : Vector length */
4284 : )
4285 : {
4286 : Word16 i;
4287 :
4288 10413 : FOR( i = 0; i < N; i++ )
4289 : {
4290 10270 : y[i] = mult_r( x1[i], x2[i] );
4291 10270 : move16();
4292 : }
4293 :
4294 143 : return;
4295 : }
4296 :
4297 : /*---------------------------------------------------------------------*
4298 : * set_zero_fx()
4299 : *
4300 : * Set a vector vec[] of dimension lvec to zero
4301 : *---------------------------------------------------------------------*/
4302 :
4303 222368473 : void set_zero_fx(
4304 : Word32 *vec, /* o : input vector */
4305 : const Word16 lvec /* i : length of the vector */
4306 : )
4307 : {
4308 : Word16 i;
4309 :
4310 26720566291 : FOR( i = 0; i < lvec; i++ )
4311 : {
4312 26498197818 : *vec++ = 0;
4313 26498197818 : move32();
4314 : }
4315 :
4316 222368473 : return;
4317 : }
4318 953 : void set_zero2_fx(
4319 : Word32 *vec, /* o : input vector */
4320 : const Word32 lvec /* i : length of the vector */
4321 : )
4322 : {
4323 : Word32 i;
4324 :
4325 7598393 : FOR( i = 0; i < lvec; i++ )
4326 : {
4327 7597440 : *vec++ = 0;
4328 7597440 : move32();
4329 : }
4330 :
4331 953 : return;
4332 : }
4333 :
4334 6454945 : void set16_zero_fx(
4335 : Word16 *vec, /* o : input vector */
4336 : const Word16 lvec /* i : length of the vector */
4337 : )
4338 : {
4339 : Word16 i;
4340 :
4341 2700035362 : FOR( i = 0; i < lvec; i++ )
4342 : {
4343 2693580417 : *vec++ = 0;
4344 2693580417 : move16();
4345 : }
4346 :
4347 6454945 : return;
4348 : }
4349 :
4350 1468441 : UWord32 mvl2s_r(
4351 : const Word32 x[], /* i : input vector */
4352 : const Word16 q_x,
4353 : Word16 y[], /* o : output vector */
4354 : const Word16 n /* i : vector size */
4355 : )
4356 : {
4357 : Word16 i;
4358 : Word32 temp;
4359 1468441 : UWord32 noClipping = 0;
4360 1468441 : move32();
4361 :
4362 1468441 : IF( n <= 0 )
4363 : {
4364 : /* cannot transfer vectors with size 0 */
4365 158 : return 0;
4366 : }
4367 :
4368 1468283 : IF( (void *) y <= (const void *) x )
4369 : {
4370 44 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
4371 4244 : FOR( i = 0; i < n; i++ )
4372 : {
4373 4200 : temp = L_add( x[i], tempd );
4374 4200 : temp = L_shr( temp, q_x );
4375 :
4376 4200 : IF( GT_32( temp, MAX16B ) )
4377 : {
4378 0 : temp = MAX16B;
4379 0 : move32();
4380 0 : noClipping = L_add( (Word32) noClipping, 1 );
4381 : }
4382 4200 : ELSE IF( LT_32( temp, MIN16B ) )
4383 : {
4384 0 : temp = MIN16B;
4385 0 : move32();
4386 0 : noClipping = L_add( (Word32) noClipping, 1 );
4387 : }
4388 :
4389 4200 : y[i] = extract_l( temp );
4390 4200 : move16();
4391 : }
4392 : }
4393 : ELSE
4394 : {
4395 1468239 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
4396 1172921319 : FOR( i = n - 1; i >= 0; i-- )
4397 : {
4398 1171453080 : temp = L_add( x[i], tempd );
4399 1171453080 : temp = L_shr( temp, q_x );
4400 :
4401 1171453080 : IF( GT_32( temp, MAX16B ) )
4402 : {
4403 238 : temp = MAX16B;
4404 238 : move32();
4405 238 : noClipping = L_add( (Word32) noClipping, 1 );
4406 : }
4407 1171452842 : ELSE IF( LT_32( temp, MIN16B ) )
4408 : {
4409 312 : temp = MIN16B;
4410 312 : move32();
4411 312 : noClipping = L_add( (Word32) noClipping, 1 );
4412 : }
4413 :
4414 1171453080 : y[i] = extract_l( temp );
4415 1171453080 : move16();
4416 : }
4417 : }
4418 :
4419 1468283 : return noClipping;
4420 : }
4421 :
4422 45159360 : Word32 dotp_me_fx(
4423 : const Word32 x[], /* i : vector x[] */
4424 : const Word32 y[], /* i : vector y[] */
4425 : const Word16 n, /* i : vector length */
4426 : Word16 exp_x,
4427 : Word16 exp_y,
4428 : Word16 *exp_suma )
4429 : {
4430 : Word16 i;
4431 : Word32 suma;
4432 : Word32 mul;
4433 45159360 : Word16 mul_exp = add( exp_x, exp_y );
4434 45159360 : suma = Mpy_32_32( x[0], y[0] );
4435 45159360 : *exp_suma = mul_exp;
4436 277730064 : FOR( i = 1; i < n; i++ )
4437 : {
4438 232570704 : mul = Mpy_32_32( x[i], y[i] );
4439 232570704 : suma = BASOP_Util_Add_Mant32Exp( suma, *exp_suma, mul, mul_exp, exp_suma ); // exp_x+exp_A
4440 : }
4441 :
4442 45159360 : return suma;
4443 : }
4444 :
4445 17500 : Word32 dotp_fixed_guarded(
4446 : const Word32 x[], /* i : vector x[] */
4447 : const Word32 y[], /* i : vector y[] */
4448 : const Word16 n /* i : vector length */
4449 : )
4450 : {
4451 : Word16 i;
4452 : Word32 suma;
4453 17500 : Word16 guarded_bits = find_guarded_bits_fx( n );
4454 17500 : suma = L_shr( Mpy_32_32( x[0], y[0] ), guarded_bits );
4455 :
4456 280000 : FOR( i = 1; i < n; i++ )
4457 : {
4458 262500 : suma = L_add( suma, L_shr( Mpy_32_32( x[i], y[i] ), guarded_bits ) );
4459 : }
4460 :
4461 17500 : return suma;
4462 : }
4463 :
4464 :
4465 372 : Word32 dotp_fixed_ivas_fx(
4466 : const Word32 x[], /* i : vector x[] */
4467 : Word16 x_e,
4468 : const Word32 y[], /* i : vector y[] */
4469 : Word16 y_e,
4470 : const Word16 n, /* i : vector length */
4471 : Word16 *out_e )
4472 : {
4473 : Word16 i, exp;
4474 372 : Word32 suma = 0;
4475 :
4476 372 : exp = 31;
4477 372 : move16();
4478 :
4479 118544 : FOR( i = 0; i < n; i++ )
4480 : {
4481 118172 : suma = BASOP_Util_Add_Mant32Exp( suma, exp, Mpy_32_32( x[i], y[i] ), x_e + y_e, &exp );
4482 : }
4483 :
4484 372 : *out_e = exp;
4485 372 : move16();
4486 372 : return suma;
4487 : }
4488 :
4489 : /*-------------------------------------------------------------------*
4490 : * v_mult()
4491 : *
4492 : * Multiplication of two vectors
4493 : *-------------------------------------------------------------------*/
4494 :
4495 :
4496 15305274 : void v_mult_fixed(
4497 : const Word32 x1[], /* i : Input vector 1 */
4498 : const Word32 x2[], /* i : Input vector 2 */
4499 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4500 : const Word16 N /* i : Vector length */
4501 : )
4502 : {
4503 : Word16 i;
4504 :
4505 577006162 : FOR( i = 0; i < N; i++ )
4506 : {
4507 561700888 : y[i] = Mpy_32_32( x1[i], x2[i] );
4508 561700888 : move32();
4509 : }
4510 :
4511 15305274 : return;
4512 : }
4513 :
4514 : /*-------------------------------------------------------------------*
4515 : * anint_fixed()
4516 : *
4517 : * Round to the nearest integer.
4518 : *-------------------------------------------------------------------*/
4519 9840 : Word32 anint_fixed( Word32 x, Word16 exp )
4520 : {
4521 9840 : IF( x == 0 )
4522 : {
4523 888 : return 0;
4524 : }
4525 8952 : IF( x >= 0 )
4526 : {
4527 8952 : return L_add( x, L_shl( 1, sub( exp, 1 ) ) );
4528 : }
4529 : ELSE
4530 : {
4531 0 : return L_sub( x, L_shl( 1, sub( exp, 1 ) ) );
4532 : }
4533 : }
4534 :
4535 : /*-------------------------------------------------------------------*
4536 : * ceil_fixed()
4537 : *
4538 : * Ceil to the next multiple of (1 << exp).
4539 : *-------------------------------------------------------------------*/
4540 25698 : Word32 ceil_fixed( Word32 x, Word16 exp )
4541 : {
4542 : Word32 step;
4543 : // step = x / L_shl( 1, exp );
4544 25698 : step = L_shr( x, exp );
4545 25698 : IF( ( x % L_shl( 1, exp ) ) > 0 )
4546 : {
4547 15523 : step = L_add( step, 1 );
4548 : }
4549 25698 : return L_shl( step, exp );
4550 : }
4551 :
4552 827 : void sort_l(
4553 : Word32 *x, /* i/o: Vector to be sorted */
4554 : Word16 len /* i/o: vector length */
4555 : )
4556 : {
4557 : Word16 i, j;
4558 : Word32 tempr;
4559 :
4560 7479 : FOR( i = len - 2; i >= 0; i-- )
4561 : {
4562 6652 : tempr = x[i];
4563 6652 : move32();
4564 24221 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
4565 : {
4566 17569 : x[j - 1] = x[j];
4567 17569 : move32();
4568 : }
4569 6652 : x[j - 1] = tempr;
4570 6652 : move32();
4571 : }
4572 :
4573 827 : return;
4574 : }
4575 :
4576 : /*-------------------------------------------------------------------*
4577 : * v_add_fixed()
4578 : *
4579 : * Subtraction of two vectors sample by sample
4580 : *-------------------------------------------------------------------*/
4581 :
4582 5105764 : void v_add_fixed(
4583 : const Word32 x1[], /* i : Input vector 1 */
4584 : const Word32 x2[], /* i : Input vector 2 */
4585 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
4586 : const Word16 N, /* i : Vector length */
4587 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
4588 : )
4589 : {
4590 : Word16 i;
4591 :
4592 195587786 : FOR( i = 0; i < N; i++ )
4593 : {
4594 190482022 : y[i] = L_add( L_shr( x1[i], hdrm ), L_shr( x2[i], hdrm ) );
4595 190482022 : move32();
4596 : }
4597 :
4598 5105764 : return;
4599 : }
4600 :
4601 142429052 : void v_add_fixed_no_hdrm(
4602 : const Word32 x1[], /* i : Input vector 1 */
4603 : const Word32 x2[], /* i : Input vector 2 */
4604 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
4605 : const Word16 N /* i : Vector length */
4606 : )
4607 : {
4608 : Word16 i;
4609 :
4610 865355334 : FOR( i = 0; i < N; i++ )
4611 : {
4612 722926282 : y[i] = L_add( x1[i], x2[i] );
4613 722926282 : move32();
4614 : }
4615 :
4616 142429052 : return;
4617 : }
4618 :
4619 7668057 : void v_add_fixed_me(
4620 : const Word32 x1[], /* i : Input vector 1 */
4621 : const Word16 x1_e, /* i : Exponent for input vector 1 */
4622 : const Word32 x2[], /* i : Input vector 2 */
4623 : const Word16 x2_e, /* i : Exponent for input vector 2 */
4624 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
4625 : Word16 *y_e, /* i : Exponent for output vector */
4626 : const Word16 N, /* i : Vector length */
4627 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
4628 : )
4629 : {
4630 : Word16 i;
4631 7668057 : Word16 x1_shift = sub( s_max( x1_e, x2_e ), x1_e );
4632 7668057 : Word16 x2_shift = sub( s_max( x1_e, x2_e ), x2_e );
4633 :
4634 120883264 : FOR( i = 0; i < N; i++ )
4635 : {
4636 113215207 : y[i] = L_add( L_shr( x1[i], hdrm + x1_shift ), L_shr( x2[i], hdrm + x2_shift ) );
4637 113215207 : move32();
4638 : }
4639 :
4640 7668057 : *y_e = add( s_max( x1_e, x2_e ), hdrm );
4641 7668057 : move16();
4642 :
4643 7668057 : return;
4644 : }
4645 :
4646 73973035 : Word16 find_guarded_bits_fx( Word32 n )
4647 : {
4648 : // return n <= 1 ? 0 : n <= 2 ? 1
4649 : // : n <= 4 ? 2
4650 : // : n <= 8 ? 3
4651 : // : n <= 16 ? 4
4652 : // : n <= 32 ? 5
4653 : // : n <= 64 ? 6
4654 : // : n <= 128 ? 7
4655 : // : n <= 256 ? 8
4656 : // : n <= 512 ? 9
4657 : // : n <= 1024 ? 10
4658 : // : n <= 2048 ? 11
4659 : // : n <= 4096 ? 12
4660 : // : n <= 8192 ? 13
4661 : // : n <= 16384 ? 14
4662 : // : 15;
4663 : /*Word16 val = 0;
4664 : move32();
4665 : test();
4666 : WHILE( GT_32( n, L_shl( 1, val ) ) && LT_32( val, 16 ) )
4667 : {
4668 : val = add( val, 1 );
4669 : }*/
4670 73973035 : IF( LE_32( n, 1 ) )
4671 : {
4672 5539285 : return 0;
4673 : }
4674 : ELSE
4675 : {
4676 :
4677 68433750 : return sub( 31, norm_l( L_sub( n, 1 ) ) );
4678 : }
4679 : }
4680 :
4681 621725492 : Word16 L_norm_arr( const Word32 *arr, Word16 size )
4682 : {
4683 621725492 : Word16 q = 31;
4684 621725492 : move16();
4685 31731214844 : FOR( Word16 i = 0; i < size; i++ )
4686 : {
4687 : Word16 q_tst;
4688 :
4689 31109489352 : q_tst = norm_l( arr[i] );
4690 31109489352 : if ( arr[i] != 0 )
4691 : {
4692 25265803721 : q = s_min( q, q_tst );
4693 : }
4694 : }
4695 :
4696 621725492 : return q;
4697 : }
4698 :
4699 19556312 : Word16 norm_arr( Word16 *arr, Word16 size )
4700 : {
4701 19556312 : Word16 q = 15;
4702 19556312 : Word16 exp = 0;
4703 19556312 : move16();
4704 19556312 : move16();
4705 7705399108 : FOR( Word16 i = 0; i < size; i++ )
4706 : {
4707 7685842796 : if ( arr[i] != 0 )
4708 : {
4709 6436697331 : exp = norm_s( arr[i] );
4710 : }
4711 7685842796 : if ( arr[i] != 0 )
4712 : {
4713 6436697331 : q = s_min( q, exp );
4714 : }
4715 : }
4716 19556312 : return q;
4717 : }
4718 :
4719 1276480 : Word16 W_norm_arr( Word64 *arr, Word16 size )
4720 : {
4721 1276480 : Word16 q = 63;
4722 1276480 : Word16 exp = 0;
4723 1276480 : move16();
4724 1276480 : move16();
4725 524043680 : FOR( Word16 i = 0; i < size; i++ )
4726 : {
4727 522767200 : if ( arr[i] != 0 )
4728 : {
4729 511580357 : exp = W_norm( arr[i] );
4730 : }
4731 522767200 : if ( arr[i] != 0 )
4732 : {
4733 511580357 : q = s_min( q, exp );
4734 : }
4735 : }
4736 1276480 : return q;
4737 : }
4738 :
4739 263501550 : Word16 get_min_scalefactor( Word32 x, Word32 y )
4740 : {
4741 : Word16 scf_y;
4742 263501550 : Word16 scf = Q31;
4743 263501550 : move16();
4744 :
4745 263501550 : test();
4746 263501550 : if ( x == 0 && y == 0 )
4747 : {
4748 19399677 : scf = 0;
4749 19399677 : move16();
4750 : }
4751 :
4752 263501550 : if ( x != 0 )
4753 : {
4754 244040036 : scf = norm_l( x );
4755 : }
4756 :
4757 263501550 : scf_y = norm_l( y );
4758 263501550 : if ( y != 0 )
4759 : {
4760 187973697 : scf = s_min( scf_y, scf );
4761 : }
4762 :
4763 263501550 : return scf;
4764 : }
4765 :
4766 :
4767 348006900 : Flag is_zero_arr( Word32 *arr, Word16 size )
4768 : {
4769 630871922 : FOR( Word16 i = 0; i < size; i++ )
4770 569463441 : IF( arr[i] != 0 )
4771 : {
4772 286598419 : return 0;
4773 : }
4774 :
4775 61408481 : return 1;
4776 : }
4777 :
4778 1853117 : Flag is_zero_arr16( Word16 *arr, Word16 size )
4779 : {
4780 254226848 : FOR( Word16 i = 0; i < size; i++ )
4781 253994737 : IF( arr[i] != 0 )
4782 : {
4783 1621006 : return 0;
4784 : }
4785 :
4786 232111 : return 1;
4787 : }
4788 :
4789 0 : Flag is_zero_arr64( Word64 *arr, Word16 size )
4790 : {
4791 0 : FOR( Word16 i = 0; i < size; i++ )
4792 : {
4793 0 : IF( arr[i] != 0 )
4794 : {
4795 0 : return 0;
4796 : }
4797 : }
4798 0 : return 1;
4799 : }
4800 :
4801 0 : void Scale_sig64(
4802 : Word64 x[], /* i/o: signal to scale Qx */
4803 : Word16 len, /* i : size of x[] Q0 */
4804 : Word16 exp /* i : exponent: x = round(x << exp) Qx exp */
4805 : )
4806 : {
4807 : Word16 i;
4808 0 : assert( exp <= 63 && exp >= -63 );
4809 0 : IF( exp == 0 )
4810 : {
4811 0 : return;
4812 : }
4813 :
4814 0 : FOR( i = 0; i < len; i++ )
4815 : {
4816 : /* saturation can occur here */
4817 0 : x[i] = W_shl( x[i], exp );
4818 0 : move64();
4819 : }
4820 : }
|
