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