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