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