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 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
35 : ====================================================================================*/
36 :
37 : #include <stdlib.h>
38 : #include <assert.h>
39 : #include <stdint.h>
40 : #include "options.h"
41 : #include "stl.h"
42 : #include <math.h>
43 : #include "cnst.h"
44 : #include "prot_fx.h"
45 : #include "basop_util.h"
46 : #include "basop32.h"
47 : #include "wmc_auto.h"
48 : #include "prot_fx_enc.h"
49 : #include "ivas_prot_fx.h"
50 :
51 :
52 : #define INV_BANDS10 3277 /* 1/10 in Q15 */
53 : #define INV_BANDS9 3641 /* 1/9 in Q15 */
54 : #define INV_BANDS3 10923 /* 1/9 in Q15 */
55 : static const Word16 b_hp400_fx[3] = { 3660, -7320, 3660 }; /* Q12 (/4) */
56 : static const Word16 a_hp400_fx[3] = { 16384, 29280, -14160 };
57 : static const Word16 a_hp400_ivas_fx[3] = { 4096, 7320, -3540 }; /*Q12*/
58 :
59 : /*------------------------------------------------------------------*
60 : * own_random()
61 : *
62 : * Random generator
63 : *------------------------------------------------------------------*/
64 :
65 : /*! r: output random value */
66 290926435 : Word16 own_random(
67 : Word16 *seed /* i/o: random seed */
68 : )
69 : {
70 290926435 : *seed = (Word16) ( *seed * 31821L + 13849L );
71 :
72 290926435 : return ( *seed );
73 : }
74 :
75 : /*---------------------------------------------------------------------
76 : * norm_ul_float()
77 : *
78 : *---------------------------------------------------------------------*/
79 :
80 0 : Word16 norm_ul_float( UWord32 UL_var1 )
81 : {
82 : Word16 var_out;
83 :
84 0 : if ( UL_var1 == 0 )
85 : {
86 0 : var_out = 0;
87 : }
88 : else
89 : {
90 0 : for ( var_out = 0; UL_var1 < (UWord32) 0x80000000U; var_out++ )
91 : {
92 0 : UL_var1 <<= 1;
93 : }
94 : }
95 : BASOP_CHECK();
96 :
97 0 : return ( var_out );
98 : }
99 :
100 :
101 : /*---------------------------------------------------------------------
102 : * sum_s()
103 : * sum_l()
104 : *
105 : *---------------------------------------------------------------------*/
106 :
107 : /*! r: sum of all vector elements */
108 363083 : Word16 sum_s(
109 : const Word16 *vec, /* i : input vector */
110 : const Word16 lvec /* i : length of input vector */
111 : )
112 : {
113 : Word16 i;
114 : Word16 tmp;
115 :
116 363083 : tmp = 0;
117 8567097 : for ( i = 0; i < lvec; i++ )
118 : {
119 8204014 : tmp += vec[i];
120 : }
121 :
122 363083 : return tmp;
123 : }
124 :
125 : /*! r: sum of all vector elements */
126 6696 : Word32 sum_l_fx(
127 : const Word32 *vec, /* i : input vector */
128 : const Word16 lvec /* i : length of input vector */
129 : )
130 : {
131 : Word16 i;
132 : Word32 tmpL;
133 :
134 6696 : tmpL = 0;
135 6696 : move32();
136 80352 : FOR( i = 0; i < lvec; i++ )
137 : {
138 73656 : tmpL = L_add( tmpL, vec[i] );
139 : }
140 :
141 6696 : return tmpL;
142 : }
143 :
144 : /*----------------------------------------------------------------------
145 : * sum2_f()
146 : *
147 : *---------------------------------------------------------------------*/
148 :
149 : /*! r: sum of all squared vector elements */
150 870 : Word32 sum2_f_16_fx(
151 : const Word16 *vec, /* i : input vector */
152 : const Word16 lvec /* i : length of input vector */
153 : )
154 : {
155 : Word16 i;
156 : Word32 tmp;
157 :
158 870 : tmp = 0;
159 870 : move32();
160 14790 : FOR( i = 0; i < lvec; i++ )
161 : {
162 13920 : tmp = L_add( tmp, L_mult0( vec[i], vec[i] ) );
163 : }
164 :
165 870 : return tmp;
166 : }
167 148 : Word32 sum2_f_16_gb_fx(
168 : const Word16 *vec, /* i : input vector */
169 : const Word16 lvec, /* i : length of input vector */
170 : Word16 gb )
171 : {
172 : Word16 i;
173 : Word32 tmp;
174 :
175 148 : tmp = 0;
176 96148 : FOR( i = 0; i < lvec; i++ )
177 : {
178 96000 : tmp = L_add( tmp, L_shr( L_mult0( vec[i], vec[i] ), gb ) );
179 : }
180 :
181 148 : return tmp;
182 : }
183 :
184 :
185 35493 : Word32 sum2_16_exp_fx(
186 : const Word16 *vec, /* i : input vector Q(15 - exp) */
187 : const Word16 lvec, /* i : length of input vector */
188 : Word16 *exp, /* i/o: exponent of vector */
189 : Word16 gb /* i : guard bits */
190 : )
191 : {
192 : Word16 i, s;
193 : Word32 L_tmp, var_a;
194 :
195 35493 : L_tmp = 0;
196 35493 : move32();
197 35493 : var_a = 0;
198 35493 : move32();
199 6534913 : FOR( i = 0; i < lvec; i++ )
200 : {
201 6499420 : var_a = L_mult0( vec[i], vec[i] ); /* 2 * Q(15 - exp) */
202 6499420 : L_tmp = L_add( L_tmp, L_shr( var_a, gb ) ); /* 2 * Q(15 - exp) - gb */
203 : }
204 :
205 35493 : s = norm_l( L_tmp );
206 35493 : L_tmp = L_shl( L_tmp, s ); /* 2 * Q(15 - exp) - gb + s */
207 :
208 35493 : *exp = add( sub( add( shl( *exp, 1 ), gb ), s ), 1 );
209 35493 : move16();
210 :
211 35493 : return L_tmp;
212 : }
213 :
214 :
215 8458959 : Word32 sum2_32_exp_fx(
216 : const Word32 *vec, /* i : input vector, Qx */
217 : const Word16 lvec, /* i : length of input vector */
218 : Word16 *exp, /* i/o: exponent of vector */
219 : Word16 gb /* i : guard bits */
220 : )
221 : {
222 : Word16 i, s, norm;
223 : Word64 W_tmp;
224 :
225 8458959 : W_tmp = 0;
226 8458959 : Word64 var_a = 0;
227 8458959 : move64();
228 8458959 : move64();
229 :
230 8458959 : norm = L_norm_arr( vec, lvec );
231 :
232 8458959 : gb = sub( gb, norm );
233 :
234 1766800949 : FOR( i = 0; i < lvec; i++ )
235 : {
236 1758341990 : var_a = W_mult0_32_32( vec[i], vec[i] ); // 2x
237 1758341990 : W_tmp = W_add( W_tmp, W_shr( var_a, gb ) ); // 2x-gb
238 : }
239 :
240 8458959 : s = W_norm( W_tmp );
241 8458959 : W_tmp = W_shl( W_tmp, s ); // 2x - gb + s
242 :
243 : //*exp = 31 - (2*(31 - *exp) - gb + s) + 32;
244 8458959 : *exp = add( sub( add( shl( *exp, 1 ), gb ), s ), 1 );
245 8458959 : move16();
246 :
247 8458959 : return W_extract_h( W_tmp );
248 : }
249 :
250 :
251 : /* o : Q(2x - 31 - gb) */
252 8369773 : Word32 sum2_f_32_fx(
253 : const Word32 *vec, /* i : input vector, Qx */
254 : const Word16 lvec, /* i : length of input vector */
255 : Word16 gb /* i : guard bits */
256 : )
257 : {
258 : Word16 i;
259 : Word32 tmp;
260 :
261 8369773 : tmp = 0;
262 8369773 : Word32 var_a = 0;
263 8369773 : move32();
264 8369773 : move32();
265 85217098 : FOR( i = 0; i < lvec; i++ )
266 : {
267 76847325 : var_a = Mpy_32_32( vec[i], vec[i] ); // 2x-31
268 76847325 : tmp = L_add( tmp, L_shr( var_a, gb ) ); // 2x-31-gb
269 : }
270 :
271 8369773 : return tmp;
272 : }
273 :
274 1953968 : Word32 sum2_32_fx(
275 : const Word32 *vec, /* i : input vector */
276 : const Word16 lvec, /* i : length of input vector */
277 : Word16 *e )
278 : {
279 : Word16 i;
280 : Word32 tmp;
281 :
282 1953968 : tmp = 0;
283 1953968 : Word32 var_a = 0;
284 1953968 : Word16 exp = 0, exp_tmp;
285 1953968 : move32();
286 1953968 : move32();
287 1953968 : move16();
288 150546504 : FOR( i = 0; i < lvec; i++ )
289 : {
290 148592536 : exp_tmp = norm_l( vec[i] );
291 148592536 : var_a = L_shl( vec[i], exp_tmp );
292 148592536 : var_a = Mpy_32_32( var_a, var_a );
293 148592536 : exp_tmp = shl( sub( *e, exp_tmp ), 1 );
294 148592536 : tmp = BASOP_Util_Add_Mant32Exp( tmp, exp, var_a, exp_tmp, &exp );
295 : }
296 1953968 : *e = exp;
297 1953968 : move16();
298 :
299 1953968 : return tmp;
300 : }
301 :
302 :
303 : /*-------------------------------------------------------------------*
304 : * set_c()
305 : * set_s()
306 : * set_l()
307 : * set_d()
308 : *
309 : * Set the vector elements to a value
310 : *-------------------------------------------------------------------*/
311 :
312 120587 : void set_c(
313 : Word8 y[], /* i/o: Vector to set */
314 : const Word8 a, /* i : Value to set the vector to */
315 : const Word32 N /* i : Length of the vector */
316 : )
317 : {
318 : Word16 i;
319 :
320 1517455 : for ( i = 0; i < N; i++ )
321 : {
322 1396868 : y[i] = a;
323 : }
324 :
325 120587 : return;
326 : }
327 :
328 :
329 2590696 : void set_s(
330 : Word16 y[], /* i/o: Vector to set */
331 : const Word16 a, /* i : Value to set the vector to */
332 : const Word16 N /* i : Length of the vector */
333 : )
334 : {
335 : Word16 i;
336 :
337 30287450 : for ( i = 0; i < N; i++ )
338 : {
339 27696754 : y[i] = a;
340 : }
341 :
342 2590696 : return;
343 : }
344 :
345 :
346 9480 : void set_l(
347 : Word32 y[], /* i/o: Vector to set */
348 : const Word32 a, /* i : Value to set the vector to */
349 : const Word16 N /* i : Length of the vector */
350 : )
351 : {
352 : Word16 i;
353 :
354 578280 : for ( i = 0; i < N; i++ )
355 : {
356 568800 : y[i] = a;
357 : }
358 :
359 9480 : return;
360 : }
361 :
362 : /*---------------------------------------------------------------------*
363 : * set_zero()
364 : *
365 : * Set a vector vec[] of dimension lvec to zero
366 : *---------------------------------------------------------------------*/
367 :
368 0 : void set_zero(
369 : float *vec, /* o : input vector */
370 : const Word16 lvec /* i : length of the vector */
371 : )
372 : {
373 : Word16 i;
374 :
375 0 : for ( i = 0; i < lvec; i++ )
376 : {
377 0 : *vec++ = 0.0f;
378 : }
379 :
380 0 : return;
381 : }
382 :
383 :
384 : /*---------------------------------------------------------------------*
385 : * mvr2r()
386 : * mvs2s()
387 : * mvr2d()
388 : * mvd2r()
389 : *
390 : * Transfer the contents of vector x[] to vector y[]
391 : *---------------------------------------------------------------------*/
392 :
393 0 : void mvr2r(
394 : const float x[], /* i : input vector */
395 : float y[], /* o : output vector */
396 : const Word16 n /* i : vector size */
397 : )
398 : {
399 : Word16 i;
400 :
401 0 : if ( n <= 0 )
402 : {
403 : /* cannot transfer vectors with size 0 */
404 0 : return;
405 : }
406 :
407 0 : if ( y < x )
408 : {
409 0 : for ( i = 0; i < n; i++ )
410 : {
411 0 : y[i] = x[i];
412 : }
413 : }
414 : else
415 : {
416 0 : for ( i = n - 1; i >= 0; i-- )
417 : {
418 0 : y[i] = x[i];
419 : }
420 : }
421 :
422 0 : return;
423 : }
424 :
425 1155051 : void mvs2s(
426 : const Word16 x[], /* i : input vector */
427 : Word16 y[], /* o : output vector */
428 : const Word16 n /* i : vector size */
429 : )
430 : {
431 : Word16 i;
432 :
433 1155051 : if ( n <= 0 )
434 : {
435 : /* cannot transfer vectors with size 0 */
436 0 : return;
437 : }
438 :
439 1155051 : if ( y < x )
440 : {
441 0 : for ( i = 0; i < n; i++ )
442 : {
443 0 : y[i] = x[i];
444 : }
445 : }
446 : else
447 : {
448 8085060 : for ( i = n - 1; i >= 0; i-- )
449 : {
450 6930009 : y[i] = x[i];
451 : }
452 : }
453 :
454 1155051 : return;
455 : }
456 :
457 120 : void mvl2l(
458 : const Word32 x[], /* i : input vector */
459 : Word32 y[], /* o : output vector */
460 : const Word16 n /* i : vector size */
461 : )
462 : {
463 : Word16 i;
464 :
465 120 : if ( n <= 0 )
466 : {
467 : /* no need to transfer vectors with size 0 */
468 0 : return;
469 : }
470 :
471 120 : if ( y < x )
472 : {
473 38480 : for ( i = 0; i < n; i++ )
474 : {
475 38400 : y[i] = x[i];
476 : }
477 : }
478 : else
479 : {
480 38440 : for ( i = n - 1; i >= 0; i-- )
481 : {
482 38400 : y[i] = x[i];
483 : }
484 : }
485 :
486 120 : return;
487 : }
488 :
489 : /*! r: index of the maximum value in the input vector */
490 639506 : Word16 maximum_s(
491 : const Word16 *vec, /* i : input vector */
492 : const Word16 lvec, /* i : length of input vector */
493 : Word16 *max /* o : maximum value in the input vector */
494 : )
495 : {
496 : Word16 i, ind;
497 : Word16 tmp;
498 :
499 639506 : ind = 0;
500 639506 : move16();
501 639506 : tmp = vec[0];
502 639506 : move16();
503 :
504 6467653 : FOR( i = 1; i < lvec; i++ )
505 : {
506 5828147 : IF( GT_16( vec[i], tmp ) )
507 : {
508 210939 : ind = i;
509 210939 : move16();
510 210939 : tmp = vec[i];
511 210939 : move16();
512 : }
513 : }
514 :
515 639506 : if ( max != NULL )
516 : {
517 632892 : *max = tmp;
518 632892 : move16();
519 : }
520 :
521 639506 : return ind;
522 : }
523 :
524 : /*! r: index of the maximum value in the input vector */
525 255361 : Word16 maximum_l(
526 : const Word32 *vec, /* i : input vector */
527 : const Word16 lvec, /* i : length of input vector */
528 : Word32 *max_val /* o : maximum value in the input vector */
529 : )
530 : {
531 : Word16 i, ind;
532 : Word32 tmp;
533 :
534 255361 : ind = 0;
535 255361 : tmp = vec[0];
536 255361 : move16();
537 255361 : move32();
538 68260030 : FOR( i = 1; i < lvec; i++ )
539 : {
540 68004669 : IF( GT_32( vec[i], tmp ) )
541 : {
542 1338149 : ind = i;
543 1338149 : tmp = vec[i];
544 1338149 : move16();
545 1338149 : move32();
546 : }
547 : }
548 :
549 255361 : if ( max_val != NULL )
550 : {
551 255361 : *max_val = tmp;
552 255361 : move32();
553 : }
554 :
555 255361 : return ind;
556 : }
557 :
558 : /*! r: index of the maximum value in the input vector */
559 1613022 : Word16 maximumAbs_l(
560 : const Word32 *vec, /* i : input vector */
561 : const Word16 lvec, /* i : length of input vector */
562 : Word32 *max_val /* o : maximum value in the input vector */
563 : )
564 : {
565 : Word16 j, ind;
566 : Word32 tmp;
567 :
568 1613022 : ind = 0;
569 1613022 : move16();
570 1613022 : tmp = L_abs( vec[0] );
571 :
572 59469555 : FOR( j = 1; j < lvec; j++ )
573 : {
574 57856533 : IF( GT_32( L_abs( vec[j] ), tmp ) )
575 : {
576 5478281 : ind = j;
577 5478281 : move16();
578 5478281 : tmp = L_abs( vec[j] );
579 : }
580 : }
581 :
582 1613022 : IF( max_val != NULL )
583 : {
584 1613022 : *max_val = tmp;
585 1613022 : move32();
586 : }
587 :
588 1613022 : return ind;
589 : }
590 :
591 : /*-------------------------------------------------------------------*
592 : * minimum_s()
593 : *
594 : * Finds minimum 16-bit signed integer value in the array and returns it.
595 : *-------------------------------------------------------------------*/
596 :
597 : /*! r: index of the minimum value in the input vector */
598 93671000 : Word16 minimum_s(
599 : const Word16 *vec, /* i : Input vector */
600 : const Word16 lvec, /* i : Vector length */
601 : Word16 *min_val /* o : minimum value in the input vector */
602 : )
603 : {
604 : Word16 i, ind;
605 :
606 93671000 : ind = 0;
607 93671000 : move16();
608 :
609 445246493 : FOR( i = 1; i < lvec; i++ )
610 : {
611 351575493 : if ( LT_16( vec[i], vec[ind] ) )
612 : {
613 32635461 : ind = i;
614 32635461 : move16();
615 : }
616 : }
617 :
618 93671000 : if ( min_val != NULL )
619 : {
620 93671000 : *min_val = vec[ind];
621 93671000 : move16();
622 : }
623 :
624 93671000 : return ind;
625 : }
626 :
627 : /*-------------------------------------------------------------------*
628 : * minimum_l()
629 : *
630 : * Finds minimum 16-bit signed integer value in the array and returns it.
631 : *-------------------------------------------------------------------*/
632 :
633 : /*! r: index of the minimum value in the input vector */
634 158648 : Word16 minimum_l(
635 : const Word32 *vec, /* i : Input vector */
636 : const Word16 lvec, /* i : Vector length */
637 : Word32 *min_val /* o : minimum value in the input vector */
638 : )
639 : {
640 : Word16 i, ind;
641 :
642 158648 : ind = 0;
643 158648 : move16();
644 :
645 1993396 : FOR( i = 1; i < lvec; i++ )
646 : {
647 1834748 : if ( LT_32( vec[i], vec[ind] ) )
648 : {
649 388434 : ind = i;
650 388434 : move16();
651 : }
652 : }
653 :
654 158648 : if ( min_val != NULL )
655 : {
656 158648 : *min_val = vec[ind];
657 158648 : move32();
658 : }
659 :
660 158648 : return ind;
661 : }
662 :
663 : /*---------------------------------------------------------------------*
664 : * dotp()
665 : *
666 : * Dot product of vector x[] and vector y[]
667 : *---------------------------------------------------------------------*/
668 :
669 : /*! r: dot product of x[] and y[] */
670 14585942 : Word32 dotp_fx32(
671 : const Word32 x[], /* i : vector x[] Qx */
672 : const Word32 y[], /* i : vector y[] Qy */
673 : const Word16 n /* i : vector length */
674 : )
675 : {
676 : Word16 i;
677 : Word32 suma;
678 :
679 14585942 : suma = Mpy_32_32( x[0], y[0] );
680 :
681 38507788 : FOR( i = 1; i < n; i++ )
682 : {
683 23921846 : suma = L_add( suma, Mpy_32_32( x[i], y[i] ) );
684 : }
685 :
686 14585942 : return suma;
687 : }
688 :
689 : /*To calculate dot product of two 32 bit arrays in case of overflow*/
690 2972939 : Word32 dotp_fx32_o(
691 : const Word32 x[], /* i : vector x[] */
692 : const Word32 y[], /* i : vector y[] */
693 : const Word16 n, /* i : vector length */
694 : const Word16 log_len, /* i : max factor added to result q after dot product (equal to log2(n)) */
695 : Word16 *res_q /*stores resultant Q*/
696 : )
697 : {
698 : Word16 i;
699 : Word64 suma; /*resultant q= q(x)+q(y)-9-x such that q<=31*/
700 :
701 2972939 : suma = W_shr( W_mult_32_32( x[0], y[0] ), log_len );
702 :
703 154003334 : FOR( i = 1; i < n; i++ )
704 : {
705 151030395 : suma = W_add( suma, W_shr( W_mult_32_32( x[i], y[i] ), log_len ) );
706 : }
707 2972939 : *res_q = add( sub( *res_q, log_len ), 1 );
708 2972939 : move16();
709 2972939 : test();
710 2972939 : test();
711 35811161 : FOR( ; ( suma > MAX_32 ) || ( suma < MIN_32 ) || ( *res_q > 31 ); )
712 : {
713 32838222 : suma = W_shr( suma, 1 );
714 32838222 : *res_q = sub( *res_q, 1 );
715 32838222 : move16();
716 : }
717 :
718 2972939 : return W_extract_l( suma );
719 : }
720 :
721 :
722 1503192 : Word32 dotp_fx32_fac(
723 : const Word32 x[], /* i : vector x[] */
724 : const Word32 y[], /* i : vector y[] */
725 : const Word16 n, /* i : vector length */
726 : const Word16 log_len, /* i : max factor added to result q after dot product (equal to log2(n)) */
727 : Word16 *res_q /*stores resultant Q*/
728 : )
729 : {
730 : Word16 i;
731 : Word64 suma; /*resultant q= q(x)+q(y)-9-x such that q<=31*/
732 :
733 1503192 : suma = W_shr( W_mult_32_32( x[0], y[0] ), log_len );
734 :
735 1294874880 : FOR( i = 1; i < n; i++ )
736 : {
737 1293371688 : suma = W_add( suma, W_shr( W_mult_32_32( x[i], y[i] ), log_len ) );
738 : }
739 1503192 : *res_q = add( *res_q, add( sub( *res_q, log_len ), 1 ) );
740 1503192 : move16();
741 1503192 : test();
742 1503192 : test();
743 31212389 : FOR( ; ( suma > MAX_32 ) || ( suma < MIN_32 ) || ( *res_q > 31 ); )
744 : {
745 29709197 : suma = W_shr( suma, 1 );
746 29709197 : *res_q = sub( *res_q, 1 );
747 29709197 : move16();
748 : }
749 1503192 : return W_extract_l( suma );
750 : }
751 :
752 :
753 : /*-------------------------------------------------------------------*
754 : * v_add_w64()
755 : *
756 : * Subtraction of two vectors sample by sample
757 : *-------------------------------------------------------------------*/
758 :
759 0 : void v_add_w64(
760 : const Word64 x1[], /* i : Input vector 1 */
761 : const Word64 x2[], /* i : Input vector 2 */
762 : Word64 y[], /* o : Output vector that contains vector 1 - vector 2 */
763 : const Word16 N, /* i : Vector length */
764 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
765 : )
766 : {
767 : Word16 i;
768 :
769 0 : FOR( i = 0; i < N; i++ )
770 : {
771 0 : y[i] = W_add( W_shr( x1[i], hdrm ), W_shr( x2[i], hdrm ) );
772 0 : move64();
773 : }
774 :
775 0 : return;
776 : }
777 :
778 :
779 : /*-------------------------------------------------------------------*
780 : * v_sub_fx()
781 : *
782 : * Subtraction of two vectors sample by sample
783 : *-------------------------------------------------------------------*/
784 :
785 7085359 : void v_sub_fx(
786 : const Word32 x1[], /* i : Input vector 1 */
787 : const Word32 x2[], /* i : Input vector 2 */
788 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
789 : const Word16 N, /* i : Vector length */
790 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
791 : )
792 : {
793 : Word16 i;
794 :
795 28341436 : FOR( i = 0; i < N; i++ )
796 : {
797 21256077 : y[i] = L_sub( L_shr( x1[i], hdrm ), L_shr( x2[i], hdrm ) );
798 21256077 : move32();
799 : }
800 :
801 7085359 : return;
802 : }
803 :
804 549974223 : void v_sub_fx_no_hdrm(
805 : const Word32 x1[], /* i : Input vector 1 */
806 : const Word32 x2[], /* i : Input vector 2 */
807 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
808 : const Word16 N /* i : Vector length */
809 : )
810 : {
811 : Word16 i;
812 :
813 2655791574 : FOR( i = 0; i < N; i++ )
814 : {
815 2105817351 : y[i] = L_sub( x1[i], x2[i] );
816 2105817351 : move32();
817 : }
818 :
819 549974223 : return;
820 : }
821 :
822 : /*-------------------------------------------------------------------*
823 : * v_multc()
824 : *
825 : * Multiplication of vector by constant
826 : *-------------------------------------------------------------------*/
827 :
828 52675958 : void v_multc_fx(
829 : const Word32 x[], /* i : Input vector */
830 : const Word32 c, /* i : Constant */
831 : Word32 y[], /* o : Output vector that contains c*x */
832 : const Word16 N /* i : Vector length */
833 : )
834 : {
835 : Word16 i;
836 :
837 6650332096 : FOR( i = 0; i < N; i++ )
838 : {
839 6597656138 : y[i] = Mpy_32_32( c, x[i] );
840 6597656138 : move32();
841 : }
842 :
843 52675958 : return;
844 : }
845 :
846 1862530 : void v_multc_fx_16(
847 : const Word32 x[], /* i : Input vector */
848 : const Word16 c, /* i : Constant */
849 : Word32 y[], /* o : Output vector that contains c*x */
850 : const Word16 N /* i : Vector length */
851 : )
852 : {
853 : Word16 i;
854 :
855 208477974 : FOR( i = 0; i < N; i++ )
856 : {
857 206615444 : y[i] = Mpy_32_16_1( x[i], c );
858 206615444 : move32();
859 : }
860 :
861 1862530 : return;
862 : }
863 :
864 12194 : void v_multc_fx_16_16(
865 : const Word16 x[], /* i : Input vector */
866 : const Word16 c, /* i : Constant */
867 : Word16 y[], /* o : Output vector that contains c*x */
868 : const Word16 N /* i : Vector length */
869 : )
870 : {
871 : Word16 i;
872 :
873 5050322 : FOR( i = 0; i < N; i++ )
874 : {
875 5038128 : y[i] = mult_r( x[i], c );
876 5038128 : move16();
877 : }
878 :
879 12194 : return;
880 : }
881 :
882 : /*-------------------------------------------------------------------*
883 : * usdequant()
884 : *
885 : * Uniform scalar de-quantizer routine
886 : *
887 : * Applies de-quantization based on scale and round operations.
888 : *-------------------------------------------------------------------*/
889 :
890 0 : float usdequant(
891 : const Word16 idx, /* i : quantizer index */
892 : const float qlow, /* i : lowest codebook entry (index 0) */
893 : const float delta /* i : quantization step */
894 : )
895 : {
896 : float g;
897 :
898 0 : g = idx * delta + qlow;
899 :
900 0 : return ( g );
901 : }
902 :
903 896 : void sort(
904 : UWord16 *x, /* i/o: Vector to be sorted */
905 : UWord16 len /* i/o: vector length */
906 : )
907 : {
908 : Word16 i;
909 : UWord16 j, tempr;
910 :
911 5190 : FOR( i = len - 2; i >= 0; i-- )
912 : {
913 4294 : tempr = x[i];
914 4294 : move16();
915 9299 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
916 : {
917 5005 : test();
918 5005 : x[j - 1] = x[j];
919 5005 : move16();
920 : }
921 4294 : x[j - 1] = tempr;
922 4294 : move16();
923 : }
924 :
925 896 : return;
926 : }
927 :
928 : #define WMC_TOOL_SKIP
929 :
930 : // conversion functions:
931 467696 : Word32 float_to_fix( float number, Word32 Q )
932 : {
933 467696 : assert( Q >= 0 );
934 467696 : if ( number == 1.0f && Q == Q31 )
935 : {
936 0 : return ONE_IN_Q31;
937 : }
938 467696 : if ( isnan( number ) )
939 : {
940 0 : number = 0;
941 : }
942 467696 : assert( fabs( number ) < pow( 2, 31 - Q ) );
943 467696 : Word32 ret = (Word32) ( number * ( (UWord32) 1 << Q ) );
944 467696 : return ret;
945 : }
946 :
947 0 : float fix_to_float(
948 : Word32 number,
949 : Word32 Q )
950 : {
951 0 : assert( Q >= 0 );
952 0 : float ret = (float) number / ( (UWord32) 1 << Q );
953 0 : return ret;
954 : }
955 :
956 0 : Word16 float_to_fix16(
957 : float number,
958 : Word16 Q )
959 : {
960 0 : assert( Q >= 0 );
961 0 : IF( isnan( number ) )
962 0 : return 0;
963 0 : if ( number == 1.0f && Q == Q15 )
964 0 : return MAX16B;
965 0 : if ( number == -1.0f && Q == Q15 )
966 0 : return MIN16B;
967 0 : assert( fabs( number ) < pow( 2, 15 - Q ) );
968 0 : Word16 ret = (Word16) ( number * ( (UWord16) 1 << Q ) );
969 0 : return ret;
970 : }
971 :
972 0 : Word16 float_to_fix16_thrld( float number, Word16 Q )
973 : {
974 0 : assert( Q >= 0 );
975 0 : if ( number == 1.0f && Q == Q15 )
976 0 : return MAX16B;
977 0 : float limit = (float) pow( 2, 15 - Q );
978 : /*Add threshold*/
979 0 : if ( number > MAX16B_FLT )
980 : {
981 0 : number = MAX16B_FLT;
982 : }
983 0 : else if ( number < MIN16B_FLT )
984 : {
985 0 : number = MIN16B_FLT;
986 : }
987 0 : assert( number <= limit && number >= -limit );
988 0 : Word16 ret = (Word16) ( number * ( (UWord16) 1 << Q ) );
989 0 : return ret;
990 : }
991 :
992 68942 : float fix16_to_float( Word16 number, Word16 Q )
993 : {
994 68942 : assert( Q >= 0 );
995 68942 : float ret = (float) number / ( (UWord16) 1 << Q );
996 68942 : return ret;
997 : }
998 :
999 : // Float to 32-bit Mantissa and Exponent
1000 0 : void f2me( float n, Word32 *mantissa, Word16 *expo )
1001 : {
1002 : Word32 e;
1003 0 : float mf = (float) frexp( n, &e );
1004 0 : *expo = (Word16) e;
1005 0 : *mantissa = float_to_fix( mf, Q31 );
1006 0 : }
1007 :
1008 : // 32-bit Mantissa and Exponent to Float
1009 0 : float me2f( Word32 m, Word16 expo )
1010 : {
1011 0 : float mf = fix_to_float( m, Q31 );
1012 0 : return (float) ldexp( mf, expo );
1013 : }
1014 :
1015 : // Float buffer to 32-bit mantissa buffer and common exponent.
1016 0 : void f2me_buf( const float *x, Word32 *m, Word16 *e, const Word32 n )
1017 : {
1018 0 : Word16 max_e = -32, tmp_e;
1019 : Word32 i;
1020 :
1021 0 : for ( i = 0; i < n; i++ )
1022 : {
1023 0 : f2me( x[i], &m[i], &tmp_e );
1024 0 : max_e = ( max_e > tmp_e ) ? max_e : tmp_e;
1025 : }
1026 :
1027 0 : for ( i = 0; i < n; i++ )
1028 : {
1029 0 : f2me( x[i], &m[i], &tmp_e );
1030 0 : m[i] = L_shr( m[i], max_e - tmp_e );
1031 : }
1032 :
1033 0 : *e = max_e;
1034 0 : }
1035 :
1036 : // 32-bit Mantissa buffer and exponent into float buffer.
1037 0 : void me2f_buf( const Word32 *m, const Word16 e, float *out, const Word32 n )
1038 : {
1039 0 : for ( int i = 0; i < n; i++ )
1040 : {
1041 0 : out[i] = me2f( m[i], e );
1042 : }
1043 0 : }
1044 :
1045 : // Float to 16-bit Mantissa and Exponent
1046 0 : void f2me_16( float n, Word16 *mantissa, Word16 *expo )
1047 : {
1048 : Word32 e;
1049 0 : float mf = (float) frexp( n, &e );
1050 0 : *expo = (Word16) e;
1051 0 : *mantissa = float_to_fix16( mf, 15 );
1052 0 : }
1053 :
1054 : // 16-bit Mantissa and Exponent to Float
1055 0 : float me2f_16( Word16 m, Word16 expo )
1056 : {
1057 0 : float mf = fix16_to_float( m, 15 );
1058 0 : return (float) ldexp( mf, expo );
1059 : }
1060 :
1061 : // Float buffer to 16-bit mantissa buffer and common exponent.
1062 0 : void f2me_buf_16( const float *x, Word16 *m, Word16 *e, const Word32 n )
1063 : {
1064 0 : Word16 max_e = -16, tmp_e;
1065 : Word32 i;
1066 :
1067 0 : for ( i = 0; i < n; i++ )
1068 : {
1069 0 : f2me_16( x[i], &m[i], &tmp_e );
1070 0 : max_e = ( max_e > tmp_e ) ? max_e : tmp_e;
1071 : }
1072 :
1073 0 : for ( i = 0; i < n; i++ )
1074 : {
1075 0 : f2me_16( x[i], &m[i], &tmp_e );
1076 0 : m[i] = shr( m[i], max_e - tmp_e );
1077 : }
1078 :
1079 0 : *e = max_e;
1080 0 : }
1081 :
1082 : // 16-bit Mantissa buffer and exponent into float buffer.
1083 0 : void me2f_buf_16( const Word16 *m, const Word16 e, float *out, const Word32 n )
1084 : {
1085 0 : for ( int i = 0; i < n; i++ )
1086 : {
1087 0 : out[i] = me2f_16( m[i], e );
1088 : }
1089 0 : }
1090 0 : void f2fix( float *var_flt, Word32 *var_fix, Word32 expo )
1091 : {
1092 0 : *var_fix = (Word32) ( *var_flt * pow( 2, 31 - expo ) );
1093 0 : }
1094 :
1095 0 : void fix2f( Word32 *var_fix, float *var_flt, Word32 expo )
1096 : {
1097 0 : float mf = fix_to_float( *var_fix, 31 );
1098 0 : *var_flt = (float) ldexp( mf, expo );
1099 0 : }
1100 :
1101 0 : void f2fix_16( float *var_flt, Word16 *var_fix, Word32 expo )
1102 : {
1103 0 : *var_fix = (Word16) ( *var_flt * pow( 2, 15 - expo ) );
1104 0 : }
1105 :
1106 0 : void fix2f_16( Word16 *var_fix, float *var_flt, Word32 expo )
1107 : {
1108 0 : float mf = fix16_to_float( *var_fix, 15 );
1109 0 : *var_flt = (float) ldexp( mf, expo );
1110 0 : }
1111 :
1112 : #undef WMC_TOOL_SKIP
1113 :
1114 :
1115 : /*-------------------------------------------------------------------*
1116 : * usdequant_fx()
1117 : *
1118 : * Uniform scalar de-quantizer routine
1119 : *
1120 : * Applies de-quantization based on scale and round operations.
1121 : *-------------------------------------------------------------------*/
1122 :
1123 : /* Qx*/
1124 325264 : Word16 usdequant_fx(
1125 : const Word16 idx, /* i: quantizer index Q0*/
1126 : const Word16 qlow, /* i: lowest codebook entry (index 0) Qx*/
1127 : const Word16 delta /* i: quantization step Qx-1*/
1128 : )
1129 : {
1130 : Word16 g;
1131 : Word32 L_tmp;
1132 :
1133 : /*g = idx * delta + qlow;*/
1134 325264 : L_tmp = L_deposit_l( qlow ); /*Qx */
1135 325264 : L_tmp = L_mac( L_tmp, idx, delta ); /*Qx */
1136 325264 : g = round_fx_sat( L_shl_sat( L_tmp, 16 ) ); /*Qx */
1137 325264 : return ( g );
1138 : }
1139 :
1140 : /* Qx*/
1141 190964 : Word32 usdequant32_fx(
1142 : const Word16 idx, /* i: quantizer index Q0*/
1143 : const Word32 qlow, /* i: lowest codebook entry (index 0) Qx*/
1144 : const Word32 delta /* i: quantization step Qx-1*/
1145 : )
1146 : {
1147 : Word32 g;
1148 : Word64 L_tmp;
1149 :
1150 : /*g = idx * delta + qlow;*/
1151 190964 : L_tmp = W_deposit32_l( qlow ); /*Qx */
1152 190964 : L_tmp = W_mac_32_16( L_tmp, delta, idx );
1153 190964 : IF( GE_64( L_tmp, MAX_32 ) )
1154 : {
1155 41693 : g = MAX_32;
1156 41693 : move32();
1157 : }
1158 : ELSE
1159 : {
1160 149271 : g = W_extract_l( L_tmp ); /*Qx */
1161 : }
1162 190964 : return ( g );
1163 : }
1164 :
1165 :
1166 : /*-------------------------------------------------------------------*
1167 : * usquant()
1168 : *
1169 : * Uniform scalar quantizer according to MMSE criterion
1170 : * (nearest neighbour in Euclidean space)
1171 : *
1172 : * Applies quantization based on scale and round operations.
1173 : * Index of the winning codeword and the winning codeword itself are returned.
1174 : *-------------------------------------------------------------------*/
1175 :
1176 : /* o: index of the winning codeword */
1177 1199423 : Word16 usquant_fx(
1178 : const Word16 x, /* i: scalar value to quantize Qx*/
1179 : Word16 *xq, /* o: quantized value Qx*/
1180 : const Word16 qlow, /* i: lowest codebook entry (index 0) Qx*/
1181 : const Word16 delta, /* i: quantization step Qx-1*/
1182 : const Word16 cbsize /* i: codebook size */
1183 : )
1184 : {
1185 : Word16 idx;
1186 : Word16 tmp, exp;
1187 : Word32 L_tmp;
1188 :
1189 : /* idx = (short)( (x - qlow)/delta + 0.5f); */
1190 1199423 : exp = norm_s( delta );
1191 1199423 : tmp = div_s( shl( 1, sub( 14, exp ) ), delta ); /*Q(29-exp-(Qx-1))->Q(30-exp-Qx) */
1192 1199423 : L_tmp = L_mult( sub_sat( x, qlow ), tmp ); /*Q(31-exp) */
1193 1199423 : idx = extract_l( L_shr_r( L_add( L_tmp, shl_sat( 1, sub( 30, exp ) ) ), sub( 31, exp ) ) ); /*Q0 */
1194 :
1195 1199423 : idx = s_min( idx, sub( cbsize, 1 ) );
1196 1199423 : idx = s_max( idx, 0 );
1197 :
1198 : /* *xq = idx*delta + qlow; */
1199 1199423 : L_tmp = L_deposit_l( qlow ); /*Qx */
1200 1199423 : L_tmp = L_mac( L_tmp, idx, delta ); /*Qx */
1201 1199423 : *xq = round_fx_sat( L_shl_sat( L_tmp, 16 ) ); /*Qx */
1202 :
1203 1199423 : return idx;
1204 : }
1205 :
1206 :
1207 : /*-------------------------------------------------------------------*
1208 : * Dot_product:
1209 : *
1210 : * Compute scalar product of <x[],y[]> using accumulator.
1211 : * Performs no normalization, as opposed to Dot_product12()
1212 : *-------------------------------------------------------------------*/
1213 : /* o : Sum */
1214 52196021 : Word32 Dot_product(
1215 : const Word16 x[], /* i : 12bits: x vector */
1216 : const Word16 y[], /* i : 12bits: y vector */
1217 : const Word16 lg /* i : vector length */
1218 : )
1219 : {
1220 : Word16 i;
1221 : Word32 L_sum;
1222 : Word64 L64_sum;
1223 :
1224 52196021 : L64_sum = 1;
1225 52196021 : move64();
1226 3159558780 : FOR( i = 0; i < lg; i++ )
1227 : {
1228 3107362759 : L64_sum = W_mac_16_16( L64_sum, x[i], y[i] );
1229 : }
1230 52196021 : L_sum = W_sat_l( L64_sum );
1231 :
1232 52196021 : return L_sum;
1233 : }
1234 :
1235 :
1236 : /*---------------------------------------------------------------------*
1237 : * dotp_fx()
1238 : *
1239 : * Dot product of vector x[] and vector y[]
1240 : *---------------------------------------------------------------------*/
1241 :
1242 : /* o : dot product of x[] and y[] */
1243 1394050 : Word32 dotp_fx(
1244 : const Word16 x[], /* i : vector x[] */
1245 : const Word16 y[], /* i : vector y[] */
1246 : const Word16 n, /* i : vector length */
1247 : Word16 *exp /* o : exponent of result (0..+30) */
1248 : )
1249 : {
1250 : Word16 sft;
1251 : Word32 L_sum;
1252 :
1253 1394050 : assert( *exp == 0 );
1254 :
1255 1394050 : L_sum = L_add( L_shr( Dot_product( x, y, n ), 1 ), 1 );
1256 :
1257 : /* Normalize acc in Q31 */
1258 :
1259 1394050 : sft = norm_l( L_sum );
1260 1394050 : L_sum = L_shl( L_sum, sft );
1261 :
1262 1394050 : *exp = sub( 30, sft );
1263 1394050 : move16(); /* exponent = 0..30 */
1264 :
1265 1394050 : return L_sum;
1266 : }
1267 :
1268 : /* o : sum of all squared vector elements Q(2x+1)*/
1269 407834 : Word32 sum2_fx(
1270 : const Word16 *vec, /* i : input vector Qx*/
1271 : const Word16 lvec /* i : length of input vector */
1272 : )
1273 : {
1274 : Word16 i;
1275 : Word32 L_tmp;
1276 :
1277 407834 : L_tmp = L_deposit_l( 0 );
1278 32019466 : FOR( i = 0; i < lvec; i++ )
1279 : {
1280 31611632 : L_tmp = L_mac_sat( L_tmp, vec[i], vec[i] ); /*Q(2x+1) */
1281 : }
1282 :
1283 407834 : return L_tmp;
1284 : }
1285 :
1286 :
1287 : /* o : sum of all squared vector elements Q(2*Qx)*/
1288 131460 : Word64 sum2_fx_no_sat(
1289 : const Word16 *vec, /* i : input vector Qx*/
1290 : const Word16 lvec /* i : length of input vector */
1291 : )
1292 : {
1293 : Word16 i;
1294 : Word64 sum;
1295 :
1296 131460 : sum = 0;
1297 131460 : move64();
1298 56221060 : FOR( i = 0; i < lvec; i++ )
1299 : {
1300 56089600 : sum = W_mac0_16_16( sum, vec[i], vec[i] ); // 2*Qx
1301 : }
1302 :
1303 131460 : return sum;
1304 : }
1305 :
1306 93444 : Word32 sum_32_fx(
1307 : const Word32 *vec, /* i : input vector */
1308 : const Word16 lvec, /* i : length of input vector */
1309 : Word16 *e )
1310 : {
1311 : Word16 i, shift;
1312 93444 : Word64 tmp = 0;
1313 93444 : move64();
1314 : Word32 ans;
1315 :
1316 301984 : FOR( i = 0; i < lvec; i++ )
1317 : {
1318 208540 : tmp = W_add( tmp, vec[i] ); // e
1319 : }
1320 93444 : shift = W_norm( tmp );
1321 93444 : tmp = W_shl( tmp, shift ); // shift + (31 - e)
1322 93444 : ans = W_extract_h( tmp ); // shift + (31 - e) - 32
1323 93444 : *e = add( sub( *e, shift ), 32 );
1324 93444 : move16();
1325 :
1326 93444 : return ans;
1327 : }
1328 :
1329 : /* o : sum of all squared vector elements Q(2x+1 -5)*/
1330 75408 : Word32 sum2_fx_mod(
1331 : const Word16 *vec, /* i : input vector Qx*/
1332 : const Word16 lvec /* i : length of input vector */
1333 : )
1334 : {
1335 : Word16 i;
1336 : Word32 L_tmp;
1337 :
1338 75408 : L_tmp = L_deposit_l( 0 );
1339 48336528 : FOR( i = 0; i < lvec; i++ )
1340 : {
1341 48261120 : L_tmp = L_add_sat( L_tmp, L_shr( L_mult_sat( vec[i], vec[i] ), 9 ) );
1342 : }
1343 :
1344 75408 : return L_tmp;
1345 : }
1346 :
1347 :
1348 : /*-------------------------------------------------------------------*
1349 : * Copy:
1350 : *
1351 : * Copy vector x[] to y[]
1352 : *
1353 : *-------------------------------------------------------------------*/
1354 :
1355 180442669 : void Copy(
1356 : const Word16 x[], /* i : input vector */
1357 : Word16 y[], /* o : output vector */
1358 : const Word16 L /* i : vector length */
1359 : )
1360 : {
1361 : Word16 i;
1362 :
1363 180442669 : IF( y < x )
1364 : {
1365 17914340019 : FOR( i = 0; i < L; i++ )
1366 : {
1367 17822978338 : y[i] = x[i];
1368 17822978338 : move16();
1369 : }
1370 :
1371 : /* 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. */
1372 91361681 : return;
1373 : }
1374 :
1375 17533155498 : FOR( i = L - 1; i >= 0; i-- )
1376 : {
1377 17444074510 : y[i] = x[i];
1378 17444074510 : move16();
1379 : }
1380 :
1381 89080988 : return;
1382 : }
1383 :
1384 :
1385 : /*-------------------------------------------------------------------*
1386 : * Copy64:
1387 : *
1388 : * Copy vector x[] to y[] (64 bits)
1389 : *-------------------------------------------------------------------*/
1390 :
1391 8154 : void Copy64(
1392 : const Word64 x[], /* i : input vector */
1393 : Word64 y[], /* o : output vector */
1394 : const Word16 L /* i : vector length */
1395 : )
1396 : {
1397 : Word16 i;
1398 8154 : IF( y < x )
1399 : {
1400 0 : FOR( i = 0; i < L; i++ )
1401 : {
1402 0 : y[i] = x[i];
1403 0 : move64();
1404 : }
1405 :
1406 : /* 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. */
1407 0 : return;
1408 : }
1409 :
1410 35694 : FOR( i = L - 1; i >= 0; i-- )
1411 : {
1412 27540 : y[i] = x[i];
1413 27540 : move64();
1414 : }
1415 :
1416 8154 : return;
1417 : }
1418 :
1419 10158644 : void set64_fx(
1420 : Word64 y[], /* i/o: Vector to set */
1421 : const Word64 a, /* i : Value to set the vector to */
1422 : const Word16 N /* i : Lenght of the vector */
1423 : )
1424 : {
1425 : Word16 i;
1426 398667697 : FOR( i = 0; i < N; i++ )
1427 : {
1428 388509053 : y[i] = a;
1429 388509053 : move64();
1430 : }
1431 :
1432 10158644 : return;
1433 : }
1434 :
1435 150320 : void Copy_pword(
1436 : const PWord16 x[], /* i : input vector */
1437 : PWord16 y[], /* o : output vector */
1438 : const Word16 L /* i : vector length */
1439 : )
1440 : {
1441 : Word16 i;
1442 :
1443 150320 : IF( y < x )
1444 : {
1445 2940912 : FOR( i = 0; i < L; i++ )
1446 : {
1447 2803354 : y[i].v.im = x[i].v.im;
1448 2803354 : y[i].v.re = x[i].v.re;
1449 2803354 : move16();
1450 2803354 : move16();
1451 : }
1452 :
1453 : /* 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. */
1454 137558 : return;
1455 : }
1456 :
1457 793102 : FOR( i = L - 1; i >= 0; i-- )
1458 : {
1459 780340 : y[i].v.im = x[i].v.im;
1460 780340 : y[i].v.re = x[i].v.re;
1461 780340 : move16();
1462 780340 : move16();
1463 : }
1464 :
1465 12762 : return;
1466 : }
1467 : /*-------------------------------------------------------------------*
1468 : * Copy32:
1469 : *
1470 : * Copy vector x[] to y[] (32 bits)
1471 : *-------------------------------------------------------------------*/
1472 329675228 : void Copy32(
1473 : const Word32 x[], /* i : input vector */
1474 : Word32 y[], /* o : output vector */
1475 : const Word16 L /* i : vector length */
1476 : )
1477 : {
1478 : Word16 i;
1479 329675228 : IF( y < x )
1480 : {
1481 36421114248 : FOR( i = 0; i < L; i++ )
1482 : {
1483 36258044456 : y[i] = x[i];
1484 36258044456 : move32();
1485 : }
1486 :
1487 163069792 : return;
1488 : }
1489 :
1490 33719747405 : FOR( i = L - 1; i >= 0; i-- )
1491 : {
1492 33553141969 : y[i] = x[i];
1493 33553141969 : move32();
1494 : }
1495 : }
1496 :
1497 87011 : void set8_fx(
1498 : Word8 y[], /* i/o: Vector to set */
1499 : const Word8 a, /* i : Value to set the vector to */
1500 : const Word16 N /* i : Lenght of the vector */
1501 : )
1502 : {
1503 : Word16 i;
1504 :
1505 49477261 : FOR( i = 0; i < N; i++ )
1506 : {
1507 49390250 : y[i] = a;
1508 49390250 : move16();
1509 : }
1510 :
1511 87011 : return;
1512 : }
1513 :
1514 :
1515 : /*-------------------------------------------------------------------*
1516 : * set16_fx()
1517 : * set32_fx()
1518 : *
1519 : * Set the vector elements to a value
1520 : *-------------------------------------------------------------------*/
1521 :
1522 274087078 : void set16_fx(
1523 : Word16 y[], /* i/o: Vector to set */
1524 : const Word16 a, /* i : Value to set the vector to */
1525 : const Word16 N /* i : Lenght of the vector */
1526 : )
1527 : {
1528 : Word16 i;
1529 :
1530 35280843313 : FOR( i = 0; i < N; i++ )
1531 : {
1532 35006756235 : y[i] = a;
1533 35006756235 : move16();
1534 : }
1535 :
1536 274087078 : return;
1537 : }
1538 :
1539 689723142 : void set32_fx(
1540 : Word32 y[], /* i/o: Vector to set */
1541 : const Word32 a, /* i : Value to set the vector to */
1542 : const Word16 N /* i : Lenght of the vector */
1543 : )
1544 : {
1545 : Word16 i;
1546 :
1547 65327116847 : FOR( i = 0; i < N; i++ )
1548 : {
1549 64637393705 : y[i] = a;
1550 64637393705 : move32();
1551 : }
1552 :
1553 689723142 : return;
1554 : }
1555 :
1556 :
1557 : /*-------------------------------------------------------------------*
1558 : * Copy_Scale_sig
1559 : *
1560 : * Up/down scale a 16 bits vector x and move it into y
1561 : *-------------------------------------------------------------------*/
1562 :
1563 20588562 : void Copy_Scale_sig(
1564 : const Word16 x[], /* i : signal to scale input Qx */
1565 : Word16 y[], /* o : scaled signal output Qx */
1566 : const Word16 lg, /* i : size of x[] Q0 */
1567 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1568 : )
1569 : {
1570 : Word16 i;
1571 : Word16 tmp;
1572 :
1573 20588562 : IF( exp0 == 0 )
1574 : {
1575 2038296250 : FOR( i = 0; i < lg; i++ )
1576 : {
1577 2034551095 : y[i] = x[i];
1578 2034551095 : move16();
1579 : }
1580 3745155 : return;
1581 : }
1582 16843407 : IF( exp0 < 0 )
1583 : {
1584 12859811 : tmp = shl( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
1585 5661252669 : FOR( i = 0; i < lg; i++ )
1586 : {
1587 5648392858 : y[i] = msu_r( 0, x[i], tmp );
1588 5648392858 : move16();
1589 : }
1590 12859811 : return;
1591 : }
1592 2080570498 : FOR( i = 0; i < lg; i++ )
1593 : {
1594 2076586902 : y[i] = shl_sat( x[i], exp0 );
1595 2076586902 : move16(); /* saturation can occur here */
1596 : }
1597 :
1598 3983596 : return;
1599 : }
1600 :
1601 :
1602 : /*-------------------------------------------------------------------*
1603 : * Copy_Scale_sig
1604 : *
1605 : * Up/down scale a 16 bits vector x and move it into y
1606 : *-------------------------------------------------------------------*/
1607 :
1608 3827200 : void Copy_Scale_sig_16_32_DEPREC(
1609 : const Word16 x[], /* i : signal to scale input Qx */
1610 : Word32 y[], /* o : scaled signal output Qx */
1611 : const Word16 lg, /* i : size of x[] Q0 */
1612 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1613 : )
1614 : {
1615 : Word16 i;
1616 : Word16 tmp;
1617 :
1618 3827200 : IF( exp0 == 0 )
1619 : {
1620 1145534191 : FOR( i = 0; i < lg; i++ )
1621 : {
1622 1143286824 : y[i] = L_deposit_l( x[i] );
1623 1143286824 : move32();
1624 : }
1625 2247367 : return;
1626 : }
1627 1579833 : IF( exp0 < 0 )
1628 : {
1629 : /*Should not happen */
1630 65029961 : FOR( i = 0; i < lg; i++ )
1631 : {
1632 64839552 : y[i] = L_deposit_l( shl_sat( x[i], exp0 ) );
1633 64839552 : move32();
1634 : }
1635 190409 : return;
1636 : }
1637 : #ifdef DEBUGGING
1638 : if ( exp0 >= 16 )
1639 : {
1640 : printf( "Issue in Copy_Scale_sig_16_32_DEPREC\n" );
1641 : }
1642 : #else
1643 1389424 : assert( exp0 < 16 );
1644 : #endif
1645 1389424 : tmp = shl_sat( 1, exp0 );
1646 443744337 : FOR( i = 0; i < lg; i++ )
1647 : {
1648 442354913 : y[i] = L_mult0( x[i], tmp );
1649 442354913 : move32(); /* saturation can occur here */
1650 : }
1651 :
1652 1389424 : return;
1653 : }
1654 :
1655 149990 : void Copy_Scale_sig_16_32_r(
1656 : const Word16 x[], /* i : signal to scale input Qx */
1657 : Word32 y[], /* o : scaled signal output Qx */
1658 : const Word16 lg, /* i : size of x[] Q0 */
1659 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1660 : )
1661 : {
1662 : Word16 i;
1663 :
1664 62270852 : FOR( i = 0; i < lg; i++ )
1665 : {
1666 62120862 : y[i] = L_shl_r( L_deposit_l( x[i] ), exp0 );
1667 : }
1668 :
1669 149990 : return;
1670 : }
1671 :
1672 17401795 : void Copy_Scale_sig_16_32_no_sat(
1673 : const Word16 x[], /* i : signal to scale input Qx */
1674 : Word32 y[], /* o : scaled signal output Qx */
1675 : const Word16 lg, /* i : size of x[] Q0 */
1676 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1677 : )
1678 : {
1679 : Word16 i;
1680 : Word32 L_tmp;
1681 :
1682 17401795 : IF( exp0 == 0 )
1683 : {
1684 573797823 : FOR( i = 0; i < lg; i++ )
1685 : {
1686 571574336 : y[i] = L_deposit_l( x[i] );
1687 571574336 : move32();
1688 : }
1689 2223487 : return;
1690 : }
1691 15178308 : IF( exp0 < 0 )
1692 : {
1693 : /*Should not happen */
1694 282796469 : FOR( i = 0; i < lg; i++ )
1695 : {
1696 282110428 : y[i] = L_deposit_l( shl_sat( x[i], exp0 ) );
1697 282110428 : move32();
1698 : }
1699 686041 : return;
1700 : }
1701 :
1702 14492267 : L_tmp = L_shl_sat( 1, exp0 - 1 );
1703 :
1704 14492267 : IF( L_tmp >= 0x7FFF )
1705 : {
1706 1549129775 : FOR( i = 0; i < lg; i++ )
1707 : {
1708 : // y[i] = L_mult0(x[i], L_tmp);
1709 1546485022 : y[i] = W_extract_l( W_mult_32_16( L_tmp, x[i] ) );
1710 1546485022 : move32(); /* Overflow can occur here */
1711 : }
1712 2644753 : return;
1713 : }
1714 : // ELSE
1715 : {
1716 11847514 : Word16 tmp = extract_l( L_tmp );
1717 5459818232 : FOR( i = 0; i < lg; i++ )
1718 : {
1719 5447970718 : y[i] = L_mult( x[i], tmp );
1720 5447970718 : move32();
1721 : }
1722 : }
1723 :
1724 11847514 : return;
1725 : }
1726 :
1727 :
1728 10569045 : void Copy_Scale_sig_32_16(
1729 : const Word32 x[], /* i : signal to scale input Qx */
1730 : Word16 y[], /* o : scaled signal output Qx */
1731 : const Word16 lg, /* i : size of x[] Q0 */
1732 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1733 : )
1734 : {
1735 : Word16 i;
1736 : Word16 tmp;
1737 :
1738 10569045 : tmp = add( 16, exp0 );
1739 10569045 : IF( tmp != 0 )
1740 : {
1741 6938603266 : FOR( i = 0; i < lg; i++ )
1742 : {
1743 6928364480 : y[i] = round_fx_sat( L_shl_sat( x[i], tmp ) );
1744 6928364480 : move16();
1745 : }
1746 : }
1747 : ELSE
1748 : {
1749 87067183 : FOR( i = 0; i < lg; i++ )
1750 : {
1751 86736924 : y[i] = round_fx_sat( x[i] );
1752 86736924 : move16();
1753 : }
1754 : }
1755 :
1756 10569045 : return;
1757 : }
1758 :
1759 :
1760 : /*-------------------------------------------------------------------*
1761 : * Scale_sig32
1762 : *
1763 : * Up/down scale a 32 bits vector
1764 : *-------------------------------------------------------------------*/
1765 :
1766 115337019 : void Scale_sig32(
1767 : Word32 x[], /* i/o: signal to scale Qx */
1768 : const Word16 lg, /* i : size of x[] Q0 */
1769 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1770 : )
1771 : {
1772 : Word16 i;
1773 :
1774 115337019 : IF( 0 == exp0 )
1775 : {
1776 33234078 : return;
1777 : }
1778 :
1779 30207769332 : FOR( i = 0; i < lg; i++ )
1780 : {
1781 30125666391 : x[i] = L_shl_sat( x[i], exp0 );
1782 30125666391 : move32(); /* saturation can occur here */
1783 : }
1784 :
1785 82102941 : return;
1786 : }
1787 :
1788 :
1789 : /*------------------------------------------------------------------*
1790 : * function Random_Fill
1791 : *
1792 : * Signed 16 bits random generator.
1793 : * (Avoids Store of Seed to Memory for 'n' Random Values and
1794 : * Combines Scaling Operation.)
1795 : *------------------------------------------------------------------*/
1796 :
1797 65526 : void Random_Fill(
1798 : Word16 *seed, /* i/o: random seed */
1799 : Word16 n, /* i : number of values */
1800 : Word16 *y, /* o : output values */
1801 : Word16 scaling /* i : scaling of values */
1802 : )
1803 : {
1804 : Word16 i;
1805 : Word16 local_seed;
1806 :
1807 65526 : local_seed = *seed;
1808 65526 : move16();
1809 6083382 : FOR( i = 0; i < n; i++ )
1810 : {
1811 6017856 : local_seed = extract_l( L_mac0( 13849L, local_seed, 31821 ) );
1812 6017856 : *y++ = shr( local_seed, scaling );
1813 6017856 : move16();
1814 : }
1815 65526 : *seed = local_seed;
1816 65526 : move16();
1817 :
1818 65526 : return;
1819 : }
1820 :
1821 :
1822 : /*-------------------------------------------------------------------*
1823 : * Scale_sig
1824 : * Up/down scale a 16 bits vector
1825 : *-------------------------------------------------------------------*/
1826 :
1827 91377701 : void Scale_sig(
1828 : Word16 x[], /* i/o: signal to scale Qx */
1829 : const Word16 lg, /* i : size of x[] Q0 */
1830 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
1831 : )
1832 : {
1833 : Word16 i;
1834 : Word16 tmp;
1835 :
1836 91377701 : IF( exp0 > 0 )
1837 : {
1838 10166339078 : FOR( i = 0; i < lg; i++ )
1839 : {
1840 10146201663 : x[i] = shl_sat( x[i], exp0 );
1841 10146201663 : move16(); /* saturation can occur here */
1842 : }
1843 20137415 : return;
1844 : }
1845 71240286 : IF( exp0 < 0 )
1846 : {
1847 36792229 : tmp = shl_sat( -32768, exp0 ); /* we use negative to correctly represent 1.0 */
1848 15042061274 : FOR( i = 0; i < lg; i++ )
1849 : {
1850 15005269045 : x[i] = msu_r_sat( 0, x[i], tmp );
1851 15005269045 : move16(); /* msu instead of mac because factor is negative */
1852 : }
1853 36792229 : return;
1854 : }
1855 : }
1856 :
1857 :
1858 : /*-------------------------------------------------------------------*
1859 : * scale_sig
1860 : * Up/down scale a 16 bits vector
1861 : *-------------------------------------------------------------------*/
1862 :
1863 9211240 : void scale_sig(
1864 : Word16 x[], /* i/o: signal to scale Qx */
1865 : const Word16 lg, /* i : size of x[] Q0 */
1866 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx exp */
1867 : )
1868 : {
1869 : Word16 i;
1870 :
1871 9211240 : IF( exp0 != 0 )
1872 : {
1873 2355958879 : FOR( i = 0; i < lg; i++ )
1874 : {
1875 2350005570 : x[i] = shl( x[i], exp0 );
1876 2350005570 : move16();
1877 : }
1878 : }
1879 9211240 : }
1880 :
1881 : /*---------------------------------------------------------------------*
1882 : * mean_fx()
1883 : *
1884 : *---------------------------------------------------------------------*/
1885 :
1886 : /* o : mean of vector */
1887 34970 : Word16 mean_fx(
1888 : const Word16 *vec_fx, /* i : input vector */
1889 : const Word16 lvec_fx /* i : length of input vector */
1890 : )
1891 : {
1892 : Word16 tmp;
1893 : // PMT("TBV : this function could be written differently to minimize the risk of saturation");
1894 34970 : tmp = sum16_fx( vec_fx, lvec_fx );
1895 34970 : tmp = mult_r( tmp, div_s( 1, lvec_fx ) );
1896 :
1897 34970 : return tmp;
1898 : }
1899 :
1900 : /* o : mean of vector Qx */
1901 21405 : Word16 mean_no_sat_fx(
1902 : const Word16 *vec_fx, /* i : input vector Qx */
1903 : const Word16 lvec_fx /* i : length of input vector */
1904 : )
1905 : {
1906 : Word16 i;
1907 21405 : Word32 L_tmp = 0;
1908 21405 : move32();
1909 706365 : FOR( i = 0; i < lvec_fx; ++i )
1910 : {
1911 684960 : L_tmp = L_add( L_tmp, vec_fx[i] );
1912 : }
1913 21405 : L_tmp = Mpy_32_16_1( L_tmp, div_s( 1, lvec_fx ) ); /* Qx */
1914 :
1915 21405 : return extract_l( L_tmp );
1916 : }
1917 :
1918 : /* o : mean of vector Qx */
1919 7424 : Word32 mean_no_sat_Word32_fx(
1920 : const Word32 *vec_fx, /* i : input vector Qx */
1921 : const Word16 lvec_fx, /* i : length of input vector */
1922 : const Word16 gb )
1923 : {
1924 : Word16 i;
1925 7424 : Word32 L_tmp = 0;
1926 7424 : move32();
1927 37120 : FOR( i = 0; i < lvec_fx; ++i )
1928 : {
1929 29696 : L_tmp = L_add( L_tmp, L_shr( vec_fx[i], gb ) );
1930 : }
1931 7424 : L_tmp = Mpy_32_16_1( L_tmp, div_s( 1, lvec_fx ) ); /* Qx-gb */
1932 :
1933 7424 : return L_tmp;
1934 : }
1935 :
1936 :
1937 : /*-------------------------------------------------------------------*
1938 : * Vr_add
1939 : *
1940 : * Add two Word16 vectors together integer by integer
1941 : *-------------------------------------------------------------------*/
1942 :
1943 4016292 : void Vr_add(
1944 : const Word16 *in1, /* i : Input vector 1 */
1945 : const Word16 *in2, /* i : Input vector 2 */
1946 : Word16 *out, /* o : Output vector that contains vector 1 + vector 2 */
1947 : Word16 Len /* i : Vector lenght */
1948 : )
1949 : {
1950 : Word16 i;
1951 :
1952 103574532 : FOR( i = 0; i < Len; i++ )
1953 : {
1954 99558240 : out[i] = add_sat( in1[i], in2[i] );
1955 99558240 : move16();
1956 : }
1957 4016292 : }
1958 :
1959 1450795 : void sort_fx(
1960 : Word16 *r, /* i/o: Vector to be sorted in place */
1961 : Word16 lo, /* i : Low limit of sorting range */
1962 : Word16 up /* I : High limit of sorting range */
1963 : )
1964 : {
1965 : Word16 i, j, i1;
1966 : Word16 tempr;
1967 :
1968 23206451 : FOR( i = sub( up, 1 ); i >= lo; i-- )
1969 : {
1970 21755656 : i1 = add( i, 1 );
1971 21755656 : tempr = r[i];
1972 21755656 : move16();
1973 21755656 : move16(); /*supplementary move for the j-1 PTR initialization*/
1974 21778775 : FOR( j = i1; j <= up; j++ )
1975 : {
1976 21777553 : IF( LE_16( tempr, r[j] ) )
1977 : {
1978 21754434 : BREAK;
1979 : }
1980 :
1981 23119 : r[j - 1] = r[j];
1982 23119 : move16();
1983 : }
1984 21755656 : r[j - 1] = tempr;
1985 21755656 : move16();
1986 : }
1987 :
1988 1450795 : return;
1989 : }
1990 :
1991 6150 : void sort_32_fx(
1992 : Word32 *r, /* i/o: Vector to be sorted in place */
1993 : const Word16 lo, /* i : Low limit of sorting range */
1994 : const Word16 up /* I : High limit of sorting range */
1995 : )
1996 : {
1997 : Word16 i, j;
1998 : Word32 tempr;
1999 164000 : FOR( i = sub( up, 1 ); i >= lo; i-- )
2000 : {
2001 157850 : tempr = r[i];
2002 157850 : move32();
2003 1416574 : FOR( j = add( i, 1 ); j <= up; j++ )
2004 : {
2005 1390083 : IF( LE_32( tempr, r[j] ) )
2006 : {
2007 131359 : BREAK;
2008 : }
2009 1258724 : r[j - 1] = r[j];
2010 1258724 : move32();
2011 : }
2012 :
2013 157850 : r[j - 1] = tempr;
2014 157850 : move32();
2015 : }
2016 :
2017 6150 : return;
2018 : }
2019 :
2020 : /* o : index of the minimum value in the input vector */
2021 2723169 : Word16 minimum_fx(
2022 : const Word16 *vec_fx, /* i : input vector */
2023 : const Word16 lvec_fx, /* i : length of input vector */
2024 : Word16 *min_fx /* o : minimum value in the input vector */
2025 : )
2026 : {
2027 : Word16 j, ind;
2028 : Word16 tmp;
2029 2723169 : ind = 0;
2030 2723169 : move16();
2031 2723169 : tmp = vec_fx[0];
2032 2723169 : move16();
2033 :
2034 76171028 : FOR( j = 1; j < lvec_fx; j++ )
2035 : {
2036 73447859 : if ( LT_16( vec_fx[j], tmp ) )
2037 : {
2038 1407339 : ind = j;
2039 1407339 : move16();
2040 : /*tmp = vec_fx[j]; move16(); */
2041 : }
2042 73447859 : tmp = s_min( tmp, vec_fx[j] );
2043 : }
2044 :
2045 2723169 : *min_fx = tmp;
2046 2723169 : move16();
2047 :
2048 2723169 : return ind;
2049 : }
2050 :
2051 : /* o : index of the maximum value in the input vector */
2052 27522050 : Word16 maximum_fx(
2053 : const Word16 *vec_fx, /* i : input vector */
2054 : const Word16 lvec_fx, /* i : length of input vector */
2055 : Word16 *max_fx /* o : maximum value in the input vector */
2056 : )
2057 : {
2058 : Word16 j, ind;
2059 : Word16 tmp;
2060 27522050 : ind = 0;
2061 27522050 : move16();
2062 27522050 : tmp = vec_fx[0];
2063 27522050 : move16();
2064 :
2065 182531192 : FOR( j = 1; j < lvec_fx; j++ )
2066 : {
2067 155009142 : if ( GT_16( vec_fx[j], tmp ) )
2068 : {
2069 26801552 : ind = j;
2070 26801552 : move16();
2071 : }
2072 155009142 : tmp = s_max( tmp, vec_fx[j] );
2073 : }
2074 27522050 : *max_fx = tmp;
2075 27522050 : move16();
2076 :
2077 27522050 : return ind;
2078 : }
2079 :
2080 : /* o : index of the maximum value in the input vector */
2081 24624996 : Word16 maximum_exp_fx(
2082 : const Word16 *vec_fx, /* i : input vector */
2083 : const Word16 *exp_vec, /* i : exponents of input vector */
2084 : const Word16 lvec_fx /* i : length of input vector */
2085 : )
2086 : {
2087 : Word16 j, ind;
2088 24624996 : ind = 0;
2089 24624996 : move16();
2090 :
2091 701237670 : FOR( j = 1; j < lvec_fx; j++ )
2092 : {
2093 676612674 : Word16 scale = sub( exp_vec[j], exp_vec[ind] );
2094 :
2095 676612674 : test();
2096 676612674 : if ( vec_fx[j] == 0 || vec_fx[ind] == 0 )
2097 : {
2098 41950450 : scale = 0;
2099 41950450 : move16();
2100 : }
2101 :
2102 676612674 : if ( L_mac0_sat( L_shl_sat( L_deposit_l( vec_fx[j] ), scale ), vec_fx[ind], -0x0001 ) > 0 )
2103 : {
2104 122274429 : ind = j;
2105 122274429 : move16();
2106 : }
2107 : }
2108 :
2109 24624996 : return ind;
2110 : }
2111 :
2112 :
2113 : /*---------------------------------------------------------------------*
2114 : * maximum_abs_16_fx()
2115 : *
2116 : * Find index and value of the absolute maximum in a vector
2117 : *---------------------------------------------------------------------*/
2118 :
2119 : /* o : index of the maximum abs value in the input vector */
2120 869860 : Word16 maximum_abs_16_fx(
2121 : const Word16 *vec, /* i : input vector */
2122 : const Word16 lvec, /* i : length of input vector */
2123 : Word16 *max_val /* o : maximum value in the input vector */
2124 : )
2125 : {
2126 : Word16 j, ind;
2127 : Word16 tmp;
2128 869860 : ind = 0;
2129 869860 : move16();
2130 869860 : tmp = abs_s( vec[0] );
2131 :
2132 104701612 : FOR( j = 1; j < lvec; j++ )
2133 : {
2134 103831752 : if ( GT_16( abs_s( vec[j] ), tmp ) )
2135 : {
2136 4320165 : ind = j;
2137 4320165 : move16();
2138 : }
2139 103831752 : tmp = s_max( tmp, abs_s( vec[j] ) );
2140 : }
2141 869860 : *max_val = tmp;
2142 869860 : move16();
2143 :
2144 869860 : return ind;
2145 : }
2146 :
2147 : /*---------------------------------------------------------------------*
2148 : * minimum_abs32_fx()
2149 : *
2150 : * Find index and value of the absolute minimum in a vector
2151 : *---------------------------------------------------------------------*/
2152 :
2153 : /* o : index of the minimum value in the input vector */
2154 0 : Word16 minimum_abs32_fx(
2155 : const Word32 *vec_fx, /* i : input vector */
2156 : const Word16 lvec_fx, /* i : length of input vector */
2157 : Word32 *min_fx /* o : minimum value in the input vector */
2158 : )
2159 : {
2160 : Word16 j, ind;
2161 : Word32 tmp;
2162 0 : ind = 0;
2163 0 : move16();
2164 0 : tmp = vec_fx[0];
2165 0 : move16();
2166 :
2167 0 : FOR( j = 1; j < lvec_fx; j++ )
2168 : {
2169 0 : IF( LT_32( L_abs( vec_fx[j] ), tmp ) )
2170 : {
2171 0 : ind = j;
2172 0 : move16();
2173 : /*tmp = vec_fx[j]; move16(); */
2174 : }
2175 0 : tmp = L_min( tmp, L_abs( vec_fx[j] ) );
2176 : }
2177 :
2178 0 : *min_fx = tmp;
2179 0 : move16();
2180 :
2181 0 : return ind;
2182 : }
2183 :
2184 :
2185 : /*---------------------------------------------------------------------*
2186 : * minimum_32_fx()
2187 : *
2188 : * Find index and value of the minimum in a vector
2189 : *---------------------------------------------------------------------*/
2190 :
2191 : /* o : index of the minimum value in the input vector */
2192 447475 : Word16 minimum_32_fx(
2193 : const Word32 *vec_fx, /* i : input vector */
2194 : const Word16 lvec_fx, /* i : length of input vector */
2195 : Word32 *min_fx /* o : minimum value in the input vector */
2196 : )
2197 : {
2198 : Word16 j, ind;
2199 : Word32 tmp;
2200 447475 : ind = 0;
2201 447475 : move16();
2202 447475 : tmp = vec_fx[0];
2203 447475 : move16();
2204 :
2205 1035521 : FOR( j = 1; j < lvec_fx; j++ )
2206 : {
2207 588046 : if ( LT_32( vec_fx[j], tmp ) )
2208 : {
2209 240597 : ind = j;
2210 240597 : move16();
2211 : /*tmp = vec_fx[j]; move32(); */
2212 : }
2213 588046 : tmp = L_min( tmp, vec_fx[j] );
2214 : }
2215 447475 : if ( min_fx != NULL )
2216 : {
2217 348056 : *min_fx = tmp;
2218 : }
2219 447475 : move32();
2220 :
2221 447475 : return ind;
2222 : }
2223 :
2224 :
2225 : /*---------------------------------------------------------------------*
2226 : * maximum_32_fx()
2227 : *
2228 : * Find index and value of the maximum in a vector
2229 : *---------------------------------------------------------------------*/
2230 :
2231 : /* o : index of the maximum value in the input vector */
2232 5662735 : Word16 maximum_32_fx(
2233 : const Word32 *vec, /* i : input vector */
2234 : const Word16 lvec, /* i : length of input vector */
2235 : Word32 *max_val /* o : maximum value in the input vector */
2236 : )
2237 : {
2238 : Word16 j, ind;
2239 : Word32 tmp;
2240 5662735 : ind = 0;
2241 5662735 : move16();
2242 5662735 : tmp = vec[0];
2243 5662735 : move16();
2244 :
2245 67041706 : FOR( j = 1; j < lvec; j++ )
2246 : {
2247 61378971 : IF( GT_32( vec[j], tmp ) )
2248 : {
2249 5450056 : ind = j;
2250 5450056 : move16();
2251 : }
2252 61378971 : tmp = L_max( tmp, vec[j] );
2253 : }
2254 5662735 : if ( max_val != NULL )
2255 : {
2256 5648811 : *max_val = tmp;
2257 : }
2258 5662735 : move32();
2259 :
2260 5662735 : return ind;
2261 : }
2262 :
2263 : /* o : index of the maximum abs value in the input vector */
2264 4967505 : Word16 maximum_abs_32_fx(
2265 : const Word32 *vec, /* i : input vector */
2266 : const Word16 lvec, /* i : length of input vector */
2267 : Word32 *max_val /* o : maximum value in the input vector */
2268 : )
2269 : {
2270 : Word16 j, ind;
2271 : Word32 tmp;
2272 4967505 : ind = 0;
2273 4967505 : move16();
2274 4967505 : tmp = L_abs( vec[0] );
2275 :
2276 1814383088 : FOR( j = 1; j < lvec; j++ )
2277 : {
2278 1809415583 : if ( GT_32( L_abs( vec[j] ), tmp ) )
2279 : {
2280 19472281 : ind = j;
2281 19472281 : move16();
2282 : }
2283 1809415583 : tmp = L_max( tmp, L_abs( vec[j] ) );
2284 : }
2285 4967505 : *max_val = tmp;
2286 4967505 : move32();
2287 :
2288 4967505 : return ind;
2289 : }
2290 :
2291 :
2292 : /*----------------------------------------------------------------
2293 : *Function:
2294 : *Finds number of shifts to normalize a 16-bit array variable.
2295 : *Return value
2296 : *Number of shifts
2297 : *----------------------------------------------------------------*/
2298 :
2299 97918 : Word16 Exp16Array(
2300 : const Word16 n, /* (i): Array size */
2301 : const Word16 *sx /* (i): Data array */
2302 : )
2303 : {
2304 : Word16 k;
2305 : Word16 exp;
2306 : Word16 sMax;
2307 : Word16 sAbs;
2308 :
2309 97918 : sMax = abs_s( sx[0] );
2310 97918 : move16();
2311 :
2312 6405504 : FOR( k = 1; k < n; k++ )
2313 : {
2314 6307586 : sAbs = abs_s( sx[k] );
2315 6307586 : sMax = s_max( sMax, sAbs );
2316 : }
2317 :
2318 97918 : exp = norm_s( sMax );
2319 97918 : return exp;
2320 : }
2321 :
2322 0 : Word16 Exp32Array(
2323 : const Word16 n, /* (i): Array size */
2324 : const Word32 *sx /* (i): Data array */
2325 : )
2326 : {
2327 : Word16 k;
2328 : Word16 exp;
2329 : Word32 L_Max;
2330 : Word32 L_Abs;
2331 :
2332 0 : L_Max = L_abs( sx[0] );
2333 0 : FOR( k = 1; k < n; k++ )
2334 : {
2335 0 : L_Abs = L_abs( sx[k] );
2336 0 : L_Max = L_max( L_Max, L_Abs );
2337 : }
2338 0 : exp = norm_l( L_Max );
2339 :
2340 0 : if ( L_Max == 0 )
2341 : {
2342 0 : exp = 31;
2343 0 : move16();
2344 : }
2345 0 : return exp;
2346 : }
2347 :
2348 : /* o : index of the maximum abs value in the input vector */
2349 16676 : Word32 sum16_32_fx(
2350 : const Word16 *vec, /* i : input vector Qx*/
2351 : const Word16 lvec /* i : length of input vector */
2352 : )
2353 : {
2354 : Word16 i;
2355 : Word32 tmp, L_var;
2356 16676 : tmp = 0;
2357 16676 : move16();
2358 1744822 : FOR( i = 0; i < lvec; i++ )
2359 : {
2360 1728146 : L_var = L_deposit_l( vec[i] );
2361 1728146 : tmp = L_add( tmp, L_var ); /*Qx */
2362 : }
2363 :
2364 16676 : return tmp;
2365 : }
2366 :
2367 :
2368 : /* o : sum of all vector elements Qx*/
2369 2658 : Word32 sum32_sat(
2370 : const Word32 *vec, /* i : input vector Qx*/
2371 : const Word16 lvec /* i : length of input vector */
2372 : )
2373 : {
2374 : Word16 i;
2375 : Word32 tmp;
2376 2658 : tmp = 0;
2377 2658 : move16();
2378 172770 : FOR( i = 0; i < lvec; i++ )
2379 : {
2380 170112 : tmp = L_add_sat( tmp, vec[i] ); /*Qx */
2381 : }
2382 :
2383 2658 : return tmp;
2384 : }
2385 :
2386 : /* o: variance of vector Qx+16*/
2387 7351 : Word32 var_fx_32(
2388 : const Word16 *x, /* i: input vector Qx*/
2389 : const Word16 Qx,
2390 : const Word16 len /* i: length of inputvector */
2391 : )
2392 : {
2393 : Word16 m;
2394 : Word32 v;
2395 : Word16 i;
2396 : Word16 tmp, exp, inv_len;
2397 : Word32 L_tmp;
2398 :
2399 7351 : L_tmp = L_add( x[0], 0 );
2400 200275 : FOR( i = 1; i < len; i++ )
2401 : {
2402 192924 : L_tmp = L_add( L_tmp, x[i] ); /*Qx */
2403 : }
2404 7351 : exp = norm_s( len );
2405 7351 : inv_len = div_s( shl( 1, sub( 14, exp ) ), len ); /*Q(29-exp) */
2406 7351 : L_tmp = Mult_32_16( L_tmp, inv_len ); /*Q(14-exp+Qx) */
2407 7351 : m = round_fx( L_shl( L_tmp, add( exp, 2 ) ) ); /*Qx */
2408 :
2409 7351 : v = L_deposit_l( 0 );
2410 207626 : FOR( i = 0; i < len; i++ )
2411 : {
2412 200275 : tmp = sub( x[i], m ); /*Qx */
2413 200275 : v = L_mac0_sat( v, tmp, tmp ); /*(Qx+Qx) */
2414 : }
2415 7351 : L_tmp = Mult_32_16( v, inv_len ); /*Q(14-exp+Qx+Qx) */
2416 7351 : v = L_shl( L_tmp, add( exp, sub( 2, Qx ) ) ); /*Qx+16 */
2417 :
2418 7351 : return v;
2419 : }
2420 :
2421 : /* o: variance of vector Qx*/
2422 3712 : Word32 var_fx_32in_32out(
2423 : const Word32 *x, /* i: input vector Qx*/
2424 : Word16 *Qx, /*i/o:Q for input/output */
2425 : const Word16 len, /* i: length of inputvector */
2426 : const Word16 gb )
2427 : {
2428 : Word16 i;
2429 : Word16 shift;
2430 : Word32 L_tmp1, L_tmp;
2431 : Word64 W_temp;
2432 :
2433 3712 : L_tmp = mean_no_sat_Word32_fx( x, len, gb ); /*Qx-gb */
2434 3712 : W_temp = 0;
2435 18560 : FOR( i = 0; i < len; i++ )
2436 : {
2437 14848 : L_tmp1 = L_sub( L_shr( x[i], gb ), L_tmp ); /*Qx-gb */
2438 14848 : W_temp = W_add( W_temp, W_mult0_32_32( L_tmp1, L_tmp1 ) ); /*Qx-gb +Qx-gb*/
2439 : }
2440 3712 : shift = W_norm( W_temp );
2441 3712 : L_tmp = Mult_32_16( W_extract_h( W_shl( W_temp, shift ) ), div_s( 1, len ) ); /*Q2*(Qx-gb)+shift -32 */
2442 :
2443 3712 : *Qx = sub( add( shl( sub( *Qx, gb ), 1 ), shift ), 32 );
2444 3712 : move16();
2445 :
2446 3712 : return L_tmp;
2447 : }
2448 :
2449 :
2450 : /*-------------------------------------------------------------------*
2451 : * conv()
2452 : *
2453 : * Convolution between vectors x[] and h[] written to y[]
2454 : * All vectors are of length L. Only the first L samples of the
2455 : * convolution are considered.
2456 : *-------------------------------------------------------------------*/
2457 :
2458 1784042 : Flag conv_fx(
2459 : const Word16 x[], /* i : input vector Q_new*/
2460 : const Word16 h[], /* i : impulse response (or second input vector) Q(15)*/
2461 : Word16 y[], /* o : output vetor (result of convolution) 12 bits*/
2462 : const Word16 L /* i : vector size */
2463 : )
2464 : {
2465 :
2466 : Word16 i, n;
2467 : Word32 L_sum;
2468 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
2469 1784042 : Flag Overflow = 0;
2470 1784042 : move32();
2471 : #endif
2472 1784042 : y[0] = mult_r( x[0], h[0] );
2473 1784042 : move16();
2474 117878272 : FOR( n = 1; n < L; n++ )
2475 : {
2476 116094230 : L_sum = L_mult( x[0], h[n] );
2477 4154484992 : FOR( i = 1; i < n; i++ )
2478 : {
2479 4038390762 : L_sum = L_mac_o( L_sum, x[i], h[n - i], &Overflow );
2480 : }
2481 116094230 : y[n] = mac_ro( L_sum, x[i], h[0], &Overflow );
2482 116094230 : move16();
2483 : }
2484 :
2485 1784042 : return Overflow;
2486 : }
2487 :
2488 :
2489 : /*-------------------------------------------------------------------*
2490 : * conv_fx_32()
2491 : *
2492 : * Convolution between vectors x[] and h[] written to y[]
2493 : * All vectors are of length L. Only the first L samples of the
2494 : * convolution are considered.
2495 : *-------------------------------------------------------------------*/
2496 :
2497 2538 : Flag conv_fx_32(
2498 : const Word16 x[], /* i : input vector Q_new*/
2499 : const Word16 h[], /* i : impulse response (or second input vector) Q(15)*/
2500 : Word32 y[], /* o : output vetor (result of convolution) 12 bits*/
2501 : const Word16 L /* i : vector size */
2502 : )
2503 : {
2504 : Word16 i, n;
2505 : Word32 L_sum;
2506 2538 : Flag Overflow = 0;
2507 2538 : y[0] = L_mult( x[0], h[0] );
2508 2538 : move32();
2509 45684 : FOR( n = 1; n < L; n++ )
2510 : {
2511 43146 : L_sum = L_mult( x[0], h[n] );
2512 388314 : FOR( i = 1; i < n; i++ )
2513 : {
2514 345168 : L_sum = L_mac_o( L_sum, x[i], h[n - i], &Overflow );
2515 : }
2516 43146 : y[n] = L_mac_o( L_sum, x[i], h[0], &Overflow );
2517 43146 : move32();
2518 : }
2519 :
2520 2538 : return Overflow;
2521 : }
2522 :
2523 : /* o: variance of vector Qx*/
2524 579563 : Word16 var_fx(
2525 : const Word16 *x, /* i: input vector Qx*/
2526 : const Word16 Qx,
2527 : const Word16 len /* i: length of inputvector */
2528 : )
2529 : {
2530 : Word16 m;
2531 : Word32 v;
2532 : Word16 v_16;
2533 : Word16 i;
2534 : Word16 tmp, exp, inv_len;
2535 : Word32 L_tmp;
2536 :
2537 579563 : L_tmp = x[0];
2538 579563 : move32();
2539 4795662 : FOR( i = 1; i < len; i++ )
2540 : {
2541 4216099 : L_tmp = L_add( L_tmp, x[i] ); /*Qx */
2542 : }
2543 579563 : exp = norm_s( len );
2544 579563 : inv_len = div_s( shl( 1, sub( 14, exp ) ), len ); /*Q(29-exp) */
2545 579563 : L_tmp = Mult_32_16( L_tmp, inv_len ); /*Q(14-exp+Qx) */
2546 579563 : m = round_fx( L_shl( L_tmp, add( exp, 2 ) ) ); /*Qx */
2547 :
2548 579563 : v = L_deposit_l( 0 );
2549 5375225 : FOR( i = 0; i < len; i++ )
2550 : {
2551 4795662 : tmp = sub_sat( x[i], m ); /*Qx */
2552 4795662 : v = L_mac0_sat( v, tmp, tmp ); /*(Qx+Qx) */
2553 : }
2554 579563 : L_tmp = Mult_32_16( v, inv_len ); /*Q(14-exp+Qx+Qx) */
2555 579563 : v_16 = round_fx_sat( L_shl_sat( L_tmp, add( exp, sub( 2, Qx ) ) ) ); /*Qx */
2556 :
2557 579563 : return v_16;
2558 : }
2559 :
2560 :
2561 : /*---------------------------------------------------------------------*
2562 : * std_fx()
2563 : *
2564 : * Calculate the standard deviation of a vector
2565 : *---------------------------------------------------------------------*/
2566 :
2567 : /* o: standard deviation */
2568 6150 : Word16 std_fx(
2569 : const Word16 x[], /* i: input vector */
2570 : const Word16 len /* i: length of the input vector */
2571 : )
2572 : {
2573 : Word16 i;
2574 : Word32 L_tmp;
2575 : Word16 exp1, exp2, tmp;
2576 : Word32 stdev;
2577 :
2578 6150 : stdev = 0;
2579 6150 : move16();
2580 55350 : FOR( i = 0; i < len; i++ )
2581 : {
2582 49200 : L_tmp = L_mult( x[i], x[i] ); /*29 */
2583 49200 : stdev = L_add( stdev, L_shr( L_tmp, 3 ) ); /*26 */
2584 : }
2585 :
2586 6150 : IF( stdev != 0 )
2587 : {
2588 6150 : exp1 = norm_l( stdev );
2589 6150 : tmp = div_s( 16384, extract_h( L_shl( stdev, exp1 ) ) ); /*15 + 14 - (26 + exp1 - 16) */
2590 6150 : L_tmp = L_mult( tmp, len ); /*15 + 14 - (26 + exp1 - 16) + 1 */
2591 6150 : exp2 = norm_l( L_tmp );
2592 6150 : exp1 = add( sub( exp1, exp2 ), 11 );
2593 6150 : stdev = Isqrt_lc( L_shl( L_tmp, exp2 ), &exp1 ); /*31-exp1 */
2594 6150 : stdev = L_shl( stdev, sub( exp1, 1 ) ); /*30 */
2595 : }
2596 :
2597 6150 : return extract_h( stdev );
2598 : }
2599 :
2600 : /* o : the dot product x'*A*x */
2601 55800 : Word32 dot_product_mat_fx(
2602 : const Word16 *x, /* i : vector x Q15 */
2603 : const Word32 *A, /* i : matrix A Q0*/
2604 : const Word16 m /* i : vector & matrix size */
2605 :
2606 : )
2607 : {
2608 : Word16 i, j;
2609 : Word64 tmp_sum_64;
2610 : Word32 suma, tmp_sum;
2611 : const Word32 *pt_A;
2612 : const Word16 *pt_x;
2613 :
2614 55800 : pt_A = A;
2615 55800 : suma = L_deposit_l( 0 );
2616 :
2617 725400 : FOR( i = 0; i < m; i++ )
2618 : {
2619 669600 : tmp_sum_64 = 0;
2620 669600 : move64();
2621 669600 : pt_x = x;
2622 8704800 : FOR( j = 0; j < m; j++ )
2623 : {
2624 8035200 : tmp_sum_64 = W_mac_32_16( tmp_sum_64, *pt_A, *pt_x ); /*Q0 */
2625 8035200 : pt_A++;
2626 8035200 : pt_x++;
2627 : }
2628 669600 : tmp_sum = W_sat_m( tmp_sum_64 );
2629 669600 : suma = Madd_32_16( suma, tmp_sum, x[i] ); /*Q0 */
2630 : }
2631 :
2632 55800 : return suma;
2633 : }
2634 :
2635 :
2636 : /*-------------------------------------------------------------------*
2637 : * Vr_subt
2638 : *
2639 : * Subtract two Word16 vectors integer by integer
2640 : *-------------------------------------------------------------------*/
2641 :
2642 2631232 : void Vr_subt(
2643 : const Word16 x1[], /* i : Input vector 1 */
2644 : const Word16 x2[], /* i : Input vector 2 */
2645 : Word16 y[], /* o : Output vector that contains vector 1 - vector 2 */
2646 : Word16 N /* i : Vector lenght */
2647 : )
2648 : {
2649 : Word16 i;
2650 :
2651 44736772 : FOR( i = 0; i < N; i++ )
2652 : {
2653 42105540 : y[i] = sub_sat( x1[i], x2[i] );
2654 42105540 : move16();
2655 : }
2656 :
2657 2631232 : return;
2658 : }
2659 :
2660 :
2661 : /*-------------------------------------------------------------------*
2662 : * vquant()
2663 : *
2664 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
2665 : *
2666 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
2667 : * Index of the winning codevector and the winning vector itself are returned.
2668 : *-------------------------------------------------------------------*/
2669 :
2670 : /* o: index of the winning codevector */
2671 1402 : Word16 vquant_ivas_fx(
2672 : Word32 x[], /* i: vector to quantize Q25 */
2673 : const Word32 x_mean[], /* i: vector mean to subtract (0 if none) Q25 */
2674 : Word32 xq[], /* o: quantized vector Q25 */
2675 : const Word32 cb[], /* i: codebook Q25 */
2676 : const Word16 dim, /* i: dimension of codebook vectors */
2677 : const Word16 cbsize /* i: codebook size */
2678 : )
2679 : {
2680 : Word16 c, d, idx, j;
2681 : Word32 L_dist, L_tmp, L_mindist;
2682 :
2683 1402 : idx = 0;
2684 1402 : move16();
2685 1402 : L_mindist = MAX_32;
2686 1402 : move32();
2687 1402 : IF( x_mean != 0 )
2688 : {
2689 0 : FOR( d = 0; d < dim; d++ )
2690 : {
2691 : /*x[d] -= x_mean[d]; */
2692 0 : x[d] = L_sub( x[d], x_mean[d] );
2693 0 : move32(); /*Qx */
2694 : }
2695 : }
2696 1402 : j = 0;
2697 1402 : move16();
2698 19330 : FOR( c = 0; c < cbsize; c++ )
2699 : {
2700 17928 : L_dist = 0;
2701 17928 : move32();
2702 :
2703 89640 : FOR( d = 0; d < dim; d++ )
2704 : {
2705 : /*tmp = x[d] - cb[j++];*/
2706 71712 : L_tmp = L_sub( x[d], cb[j++] ); // Q25
2707 71712 : L_dist = L_add( L_dist, Mpy_32_32( L_tmp, L_tmp ) ); // (Q25, Q25) -> Q19
2708 : }
2709 17928 : if ( LT_32( L_dist, L_mindist ) ) // Q19
2710 : {
2711 3119 : idx = c;
2712 3119 : move16();
2713 : }
2714 17928 : L_mindist = L_min( L_mindist, L_dist ); // Q19
2715 : }
2716 1402 : IF( xq == 0 )
2717 : {
2718 0 : return idx;
2719 : }
2720 :
2721 : /*j = idx*dim;*/
2722 1402 : j = i_mult2( idx, dim );
2723 7010 : FOR( d = 0; d < dim; d++ )
2724 : {
2725 5608 : xq[d] = cb[j++]; // Q25
2726 5608 : move32();
2727 : }
2728 1402 : IF( x_mean != 0 )
2729 : {
2730 0 : FOR( d = 0; d < dim; d++ )
2731 : {
2732 : /*xq[d] += x_mean[d]; */
2733 0 : xq[d] = L_add( xq[d], x_mean[d] ); // Q25
2734 0 : move32();
2735 : }
2736 : }
2737 :
2738 1402 : return idx;
2739 : }
2740 :
2741 : /* o: index of the winning codevector */
2742 240794 : Word16 vquant_fx(
2743 : Word16 x[], /* i: vector to quantize Q13 */
2744 : const Word16 x_mean[], /* i: vector mean to subtract (0 if none) Q13 */
2745 : Word16 xq[], /* o: quantized vector Q13 */
2746 : const Word16 cb[], /* i: codebook Q13 */
2747 : const Word16 dim, /* i: dimension of codebook vectors */
2748 : const Word16 cbsize /* i: codebook size */
2749 : )
2750 : {
2751 : Word16 tmp;
2752 : Word16 c, d, idx, j;
2753 : Word32 L_dist, /*L_tmp,*/ L_mindist;
2754 :
2755 240794 : idx = 0;
2756 240794 : move16();
2757 240794 : L_mindist = MAX_32;
2758 240794 : move32();
2759 240794 : IF( x_mean != 0 )
2760 : {
2761 478512 : FOR( d = 0; d < dim; d++ )
2762 : {
2763 : /*x[d] -= x_mean[d]; */
2764 354794 : x[d] = sub( x[d], x_mean[d] );
2765 354794 : move16(); /*Qx */
2766 : }
2767 : }
2768 240794 : j = 0;
2769 240794 : move16();
2770 8863778 : FOR( c = 0; c < cbsize; c++ )
2771 : {
2772 8622984 : L_dist = 0;
2773 8622984 : move32();
2774 :
2775 37611752 : FOR( d = 0; d < dim; d++ )
2776 : {
2777 : /*tmp = x[d] - cb[j++];*/
2778 28988768 : tmp = sub( x[d], cb[j++] ); /*Qx */
2779 28988768 : L_dist = L_mac0( L_dist, tmp, tmp );
2780 : }
2781 8622984 : if ( LT_32( L_dist, L_mindist ) )
2782 : {
2783 941229 : idx = c;
2784 941229 : move16();
2785 : }
2786 8622984 : L_mindist = L_min( L_mindist, L_dist );
2787 : }
2788 240794 : IF( xq == 0 )
2789 : {
2790 0 : return idx;
2791 : }
2792 :
2793 : /*j = idx*dim;*/
2794 240794 : j = i_mult2( idx, dim );
2795 1063892 : FOR( d = 0; d < dim; d++ )
2796 : {
2797 823098 : xq[d] = cb[j++];
2798 823098 : move16();
2799 : }
2800 240794 : IF( x_mean != 0 )
2801 : {
2802 478512 : FOR( d = 0; d < dim; d++ )
2803 : {
2804 : /*xq[d] += x_mean[d]; */
2805 354794 : xq[d] = add( xq[d], x_mean[d] );
2806 354794 : move16();
2807 : }
2808 : }
2809 :
2810 240794 : return idx;
2811 : }
2812 :
2813 :
2814 : /*-------------------------------------------------------------------*
2815 : * w_vquant_fx()
2816 : *
2817 : * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
2818 : *
2819 : * Searches a given codebook to find the nearest neighbour in Euclidean space.
2820 : * Weights are put on the error for each vector element.
2821 : * Index of the winning codevector and the winning vector itself are returned.
2822 : *-------------------------------------------------------------------*/
2823 :
2824 50070 : Word16 w_vquant_fx(
2825 : Word16 x[], /* i: vector to quantize in Q10 */
2826 : Word16 Qx,
2827 : const Word16 weights[], /* i: error weights in Q0 */
2828 : Word16 xq[], /* o: quantized vector in Q15 */
2829 : const Word16 cb[], /* i: codebook in Q15 */
2830 : const Word16 cbsize, /* i: codebook size */
2831 : const Word16 rev_vect /* i: reverse codebook vectors */
2832 : )
2833 : {
2834 : Word16 tmp;
2835 : Word16 c, idx, j;
2836 : Word32 dist, minDist;
2837 :
2838 50070 : idx = 0;
2839 50070 : move16();
2840 50070 : minDist = 0x7fffffffL;
2841 50070 : move32();
2842 50070 : Qx = sub( 15, Qx );
2843 :
2844 50070 : j = 0;
2845 50070 : move16();
2846 50070 : IF( rev_vect )
2847 : {
2848 1838488 : FOR( c = 0; c < cbsize; c++ )
2849 : {
2850 1826207 : dist = L_deposit_l( 0 );
2851 :
2852 1826207 : tmp = sub_sat( x[3], shr( cb[j++], Qx ) );
2853 1826207 : if ( weights[3] != 0 )
2854 : {
2855 1652193 : dist = L_mac0_sat( dist, tmp, tmp );
2856 : }
2857 1826207 : tmp = sub_sat( x[2], shr( cb[j++], Qx ) );
2858 1826207 : if ( weights[2] != 0 )
2859 : {
2860 1694444 : dist = L_mac0_sat( dist, tmp, tmp );
2861 : }
2862 1826207 : tmp = sub_sat( x[1], shr( cb[j++], Qx ) );
2863 1826207 : if ( weights[1] != 0 )
2864 : {
2865 1788879 : dist = L_mac0_sat( dist, tmp, tmp );
2866 : }
2867 1826207 : tmp = sub_sat( x[0], shr( cb[j++], Qx ) );
2868 1826207 : if ( weights[0] != 0 )
2869 : {
2870 1738514 : dist = L_mac0_sat( dist, tmp, tmp );
2871 : }
2872 1826207 : if ( LT_32( dist, minDist ) )
2873 : {
2874 131688 : idx = c;
2875 131688 : move16();
2876 : }
2877 1826207 : minDist = L_min( minDist, dist );
2878 : }
2879 :
2880 12281 : IF( xq == 0 )
2881 : {
2882 0 : return idx;
2883 : }
2884 :
2885 12281 : j = shl( idx, 2 );
2886 12281 : xq[3] = cb[j++];
2887 12281 : move16(); /* in Q15 */
2888 12281 : xq[2] = cb[j++];
2889 12281 : move16(); /* in Q15 */
2890 12281 : xq[1] = cb[j++];
2891 12281 : move16(); /* in Q15 */
2892 12281 : xq[0] = cb[j++];
2893 12281 : move16(); /* in Q15 */
2894 : }
2895 : ELSE
2896 : {
2897 2038426 : FOR( c = 0; c < cbsize; c++ )
2898 : {
2899 2000637 : dist = L_deposit_l( 0 );
2900 :
2901 2000637 : tmp = sub_sat( x[0], shr( cb[j++], Qx ) );
2902 2000637 : if ( weights[0] != 0 )
2903 : {
2904 1839764 : dist = L_mac0_sat( dist, tmp, tmp );
2905 : }
2906 2000637 : tmp = sub_sat( x[1], shr( cb[j++], Qx ) );
2907 2000637 : if ( weights[1] != 0 )
2908 : {
2909 1893642 : dist = L_mac0_sat( dist, tmp, tmp );
2910 : }
2911 2000637 : tmp = sub_sat( x[2], shr( cb[j++], Qx ) );
2912 2000637 : if ( weights[2] != 0 )
2913 : {
2914 1911124 : dist = L_mac0_sat( dist, tmp, tmp );
2915 : }
2916 2000637 : tmp = sub_sat( x[3], shr( cb[j++], Qx ) );
2917 2000637 : if ( weights[3] != 0 )
2918 : {
2919 1865818 : dist = L_mac0_sat( dist, tmp, tmp );
2920 : }
2921 2000637 : if ( LT_32( dist, minDist ) )
2922 : {
2923 196137 : idx = c;
2924 196137 : move16();
2925 : }
2926 2000637 : minDist = L_min( minDist, dist );
2927 : }
2928 :
2929 37789 : IF( xq == 0 )
2930 : {
2931 25035 : return idx;
2932 : }
2933 :
2934 12754 : j = shl( idx, 2 );
2935 12754 : xq[0] = cb[j++];
2936 12754 : move16(); /* in Q15 */
2937 12754 : xq[1] = cb[j++];
2938 12754 : move16(); /* in Q15 */
2939 12754 : xq[2] = cb[j++];
2940 12754 : move16(); /* in Q15 */
2941 12754 : xq[3] = cb[j++];
2942 12754 : move16(); /* in Q15 */
2943 : }
2944 :
2945 25035 : return idx;
2946 : }
2947 :
2948 :
2949 : /*-------------------------------------------------------------------*
2950 : * Emaximum:
2951 : *
2952 : * Find index of a maximum energy in a vector
2953 : *-------------------------------------------------------------------*/
2954 :
2955 : /* o : return index with max energy value in vector Q0 */
2956 20653646 : Word16 emaximum_fx(
2957 : const Word16 Qvec, /* i : Q of input vector Q0 */
2958 : const Word16 *vec, /* i : input vector Qx */
2959 : const Word16 lvec, /* i : length of input vector Q0 */
2960 : Word32 *ener_max /* o : maximum energy value Q0 */
2961 : )
2962 : {
2963 : Word16 j, ind;
2964 : Word32 L_tmp, L_tmp1;
2965 : Word32 emax;
2966 :
2967 20653646 : emax = L_mult0( vec[0], vec[0] );
2968 20653646 : ind = 0;
2969 20653646 : move16();
2970 :
2971 668952041 : FOR( j = 1; j < lvec; j++ )
2972 : {
2973 648298395 : L_tmp = L_mult0( vec[j], vec[j] );
2974 648298395 : L_tmp1 = L_sub( L_tmp, emax );
2975 648298395 : if ( L_tmp1 > 0 )
2976 : {
2977 82490937 : ind = j;
2978 82490937 : move16();
2979 : }
2980 648298395 : emax = L_max( emax, L_tmp );
2981 : }
2982 :
2983 20653646 : *ener_max = L_shr_sat( emax, add( Qvec, Qvec ) );
2984 20653646 : move32();
2985 :
2986 20653646 : return ind;
2987 : }
2988 :
2989 : /* o : return index with max energy value in vector Q0 */
2990 33916 : Word16 emaximum_32fx(
2991 : const Word16 Qvec, /* i : Q of input vector Q0 */
2992 : const Word32 *vec, /* i : input vector Qx */
2993 : const Word16 lvec, /* i : length of input vector Q0 */
2994 : Word32 *ener_max /* o : maximum energy value Q0 */
2995 : )
2996 : {
2997 : Word16 j, ind;
2998 : Word64 W_tmp, W_tmp1;
2999 : Word64 emax;
3000 :
3001 33916 : emax = W_mult0_32_32( vec[0], vec[0] );
3002 33916 : ind = 0;
3003 33916 : move16();
3004 :
3005 3241503 : FOR( j = 1; j < lvec; j++ )
3006 : {
3007 3207587 : W_tmp = W_mult0_32_32( vec[j], vec[j] );
3008 3207587 : W_tmp1 = W_sub( W_tmp, emax );
3009 3207587 : if ( W_tmp1 > 0 )
3010 : {
3011 262839 : ind = j;
3012 262839 : move16();
3013 : }
3014 3207587 : if ( LE_64( emax, W_tmp ) )
3015 : {
3016 265535 : emax = W_tmp;
3017 265535 : move64();
3018 : }
3019 : }
3020 :
3021 33916 : *ener_max = W_extract_l( W_shr( emax, add( Qvec, Qvec ) ) );
3022 33916 : move32();
3023 :
3024 33916 : return ind;
3025 : }
3026 :
3027 :
3028 : /*-------------------------------------------------------------------*
3029 : * mean32:
3030 : *
3031 : * Find the mean of a 32 bits vector
3032 : *-------------------------------------------------------------------*/
3033 : /* o : mean of the elements of the vector */
3034 4852 : Word32 Mean32(
3035 : const Word32 in[], /* i : input vector */
3036 : const Word16 L /* i : length of input vector */
3037 : )
3038 : {
3039 : Word32 Ltmp;
3040 : Word16 inv_L;
3041 :
3042 4852 : inv_L = INV_BANDS9;
3043 4852 : move16();
3044 4852 : if ( EQ_16( L, 10 ) )
3045 : {
3046 4852 : inv_L = INV_BANDS10;
3047 4852 : move16();
3048 : }
3049 :
3050 4852 : Ltmp = sum32_fx( in, L );
3051 :
3052 4852 : Ltmp = Mult_32_16( Ltmp, inv_L );
3053 :
3054 4852 : return Ltmp;
3055 : }
3056 :
3057 : /* o : sum of all vector elements Qx*/
3058 190732 : Word32 sum32_fx(
3059 : const Word32 *vec, /* i : input vector Qx*/
3060 : const Word16 lvec /* i : length of input vector */
3061 : )
3062 : {
3063 : Word16 i;
3064 : Word32 tmp;
3065 :
3066 190732 : tmp = L_deposit_l( 0 );
3067 7091061 : FOR( i = 0; i < lvec; i++ )
3068 : {
3069 6900329 : tmp = L_add_sat( tmp, vec[i] ); /*Qx */
3070 : }
3071 :
3072 190732 : return tmp;
3073 : }
3074 :
3075 : /* o : sum of all vector elements Qx*/
3076 1202809 : Word16 sum16_fx(
3077 : const Word16 *vec, /* i : input vector Qx*/
3078 : const Word16 lvec /* i : length of input vector */
3079 : )
3080 : {
3081 : Word16 i;
3082 : Word16 tmp;
3083 1202809 : tmp = 0;
3084 1202809 : move16();
3085 8796113 : FOR( i = 0; i < lvec; i++ )
3086 : {
3087 7593304 : tmp = add_sat( tmp, vec[i] ); /*Qx */
3088 : }
3089 :
3090 1202809 : return tmp;
3091 : }
3092 :
3093 :
3094 : /*------------------------------------------------------------------*
3095 : * function Random
3096 : *
3097 : * Signed 16 bits random generator.
3098 : *------------------------------------------------------------------*/
3099 :
3100 : /* o : output random value */
3101 709983777 : Word16 Random(
3102 : Word16 *seed /* i/o: random seed */
3103 : )
3104 : {
3105 709983777 : *seed = extract_l( L_mac0( 13849L, *seed, 31821 ) );
3106 709983777 : move16();
3107 :
3108 709983777 : return *seed;
3109 : }
3110 :
3111 152188 : Word16 own_random2_fx(
3112 : Word16 seed )
3113 : {
3114 152188 : return extract_l( L_mac0( 13849, seed, 31821 ) );
3115 : }
3116 :
3117 :
3118 : /*---------------------------------------------------------------------
3119 : * sign_fx()
3120 : *
3121 : *---------------------------------------------------------------------*/
3122 :
3123 : /*! r: sign of x (+1/-1) */
3124 2102172 : Word16 sign_fx(
3125 : const Word32 x /* i : input value of x */
3126 : )
3127 : {
3128 2102172 : IF( LT_32( x, 0 ) )
3129 : {
3130 697824 : return -1;
3131 : }
3132 : ELSE
3133 : {
3134 1404348 : return 1;
3135 : }
3136 : }
3137 :
3138 0 : Word16 sign16_fx(
3139 : const Word16 x /* i : input value of x */
3140 : )
3141 : {
3142 0 : IF( LT_16( x, 0 ) )
3143 : {
3144 0 : return -1;
3145 : }
3146 : ELSE
3147 : {
3148 0 : return 1;
3149 : }
3150 : }
3151 :
3152 :
3153 : /*------------------------------------------------------------------*
3154 : * function Div_32_optmz
3155 : *
3156 : * Performs 32 bits interger division
3157 : *------------------------------------------------------------------*/
3158 :
3159 1469799 : static Word32 Div_32_optmz(
3160 : Word32 L_num,
3161 : Word16 denom_hi )
3162 : {
3163 : Word16 approx, hi, lo, n_hi, n_lo;
3164 : Word32 L_32;
3165 :
3166 : /* First approximation: 1 / L_denom = 1/denom_hi */
3167 :
3168 1469799 : approx = div_s( (Word16) 0x3fff, denom_hi );
3169 :
3170 : /* 1/L_denom = approx * (2.0 - L_denom * approx) */
3171 :
3172 1469799 : L_32 = L_msu( (Word32) 0x7fffffffL, denom_hi, approx );
3173 :
3174 1469799 : lo = L_Extract_lc( L_32, &hi );
3175 1469799 : L_32 = Mpy_32_16( hi, lo, approx );
3176 :
3177 : /* L_num * (1/L_denom) */
3178 :
3179 1469799 : lo = L_Extract_lc( L_32, &hi );
3180 1469799 : n_lo = L_Extract_lc( L_num, &n_hi );
3181 1469799 : L_32 = Mpy_32( n_hi, n_lo, hi, lo );
3182 :
3183 1469799 : return ( L_32 );
3184 : }
3185 :
3186 :
3187 : /*------------------------------------------------------------------*
3188 : * function iDiv_and_mod_32
3189 : *
3190 : * return the quotient and the modulo 32 bits numerator divided by a 16 bit denominator
3191 : * The denominator might be right shifted by 1
3192 : *------------------------------------------------------------------*/
3193 :
3194 1469799 : void iDiv_and_mod_32(
3195 : const Word32 Numer, /* i : 32 bits numerator */
3196 : const Word16 Denom, /* i : 16 bits denominator */
3197 : Word32 *Int_quotient, /* o : integer result of the division (int)(num/den) */
3198 : Word32 *Int_mod, /* o : modulo result of the division num-((int)(num/den)*den)*/
3199 : const Word16 rshift /* i : 0 if no right shift / 1 if the denom is right shifted by 1 */
3200 : )
3201 : {
3202 : Word32 Quotient;
3203 : Word16 expA, expB;
3204 : Word32 N, TEMP;
3205 : Word16 D;
3206 :
3207 : /* Normalize 'Numer' & 'Denom' */
3208 : /* Use Temporary to Preserve the Original Value */
3209 1469799 : expA = norm_l( Numer );
3210 1469799 : N = L_shl( Numer, expA );
3211 1469799 : expB = norm_s( Denom );
3212 1469799 : D = shl( Denom, expB );
3213 :
3214 : /* Need to shift right 'Numer' by 1? */
3215 1469799 : if ( L_mac( N, D, -32768L ) >= 0 )
3216 : {
3217 842202 : expA = sub( expA, 1 );
3218 : }
3219 1469799 : N = L_shl( Numer, expA );
3220 :
3221 : /* Perform Approximation of the Division
3222 : * Since 'lo' part is '0' AND 'denom' is supposed to be constant in the targeted usage
3223 : * one could import the Div32 code and pre-calc the div_s and eliminate all calcs
3224 : * with 'lo' to save some complexity */
3225 :
3226 1469799 : Quotient = Div_32_optmz( N, D ); /* takes 36 clocks */
3227 : /* Bring Back to Q0 (minus 2 because we removed the left shift by 2 in the Div32_optmz) */
3228 1469799 : IF( rshift )
3229 : {
3230 0 : Quotient = L_shr( Quotient, add( 15 - 2, sub( expA, sub( expB, 1 ) ) ) );
3231 : }
3232 : ELSE
3233 : {
3234 1469799 : Quotient = L_shr( Quotient, add( 15 - 2, sub( expA, expB ) ) );
3235 : }
3236 :
3237 : /* Cross Check (do Quotient x Divisor)
3238 : * The Quotient is unsigned but we cannot just use extract_h because
3239 : * extract_l on the low part will get the sign of the bit #15.
3240 : * In a 32 bits value, what is in bits 0-15 is un unsigned 16 bits value.
3241 : * So, we shift left by 1, extract the hi part and mult it by 32768 (hence the L_shl by 16-1.
3242 : * Then we take 15 bits left (mask the others) and multiply by Denom.
3243 : * Technically this could overflow. But since we are mutiplying to get
3244 : * back to the Numer value that fitted in a 32 bits, doing Divisor x Quotient
3245 : * must necessarily fit in a 32 bits
3246 : * It is assumed that all are positive values. If not, one could
3247 : * check the sign of the numer and denom, turn them into abs values
3248 : * and restore the sign after*/
3249 1469799 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
3250 1469799 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
3251 :
3252 :
3253 : /* Here we test to see if the previous "Quotient x Divisor" (or TEMP) is too small
3254 : * that is the "Numer" minus TEMP is bigger or Equal to the Divisor.
3255 : * If it is then the quotient is too small. We need to increase it by 1.
3256 : * That is caused by our Div32 fractionnal division that can be off by 1
3257 : * sometimes.
3258 : * In some cases, when the divisor is very small (like 10 or something)
3259 : * the quotient could be off by more than 1 and we would need a loop
3260 : * to check again. That is not the case here with the current divisor */
3261 1469799 : IF( rshift )
3262 : {
3263 0 : TEMP = L_shl( TEMP, 1 );
3264 0 : WHILE( L_msu0( L_sub( Numer, TEMP ), 2, Denom ) >= 0 )
3265 : {
3266 0 : Quotient = L_add( Quotient, 1 );
3267 0 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
3268 0 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
3269 0 : TEMP = L_shl( TEMP, 1 );
3270 : }
3271 : }
3272 : ELSE{
3273 1754300 : WHILE( L_msu0( L_sub( Numer, TEMP ), 1, Denom ) >= 0 ){
3274 284501 : Quotient = L_add( Quotient, 1 );
3275 284501 : TEMP = L_shl( L_mult0( extract_h( L_shl( Quotient, 1 ) ), Denom ), 16 - 1 );
3276 284501 : TEMP = L_mac0( TEMP, extract_l( L_and( 0x7FFF, Quotient ) ), Denom );
3277 : }
3278 : }
3279 1469799 : *Int_quotient = Quotient;
3280 1469799 : move32();
3281 1469799 : IF( L_msu0( L_sub( Numer, TEMP ), 1, Denom ) == 0 )
3282 : {
3283 0 : *Int_mod = 0L;
3284 0 : move32();
3285 : }
3286 : ELSE
3287 : {
3288 1469799 : *Int_mod = L_sub( Numer, TEMP );
3289 1469799 : move32();
3290 : }
3291 1469799 : }
3292 :
3293 : /*===================================================================*/
3294 : /* FUNCTION : pz_filter_sp_fx () */
3295 : /*-------------------------------------------------------------------*/
3296 : /* PURPOSE : Generic pole-zero filter routine, with single */
3297 : /* precision memory */
3298 : /*-------------------------------------------------------------------*/
3299 : /* INPUT ARGUMENTS : */
3300 : /* */
3301 : /* _ (Word16 []) b : zero filter coefficients (Qc). */
3302 : /* _ (Word16 []) a : pole filter coefficients (Qc), a(0)=1 in Qc */
3303 : /* _ (Word16 []) x : input signal (Qn). */
3304 : /* _ (Word16) PNR : NR filter order */
3305 : /* _ (Word16) PDR : DR filter order */
3306 : /* _ (Word16) N : number of input samples. */
3307 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
3308 : /*-------------------------------------------------------------------*/
3309 : /* OUTPUT ARGUMENTS : */
3310 : /* */
3311 : /* _ (Word16 []) y : output signal (Qn) */
3312 : /*-------------------------------------------------------------------*/
3313 : /* INPUT/OUTPUT ARGUMENTS : */
3314 : /* */
3315 : /* _ (Word16 []) buf : filter memory (Qn-Qa). */
3316 : /*-------------------------------------------------------------------*/
3317 : /* RETURN ARGUMENTS : _ None. */
3318 : /*===================================================================*/
3319 :
3320 0 : void pz_filter_sp_fx(
3321 : const Word16 b[],
3322 : const Word16 a[],
3323 : Word16 x[],
3324 : Word16 y[],
3325 : Word16 buf[],
3326 : Word16 PNR,
3327 : Word16 PDR,
3328 : Word16 N,
3329 : Word16 Qa )
3330 : {
3331 : Word16 i, j;
3332 : Word16 s;
3333 : Word16 s_mem;
3334 : Word32 Ltemp1;
3335 : Word32 Lacc;
3336 0 : s = negate( Qa );
3337 0 : s = add( s, s ); /* s=-2Qa*/
3338 0 : s = add( s, 1 );
3339 0 : FOR( i = 0; i < N; i++ )
3340 : {
3341 0 : Lacc = L_deposit_h( x[i] ); /* Lacc in Q(16+Qn)*/
3342 0 : Lacc = L_shl( Lacc, s ); /* Lacc=x[i] in Q(16+Qn-2Qa+1)*/
3343 0 : FOR( j = PDR - 1; j >= 0; j-- )
3344 0 : Lacc = L_msu_sat( Lacc, buf[j], a[j + 1] ); /*Q(16+Qn-2Qa+1)*/
3345 :
3346 :
3347 0 : Lacc = L_shr( Lacc, 1 );
3348 0 : Ltemp1 = L_add_sat( L_shl_sat( Lacc, Qa ), 0x08000 );
3349 0 : s_mem = extract_h( Ltemp1 );
3350 :
3351 0 : Lacc = L_deposit_l( 0 );
3352 0 : FOR( j = PNR - 1; j >= 0; j-- )
3353 0 : Lacc = L_mac_sat( Lacc, buf[j], b[j + 1] );
3354 0 : Lacc = L_mac_sat( Lacc, s_mem, b[0] );
3355 : /* Lacc in Q(1+Qc+Qn-Qa)*/
3356 :
3357 0 : FOR( j = s_max( PDR, PNR ) - 1; j > 0; j-- )
3358 : {
3359 : /* Update filter memory */
3360 0 : buf[j] = buf[j - 1];
3361 0 : move16();
3362 : }
3363 0 : buf[0] = s_mem;
3364 0 : move16();
3365 :
3366 0 : Ltemp1 = L_add_sat( L_shr_sat( Lacc, s ), 0x08000 ); /* Ltemp1 in Qc+Qa+Qn=Q(Qn) */
3367 0 : y[i] = extract_h( Ltemp1 ); /* y[i] in Qn */
3368 0 : move16();
3369 : }
3370 :
3371 0 : return;
3372 : }
3373 :
3374 :
3375 208331 : Word32 root_a_fx(
3376 : Word32 a,
3377 : Word16 Q_a,
3378 : Word16 *exp_out )
3379 : {
3380 : Word16 exp, tmp;
3381 : Word32 L_tmp;
3382 :
3383 208331 : IF( a <= 0 )
3384 : {
3385 2790 : *exp_out = 0;
3386 2790 : move16();
3387 2790 : return 0;
3388 : }
3389 :
3390 205541 : exp = norm_l( a );
3391 205541 : tmp = extract_h( L_shl( a, exp ) );
3392 205541 : exp = sub( exp, sub( 30, Q_a ) );
3393 205541 : tmp = div_s( 16384, tmp );
3394 205541 : L_tmp = L_deposit_h( tmp );
3395 205541 : L_tmp = Isqrt_lc( L_tmp, &exp );
3396 :
3397 205541 : *exp_out = exp;
3398 205541 : move16();
3399 :
3400 205541 : return L_tmp;
3401 : }
3402 :
3403 :
3404 1980296 : Word32 root_a_over_b_fx(
3405 : Word32 a, /* Q(Q_a) */
3406 : Word16 Q_a,
3407 : Word32 b, /* Q(Q_b) */
3408 : Word16 Q_b,
3409 : Word16 *exp_out )
3410 : {
3411 : Word16 tmp, num, den, scale;
3412 : Word16 exp, exp_num, exp_den;
3413 : Word32 L_tmp;
3414 :
3415 1980296 : test();
3416 1980296 : IF( ( a <= 0 ) || ( b <= 0 ) )
3417 : {
3418 10439 : *exp_out = 0;
3419 10439 : move16();
3420 10439 : return 0;
3421 : }
3422 :
3423 1969857 : exp_num = norm_l( b );
3424 1969857 : num = round_fx_sat( L_shl_sat( b, exp_num ) );
3425 1969857 : exp_num = sub( sub( 30, exp_num ), Q_b );
3426 :
3427 1969857 : exp_den = norm_l( a );
3428 1969857 : den = round_fx_sat( L_shl_sat( a, exp_den ) );
3429 1969857 : exp_den = sub( sub( 30, exp_den ), Q_a );
3430 :
3431 1969857 : scale = shr( sub( den, num ), 15 );
3432 1969857 : num = shl_sat( num, scale );
3433 1969857 : exp_num = sub( exp_num, scale );
3434 :
3435 1969857 : tmp = div_s( num, den );
3436 1969857 : exp = sub( exp_num, exp_den );
3437 :
3438 1969857 : L_tmp = L_deposit_h( tmp );
3439 1969857 : L_tmp = Isqrt_lc( L_tmp, &exp );
3440 :
3441 1969857 : *exp_out = exp;
3442 1969857 : move16();
3443 :
3444 1969857 : return L_tmp;
3445 : }
3446 :
3447 :
3448 332850 : Word32 root_a_over_b_ivas_fx(
3449 : Word32 a, /* Q(Q_a) */
3450 : Word16 Q_a,
3451 : Word32 b, /* Q(Q_b) */
3452 : Word16 Q_b,
3453 : Word16 *q_out )
3454 : {
3455 : Word16 shift_a, shift_b, shift;
3456 : Word32 mod_a, mod_b, one_in_Q_a, one_in_Q_b, half_in_Q_a, half_in_Q_b;
3457 : Word32 a_sqr, b_sqr, p0, p1, p2, approx;
3458 : Word16 exp;
3459 :
3460 332850 : test();
3461 332850 : IF( ( a <= 0 ) || ( b <= 0 ) )
3462 : {
3463 0 : *q_out = 0;
3464 0 : move16();
3465 0 : return 0;
3466 : }
3467 :
3468 332850 : one_in_Q_a = L_shl( 1, Q_a ); // 1.0f in Q_a
3469 332850 : one_in_Q_b = L_shl( 1, Q_b ); // 1.0f in Q_b
3470 332850 : half_in_Q_a = L_shr( one_in_Q_a, 1 ); // 0.5f in Q_a
3471 332850 : half_in_Q_b = L_shr( one_in_Q_b, 1 ); // 0.5f in Q_b
3472 :
3473 332850 : a = L_add( a, one_in_Q_a );
3474 332850 : b = L_add( b, one_in_Q_b );
3475 :
3476 : /* This next piece of code implements a "norm" function */
3477 : /* and returns the shift needed to scale "a" to have a */
3478 : /* 1 in the (MSB-1) position. This is equivalent to */
3479 : /* giving a value between 0.5 & 1.0. */
3480 :
3481 332850 : mod_a = a;
3482 332850 : move32();
3483 :
3484 332850 : shift_a = 0;
3485 332850 : move16();
3486 665700 : WHILE( GT_32( mod_a, one_in_Q_a ) )
3487 : {
3488 332850 : mod_a = L_shr( mod_a, 1 );
3489 332850 : shift_a = sub( shift_a, 1 );
3490 : }
3491 :
3492 332850 : WHILE( LT_32( mod_a, half_in_Q_a ) )
3493 : {
3494 0 : mod_a = L_shl( mod_a, 1 );
3495 0 : shift_a = add( shift_a, 1 );
3496 : }
3497 :
3498 332850 : shift_a = s_and( shift_a, -2 );
3499 332850 : mod_a = L_shl( a, shift_a ); // Q_a
3500 :
3501 : /* This next piece of code implements a "norm" function */
3502 : /* and returns the shift needed to scale "b" to have a */
3503 : /* 1 in the (MSB-1) position. This is equivalent to */
3504 : /* giving a value between 0.5 & 1.0. */
3505 332850 : mod_b = b;
3506 332850 : move32();
3507 :
3508 332850 : shift_b = 0;
3509 332850 : move16();
3510 665700 : WHILE( GT_32( mod_b, one_in_Q_b ) )
3511 : {
3512 332850 : mod_b = L_shr( mod_b, 1 );
3513 332850 : shift_b = sub( shift_b, 1 );
3514 : }
3515 :
3516 332850 : WHILE( LT_32( mod_b, half_in_Q_b ) )
3517 : {
3518 0 : mod_b = L_shl( mod_b, 1 );
3519 0 : shift_b = add( shift_b, 1 );
3520 : }
3521 :
3522 332850 : shift_b = s_and( shift_b, -2 );
3523 332850 : mod_b = L_shl( b, shift_b ); // Q_b
3524 :
3525 332850 : shift = shr( sub( shift_b, shift_a ), 1 );
3526 :
3527 332850 : a_sqr = W_extract_h( W_shl( W_mult0_32_32( mod_a, mod_a ), sub( 32, Q_a ) ) ); // Q_a
3528 332850 : b_sqr = W_extract_h( W_shl( W_mult0_32_32( mod_b, mod_b ), sub( 32, Q_b ) ) ); // Q_b
3529 :
3530 332850 : p2 = L_shl( -408505077 /* -0.7609f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
3531 332850 : p1 = L_shl( 1444612250 /* 2.6908f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
3532 332850 : p0 = L_shl( 385258566 /* 0.7176f in Q29 */, sub( Q_b, 31 ) ); // Qb-2
3533 :
3534 332850 : 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
3535 332850 : 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
3536 332850 : 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
3537 :
3538 : /* approx = p0 + p1 * mod_a + p2 * mod_a * mod_a; */
3539 332850 : approx = Madd_32_32( Mpy_32_32( p1, mod_a ), p2, a_sqr ); // Q_a+Q_b-33
3540 332850 : approx = L_add( approx, L_shl( p0, sub( Q_a, 31 ) ) ); // Q_a+Q_b-33
3541 :
3542 332850 : exp = sub( norm_l( approx ), 1 );
3543 332850 : approx = L_shl( approx, exp ); // // Q_a+Q_b-33+exp
3544 :
3545 332850 : *q_out = sub( add( sub( add( Q_a, Q_b ), 33 ), exp ), shift );
3546 332850 : move16();
3547 :
3548 332850 : return approx;
3549 : }
3550 :
3551 :
3552 : /*===================================================================*/
3553 : /* FUNCTION : fir_fx () */
3554 : /*-------------------------------------------------------------------*/
3555 : /* PURPOSE : Generic FIR filter routine */
3556 : /*-------------------------------------------------------------------*/
3557 : /* INPUT ARGUMENTS : */
3558 : /* */
3559 : /* _ (Word16 []) b : filter coefficients (Qc). */
3560 : /* _ (Word16 []) x : input signal (Qn). */
3561 : /* _ (Word16) P : filter order. */
3562 : /* _ (Word16) N : number of input samples. */
3563 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
3564 : /*-------------------------------------------------------------------*/
3565 : /* OUTPUT ARGUMENTS : */
3566 : /* */
3567 : /* _ (Word16 []) y : output signal (Qn) */
3568 : /*-------------------------------------------------------------------*/
3569 : /* INPUT/OUTPUT ARGUMENTS : */
3570 : /* */
3571 : /* _ : None */
3572 : /*-------------------------------------------------------------------*/
3573 : /* RETURN ARGUMENTS : _ None. */
3574 : /*===================================================================*/
3575 :
3576 261918 : void fir_fx(
3577 : const Word16 x[], /* i : input vector Qx*/
3578 : const Word16 h[], /* i : impulse response of the FIR filter Q12*/
3579 : Word16 y[], /* o : output vector (result of filtering) Qx*/
3580 : Word16 mem[], /* i/o: memory of the input signal (L samples) Qx*/
3581 : const Word16 L, /* i : input vector size */
3582 : const Word16 K, /* i : order of the FIR filter (K+1 coefs.) */
3583 : const Word16 upd, /* i : 1 = update the memory, 0 = not */
3584 : Word16 shift /* i : difference between Q15 and scaling of h[] */
3585 : )
3586 : {
3587 :
3588 : Word16 buf_in[L_FRAME32k + L_FILT_MAX];
3589 : Word16 i, j;
3590 : Word32 s;
3591 :
3592 : /* prepare the input buffer (copy and update memory) */
3593 261918 : Copy( mem, buf_in, K );
3594 261918 : Copy( x, buf_in + K, L );
3595 261918 : IF( upd )
3596 : {
3597 134 : Copy( buf_in + L, mem, K );
3598 : }
3599 :
3600 : /* do the filtering */
3601 80743046 : FOR( i = 0; i < L; i++ )
3602 : {
3603 80481128 : s = L_mult_sat( buf_in[K + i], h[0] );
3604 :
3605 431720512 : FOR( j = 1; j <= K; j++ )
3606 : {
3607 351239384 : s = L_mac_sat( s, h[j], buf_in[K + i - j] );
3608 : }
3609 :
3610 80481128 : s = L_shl_sat( s, shift );
3611 80481128 : y[i] = round_fx_sat( s ); /*Qx */
3612 80481128 : move16();
3613 : }
3614 :
3615 261918 : return;
3616 : }
3617 :
3618 :
3619 : /*-------------------------------------------------------------------*
3620 : * v_add_32()
3621 : *
3622 : * Addition of two vectors sample by sample
3623 : *-------------------------------------------------------------------*/
3624 :
3625 5888439 : void v_add_32(
3626 : const Word32 x1[], /* i : Input vector 1 */
3627 : const Word32 x2[], /* i : Input vector 2 */
3628 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
3629 : const Word16 N /* i : Vector length */
3630 : )
3631 : {
3632 : Word16 i;
3633 :
3634 1070937318 : FOR( i = 0; i < N; i++ )
3635 : {
3636 1065048879 : y[i] = L_add( x1[i], x2[i] );
3637 1065048879 : move32();
3638 : }
3639 :
3640 5888439 : return;
3641 : }
3642 :
3643 408929 : void v_shr_32(
3644 : Word32 x1[], /* i : Input vector 1 */
3645 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
3646 : const Word16 N, /* i : Vector length */
3647 : Word16 shift /*shift value*/
3648 : )
3649 : {
3650 : Word16 i;
3651 :
3652 295540651 : FOR( i = 0; i < N; i++ )
3653 : {
3654 295131722 : y[i] = L_shr( x1[i], shift );
3655 295131722 : move32();
3656 : }
3657 :
3658 408929 : return;
3659 : }
3660 :
3661 :
3662 : /*-------------------------------------------------------------------*
3663 : * v_sub_32()
3664 : *
3665 : * Subtraction of two vectors sample by sample
3666 : *-------------------------------------------------------------------*/
3667 :
3668 2544214 : void v_sub_32(
3669 : const Word32 x1[], /* i : Input vector 1 */
3670 : const Word32 x2[], /* i : Input vector 2 */
3671 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
3672 : const Word16 N /* i : Vector length */
3673 : )
3674 : {
3675 : Word16 i;
3676 :
3677 208130036 : FOR( i = 0; i < N; i++ )
3678 : {
3679 205585822 : y[i] = L_sub( x1[i], x2[i] );
3680 205585822 : move32();
3681 : }
3682 :
3683 2544214 : return;
3684 : }
3685 :
3686 :
3687 : /*-------------------------------------------------------------------*
3688 : * v_add_16()
3689 : *
3690 : * Addition of two vectors sample by sample
3691 : *-------------------------------------------------------------------*/
3692 :
3693 81086 : void v_add_16(
3694 : const Word16 x1[], /* i : Input vector 1 */
3695 : const Word16 x2[], /* i : Input vector 2 */
3696 : Word16 y[], /* o : Output vector that contains vector 1 + vector 2 */
3697 : const Word16 N /* i : Vector length */
3698 : )
3699 : {
3700 : Word16 i;
3701 :
3702 5122696 : FOR( i = 0; i < N; i++ )
3703 : {
3704 5041610 : y[i] = add_sat( x1[i], x2[i] );
3705 5041610 : move16();
3706 : }
3707 :
3708 81086 : return;
3709 : }
3710 :
3711 :
3712 : /*-------------------------------------------------------------------*
3713 : * v_sub_16()
3714 : *
3715 : * Subtraction of two vectors sample by sample
3716 : *-------------------------------------------------------------------*/
3717 :
3718 245484 : void v_sub_16(
3719 : const Word16 x1[], /* i : Input vector 1 */
3720 : const Word16 x2[], /* i : Input vector 2 */
3721 : Word16 y[], /* o : Output vector that contains vector 1 - vector 2 */
3722 : const Word16 N /* i : Vector length */
3723 : )
3724 : {
3725 : Word16 i;
3726 :
3727 6834083 : FOR( i = 0; i < N; i++ )
3728 : {
3729 6588599 : y[i] = sub_sat( x1[i], x2[i] );
3730 6588599 : move16();
3731 : }
3732 :
3733 245484 : return;
3734 : }
3735 :
3736 :
3737 : /*--------------------------------------------------------------------------------*/
3738 : /* squant_fx() */
3739 : /*--------------------------------------------------------------------------------*/
3740 :
3741 : /* o: index of the winning codeword */
3742 5082684 : Word16 squant_fx(
3743 : const Word16 x, /* i: scalar value to quantize */
3744 : Word16 *xq, /* o: quantized value */
3745 : const Word16 cb[], /* i: codebook */
3746 : const Word16 cbsize /* i: codebook size */
3747 : )
3748 : {
3749 : Word16 tmp;
3750 : Word16 c, idx;
3751 : Word32 L_mindist, L_dist;
3752 :
3753 5082684 : idx = 0;
3754 5082684 : move16();
3755 5082684 : L_mindist = MAX_32;
3756 5082684 : move32();
3757 :
3758 25307048 : FOR( c = 0; c < cbsize; c++ )
3759 : {
3760 20224364 : L_dist = L_deposit_l( 0 );
3761 20224364 : tmp = sub_sat( x, cb[c] );
3762 :
3763 : /*dist += tmp*tmp; */
3764 20224364 : L_dist = L_mac_sat( L_dist, tmp, tmp );
3765 :
3766 20224364 : if ( LT_32( L_dist, L_mindist ) )
3767 : {
3768 11590892 : idx = c;
3769 11590892 : move16();
3770 : }
3771 20224364 : L_mindist = L_min( L_mindist, L_dist );
3772 : }
3773 :
3774 5082684 : *xq = cb[idx];
3775 5082684 : move16();
3776 :
3777 5082684 : return idx;
3778 : }
3779 :
3780 : /*! r: index of the winning codeword */
3781 401128 : Word16 squant_int_fx(
3782 : UWord8 x, /* i : scalar value to quantize */
3783 : UWord8 *xq, /* o : quantized value */
3784 : const UWord8 *cb, /* i : codebook */
3785 : const Word16 cbsize /* i : codebook size */
3786 : )
3787 : {
3788 : Word16 i, idx;
3789 : Word32 mindist, d;
3790 :
3791 401128 : idx = 0;
3792 401128 : move16();
3793 401128 : mindist = 10000000; // Q0
3794 401128 : move32();
3795 3348042 : FOR( i = 0; i < cbsize; i++ )
3796 : {
3797 2946914 : d = L_mult0( sub( x, cb[i] ), sub( x, cb[i] ) );
3798 2946914 : IF( LT_32( d, mindist ) )
3799 : {
3800 723360 : mindist = d;
3801 723360 : move32();
3802 723360 : idx = i;
3803 723360 : move16();
3804 : }
3805 : }
3806 401128 : *xq = cb[idx];
3807 401128 : move16();
3808 :
3809 401128 : return idx;
3810 : }
3811 :
3812 : /*===================================================================*/
3813 : /* FUNCTION : pz_filter_dp_fx () */
3814 : /*-------------------------------------------------------------------*/
3815 : /* PURPOSE : Generic pole-zero filter routine, with double */
3816 : /* precision memory, transposed direct form II */
3817 : /*-------------------------------------------------------------------*/
3818 : /* INPUT ARGUMENTS : */
3819 : /* */
3820 : /* _ (Word16 []) b : zero filter coefficients (Qc). */
3821 : /* _ (Word16 []) a : pole filter coefficients (Qc), a(0)=1 */
3822 : /* _ (Word16 []) x : input signal (Qx). */
3823 : /* _ (Word16) P : filter order. */
3824 : /* _ (Word16) N : number of input samples. */
3825 : /* _ (Word16) Qa : Q factor compensation (Qa=16-Qc) */
3826 : /*-------------------------------------------------------------------*/
3827 : /* OUTPUT ARGUMENTS : */
3828 : /* */
3829 : /* _ (Word16 []) y : output signal (Qx) */
3830 : /*-------------------------------------------------------------------*/
3831 : /* INPUT/OUTPUT ARGUMENTS : */
3832 : /* */
3833 : /* _ (Word32 []) buf : filter memory (Qx+Qc) */
3834 : /*-------------------------------------------------------------------*/
3835 : /* RETURN ARGUMENTS : _ None. */
3836 : /*===================================================================*/
3837 :
3838 0 : void pz_filter_dp_fx(
3839 : const Word16 b[],
3840 : const Word16 a[],
3841 : Word16 x[],
3842 : Word16 y[],
3843 : Word32 buf[],
3844 : Word16 PNR,
3845 : Word16 PDR,
3846 : Word16 N,
3847 : Word16 Qa )
3848 : {
3849 : Word16 i, j;
3850 : Word16 s;
3851 : Word32 s_mem;
3852 : Word32 Ltemp1;
3853 : Word32 Lacc;
3854 :
3855 0 : s = negate( Qa );
3856 0 : s = add( s, s ); /* s=-2Qa */
3857 0 : s = add( s, 1 );
3858 0 : FOR( i = 0; i < N; i++ )
3859 : {
3860 0 : Lacc = L_deposit_h( x[i] ); /* Lacc in Q(16+Qn)*/
3861 0 : Lacc = L_shl( Lacc, s ); /* Lacc=x[i] in Q(16+Qn-2Qa+1)*/
3862 0 : FOR( j = PDR - 1; j >= 0; j-- )
3863 0 : Lacc = Msub_32_16( Lacc, buf[j], a[j + 1] ); /*Q(16+Qn-2Qa+1)*/
3864 :
3865 0 : s_mem = L_shl_sat( Lacc, sub( Qa, 1 ) ); /*Qn-Qa+16=Qn+Qc*/
3866 0 : Lacc = L_deposit_l( 0 );
3867 0 : FOR( j = PNR - 1; j >= 0; j-- )
3868 0 : Lacc = Madd_32_16( Lacc, buf[j], b[j + 1] );
3869 0 : Lacc = Madd_32_16( Lacc, s_mem, b[0] );
3870 : /* Lacc in Q(1+Qc+Qn-Qa) */
3871 :
3872 0 : FOR( j = s_max( PDR, PNR ) - 1; j > 0; j-- )
3873 : {
3874 : /* Update filter memory */
3875 0 : buf[j] = buf[j - 1];
3876 0 : move16();
3877 : }
3878 0 : buf[0] = s_mem;
3879 0 : move16();
3880 :
3881 0 : Ltemp1 = L_shr_sat( Lacc, s ); /* Ltemp1 in Qc+Qa+Qn=Q(16+Qn) */
3882 0 : y[i] = extract_h( Ltemp1 ); /* y[i] in Qn */
3883 0 : move16();
3884 : }
3885 :
3886 0 : return;
3887 : }
3888 :
3889 :
3890 : /*-------------------------------------------------------------------*
3891 : * Copy_Scale_sig
3892 : *
3893 : * Up/down scale a 16 bits vector x and move it into y
3894 : *-------------------------------------------------------------------*/
3895 :
3896 8705614 : void Copy_Scale_sig32(
3897 : const Word32 x[], /* i : signal to scale input Qx */
3898 : Word32 y[], /* o : scaled signal output Qx */
3899 : const Word16 lg, /* i : size of x[] Q0 */
3900 : const Word16 exp0 /* i : exponent: x = round(x << exp) Qx ?exp */
3901 : )
3902 : {
3903 : Word16 i;
3904 : Word32 L_tmp;
3905 8705614 : Word16 tmp = exp0;
3906 :
3907 8705614 : IF( exp0 == 0 )
3908 : {
3909 990218857 : FOR( i = 0; i < lg; i++ )
3910 : {
3911 988740430 : y[i] = x[i];
3912 988740430 : move32();
3913 : }
3914 1478427 : return;
3915 : }
3916 7227187 : IF( exp0 < 0 )
3917 : {
3918 555664448 : FOR( i = 0; i < lg; i++ )
3919 : {
3920 550816670 : y[i] = L_shl_sat( x[i], tmp );
3921 550816670 : move16();
3922 : }
3923 4847778 : return;
3924 : }
3925 :
3926 2379409 : L_tmp = L_shl_sat( 1, exp0 - 1 );
3927 :
3928 312114354 : FOR( i = 0; i < lg; i++ )
3929 : {
3930 309734945 : y[i] = W_extract_l( W_mult_32_32( L_tmp, x[i] ) );
3931 309734945 : move32(); /* saturation can occur here */
3932 : }
3933 :
3934 2379409 : return;
3935 : }
3936 :
3937 :
3938 : /*-------------------------------------------------------------------*
3939 : * Copy_Scale_sig32_16
3940 : *
3941 : * Up/down scale a 32 bits vector and round to 16 bits vector
3942 : *-------------------------------------------------------------------*/
3943 11859813 : void Copy_Scale_sig32_16(
3944 : const Word32 *src, /* i : signal to scale Qx */
3945 : Word16 *dst, /* o : scaled signal Qx */
3946 : Word16 len, /* i : size of x[] Q0 */
3947 : Word16 exp0 ) /* i : exponent: x = round(x << exp) Qx ?exp */
3948 : {
3949 : Word16 i;
3950 : Word32 L_temp;
3951 :
3952 11859813 : IF( exp0 == 0 )
3953 : {
3954 194936928 : FOR( i = 0; i < len; i++ )
3955 : {
3956 190631105 : *dst++ = round_fx_sat( *src++ );
3957 190631105 : move16();
3958 : }
3959 4305823 : return;
3960 : }
3961 :
3962 4209079556 : FOR( i = 0; i < len; i++ )
3963 : {
3964 4201525566 : L_temp = L_shl_sat( *src++, exp0 );
3965 :
3966 4201525566 : *dst++ = round_fx_sat( L_temp );
3967 4201525566 : move16();
3968 : }
3969 :
3970 7553990 : return;
3971 : }
3972 :
3973 : /*-------------------------------------------------------------------*
3974 : * v_multc_att()
3975 : *
3976 : * Attenuation of a vector,, attenuation factor in Q15
3977 : *-------------------------------------------------------------------*/
3978 :
3979 1423 : void v_multc_att(
3980 : const Word16 x[], /* i : Input vector Qx */
3981 : const Word16 att, /* i : Constant Q15, <= MAX_16 */
3982 : Word16 y[], /* o : Output vector that contains att*x */
3983 : const Word16 N /* i : Vector length */
3984 : )
3985 : {
3986 : Word16 i;
3987 1423 : IF( LT_16( att, 32767 ) )
3988 : {
3989 0 : FOR( i = 0; i < N; i++ )
3990 : {
3991 0 : y[i] = mult_r( x[i], att );
3992 0 : move16();
3993 : }
3994 : }
3995 :
3996 1423 : return;
3997 : }
3998 :
3999 :
4000 : /*-------------------------------------------------------------------*
4001 : * v_multc_att32()
4002 : *
4003 : * Attenuation of a vector,, attenuation factor in Q15
4004 : *-------------------------------------------------------------------*/
4005 :
4006 860 : void v_multc_att32(
4007 : const Word32 x[], /* i : Input vector Qx */
4008 : const Word16 att, /* i : Constant Q15, <= MAX_16 */
4009 : Word32 y[], /* o : Output vector that contains att*x */
4010 : const Word16 N /* i : Vector length */
4011 : )
4012 : {
4013 : Word16 i;
4014 860 : IF( LT_16( att, 32767 ) )
4015 : {
4016 0 : FOR( i = 0; i < N; i++ )
4017 : {
4018 0 : y[i] = Mpy_32_16_r( x[i], att );
4019 0 : move32();
4020 : }
4021 : }
4022 :
4023 860 : return;
4024 : }
4025 :
4026 : /*-------------------------------------------------------------------*
4027 : * v_multc_att3232()
4028 : *
4029 : * Attenuation of a vector, attenuation factor in Q31
4030 : *-------------------------------------------------------------------*/
4031 :
4032 0 : void v_multc_att3232(
4033 : const Word32 x[], /* i : Input vector Qx */
4034 : const Word32 att, /* i : Constant Q32, <= MAX_32 */
4035 : Word32 y[], /* o : Output vector that contains att*x */
4036 : const Word16 N /* i : Vector length */
4037 : )
4038 : {
4039 : Word16 i;
4040 0 : IF( LE_32( att, MAX_32 ) )
4041 : {
4042 0 : FOR( i = 0; i < N; i++ )
4043 : {
4044 0 : y[i] = Mpy_32_32_r( x[i], att );
4045 0 : move32();
4046 : }
4047 : }
4048 :
4049 0 : return;
4050 : }
4051 :
4052 : /*-------------------------------------------------------------------*
4053 : * v_L_mult_1616()
4054 : *
4055 : * Multiplication of two vectors, Output in Word32
4056 : *-------------------------------------------------------------------*/
4057 :
4058 0 : void v_L_mult_1616(
4059 : const Word16 x1[], /* i : Input vector 1 */
4060 : const Word16 x2[], /* i : Input vector 2 */
4061 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4062 : const Word16 N /* i : Vector length */
4063 : )
4064 : {
4065 : Word16 i;
4066 :
4067 0 : for ( i = 0; i < N; i++ )
4068 : {
4069 0 : y[i] = L_mult( x1[i], x2[i] );
4070 0 : move32();
4071 : }
4072 :
4073 0 : return;
4074 : }
4075 :
4076 : /*-------------------------------------------------------------------*
4077 : * v_L_mult_3216()
4078 : *
4079 : * Multiplication of two vectors, Output in Word32
4080 : *-------------------------------------------------------------------*/
4081 :
4082 97328 : void v_L_mult_3216(
4083 : const Word32 x1[], /* i : Input vector 1 */
4084 : const Word16 x2[], /* i : Input vector 2 */
4085 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
4086 : const Word16 N /* i : Vector length */
4087 : )
4088 : {
4089 : Word16 i;
4090 :
4091 56449328 : for ( i = 0; i < N; i++ )
4092 : {
4093 56352000 : y[i] = Mpy_32_16_1( x1[i], x2[i] );
4094 56352000 : move32();
4095 : }
4096 :
4097 97328 : return;
4098 : }
4099 :
4100 : /*-------------------------------------------------------------------*
4101 : * add_vec()
4102 : *
4103 : * Addition of two vectors sample by sample
4104 : *-------------------------------------------------------------------*/
4105 :
4106 2058 : void add_vec_fx(
4107 : const Word16 x1[], /* i : Input vector 1 */
4108 : const Word16 Qx1, /* i : SCaling of input 1 */
4109 : const Word16 x2[], /* i : Input vector 2 */
4110 : const Word16 Qx2, /* i : SCaling of input 1 */
4111 : Word16 y[], /* o : Output vector that contains vector 1 + vector 2 */
4112 : const Word16 Qy, /* i : SCaling of output 1 */
4113 : const Word16 N /* i : Vector lenght */
4114 : )
4115 : {
4116 : Word16 i, Qyx1, Qyx2;
4117 :
4118 2058 : Qyx1 = sub( Qx1, Qy );
4119 2058 : Qyx2 = sub( Qx2, Qy );
4120 2058 : IF( Qyx1 == 0 )
4121 : {
4122 1738058 : FOR( i = 0; i < N; i++ )
4123 : {
4124 1736000 : y[i] = add_sat( x1[i], shr_r_sat( x2[i], Qyx2 ) );
4125 1736000 : move16();
4126 : }
4127 : }
4128 : ELSE
4129 : {
4130 0 : FOR( i = 0; i < N; i++ )
4131 : {
4132 0 : y[i] = add_sat( shr_r_sat( x1[i], Qyx1 ), shr_r_sat( x2[i], Qyx2 ) ); //!!sat //!!sat
4133 0 : move16();
4134 : }
4135 : }
4136 :
4137 2058 : return;
4138 : }
4139 :
4140 :
4141 : /*-------------------------------------------------------------------*
4142 : * Add_flt32_flt32
4143 : *
4144 : * Add two Pseudo Float Value that are 32 Bits Mantisa and 16 Bits Exp
4145 : *-------------------------------------------------------------------*/
4146 :
4147 : /* o: Result (Normalized) */
4148 5392 : Word32 Add_flt32_flt32(
4149 : Word32 a, /* i: 1st Value */
4150 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
4151 : Word32 b, /* i: 2nd Value */
4152 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
4153 : Word16 *exp_out /* o: Exponent of Result */
4154 : )
4155 : {
4156 : Word16 temp, temp2;
4157 : Word32 L_temp;
4158 :
4159 : /* Subract 1 to further divide by 2 to avoid overflow on L_add */
4160 5392 : temp = sub( s_min( exp_a, exp_b ), 1 );
4161 :
4162 : /* Put both to same exponent */
4163 5392 : exp_a = sub( exp_a, temp );
4164 5392 : a = L_shr( a, exp_a );
4165 5392 : exp_b = sub( exp_b, temp );
4166 5392 : b = L_shr( b, exp_b );
4167 :
4168 : /* add them together */
4169 5392 : L_temp = L_add( a, b );
4170 5392 : temp2 = norm_l( L_temp );
4171 :
4172 5392 : *exp_out = add( temp, temp2 );
4173 5392 : move16();
4174 :
4175 5392 : return L_shl( L_temp, temp2 );
4176 : }
4177 :
4178 :
4179 : /*-------------------------------------------------------------------*
4180 : * Mul_flt32_Q15
4181 : *
4182 : * Multiply one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
4183 : * with a Q15 value
4184 : *-------------------------------------------------------------------*/
4185 :
4186 : /* o: Result (Normalized) */
4187 0 : Word32 Mul_flt32_Q15(
4188 : Word32 value, /* i: Pseudo_float Value */
4189 : Word16 *exp_v, /*i/o: Exponent of Value (Q of Value) */
4190 : Word16 frac /* i: Q15 value */
4191 : )
4192 : {
4193 : Word16 temp;
4194 : Word32 L_temp;
4195 :
4196 0 : L_temp = Mult_32_16( value, frac );
4197 0 : temp = norm_l( L_temp );
4198 :
4199 0 : *exp_v = add( temp, *exp_v );
4200 0 : move16();
4201 :
4202 0 : return L_shl( L_temp, temp );
4203 : }
4204 :
4205 :
4206 : /*-------------------------------------------------------------------*
4207 : * Div_flt32_flt32
4208 : *
4209 : * Divide one Pseudo Float Value (32 Bits Mantisa and 16 Bits Exp)
4210 : * by another one
4211 : *-------------------------------------------------------------------*/
4212 :
4213 : /* o: Result (Normalized) */
4214 3 : Word32 Div_flt32_flt32(
4215 : Word32 a, /* i: 1st Value */
4216 : Word16 exp_a, /* i: Exponent of 1st Value (Q of Value) */
4217 : Word32 b, /* i: 2nd Value */
4218 : Word16 exp_b, /* i: Exponent of 2nd Value (Q of Value) */
4219 : Word16 *exp_out /* o: Exponent of Result */
4220 : )
4221 : {
4222 : Word16 temp, temp2;
4223 : Word32 L_temp;
4224 :
4225 3 : temp = div_s( 16384, round_fx( b ) );
4226 3 : L_temp = Mult_32_16( a, temp );
4227 3 : temp2 = sub( 31 - 1, exp_b );
4228 3 : temp2 = add( temp2, exp_a );
4229 :
4230 3 : temp = norm_l( L_temp );
4231 :
4232 3 : *exp_out = add( temp, temp2 );
4233 3 : move16();
4234 :
4235 3 : return L_shl( L_temp, temp );
4236 : }
4237 :
4238 :
4239 : /*-------------------------------------------------------------------*
4240 : * Calc_Energy_Autoscaled
4241 : *
4242 : * Calculate Energy with overflow protection
4243 : *-------------------------------------------------------------------*/
4244 :
4245 : /* o: Result (Energy) */
4246 3748105 : Word32 Calc_Energy_Autoscaled(
4247 : const Word16 *signal, /* i: Signal */
4248 : Word16 signal_exp, /* i: Exponent of Signal (Q of Signal) */
4249 : Word16 len, /* i: Frame Length */
4250 : Word16 *energy_exp /* o: Exponent of Energy (Q of Energy) */
4251 : )
4252 : {
4253 : Word16 temp, temp2;
4254 : Word32 L_temp, L_Energy;
4255 : Word16 i, j;
4256 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
4257 3748105 : Flag Overflow = 0;
4258 3748105 : move32();
4259 : #endif
4260 :
4261 3748105 : Overflow = 0;
4262 3748105 : move16();
4263 :
4264 3748105 : temp2 = 0;
4265 3748105 : move16();
4266 3748105 : L_Energy = L_deposit_l( 1 );
4267 3748105 : j = s_and( 7, len );
4268 3759945 : FOR( i = 0; i < j; i++ )
4269 : {
4270 : /* divide by 2 so energy will be divided by 4 */
4271 11840 : temp = mult_r( *signal++, 16384 );
4272 11840 : L_Energy = L_mac0_o( L_Energy, temp, temp, &Overflow );
4273 : }
4274 12356719 : FOR( i = j; i < len; i += 8 ) /* Process 8 Samples at a time */
4275 : {
4276 : /* divide by 2 so energy will be divided by 4 */
4277 8608614 : temp = mult_ro( *signal++, 16384, &Overflow );
4278 8608614 : L_temp = L_mult0( temp, temp );
4279 68868912 : FOR( j = 1; j < 8; j++ )
4280 : {
4281 60260298 : temp = mult_r( *signal++, 16384 );
4282 60260298 : L_temp = L_mac0_o( L_temp, temp, temp, &Overflow );
4283 : }
4284 : /*Overfloe will never happen because temp2 is always positive*/
4285 8608614 : L_temp = L_shr( L_temp, temp2 );
4286 : /* Here we try the addition just to check if we can sum
4287 : the energy of the small (8 Iterations) loop with the
4288 : total energy calculated so far without an overflow.
4289 : The result is discarded. If there is an overflow both
4290 : energies are div by 2. Otherwise, nothing is done.
4291 : After the 'IF', the sum is done again and will always
4292 : be without an overflow. */
4293 8608614 : L_add_o( L_Energy, L_temp, &Overflow );
4294 8608614 : IF( Overflow != 0 )
4295 : {
4296 41 : L_Energy = L_shr( L_Energy, 1 );
4297 41 : L_temp = L_shr( L_temp, 1 );
4298 41 : temp2 = add( temp2, 1 );
4299 41 : Overflow = 0;
4300 41 : move16();
4301 : }
4302 8608614 : L_Energy = L_add_o( L_Energy, L_temp, &Overflow );
4303 : }
4304 : /* Calc Final Exponent (sub 2 because of the mult_r by 16384 that already divs the ener by 4) */
4305 3748105 : temp2 = sub( sub( shl( signal_exp, 1 ), temp2 ), 2 );
4306 :
4307 3748105 : *energy_exp = temp2;
4308 3748105 : move16();
4309 :
4310 3748105 : return L_Energy;
4311 : }
4312 :
4313 4378679 : Word16 Find_Max_Norm16(
4314 : const Word16 *src,
4315 : Word16 len )
4316 : {
4317 : Word16 i;
4318 : Word16 max16;
4319 :
4320 : /* it starts at '0' and not '1' like in Find_Max_Norm32() */
4321 : /* and that is necessary. */
4322 4378679 : max16 = 0;
4323 4378679 : move16();
4324 :
4325 2616024816 : FOR( i = 0; i < len; i++ )
4326 : {
4327 2611646137 : max16 = s_max( max16, abs_s( *src++ ) );
4328 : }
4329 :
4330 4378679 : return norm_s( max16 );
4331 : }
4332 :
4333 4269690 : Word16 Find_Max_Norm32(
4334 : const Word32 *src,
4335 : Word16 len )
4336 : {
4337 : Word16 i;
4338 : Word32 max32;
4339 :
4340 4269690 : max32 = L_deposit_l( 1 );
4341 :
4342 3316452959 : FOR( i = 0; i < len; i++ )
4343 : {
4344 3312183269 : max32 = L_max( max32, L_abs( *src++ ) );
4345 : }
4346 :
4347 4269690 : return norm_l( max32 );
4348 : }
4349 :
4350 :
4351 : /*-------------------------------------------------------------------*
4352 : * Sqrt_Ratio32
4353 : *
4354 : * Calculate Sqrt of Val1/Val2
4355 : *-------------------------------------------------------------------*/
4356 :
4357 : /* o: Result in Q31 */
4358 5529 : Word32 Sqrt_Ratio32(
4359 : Word32 L_val1, /* i: Mantisa of Val1 */
4360 : Word16 exp1, /* i: Exp of Val1 (>0: Val was Left Shifted, <0:Right Shifted) */
4361 : Word32 L_val2, /* i: Mantisa of Val2 */
4362 : Word16 exp2, /* i: Exp of Val2 (same as exp1) */
4363 : Word16 *exp /* o: Exp of Result (# of 'L_shl' Req to get to Final Value) */
4364 : )
4365 : {
4366 : Word16 temp;
4367 :
4368 : /* Normalize Energy #1 */
4369 5529 : temp = norm_l( L_val1 );
4370 5529 : L_val1 = L_shl( L_val1, temp );
4371 : /* Adjust Exponent of Energy #1 */
4372 5529 : exp1 = add( exp1, temp );
4373 :
4374 : /* Normalize Energy #2 */
4375 5529 : temp = norm_l( L_val2 );
4376 5529 : L_val2 = L_shl( L_val2, temp );
4377 : /* Adjust Exponent of Energy #1 */
4378 5529 : exp2 = add( exp2, temp );
4379 :
4380 : /* Prepare for Inverse */
4381 5529 : temp = round_fx_sat( L_val1 );
4382 5529 : temp = div_s( 16384, temp );
4383 : /* Mult Now */
4384 5529 : L_val2 = Mult_32_16( L_val2, temp );
4385 5529 : exp1 = add( sub( exp2, exp1 ), 15 * 2 );
4386 :
4387 : /* Here Result of ('L_val2' / 2^'exp2') / ('L_val1' / 2^'exp1') is */
4388 : /* 'L_val2' / 2^'exp1' */
4389 : /* Which is val2/val1 instead of val1/val2 because we will use Inverted Square Root */
4390 : /* Normalize before Square Root */
4391 5529 : temp = norm_l( L_val2 );
4392 5529 : L_val2 = L_shl( L_val2, temp );
4393 5529 : exp1 = add( temp, exp1 );
4394 : /* Do Sqrt */
4395 5529 : temp = sub( 31, exp1 );
4396 5529 : L_val1 = Isqrt_lc( L_val2, &temp );
4397 :
4398 5529 : *exp = temp;
4399 5529 : move16();
4400 :
4401 5529 : return L_val1;
4402 : }
4403 :
4404 : /* result in Q'15 + 'exp' */
4405 4430 : Word16 Invert16(
4406 : Word16 val,
4407 : Word16 *exp )
4408 : {
4409 : Word16 temp;
4410 :
4411 : /* prevent 0 input */
4412 4430 : val = s_max( val, 1 );
4413 : /* Normalize Value */
4414 4430 : temp = norm_s( val );
4415 4430 : val = shl( val, temp );
4416 :
4417 4430 : *exp = sub( sub( 15 - 1, *exp ), temp );
4418 4430 : move16(); /* -1 because of 0x4000 is 1.0 in Q14 (and not Q15) */
4419 :
4420 4430 : temp = div_s( 0x4000, val );
4421 :
4422 4430 : return temp;
4423 : }
4424 :
4425 0 : Word16 find_rem(
4426 : Word16 n,
4427 : Word16 m,
4428 : Word16 *r )
4429 : {
4430 : Word16 i, q1, q2, qd;
4431 : Word32 Ltemp2;
4432 : Word32 Lacc;
4433 :
4434 0 : test();
4435 0 : test();
4436 0 : IF( n <= 0 || m <= 0 || n < m )
4437 : {
4438 0 : *r = n;
4439 0 : move16();
4440 0 : return ( 0 );
4441 : }
4442 :
4443 0 : q1 = norm_s( n );
4444 0 : q1 = sub( q1, 1 );
4445 0 : Lacc = L_deposit_h( shl( n, q1 ) );
4446 0 : qd = sub( q1, 1 );
4447 0 : q2 = norm_s( m );
4448 0 : q2 = sub( q2, 1 );
4449 0 : Ltemp2 = L_deposit_h( shl( m, q2 ) );
4450 0 : qd = sub( q2, qd );
4451 0 : q2 = add( q2, 1 );
4452 :
4453 0 : FOR( i = 0; i < qd; i++ )
4454 : {
4455 0 : Lacc = L_sub( Lacc, Ltemp2 );
4456 0 : IF( Lacc >= 0 )
4457 : {
4458 0 : Lacc = L_add( L_shl( Lacc, 1 ), 1 );
4459 : }
4460 : ELSE
4461 : {
4462 0 : Lacc = L_add( Lacc, Ltemp2 );
4463 0 : Lacc = L_shl( Lacc, 1 );
4464 : }
4465 : }
4466 0 : q1 = extract_l( Lacc );
4467 0 : Ltemp2 = L_shr( Lacc, q2 );
4468 0 : *r = extract_h( Ltemp2 );
4469 0 : move16();
4470 :
4471 0 : return ( q1 );
4472 : }
4473 :
4474 :
4475 0 : Word32 find_remd(
4476 : Word32 n,
4477 : Word32 m,
4478 : Word32 *r )
4479 : {
4480 : Word16 i, q1, q2, qd;
4481 : Word32 Ltemp2, qo;
4482 : Word32 Lacc;
4483 :
4484 0 : test();
4485 0 : test();
4486 0 : IF( n <= 0 || m <= 0 || n < m )
4487 : {
4488 0 : *r = n;
4489 0 : move16();
4490 0 : return ( 0 );
4491 : }
4492 :
4493 0 : q1 = norm_l( n );
4494 0 : q1 = sub( q1, 1 );
4495 0 : Lacc = L_shl( n, q1 );
4496 0 : qd = sub( q1, 1 );
4497 0 : q2 = norm_l( m );
4498 0 : q2 = sub( q2, 1 );
4499 0 : Ltemp2 = L_shl( m, q2 );
4500 0 : qd = sub( q2, qd );
4501 0 : q2 = add( q2, 1 );
4502 0 : qo = 0;
4503 0 : move16();
4504 :
4505 0 : FOR( i = 0; i < qd; i++ )
4506 : {
4507 0 : Lacc = L_sub( Lacc, Ltemp2 );
4508 0 : qo = L_shl( qo, 1 );
4509 0 : IF( Lacc >= 0 )
4510 : {
4511 0 : Lacc = L_shl( Lacc, 1 );
4512 0 : qo = L_add( qo, 1 );
4513 : }
4514 : ELSE
4515 : {
4516 0 : Lacc = L_add( Lacc, Ltemp2 );
4517 0 : Lacc = L_shl( Lacc, 1 );
4518 : }
4519 : }
4520 0 : *r = L_shr( Lacc, q2 );
4521 0 : move16();
4522 :
4523 0 : return ( qo );
4524 : }
4525 :
4526 :
4527 30442 : Word16 rint_new_fx(
4528 : Word32 x /*Q16 */
4529 : )
4530 : {
4531 : Word16 a;
4532 : Word32 L_tmp;
4533 : Word16 frac, tmp;
4534 :
4535 : /* middle value point test */
4536 30442 : frac = lshr( extract_l( x ), 1 ); /*Q15 */
4537 30442 : tmp = sub( frac, 0x4000 );
4538 :
4539 30442 : IF( !tmp )
4540 : {
4541 0 : a = add( extract_h( x ), 1 );
4542 :
4543 0 : IF( s_and( a, 1 ) == 0 )
4544 : {
4545 0 : return a;
4546 : }
4547 0 : IF( s_and( a, 1 ) != 0 )
4548 : {
4549 0 : return extract_h( x );
4550 : }
4551 0 : return extract_h( x );
4552 : }
4553 : ELSE
4554 : {
4555 30442 : L_tmp = L_add( x, 32768 ); /*Q16 */
4556 30442 : return extract_h( L_tmp );
4557 : }
4558 : }
4559 :
4560 :
4561 : /*===================================================================*/
4562 : /* FUNCTION : erb_diff_search_fx () */
4563 : /*-------------------------------------------------------------------*/
4564 : /* PURPOSE : erb amplitude VQ search for QPPP */
4565 : /*-------------------------------------------------------------------*/
4566 : /* INPUT ARGUMENTS : */
4567 : /* _ (Word16 []) prev_erb : Previous erb amplitude, Q13 */
4568 : /* _ (Word16 []) curr_erb : Current erb amplitude, Q13 */
4569 : /* _ (Word16 []) dif_erb: erb differential, Q13 */
4570 : /* _ (Word16 []) pow_spec : LPC power spectrum, Q7 */
4571 : /* _ (Word16 [][]) cb_fx : differential erb codebook, Q13 */
4572 : /* _ (Word16) cb_size : codebook size */
4573 : /* _ (Word16) cb_dim : codebook dimension */
4574 : /* _ (Word16) offset : index to current segment of erb array */
4575 : /* for quantization */
4576 : /*-------------------------------------------------------------------*/
4577 : /* OUTPUT ARGUMENTS : */
4578 : /* _ None */
4579 : /*-------------------------------------------------------------------*/
4580 : /* INPUT/OUTPUT ARGUMENTS : */
4581 : /* _ None */
4582 : /*-------------------------------------------------------------------*/
4583 : /* RETURN ARGUMENTS : */
4584 : /* _ (Word16) index: best codebook index */
4585 : /*-------------------------------------------------------------------*/
4586 : /* CALLED FROM : TX */
4587 : /*===================================================================*/
4588 :
4589 0 : Word16 erb_diff_search_fx(
4590 : Word16 *prev_erb,
4591 : const Word16 *curr_erb,
4592 : Word16 *dif_erb,
4593 : Word16 *pow_spec,
4594 : const Word16 *cb_fx,
4595 : Word16 cb_size,
4596 : Word16 cb_dim,
4597 : Word16 offset )
4598 : {
4599 : Word16 i, j, mmseindex;
4600 : Word16 dh, dl;
4601 : Word32 mmse;
4602 : Word32 Ltemp1;
4603 : Word32 Lacc;
4604 :
4605 0 : mmse = EVS_LW_MAX;
4606 0 : move32();
4607 0 : mmseindex = -1;
4608 0 : move16();
4609 0 : FOR( j = 0; j < cb_size; j++ )
4610 : {
4611 :
4612 0 : Lacc = L_deposit_l( 0 );
4613 0 : FOR( i = 0; i < cb_dim; i++ )
4614 : {
4615 0 : IF( add_sat( cb_fx[j * cb_dim + i], prev_erb[i + offset] ) < 0 )
4616 : {
4617 0 : Ltemp1 = L_mult( curr_erb[i + offset], curr_erb[i + offset] ); /* Q27 */
4618 0 : dh = extract_h( Ltemp1 );
4619 0 : dl = extract_l( Ltemp1 );
4620 0 : IF( dl < 0 )
4621 : {
4622 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
4623 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
4624 0 : Ltemp1 = L_shl( Ltemp1, 1 );
4625 : }
4626 : ELSE
4627 : {
4628 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl );
4629 : }
4630 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) );
4631 : }
4632 : ELSE
4633 : {
4634 0 : dh = sub_sat( dif_erb[i + offset], cb_fx[j * cb_dim + i] ); /* Q13 */
4635 0 : Ltemp1 = L_mult_sat( dh, dh ); /* Q27 */
4636 0 : dh = extract_h( Ltemp1 );
4637 0 : dl = extract_l( Ltemp1 );
4638 :
4639 0 : IF( dl < 0 )
4640 : {
4641 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
4642 0 : Ltemp1 = Mult_32_16( Ltemp1, pow_spec[i + offset] );
4643 0 : Ltemp1 = L_shl( Ltemp1, 1 );
4644 : }
4645 : ELSE
4646 : {
4647 0 : Ltemp1 = (Word32) L_mult0( pow_spec[i + offset], dl ); /* Q33 */
4648 : }
4649 :
4650 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( pow_spec[i + offset], dh ) ); /* Q18 */
4651 : }
4652 :
4653 0 : IF( LT_16( cb_fx[j * cb_dim + i], dif_erb[i + offset] ) )
4654 : {
4655 0 : dh = extract_h( Ltemp1 );
4656 0 : dl = extract_l( Ltemp1 );
4657 0 : IF( dl < 0 )
4658 : {
4659 0 : Ltemp1 = L_shl( L_add( 65536, dl ), 14 ); /* */
4660 0 : Ltemp1 = Mult_32_16( Ltemp1, 29491 );
4661 0 : Ltemp1 = L_shl( Ltemp1, 1 );
4662 : }
4663 : ELSE
4664 : {
4665 0 : Ltemp1 = (Word32) L_mult0( 29491, dl ); /* 29491=0.9 in Q15 */
4666 : }
4667 0 : Ltemp1 = L_add( L_shr( Ltemp1, 15 ), L_mult( dh, 29491 ) );
4668 : }
4669 0 : Lacc = L_add( Lacc, Ltemp1 ); /* Q18 */
4670 : }
4671 :
4672 0 : IF( LT_32( Lacc, mmse ) )
4673 : {
4674 0 : mmse = L_add( Lacc, 0 );
4675 0 : mmseindex = j;
4676 0 : move16();
4677 : }
4678 : }
4679 :
4680 0 : return ( mmseindex );
4681 : }
4682 :
4683 :
4684 518022 : void Acelp_dec_total_exc(
4685 : Word16 *exc_fx, /* i/o: adapt. excitation exc */
4686 : Word16 *exc2_fx, /* i/o: adapt. excitation/total exc */
4687 : const Word16 gain_code16, /* i : Gain code Q0 */
4688 : const Word16 gain_pit_fx, /* i ; Pitch gain in Q14 */
4689 : const Word16 i_subfr, /* i ; subfr */
4690 : const Word16 *code_fx, /* i : code in Q9 */
4691 : const Word16 L_subfr /* i : Subframne lenght */
4692 : )
4693 : {
4694 : Word16 i;
4695 : Word32 L_tmp;
4696 :
4697 33672070 : FOR( i = 0; i < L_subfr; i++ )
4698 : {
4699 33154048 : L_tmp = L_shl_sat( L_mult( gain_pit_fx, exc_fx[i + i_subfr] ), 1 ); /*Q16+Q_exc*/
4700 33154048 : exc2_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
4701 33154048 : L_tmp = L_add_sat( L_tmp, L_shl_sat( L_mult( gain_code16, code_fx[i] ), 6 ) ); /*Q16+Q_exc*/
4702 33154048 : exc_fx[i + i_subfr] = round_fx_sat( L_tmp ); /*Q_exc*/
4703 33154048 : move16();
4704 33154048 : move16();
4705 : }
4706 :
4707 518022 : return;
4708 : }
4709 :
4710 :
4711 : /*-------------------------------------------------------------------*
4712 : * UL_inverse
4713 : *
4714 : * Calculate inverse of UL_val. Output in Q_exp.
4715 : *-------------------------------------------------------------------*/
4716 :
4717 644508 : UWord32 UL_inverse(
4718 : const UWord32 UL_val,
4719 : Word16 *exp )
4720 : {
4721 : UWord32 UL_tmp;
4722 :
4723 644508 : *exp = norm_ul( UL_val );
4724 644508 : move16();
4725 644508 : UL_tmp = UL_lshl( UL_val, *exp ); /* Q32 */
4726 :
4727 644508 : *exp = add( 32, sub( 31, *exp ) );
4728 644508 : move16();
4729 :
4730 644508 : return UL_div( 0x80000000, UL_tmp );
4731 : }
4732 :
4733 : /*-------------------------------------------------------------------*
4734 : * UL_div
4735 : *
4736 : * Calculate UL_num/UL_den. UL_num assumed to be Q31, UL_den assumed
4737 : * to be Q32, then result is in Q32.
4738 : *-------------------------------------------------------------------*/
4739 :
4740 964232 : UWord32 UL_div(
4741 : const UWord32 UL_num,
4742 : const UWord32 UL_den )
4743 : {
4744 : UWord32 UL_e, UL_Q;
4745 : UWord32 UL_msb, UL_lsb;
4746 : Word16 i;
4747 :
4748 964232 : UL_e = UL_subNsD( 0xffffffff, UL_den );
4749 964232 : UL_Q = UL_num;
4750 964232 : move32();
4751 :
4752 5785392 : FOR( i = 0; i < 5; i++ )
4753 : {
4754 4821160 : Mpy_32_32_uu( UL_Q, UL_e, &UL_msb, &UL_lsb ); /*31+32-32=31 */
4755 4821160 : UL_Q = UL_addNsD( UL_Q, UL_msb );
4756 4821160 : Mpy_32_32_uu( UL_e, UL_e, &UL_e, &UL_lsb ); /*32+32-32=32 */
4757 : }
4758 :
4759 964232 : return UL_Q;
4760 : }
4761 :
4762 : /*-----------------------------------------------------------------------------
4763 : * ratio()
4764 : *
4765 : * Divide the numerator by the denominator.
4766 : *----------------------------------------------------------------------------*/
4767 7187181 : Word16 ratio( const Word32 numer, const Word32 denom, Word16 *expo )
4768 : {
4769 : Word16 expNumer, expDenom;
4770 : Word16 manNumer, manDenom;
4771 : Word16 quotient;
4772 :
4773 7187181 : expDenom = norm_l( denom ); /* exponent */
4774 7187181 : manDenom = extract_h( L_shl( denom, expDenom ) ); /* mantissa */
4775 7187181 : expNumer = norm_l( numer ); /* exponent */
4776 7187181 : manNumer = extract_h( L_shl( numer, expNumer ) ); /* mantissa */
4777 7187181 : manNumer = shr( manNumer, 1 ); /* Ensure the numerator < the denominator */
4778 7187181 : quotient = div_s( manNumer, manDenom ); /* in Q14 */
4779 :
4780 7187181 : *expo = sub( expNumer, expDenom );
4781 7187181 : move16();
4782 :
4783 7187181 : return quotient; /* Q14 */
4784 : }
4785 :
4786 :
4787 : /*-----------------------------------------------------------------------*
4788 : * Function hp400_12k8() *
4789 : * *
4790 : * 2nd order Cheb2 high pass filter with cut off frequency at 400 Hz. *
4791 : * Optimized for fixed-point to get the following frequency response : *
4792 : * *
4793 : * frequency : 0Hz 100Hz 200Hz 300Hz 400Hz 630Hz 1.5kHz 3kHz *
4794 : * dB loss : -infdB -30dB -20dB -10dB -3dB +6dB +1dB 0dB *
4795 : * *
4796 : * Algorithm : *
4797 : * *
4798 : * y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] *
4799 : * + a[1]*y[i-1] + a[2]*y[i-2]; *
4800 : * *
4801 : * short b[3] = {3660, -7320, 3660}; in Q12 *
4802 : * short a[3] = {4096, 7320, -3540}; in Q12 *
4803 : * *
4804 : * float --> b[3] = {0.893554687, -1.787109375, 0.893554687}; *
4805 : * a[3] = {1.000000000, 1.787109375, -0.864257812}; *
4806 : *-----------------------------------------------------------------------*/
4807 :
4808 0 : void hp400_12k8_fx(
4809 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
4810 : const Word16 lg, /* i : lenght of signal */
4811 : Word16 mem[] /* i/o: filter memory [6] */
4812 : )
4813 : {
4814 : Word16 i;
4815 : Word16 y1_hi, y1_lo;
4816 : Word32 L_tmp, L_tmp2, L_tmp3;
4817 :
4818 0 : y1_hi = mem[2];
4819 0 : move16();
4820 0 : y1_lo = mem[3];
4821 0 : move16();
4822 :
4823 0 : L_tmp3 = L_mac( 16384L, mem[1], a_hp400_fx[2] ); /* rounding to maximize precision */
4824 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
4825 0 : L_tmp3 = L_shr( L_tmp3, 15 );
4826 0 : L_tmp2 = L_mac( L_tmp3, mem[0], a_hp400_fx[2] );
4827 0 : L_tmp2 = L_mac( L_tmp2, mem[5], b_hp400_fx[2] );
4828 0 : L_tmp2 = L_mac( L_tmp2, mem[4], b_hp400_fx[1] );
4829 0 : L_tmp3 = L_mult( mem[4], b_hp400_fx[2] );
4830 :
4831 0 : mem[5] = signal[lg - 2];
4832 0 : move16();
4833 0 : FOR( i = 1; i < lg; i++ )
4834 : {
4835 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
4836 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
4837 :
4838 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
4839 0 : L_tmp = L_mac( L_tmp, *signal, b_hp400_fx[0] );
4840 :
4841 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
4842 :
4843 0 : L_tmp2 = L_mac( L_tmp3, y1_hi, a_hp400_fx[2] );
4844 0 : L_tmp2 = L_mac( L_tmp2, *signal, b_hp400_fx[1] );
4845 0 : L_tmp3 = L_mac( 16384L, y1_lo, a_hp400_fx[2] ); /* rounding to maximize precision */
4846 :
4847 0 : y1_lo = L_Extract_lc( L_tmp, &y1_hi );
4848 :
4849 0 : L_tmp3 = L_mac( L_tmp3, y1_lo, a_hp400_fx[1] );
4850 0 : L_tmp3 = L_shr( L_tmp3, 15 );
4851 :
4852 0 : L_tmp2 = L_add( L_tmp3, L_tmp2 );
4853 :
4854 0 : L_tmp3 = L_mult( *signal, b_hp400_fx[2] );
4855 :
4856 : /* signal is divided by 16 to avoid overflow in energy computation */
4857 0 : *signal++ = round_fx( L_tmp );
4858 0 : move16();
4859 : }
4860 :
4861 : /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
4862 : /* + a[1]*y[i-1] + a[2] * y[i-2] */
4863 :
4864 0 : L_tmp = L_mac( L_tmp2, y1_hi, a_hp400_fx[1] );
4865 :
4866 0 : mem[4] = *signal;
4867 0 : move16();
4868 0 : L_tmp = L_mac( L_tmp, mem[4], b_hp400_fx[0] );
4869 :
4870 0 : L_tmp = L_shl( L_tmp, 1 ); /* coeff Q12 --> Q13 */
4871 :
4872 0 : mem[0] = y1_hi;
4873 0 : move16();
4874 0 : mem[1] = y1_lo;
4875 0 : move16();
4876 0 : L_Extract( L_tmp, &mem[2], &mem[3] );
4877 :
4878 : /* signal is divided by 16 to avoid overflow in energy computation */
4879 0 : *signal++ = round_fx( L_tmp );
4880 0 : move16();
4881 :
4882 0 : return;
4883 : }
4884 :
4885 :
4886 61916 : void hp400_12k8_ivas_fx(
4887 : Word16 signal[], /* i/o: input signal / output is divided by 16 */
4888 : const Word16 lg, /* i : lenght of signal */
4889 : Word16 mem[] /* i/o: filter memory [6] */
4890 : )
4891 : {
4892 : Word16 i;
4893 : Word16 x0, x1, x2;
4894 : Word32 L_tmp, yy1, y2;
4895 :
4896 61916 : yy1 = L_Comp( mem[2], mem[3] ); /* Q_syn + 13 */
4897 61916 : y2 = L_Comp( mem[0], mem[1] ); /* Q_syn + 13 */
4898 61916 : x0 = mem[4]; /* Q_syn */
4899 61916 : move16();
4900 61916 : x1 = mem[5]; /* Q_syn */
4901 61916 : move16();
4902 :
4903 4024540 : FOR( i = 0; i < lg; i++ )
4904 : {
4905 3962624 : x2 = x1; /* Q_syn */
4906 3962624 : move16();
4907 3962624 : x1 = x0; /* Q_syn */
4908 3962624 : move16();
4909 3962624 : x0 = signal[i]; /* Q_syn */
4910 3962624 : move16();
4911 :
4912 3962624 : 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)*/
4913 3962624 : L_tmp = Madd_32_16( L_tmp, y2, a_hp400_ivas_fx[2] ); /*y2 * a_hp400[2]*/ /* Qx + 10 ---->( (Q_syn+13) + 12 - 15)*/
4914 3962624 : L_tmp = L_shl( L_tmp, 3 ); /* shifting by 3 to maintain same Q (Q_syn+13) */
4915 :
4916 3962624 : L_tmp = L_mac( L_tmp, x0, b_hp400_fx[0] ); /* Q_syn + 13 */
4917 3962624 : L_tmp = L_mac( L_tmp, x1, b_hp400_fx[1] ); /* Q_syn + 13 */
4918 3962624 : L_tmp = L_mac( L_tmp, x2, b_hp400_fx[2] ); /* Q_syn + 13 */
4919 :
4920 3962624 : y2 = yy1; /* Q_syn + 13 */
4921 3962624 : move32();
4922 3962624 : yy1 = L_tmp; /* Q_syn + 13 */
4923 3962624 : move32();
4924 :
4925 3962624 : signal[i] = round_fx( L_tmp ); /* Q_syn - 3 */
4926 3962624 : move16();
4927 : }
4928 :
4929 61916 : L_Extract( yy1, &mem[2], &mem[3] );
4930 61916 : L_Extract( y2, &mem[0], &mem[1] );
4931 61916 : mem[4] = x0; /* Q_syn */
4932 61916 : mem[5] = x1; /* Q_syn */
4933 61916 : move16();
4934 61916 : move16();
4935 :
4936 61916 : return;
4937 : }
4938 :
4939 :
4940 0 : Word16 dot_prod_satcontr(
4941 : const Word16 *x,
4942 : const Word16 *y,
4943 : Word16 qx,
4944 : Word16 qy,
4945 : Word16 *qo,
4946 : Word16 len )
4947 : {
4948 : Word16 tmp_tab_x[L_FRAME16k];
4949 : Word16 tmp_tab_y[L_FRAME16k];
4950 : Word16 shift, q, ener, i;
4951 : Word32 L_tmp;
4952 : Word16 *pt1, *pt2;
4953 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
4954 0 : Flag Overflow = 0;
4955 0 : move32();
4956 : #endif
4957 :
4958 0 : Copy( x, tmp_tab_x, len ); /* OPTIMIZE !!!!! the copy into local table is not necessary */
4959 0 : Copy( y, tmp_tab_y, len ); /* could be reworked to do a 1st iteration with the original x[] and y[] */
4960 : /* then check if there is an overflow and do a more complex 2nd, 3rd, ... processing */
4961 0 : shift = 0;
4962 0 : move16();
4963 : BASOP_SATURATE_WARNING_OFF_EVS
4964 : DO
4965 : {
4966 0 : Overflow = 0;
4967 0 : move16();
4968 0 : L_tmp = L_shl_o( 1, s_max( sub( add( add( qx, qy ), 7 ), shift ), 0 ), &Overflow );
4969 0 : pt1 = tmp_tab_x;
4970 0 : pt2 = tmp_tab_y;
4971 0 : FOR( i = 0; i < len; i++ )
4972 : {
4973 0 : L_tmp = L_mac0_o( L_tmp, *pt1++, *pt2++, &Overflow ); /*Q(qx+qy-shift) */
4974 : }
4975 :
4976 0 : IF( Overflow != 0 )
4977 : {
4978 0 : Scale_sig( tmp_tab_x, len, -2 );
4979 0 : Scale_sig( tmp_tab_y, len, -2 );
4980 0 : shift = add( shift, 4 );
4981 : }
4982 : }
4983 0 : WHILE( Overflow != 0 );
4984 : BASOP_SATURATE_WARNING_ON_EVS
4985 :
4986 0 : q = norm_l( L_tmp );
4987 0 : L_tmp = L_shl( L_tmp, q ); /*Q(qx+qy-shift+q) */
4988 0 : ener = extract_h( L_tmp ); /*Q(qx+qy-shift+q-16) */
4989 0 : q = add( q, add( qx, qy ) );
4990 0 : *qo = sub( q, add( shift, 16 ) );
4991 0 : move16();
4992 :
4993 0 : return ener;
4994 : }
4995 :
4996 :
4997 : /*
4998 : * E_UTIL_f_convolve
4999 : *
5000 : * Parameters:
5001 : * x I: input vector <14bits
5002 : * h I: impulse response (or second input vector) (1Q14)
5003 : * y O: output vetor (result of convolution)
5004 : *
5005 : * Function:
5006 : * Perform the convolution between two vectors x[] and h[] and
5007 : * write the result in the vector y[]. All vectors are of length L.
5008 : * Only the first L samples of the convolution are considered.
5009 : * Vector size = L_SUBFR
5010 : *
5011 : * Returns:
5012 : * void
5013 : */
5014 15530 : void E_UTIL_f_convolve(
5015 : const Word16 x[],
5016 : const Word16 h[],
5017 : Word16 y[],
5018 : const Word16 size )
5019 : {
5020 : Word16 i, n;
5021 : Word32 L_sum;
5022 : Word64 L64_sum;
5023 :
5024 1009450 : FOR( n = 0; n < size; n++ )
5025 : {
5026 993920 : L64_sum = 0;
5027 993920 : move64();
5028 32302400 : FOR( i = 0; i < n; i++ )
5029 : {
5030 31308480 : L64_sum = W_mac_16_16( L64_sum, x[i], h[n - i] );
5031 : }
5032 993920 : L_sum = W_sat_l( L64_sum );
5033 993920 : y[n] = mac_r( L_sum, x[i], h[0] );
5034 993920 : move16();
5035 : }
5036 :
5037 15530 : return;
5038 : }
5039 :
5040 :
5041 : /*-----------------------------------------------------------------------------
5042 : * floating_point_add:
5043 : *
5044 : * Add two floating point numbers: x <- x + y.
5045 : *----------------------------------------------------------------------------*/
5046 :
5047 23336664 : void floating_point_add(
5048 : Word32 *mx, /* io: mantissa of the addend Q31 */
5049 : Word16 *ex, /* io: exponent of the addend Q0 */
5050 : const Word32 my, /* i: mantissa of the adder Q31 */
5051 : const Word16 ey /* i: exponent of the adder Q0 */
5052 : )
5053 : {
5054 : Word32 accX, accY;
5055 : Word16 align, expo;
5056 : /* NB: This function will not work properly if the mantissa is zero and the exponent is not 32.
5057 : It is up to the caller function to avoid this condition. */
5058 : /* Ensure 1 bit headroom before addition. */
5059 23336664 : accX = L_shr( *mx, 1 );
5060 23336664 : accY = L_shr( my, 1 );
5061 : /* First, align the Q-points of the two operands. Then, add. */
5062 23336664 : align = sub( *ex, ey );
5063 :
5064 23336664 : IF( align < 0 )
5065 : {
5066 22418526 : accX = L_add( accX, L_shl( accY, align ) );
5067 : }
5068 : ELSE
5069 : {
5070 918138 : accX = L_add( accY, L_shr( accX, align ) );
5071 918138 : *ex = ey;
5072 918138 : move16();
5073 : }
5074 : /* Normalize the result and update the mantissa and exponent. */
5075 23336664 : expo = norm_l( accX );
5076 23336664 : *mx = L_shl( accX, expo );
5077 23336664 : *ex = sub( add( *ex, expo ), 1 ); /* Subtract 1 due to 1-bit down-shift above ensuring 1 bit headroom before addition. */
5078 23336664 : move32();
5079 23336664 : move16();
5080 :
5081 23336664 : return;
5082 : }
5083 :
5084 :
5085 : /*-------------------------------------------------------------------*
5086 : * delay_signal_fx()
5087 : *
5088 : * Delay buffer by defined number of samples
5089 : *-------------------------------------------------------------------*/
5090 :
5091 1000730 : void delay_signal_fx(
5092 : Word16 x[], /* i/o: signal to be delayed */
5093 : const Word16 len, /* i : length of the input signal */
5094 : Word16 mem[], /* i/o: synchronization memory */
5095 : const Word16 delay /* i : delay in samples */
5096 : )
5097 : {
5098 : Word16 tmp_buffer[L_FRAME48k];
5099 :
5100 1000730 : Copy( mem, tmp_buffer, delay );
5101 1000730 : Copy( x + sub( len, delay ), mem, delay );
5102 1000730 : Copy( x, x + delay, sub( len, delay ) );
5103 1000730 : Copy( tmp_buffer, x, delay );
5104 :
5105 1000730 : return;
5106 : }
5107 :
5108 2646577 : void delay_signal32_fx(
5109 : Word32 x[], /* i/o: signal to be delayed */
5110 : const Word16 len, /* i : length of the input signal */
5111 : Word32 mem[], /* i/o: synchronization memory */
5112 : const Word16 delay /* i : delay in samples */
5113 : )
5114 : {
5115 : Word32 tmp_buffer[L_FRAME48k];
5116 :
5117 2646577 : Copy32( mem, tmp_buffer, delay );
5118 2646577 : Copy32( x + sub( len, delay ), mem, delay );
5119 2646577 : Copy32( x, x + delay, sub( len, delay ) );
5120 2646577 : Copy32( tmp_buffer, x, delay );
5121 :
5122 2646577 : return;
5123 : }
5124 :
5125 54200 : void delay_signal_q_adj_fx(
5126 : Word32 x[], /* i/o: signal to be delayed */
5127 : const Word16 len, /* i : length of the input signal */
5128 : Word32 mem[], /* i/o: synchronization memory */
5129 : const Word16 delay, /* i : delay in samples */
5130 : const Word16 q_x,
5131 : const Word16 q_mem )
5132 : {
5133 :
5134 : Word32 tmp_buffer[L_FRAME48k];
5135 :
5136 54200 : Copy32( mem, tmp_buffer, delay );
5137 54200 : Copy32( x + sub( len, delay ), mem, delay );
5138 54200 : Copy32( x, x + delay, sub( len, delay ) );
5139 :
5140 :
5141 54200 : IF( EQ_16( q_x, q_mem ) )
5142 : {
5143 46239 : Copy32( tmp_buffer, x, delay );
5144 : }
5145 : ELSE
5146 : {
5147 7961 : v_shr( tmp_buffer, sub( q_mem, q_x ), x, delay );
5148 : }
5149 :
5150 54200 : return;
5151 : }
5152 :
5153 :
5154 3809 : Word16 floor_log_2( Word32 num )
5155 : {
5156 :
5157 3809 : IF( num == 0 )
5158 : {
5159 0 : return 0;
5160 : }
5161 :
5162 3809 : return ( sub( 30, norm_l( num ) ) );
5163 : }
5164 :
5165 3646663 : void v_shr(
5166 : const Word32 x[], /* i : Input vector */
5167 : const Word16 shift, /* i : Constant */
5168 : Word32 y[], /* o : Output vector that contains x >> shift */
5169 : const Word16 N /* i : Vector length */
5170 : )
5171 : {
5172 : Word16 i;
5173 :
5174 647600598 : FOR( i = 0; i < N; i++ )
5175 : {
5176 643953935 : y[i] = L_shr( x[i], shift );
5177 643953935 : move32();
5178 : }
5179 :
5180 3646663 : return;
5181 : }
5182 :
5183 0 : void v_shr_16(
5184 : const Word16 x[], /* i : Input vector */
5185 : const Word16 shift, /* i : Constant */
5186 : Word16 y[], /* o : Output vector that contains x >> shift */
5187 : const Word16 N /* i : Vector length */
5188 : )
5189 : {
5190 : Word16 i;
5191 :
5192 0 : FOR( i = 0; i < N; i++ )
5193 : {
5194 0 : y[i] = shr( x[i], shift );
5195 0 : move16();
5196 : }
5197 :
5198 0 : return;
5199 : }
5200 :
5201 :
5202 : /*---------------------------------------------------------------------*
5203 : * lin_interp_fx()
5204 : *
5205 : * Linearly maps x from source range <x1, x2> to the target range <y1, y2>
5206 : *---------------------------------------------------------------------*/
5207 :
5208 : /*! r: mapped output value */
5209 713 : Word16 lin_interp_fx(
5210 : const Word16 x, /* i : Q15 the value to be mapped */
5211 : const Word16 x1, /* i : Q15 source range interval: low end */
5212 : const Word16 y1, /* i : Q15 source range interval: high end */
5213 : const Word16 x2, /* i : Q15 target range interval: low */
5214 : const Word16 y2, /* i : Q15 target range interval: high */
5215 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
5216 : )
5217 : {
5218 713 : IF( sub( x2, x1 ) == 0 )
5219 : {
5220 0 : return y1;
5221 : }
5222 713 : ELSE IF( flag_sat )
5223 : {
5224 713 : IF( GE_16( x, s_max( x1, x2 ) ) )
5225 : {
5226 0 : IF( GT_16( x1, x2 ) )
5227 : {
5228 0 : return y1;
5229 : }
5230 : ELSE
5231 : {
5232 0 : return y2;
5233 : }
5234 : }
5235 713 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
5236 : {
5237 2 : IF( LT_16( x1, x2 ) )
5238 : {
5239 2 : return y1;
5240 : }
5241 : ELSE
5242 : {
5243 0 : return y2;
5244 : }
5245 : }
5246 : }
5247 :
5248 711 : return add_sat( y1, mult( sub( x, x1 ), div_s( sub( y2, y1 ), sub( x2, x1 ) ) ) );
5249 : }
5250 :
5251 :
5252 164931 : Word16 lin_interp_ivas_fx(
5253 : const Word16 x, /* i : Q15 the value to be mapped */
5254 : const Word16 x1, /* i : Q15 source range interval: low end */
5255 : const Word16 y1, /* i : Q15 source range interval: high end */
5256 : const Word16 x2, /* i : Q15 target range interval: low */
5257 : const Word16 y2, /* i : Q15 target range interval: high */
5258 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
5259 : )
5260 : {
5261 164931 : IF( EQ_16( sub( x2, x1 ), 0 ) )
5262 : {
5263 0 : return y1;
5264 : }
5265 164931 : ELSE IF( flag_sat )
5266 : {
5267 162155 : IF( GE_16( x, s_max( x1, x2 ) ) )
5268 : {
5269 780 : return GT_16( x1, x2 ) ? y1 : y2;
5270 : }
5271 161375 : ELSE IF( LE_16( x, s_min( x1, x2 ) ) )
5272 : {
5273 124427 : return LT_16( x1, x2 ) ? y1 : y2;
5274 : }
5275 : }
5276 : Word16 div_res_e;
5277 39724 : Word16 div_res = BASOP_Util_Divide1616_Scale( sub( y2, y1 ), sub( x2, x1 ), &div_res_e );
5278 : // div_res = shl( div_res, div_res_e );
5279 39724 : return add_sat( y1, round_fx( L_shl( L_mult( sub( x, x1 ), div_res ), div_res_e ) ) );
5280 : }
5281 :
5282 :
5283 : /*---------------------------------------------------------------------
5284 : * sign_l()
5285 : *
5286 : *---------------------------------------------------------------------*/
5287 :
5288 : /*! r: sign of x (+1/-1) */
5289 82440 : Word32 sign_l(
5290 : const Word32 x /* i : input value of x */
5291 : )
5292 : {
5293 82440 : IF( x < 0 )
5294 : {
5295 38509 : return MIN_32;
5296 : }
5297 : ELSE
5298 : {
5299 43931 : return MAX_32;
5300 : }
5301 : }
5302 :
5303 199 : void v_mult16_fx(
5304 : const Word16 x1[], /* i : Input vector 1 */
5305 : const Word16 x2[], /* i : Input vector 2 */
5306 : Word16 y[], /* o : Output vector that contains vector 1 .* vector 2 */
5307 : const Word16 N /* i : Vector length */
5308 : )
5309 : {
5310 : Word16 i;
5311 :
5312 14317 : FOR( i = 0; i < N; i++ )
5313 : {
5314 14118 : y[i] = mult_r( x1[i], x2[i] );
5315 14118 : move16();
5316 : }
5317 :
5318 199 : return;
5319 : }
5320 :
5321 : /*---------------------------------------------------------------------*
5322 : * set_zero_fx()
5323 : *
5324 : * Set a vector vec[] of dimension lvec to zero
5325 : *---------------------------------------------------------------------*/
5326 :
5327 235278211 : void set_zero_fx(
5328 : Word32 *vec, /* o : input vector */
5329 : const Word16 lvec /* i : length of the vector */
5330 : )
5331 : {
5332 : Word16 i;
5333 :
5334 14802160050 : FOR( i = 0; i < lvec; i++ )
5335 : {
5336 14566881839 : *vec++ = 0;
5337 14566881839 : move32();
5338 : }
5339 :
5340 235278211 : return;
5341 : }
5342 :
5343 9965 : void set_zero2_fx(
5344 : Word32 *vec, /* o : input vector */
5345 : const Word32 lvec /* i : length of the vector */
5346 : )
5347 : {
5348 : Word32 i;
5349 :
5350 36465802 : FOR( i = 0; i < lvec; i++ )
5351 : {
5352 36455837 : *vec++ = 0;
5353 36455837 : move32();
5354 : }
5355 :
5356 9965 : return;
5357 : }
5358 :
5359 6594039 : void set16_zero_fx(
5360 : Word16 *vec, /* o : input vector */
5361 : const Word16 lvec /* i : length of the vector */
5362 : )
5363 : {
5364 : Word16 i;
5365 :
5366 2761856292 : FOR( i = 0; i < lvec; i++ )
5367 : {
5368 2755262253 : *vec++ = 0;
5369 2755262253 : move16();
5370 : }
5371 :
5372 6594039 : return;
5373 : }
5374 :
5375 1603523 : UWord32 mvl2s_r(
5376 : const Word32 x[], /* i : input vector */
5377 : const Word16 q_x,
5378 : Word16 y[], /* o : output vector */
5379 : const Word16 n /* i : vector size */
5380 : )
5381 : {
5382 : Word16 i;
5383 : Word32 temp;
5384 1603523 : UWord32 noClipping = 0;
5385 1603523 : move32();
5386 :
5387 1603523 : IF( n <= 0 )
5388 : {
5389 : /* cannot transfer vectors with size 0 */
5390 182 : return 0;
5391 : }
5392 :
5393 1603341 : IF( (void *) y <= (const void *) x )
5394 : {
5395 0 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
5396 0 : FOR( i = 0; i < n; i++ )
5397 : {
5398 0 : temp = L_add( x[i], tempd );
5399 0 : temp = L_shr( temp, q_x );
5400 :
5401 0 : IF( GT_32( temp, MAX16B ) )
5402 : {
5403 0 : temp = MAX16B;
5404 0 : move32();
5405 0 : noClipping = L_add( (Word32) noClipping, 1 );
5406 : }
5407 0 : ELSE IF( LT_32( temp, MIN16B ) )
5408 : {
5409 0 : temp = MIN16B;
5410 0 : move32();
5411 0 : noClipping = L_add( (Word32) noClipping, 1 );
5412 : }
5413 :
5414 0 : y[i] = extract_l( temp );
5415 0 : move16();
5416 : }
5417 : }
5418 : ELSE
5419 : {
5420 1603341 : Word32 tempd = L_shl( 1, sub( q_x, 1 ) );
5421 1274937441 : FOR( i = n - 1; i >= 0; i-- )
5422 : {
5423 1273334100 : temp = L_add( x[i], tempd );
5424 1273334100 : temp = L_shr( temp, q_x );
5425 :
5426 1273334100 : IF( GT_32( temp, MAX16B ) )
5427 : {
5428 403 : temp = MAX16B;
5429 403 : move32();
5430 403 : noClipping = L_add( (Word32) noClipping, 1 );
5431 : }
5432 1273333697 : ELSE IF( LT_32( temp, MIN16B ) )
5433 : {
5434 467 : temp = MIN16B;
5435 467 : move32();
5436 467 : noClipping = L_add( (Word32) noClipping, 1 );
5437 : }
5438 :
5439 1273334100 : y[i] = extract_l( temp );
5440 1273334100 : move16();
5441 : }
5442 : }
5443 :
5444 1603341 : return noClipping;
5445 : }
5446 :
5447 46190160 : Word32 dotp_me_fx(
5448 : const Word32 x[], /* i : vector x[] */
5449 : const Word32 y[], /* i : vector y[] */
5450 : const Word16 n, /* i : vector length */
5451 : Word16 exp_x,
5452 : Word16 exp_y,
5453 : Word16 *exp_suma )
5454 : {
5455 : Word16 i;
5456 : Word32 suma;
5457 : Word32 mul;
5458 46190160 : Word16 mul_exp = add( exp_x, exp_y );
5459 46190160 : suma = Mpy_32_32( x[0], y[0] );
5460 46190160 : *exp_suma = mul_exp;
5461 284069484 : FOR( i = 1; i < n; i++ )
5462 : {
5463 237879324 : mul = Mpy_32_32( x[i], y[i] );
5464 237879324 : suma = BASOP_Util_Add_Mant32Exp( suma, *exp_suma, mul, mul_exp, exp_suma ); // exp_x+exp_A
5465 : }
5466 :
5467 46190160 : return suma;
5468 : }
5469 :
5470 17500 : Word32 dotp_fx_guarded(
5471 : const Word32 x[], /* i : vector x[] */
5472 : const Word32 y[], /* i : vector y[] */
5473 : const Word16 n /* i : vector length */
5474 : )
5475 : {
5476 : Word16 i;
5477 : Word32 suma;
5478 17500 : Word16 guarded_bits = find_guarded_bits_fx( n );
5479 17500 : suma = L_shr( Mpy_32_32( x[0], y[0] ), guarded_bits );
5480 :
5481 280000 : FOR( i = 1; i < n; i++ )
5482 : {
5483 262500 : suma = L_add( suma, L_shr( Mpy_32_32( x[i], y[i] ), guarded_bits ) );
5484 : }
5485 :
5486 17500 : return suma;
5487 : }
5488 :
5489 :
5490 372 : Word32 dotp_fx_ivas_fx(
5491 : const Word32 x[], /* i : vector x[] */
5492 : Word16 x_e,
5493 : const Word32 y[], /* i : vector y[] */
5494 : Word16 y_e,
5495 : const Word16 n, /* i : vector length */
5496 : Word16 *out_e )
5497 : {
5498 : Word16 i, exp;
5499 372 : Word32 suma = 0;
5500 :
5501 372 : exp = 31;
5502 372 : move16();
5503 :
5504 118544 : FOR( i = 0; i < n; i++ )
5505 : {
5506 118172 : suma = BASOP_Util_Add_Mant32Exp( suma, exp, Mpy_32_32( x[i], y[i] ), x_e + y_e, &exp );
5507 : }
5508 :
5509 372 : *out_e = exp;
5510 372 : move16();
5511 :
5512 372 : return suma;
5513 : }
5514 :
5515 :
5516 : /*-------------------------------------------------------------------*
5517 : * v_mult()
5518 : *
5519 : * Multiplication of two vectors
5520 : *-------------------------------------------------------------------*/
5521 :
5522 14274129 : void v_mult_fx(
5523 : const Word32 x1[], /* i : Input vector 1 */
5524 : const Word32 x2[], /* i : Input vector 2 */
5525 : Word32 y[], /* o : Output vector that contains vector 1 .* vector 2 */
5526 : const Word16 N /* i : Vector length */
5527 : )
5528 : {
5529 : Word16 i;
5530 :
5531 563106407 : FOR( i = 0; i < N; i++ )
5532 : {
5533 548832278 : y[i] = Mpy_32_32( x1[i], x2[i] );
5534 548832278 : move32();
5535 : }
5536 :
5537 14274129 : return;
5538 : }
5539 :
5540 :
5541 : /*-------------------------------------------------------------------*
5542 : * anint_fx()
5543 : *
5544 : * Round to the nearest integer.
5545 : *-------------------------------------------------------------------*/
5546 :
5547 10414 : Word32 anint_fx(
5548 : const Word32 x,
5549 : const Word16 exp )
5550 : {
5551 10414 : IF( x == 0 )
5552 : {
5553 1012 : return 0;
5554 : }
5555 9402 : IF( x >= 0 )
5556 : {
5557 9402 : return L_add( x, L_shl( 1, sub( exp, 1 ) ) );
5558 : }
5559 : ELSE
5560 : {
5561 0 : return L_sub( x, L_shl( 1, sub( exp, 1 ) ) );
5562 : }
5563 : }
5564 :
5565 :
5566 : /*-------------------------------------------------------------------*
5567 : * ceil_fx()
5568 : *
5569 : * Ceil to the next multiple of (1 << exp).
5570 : *-------------------------------------------------------------------*/
5571 :
5572 31817 : Word32 ceil_fx(
5573 : const Word32 x,
5574 : const Word16 exp )
5575 : {
5576 : Word32 step;
5577 :
5578 : // step = x / L_shl( 1, exp );
5579 31817 : step = L_shr( x, exp );
5580 31817 : IF( ( x % L_shl( 1, exp ) ) > 0 )
5581 : {
5582 19847 : step = L_add( step, 1 );
5583 : }
5584 :
5585 31817 : return L_shl( step, exp );
5586 : }
5587 :
5588 861 : void sort_l(
5589 : Word32 *x, /* i/o: Vector to be sorted */
5590 : Word16 len /* i/o: vector length */
5591 : )
5592 : {
5593 : Word16 i, j;
5594 : Word32 tempr;
5595 :
5596 7991 : FOR( i = len - 2; i >= 0; i-- )
5597 : {
5598 7130 : tempr = x[i];
5599 7130 : move32();
5600 26159 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
5601 : {
5602 19029 : x[j - 1] = x[j];
5603 19029 : move32();
5604 : }
5605 7130 : x[j - 1] = tempr;
5606 7130 : move32();
5607 : }
5608 :
5609 861 : return;
5610 : }
5611 :
5612 :
5613 : /*-------------------------------------------------------------------*
5614 : * v_add()
5615 : *
5616 : * Subtraction of two vectors sample by sample
5617 : *-------------------------------------------------------------------*/
5618 :
5619 25047314 : void v_add_fx(
5620 : const Word32 x1[], /* i : Input vector 1 */
5621 : const Word32 x2[], /* i : Input vector 2 */
5622 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
5623 : const Word16 N /* i : Vector length */
5624 : )
5625 : {
5626 : Word16 i;
5627 :
5628 3065066026 : FOR( i = 0; i < N; i++ )
5629 : {
5630 3040018712 : y[i] = L_add_sat( x1[i], x2[i] );
5631 3040018712 : move32();
5632 : }
5633 :
5634 25047314 : return;
5635 : }
5636 :
5637 :
5638 4621409 : void v_add_fx_hdrm(
5639 : const Word32 x1[], /* i : Input vector 1 */
5640 : const Word32 x2[], /* i : Input vector 2 */
5641 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
5642 : const Word16 N, /* i : Vector length */
5643 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
5644 : )
5645 : {
5646 : Word16 i;
5647 :
5648 146508935 : FOR( i = 0; i < N; i++ )
5649 : {
5650 141887526 : y[i] = L_add( L_shr( x1[i], hdrm ), L_shr( x2[i], hdrm ) );
5651 141887526 : move32();
5652 : }
5653 :
5654 4621409 : return;
5655 : }
5656 :
5657 484425740 : void v_add_fx_no_hdrm(
5658 : const Word32 x1[], /* i : Input vector 1 */
5659 : const Word32 x2[], /* i : Input vector 2 */
5660 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 */
5661 : const Word16 N /* i : Vector length */
5662 : )
5663 : {
5664 : Word16 i;
5665 :
5666 2659758979 : FOR( i = 0; i < N; i++ )
5667 : {
5668 2175333239 : y[i] = L_add( x1[i], x2[i] );
5669 2175333239 : move32();
5670 : }
5671 :
5672 484425740 : return;
5673 : }
5674 :
5675 8875070 : void v_add_fx_me(
5676 : const Word32 x1[], /* i : Input vector 1 */
5677 : const Word16 x1_e, /* i : Exponent for input vector 1 */
5678 : const Word32 x2[], /* i : Input vector 2 */
5679 : const Word16 x2_e, /* i : Exponent for input vector 2 */
5680 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 */
5681 : Word16 *y_e, /* i : Exponent for output vector */
5682 : const Word16 N, /* i : Vector length */
5683 : const Word16 hdrm /* i : headroom for when subtraction result > 1 or < -1 */
5684 : )
5685 : {
5686 : Word16 i;
5687 8875070 : Word16 x1_shift = sub( s_max( x1_e, x2_e ), x1_e );
5688 8875070 : Word16 x2_shift = sub( s_max( x1_e, x2_e ), x2_e );
5689 :
5690 139335277 : FOR( i = 0; i < N; i++ )
5691 : {
5692 130460207 : y[i] = L_add( L_shr( x1[i], hdrm + x1_shift ), L_shr( x2[i], hdrm + x2_shift ) );
5693 130460207 : move32();
5694 : }
5695 :
5696 8875070 : *y_e = add( s_max( x1_e, x2_e ), hdrm );
5697 8875070 : move16();
5698 :
5699 8875070 : return;
5700 : }
5701 :
5702 74970709 : Word16 find_guarded_bits_fx(
5703 : const Word32 n )
5704 : {
5705 : // return n <= 1 ? 0 : n <= 2 ? 1
5706 : // : n <= 4 ? 2
5707 : // : n <= 8 ? 3
5708 : // : n <= 16 ? 4
5709 : // : n <= 32 ? 5
5710 : // : n <= 64 ? 6
5711 : // : n <= 128 ? 7
5712 : // : n <= 256 ? 8
5713 : // : n <= 512 ? 9
5714 : // : n <= 1024 ? 10
5715 : // : n <= 2048 ? 11
5716 : // : n <= 4096 ? 12
5717 : // : n <= 8192 ? 13
5718 : // : n <= 16384 ? 14
5719 : // : 15;
5720 : /*Word16 val = 0;
5721 : move32();
5722 : test();
5723 : WHILE( GT_32( n, L_shl( 1, val ) ) && LT_32( val, 16 ) )
5724 : {
5725 : val = add( val, 1 );
5726 : }*/
5727 74970709 : IF( LE_32( n, 1 ) )
5728 : {
5729 5466684 : return 0;
5730 : }
5731 : ELSE
5732 : {
5733 :
5734 69504025 : return sub( 31, norm_l( L_sub( n, 1 ) ) );
5735 : }
5736 : }
5737 :
5738 :
5739 678963554 : Word16 L_norm_arr(
5740 : const Word32 *arr,
5741 : Word16 size )
5742 : {
5743 678963554 : Word16 q = 31;
5744 678963554 : move16();
5745 :
5746 34963968720 : FOR( Word16 i = 0; i < size; i++ )
5747 : {
5748 : Word16 q_tst;
5749 :
5750 34285005166 : q_tst = norm_l( arr[i] );
5751 34285005166 : if ( arr[i] != 0 )
5752 : {
5753 28005742588 : q = s_min( q, q_tst );
5754 : }
5755 : }
5756 :
5757 678963554 : return q;
5758 : }
5759 :
5760 20828654 : Word16 norm_arr(
5761 : Word16 *arr,
5762 : Word16 size )
5763 : {
5764 20828654 : Word16 q = 15;
5765 20828654 : Word16 exp = 0;
5766 20828654 : move16();
5767 20828654 : move16();
5768 :
5769 8234046574 : FOR( Word16 i = 0; i < size; i++ )
5770 : {
5771 8213217920 : if ( arr[i] != 0 )
5772 : {
5773 6867217682 : exp = norm_s( arr[i] );
5774 : }
5775 8213217920 : if ( arr[i] != 0 )
5776 : {
5777 6867217682 : q = s_min( q, exp );
5778 : }
5779 : }
5780 :
5781 20828654 : return q;
5782 : }
5783 :
5784 3350327 : Word16 W_norm_arr(
5785 : Word64 *arr,
5786 : Word16 size )
5787 : {
5788 3350327 : Word16 q = 63;
5789 3350327 : Word16 exp = 0;
5790 3350327 : move16();
5791 3350327 : move16();
5792 :
5793 1591256759 : FOR( Word16 i = 0; i < size; i++ )
5794 : {
5795 1587906432 : if ( arr[i] != 0 )
5796 : {
5797 1553230300 : exp = W_norm( arr[i] );
5798 : }
5799 1587906432 : if ( arr[i] != 0 )
5800 : {
5801 1553230300 : q = s_min( q, exp );
5802 : }
5803 : }
5804 :
5805 3350327 : return q;
5806 : }
5807 :
5808 287495560 : Word16 get_min_scalefactor(
5809 : Word32 x,
5810 : Word32 y )
5811 : {
5812 : Word16 scf_y;
5813 287495560 : Word16 scf = Q31;
5814 287495560 : move16();
5815 :
5816 287495560 : test();
5817 287495560 : if ( x == 0 && y == 0 )
5818 : {
5819 21042133 : scf = 0;
5820 21042133 : move16();
5821 : }
5822 :
5823 287495560 : if ( x != 0 )
5824 : {
5825 266393835 : scf = norm_l( x );
5826 : }
5827 :
5828 287495560 : scf_y = norm_l( y );
5829 287495560 : if ( y != 0 )
5830 : {
5831 205889446 : scf = s_min( scf_y, scf );
5832 : }
5833 :
5834 287495560 : return scf;
5835 : }
5836 :
5837 :
5838 380725308 : Flag is_zero_arr(
5839 : Word32 *arr,
5840 : Word16 size )
5841 : {
5842 686333816 : FOR( Word16 i = 0; i < size; i++ )
5843 : {
5844 620831954 : IF( arr[i] != 0 )
5845 : {
5846 315223446 : return 0;
5847 : }
5848 : }
5849 :
5850 65501862 : return 1;
5851 : }
5852 :
5853 1952037 : Flag is_zero_arr16(
5854 : Word16 *arr,
5855 : Word16 size )
5856 : {
5857 257285196 : FOR( Word16 i = 0; i < size; i++ )
5858 257058641 : IF( arr[i] != 0 )
5859 : {
5860 1725482 : return 0;
5861 : }
5862 :
5863 226555 : return 1;
5864 : }
5865 :
5866 0 : Flag is_zero_arr64(
5867 : Word64 *arr,
5868 : Word16 size )
5869 : {
5870 0 : FOR( Word16 i = 0; i < size; i++ )
5871 : {
5872 0 : IF( arr[i] != 0 )
5873 : {
5874 0 : return 0;
5875 : }
5876 : }
5877 0 : return 1;
5878 : }
5879 :
5880 0 : void Scale_sig64(
5881 : Word64 x[], /* i/o: signal to scale Qx */
5882 : Word16 len, /* i : size of x[] Q0 */
5883 : Word16 exp /* i : exponent: x = round(x << exp) Qx exp */
5884 : )
5885 : {
5886 : Word16 i;
5887 :
5888 0 : assert( exp <= 63 && exp >= -63 );
5889 :
5890 0 : IF( exp == 0 )
5891 : {
5892 0 : return;
5893 : }
5894 :
5895 0 : FOR( i = 0; i < len; i++ )
5896 : {
5897 : /* saturation can occur here */
5898 0 : x[i] = W_shl( x[i], exp );
5899 0 : move64();
5900 : }
5901 :
5902 0 : return;
5903 : }
|
