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