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 : #include <assert.h>
34 : #include <stdint.h>
35 : #include "options.h"
36 : #include <math.h>
37 : #include "prot_fx.h"
38 : #include "wmc_auto.h"
39 : #include "ivas_rom_com.h"
40 : #include "ivas_prot_fx.h"
41 :
42 : #define ANGLE_90_DEG_Q22 377487360
43 : #define ANGLE_180_DEG_Q22 754974720
44 : #define ANGLE_360_DEG_Q22 1509949440
45 :
46 : /*---------------------------------------------------------------
47 : * sumAbs()
48 : *
49 : * sum of absolute values
50 : * ---------------------------------------------------------------*/
51 :
52 0 : Word32 sumAbs_fx(
53 : const Word32 *vec, /* i : input vector Qx*/
54 : const Word16 lvec /* i : length of input vector Q0*/
55 : )
56 : {
57 : Word16 i;
58 : Word32 tmp; /*Qx*/
59 :
60 0 : tmp = 0;
61 0 : move32();
62 0 : FOR( i = 0; i < lvec; i++ )
63 : {
64 0 : tmp = L_add( tmp, L_abs( vec[i] ) ); /*Qx*/
65 : }
66 :
67 0 : return tmp;
68 : }
69 :
70 : /*---------------------------------------------------------------------*
71 : * mvc2c()
72 : *
73 : * Transfer the contents of vector x[] to vector y[] in char format
74 : *---------------------------------------------------------------------*/
75 :
76 38131 : void mvc2c(
77 : const UWord8 x[], /* i : input vector */
78 : UWord8 y[], /* o : output vector */
79 : const Word16 n /* i : vector size */
80 : )
81 : {
82 : Word16 i;
83 :
84 38131 : IF( n <= 0 )
85 : {
86 : /* no need to transfer vectors with size 0 */
87 0 : return;
88 : }
89 :
90 38131 : IF( y < x )
91 : {
92 3779842 : FOR( i = 0; i < n; i++ )
93 : {
94 3762171 : y[i] = x[i];
95 3762171 : move16();
96 : }
97 : }
98 : ELSE
99 : {
100 88035 : FOR( i = n - 1; i >= 0; i-- )
101 : {
102 67575 : y[i] = x[i];
103 67575 : move16();
104 : }
105 : }
106 :
107 38131 : return;
108 : }
109 :
110 :
111 : /*-------------------------------------------------------------------*
112 : * ivas_syn_output()
113 : *
114 : * Output ivas synthesis signal with compensation for saturation
115 : * returns number of clipped samples
116 : *-------------------------------------------------------------------*/
117 :
118 : /*! r: number of clipped samples */
119 424456 : UWord32 ivas_syn_output_fx(
120 : Word32 *synth[], /* i/o: float synthesis signal q_synth*/
121 : const Word16 q_synth,
122 : const Word16 output_frame, /* i : output frame length (one channel) Q0*/
123 : const Word16 n_channels, /* i : number of output channels Q0*/
124 : Word16 *synth_out /* o : integer 16 bits synthesis signal Q0*/
125 : )
126 : {
127 : Word16 i, n;
128 : Word16 synth_loc[MAX_JBM_L_FRAME48k];
129 : UWord32 tmp;
130 424456 : UWord32 noClipping = 0;
131 424456 : move32();
132 :
133 : /*-----------------------------------------------------------------*
134 : * float to integer conversion with saturation control
135 : *-----------------------------------------------------------------*/
136 :
137 1957181 : FOR( n = 0; n < n_channels; n++ )
138 : {
139 1532725 : tmp = mvl2s_r( synth[n], q_synth, synth_loc, output_frame ); /*Q0*/
140 1532725 : noClipping = UL_addNsD( noClipping, tmp );
141 :
142 1227499805 : FOR( i = 0; i < output_frame; i++ )
143 : {
144 1225967080 : synth_out[( ( i * n_channels ) + n )] = synth_loc[i]; /*q_synth*/
145 1225967080 : move16();
146 : }
147 : }
148 :
149 424456 : return noClipping; /*Q0*/
150 : }
151 :
152 :
153 : /*-------------------------------------------------------------------*
154 : * ivas_syn_output_f()
155 : *
156 : * Output ivas synthesis signal with compensation for saturation
157 : * returns number of clipped samples
158 : *-------------------------------------------------------------------*/
159 :
160 : /*! r: number of clipped samples */
161 19553 : void ivas_syn_output_f_fx(
162 : Word32 *synth[], /* i/o: float synthesis signal Q11*/
163 : const Word16 output_frame, /* i : output frame length (one channel) Q0*/
164 : const Word16 n_channels, /* i : number of output channels Q0*/
165 : Word32 *synth_out /* o : integer 16 bits synthesis signal Q11*/
166 : )
167 : {
168 : Word16 i, n;
169 :
170 : /*-----------------------------------------------------------------*
171 : * float to integer conversion with saturation control
172 : *-----------------------------------------------------------------*/
173 :
174 64444 : FOR( n = 0; n < n_channels; n++ )
175 : {
176 35835931 : FOR( i = 0; i < output_frame; i++ )
177 : {
178 35791040 : synth_out[( ( i * n_channels ) + n )] = synth[n][i]; /*Q11*/
179 35791040 : move16();
180 : }
181 : }
182 :
183 19553 : return;
184 : }
185 :
186 :
187 : /*-------------------------------------------------------------------*
188 : * mvr2r_inc()
189 : *
190 : *
191 : *-------------------------------------------------------------------*/
192 0 : void mvr2r_inc_fixed_one(
193 : const Word32 x_fx[], /* i : input vector Q29*/
194 : const Word16 x_inc, /* i : increment for vector x[] Q0*/
195 : Word32 y_fx[], /* o : output vector Q29*/
196 : const Word16 y_inc, /* i : increment for vector y[] Q0*/
197 : const Word16 n /* i : vector size Q0*/
198 : )
199 : {
200 : Word16 i;
201 : Word16 ix;
202 : Word16 iy;
203 :
204 0 : IF( n <= 0 )
205 : {
206 : /* cannot transfer vectors with size 0 */
207 0 : return;
208 : }
209 :
210 0 : IF( y_fx < x_fx )
211 : {
212 0 : ix = 0;
213 0 : move16();
214 0 : iy = 0;
215 0 : move16();
216 0 : FOR( i = 0; i < n; i++ )
217 : {
218 0 : y_fx[iy] = x_fx[ix]; /*Q29*/
219 0 : move32();
220 :
221 0 : ix = ix + x_inc;
222 0 : iy = iy + y_inc;
223 : }
224 : }
225 : ELSE
226 : {
227 0 : ix = imult1616( sub( n, 1 ), x_inc ); /*Q0*/
228 0 : iy = imult1616( sub( n, 1 ), y_inc ); /*Q0*/
229 0 : FOR( i = ( n - 1 ); i >= 0; i-- )
230 : {
231 0 : y_fx[iy] = x_fx[ix]; /*Q29*/
232 0 : move32();
233 :
234 0 : ix = ix - x_inc;
235 0 : iy = iy - y_inc;
236 : }
237 : }
238 :
239 0 : return;
240 : }
241 :
242 82213179 : void mvr2r_inc_fixed(
243 : const Word32 x_fx[], /* i : input vector Q29*/
244 : const Word16 x_inc, /* i : increment for vector x[] Q0*/
245 : Word32 y_fx[], /* o : output vector Q29*/
246 : const Word16 y_inc, /* i : increment for vector y[] Q0*/
247 : const Word16 n /* i : vector size Q0*/
248 : )
249 : {
250 : Word16 i;
251 : Word16 ix;
252 : Word16 iy;
253 :
254 82213179 : IF( n <= 0 )
255 : {
256 : /* cannot transfer vectors with size 0 */
257 0 : return;
258 : }
259 :
260 82213179 : IF( y_fx < x_fx )
261 : {
262 74488348 : ix = 0;
263 74488348 : move16();
264 74488348 : iy = 0;
265 74488348 : move16();
266 1152055005 : FOR( i = 0; i < n; i++ )
267 : {
268 1077566657 : y_fx[iy] = x_fx[ix]; /*Q29*/
269 1077566657 : move32();
270 :
271 1077566657 : ix = ix + x_inc;
272 1077566657 : iy = iy + y_inc;
273 : }
274 : }
275 : ELSE
276 : {
277 7724831 : ix = i_mult( sub( n, 1 ), x_inc ); /*Q0*/
278 7724831 : iy = i_mult( sub( n, 1 ), y_inc );
279 58652057 : FOR( i = sub( n, 1 ); i >= 0; i-- )
280 : {
281 50927226 : y_fx[iy] = x_fx[ix]; /*Q29*/
282 50927226 : move32();
283 :
284 50927226 : ix = ix - x_inc;
285 50927226 : iy = iy - y_inc;
286 : }
287 : }
288 :
289 82213179 : return;
290 : }
291 :
292 : /*-------------------------------------------------------------------*
293 : * v_add_inc()
294 : *
295 : * Addition of two vectors sample by sample with explicit increments
296 : *-------------------------------------------------------------------*/
297 :
298 : // for same q//
299 8471993 : void v_add_inc_fx(
300 : const Word32 x1[], /* i : Input vector 1 Qx*/
301 : const Word16 x_inc, /* i : Increment for input vector 1 Q0*/
302 : const Word32 x2[], /* i : Input vector 2 Qx*/
303 : const Word16 x2_inc, /* i : Increment for input vector 2 Q0*/
304 : Word32 y[], /* o : Output vector that contains vector 1 + vector 2 Qx*/
305 : const Word16 y_inc, /* i : increment for vector y[] Q0*/
306 : const Word16 N /* i : Vector length Q0*/
307 : )
308 : {
309 : Word16 i, ix1, ix2, iy;
310 :
311 : /* The use of this function is currently always for the interleaved input format, */
312 : /* that means, the following conditions are always true and thus obsolete. */
313 8471993 : test();
314 8471993 : test();
315 8471993 : test();
316 8471993 : test();
317 8471993 : IF( ( sub( x_inc, 2 ) == 0 ) && ( sub( x2_inc, 2 ) == 0 ) && ( sub( y_inc, 1 ) == 0 ) && ( &x1[1] == &x2[0] ) )
318 : {
319 : /* Interleaved input case, linear output */
320 200320650 : FOR( i = 0; i < N; i++ )
321 : {
322 191848657 : y[i] = L_add( x1[2 * i + 0], x1[2 * i + 1] ); /*Qx*/
323 191848657 : move32();
324 : }
325 8471993 : return;
326 : }
327 :
328 0 : ix1 = 0;
329 0 : ix2 = 0;
330 0 : iy = 0;
331 0 : move16();
332 0 : move16();
333 0 : move16();
334 0 : FOR( i = 0; i < N; i++ )
335 : {
336 0 : y[iy] = L_add( x1[ix1], x2[ix2] ); /*Qx*/
337 0 : move32();
338 0 : ix1 = add( ix1, x_inc ); /*Q0*/
339 0 : ix2 = add( ix2, x2_inc ); /*Q0*/
340 0 : iy = add( iy, y_inc ); /*Q0*/
341 : }
342 0 : return;
343 : }
344 :
345 : /*-------------------------------------------------------------------*
346 : * v_mult_inc_fx()
347 : *
348 : * Multiplication of two vectors with explicit increments
349 : *-------------------------------------------------------------------*/
350 :
351 0 : void v_mult_inc_fx(
352 : const Word32 x1_fx[], /* i : Input vector 1 x1_q_fx*/
353 : Word16 *x1_q_fx,
354 : const Word16 x1_inc, /* i : Increment for input vector 1 Q0*/
355 : const Word32 x2_fx[], /* i : Input vector 2 x2_q_fx*/
356 : Word16 *x2_q_fx,
357 : const Word16 x2_inc, /* i : Increment for input vector 1 Q0*/
358 : Word32 y_fx[], /* o : Output vector that contains vector 1 .* vector 2 y_q_fx*/
359 : Word16 *y_q_fx,
360 : const Word16 y_inc, /* i : increment for vector y[i] Q0*/
361 : const Word16 N /* i : Vector length Q0*/
362 : )
363 : {
364 : Word16 i;
365 0 : Word16 ix1 = 0;
366 0 : Word16 ix2 = 0;
367 0 : Word16 iy = 0;
368 :
369 0 : move16();
370 0 : move16();
371 0 : move16();
372 :
373 0 : FOR( i = 0; i < N; i++ )
374 : {
375 0 : y_fx[iy] = Mpy_32_32( x1_fx[ix1], x2_fx[ix2] ); /* x1_q_fx + x2_q_fx - 31 */
376 0 : move32();
377 0 : y_q_fx[iy] = sub( add( x1_q_fx[ix1], x2_q_fx[ix2] ), 31 );
378 0 : move16();
379 :
380 0 : ix1 = add( ix1, x1_inc ); /*Q0*/
381 0 : ix2 = add( ix2, x2_inc ); /*Q0*/
382 0 : iy = add( iy, y_inc ); /*Q0*/
383 : }
384 :
385 0 : return;
386 : }
387 :
388 : // when buffers have constant q/
389 4469236 : void v_mult_inc_fixed(
390 : const Word32 x1_fx[], /* i : Input vector 1 Qx1*/
391 : const Word16 x1_inc, /* i : Increment for input vector 1 Q0*/
392 : const Word32 x2_fx[], /* i : Input vector 2 Qx2*/
393 : const Word16 x2_inc, /* i : Increment for input vector 1 Q0*/
394 : Word32 y_fx[], /* o : Output vector that contains vector 1 .* vector 2 Qx1 + Qx2 - 31*/
395 : const Word16 y_inc, /* i : increment for vector y[i] Q0*/
396 : const Word16 N /* i : Vector length Q0*/
397 : )
398 : {
399 : Word16 i;
400 4469236 : Word16 ix1 = 0;
401 4469236 : Word16 ix2 = 0;
402 4469236 : Word16 iy = 0;
403 :
404 4469236 : move16();
405 4469236 : move16();
406 4469236 : move16();
407 :
408 77151842 : FOR( i = 0; i < N; i++ )
409 : {
410 72682606 : y_fx[iy] = Mpy_32_32( x1_fx[ix1], x2_fx[ix2] ); /*Qx1 + Qx2 - 31*/
411 72682606 : move32();
412 :
413 72682606 : ix1 = add( ix1, x1_inc );
414 72682606 : ix2 = add( ix2, x2_inc );
415 72682606 : iy = add( iy, y_inc );
416 : }
417 :
418 4469236 : return;
419 : }
420 :
421 : /*-------------------------------------------------------------------*
422 : * v_addc_fx()
423 : *
424 : * Addition of constant to vector
425 : *-------------------------------------------------------------------*/
426 :
427 0 : void v_addc_fx(
428 : const Word32 x_fx[], /* i : Input vector Qx*/
429 : const Word32 c_fx, /* i : Constant Qx*/
430 : Word32 y_fx[], /* o : Output vector that contains c*x Qx*/
431 : const Word16 N /* i : Vector length Q0*/
432 : )
433 : {
434 : Word16 i;
435 :
436 0 : FOR( i = 0; i < N; i++ )
437 : {
438 0 : y_fx[i] = L_add( c_fx, x_fx[i] ); /*Qx*/
439 0 : move32();
440 : }
441 :
442 0 : return;
443 : }
444 : /*-------------------------------------------------------------------*
445 : * v_addc()
446 : *
447 : * Addition of constant to vector
448 : *-------------------------------------------------------------------*/
449 2067438 : void v_addc_fixed(
450 : const Word32 x[], /* i : Input vector Qx*/
451 : const Word32 c, /* i : Constant Qx*/
452 : Word32 y[], /* o : Output vector that contains c*x Qx*/
453 : const Word16 N /* i : Vector length Q0*/
454 : )
455 : {
456 : Word16 i;
457 :
458 57804083 : FOR( i = 0; i < N; i++ )
459 : {
460 55736645 : y[i] = L_add( c, x[i] ); /*Qx*/
461 55736645 : move32();
462 : }
463 :
464 2067438 : return;
465 : }
466 :
467 : /*-------------------------------------------------------------------*
468 : * v_min_fx()
469 : *
470 : * minimum of two vectors
471 : *-------------------------------------------------------------------*/
472 :
473 2400 : void v_min_fx(
474 : const Word32 x1_fx[], /* i : Input vector 1 x1_q_fx*/
475 : Word16 *x1_q_fx,
476 : const Word32 x2_fx[], /* i : Input vector 2 x2_q_fx*/
477 : Word16 *x2_q_fx,
478 : Word32 y_fx[], /* o : Output vector that contains vector 1 .* vector 2 y_q_fx*/
479 : Word16 *y_q_fx,
480 : const Word16 N /* i : Vector length Q0*/
481 : )
482 : {
483 : Word16 i;
484 :
485 60000 : FOR( i = 0; i < N; i++ )
486 : {
487 57600 : IF( GT_16( x1_q_fx[i], x2_q_fx[i] ) )
488 : {
489 0 : IF( LT_32( L_shr( x1_fx[i], sub( x1_q_fx[i], x2_q_fx[i] ) ), x2_fx[i] ) )
490 : {
491 0 : y_fx[i] = x1_fx[i]; /*x1_q_fx*/
492 0 : move32();
493 0 : y_q_fx[i] = x1_q_fx[i];
494 0 : move16();
495 : }
496 : ELSE
497 : {
498 0 : y_fx[i] = x2_fx[i]; /*x2_q_fx*/
499 0 : move32();
500 0 : y_q_fx[i] = x2_q_fx[i];
501 0 : move16();
502 : }
503 : }
504 : ELSE
505 : {
506 57600 : IF( LT_32( x1_fx[i], L_shr( x2_fx[i], sub( x2_q_fx[i], x1_q_fx[i] ) ) ) )
507 : {
508 54849 : y_fx[i] = x1_fx[i]; /*x1_q_fx*/
509 54849 : move32();
510 54849 : y_q_fx[i] = x1_q_fx[i];
511 54849 : move16();
512 : }
513 : ELSE
514 : {
515 2751 : y_fx[i] = x2_fx[i]; /*x2_q_fx*/
516 2751 : move32();
517 2751 : y_q_fx[i] = x2_q_fx[i];
518 2751 : move16();
519 : }
520 : }
521 : }
522 :
523 2400 : return;
524 : }
525 :
526 : /*-------------------------------------------------------------------*
527 : * v_sqrt()
528 : *
529 : * square root of vector
530 : *-------------------------------------------------------------------*/
531 315180 : void v_sqrt_fx(
532 : const Word32 x[], /* i : Input vector Qx*/
533 : Word16 exp[],
534 : Word32 y[], /* o : Output vector that contains sqrt(x) Q31 - exp[]*/
535 : const Word16 N /* i : Vector length Q0*/
536 : )
537 : {
538 : Word16 i;
539 :
540 945540 : FOR( i = 0; i < N; i++ )
541 : {
542 630360 : y[i] = Sqrt32( x[i], &exp[i] ); /*Q31 - exp[i]*/
543 630360 : move32();
544 : }
545 :
546 315180 : return;
547 : }
548 :
549 : /*-------------------------------------------------------------------*
550 : * v_sub_s()
551 : *
552 : * Subtraction of two vectors
553 : *-------------------------------------------------------------------*/
554 :
555 0 : void v_sub_s16_fx(
556 : const Word16 x1[], /* i : Input vector 1 Qx*/
557 : const Word16 x2[], /* i : Input vector 2 Qx*/
558 : Word16 y[], /* o : Output vector that contains vector 1 - vector 2 Qx*/
559 : const Word16 N /* i : Vector length Q0*/
560 : )
561 : {
562 : Word16 i;
563 :
564 0 : FOR( i = 0; i < N; i++ )
565 : {
566 0 : y[i] = sub( x1[i], x2[i] ); /*Qx*/
567 0 : move16();
568 : }
569 :
570 0 : return;
571 : }
572 :
573 :
574 0 : void v_sub32_fx(
575 : const Word32 x1[], /* i : Input vector 1 Qx*/
576 : const Word32 x2[], /* i : Input vector 2 Qx*/
577 : Word32 y[], /* o : Output vector that contains vector 1 - vector 2 Qx*/
578 : const Word16 N /* i : Vector length Q0*/
579 : )
580 : {
581 : Word16 i;
582 :
583 0 : FOR( i = 0; i < N; i++ )
584 : {
585 0 : y[i] = L_sub( x1[i], x2[i] ); /*Qx*/
586 0 : move32();
587 : }
588 :
589 0 : return;
590 : }
591 :
592 : /*---------------------------------------------------------------------*
593 : * dot_product_cholesky()
594 : *
595 : * Calculates dot product of type x'*A*A'*x, where x is column vector of size m,
596 : * and A is a Cholesky decomposition of some Hermitian matrix S whose size is m*m.
597 : * Therefore, S=A*A' where A is upper triangular matrix of size (m*m+m)/2 (zeros ommitted, column-wise)
598 : *---------------------------------------------------------------------*/
599 :
600 : /*! r: the dot product x'*A*A'*x */
601 0 : Word64 dot_product_cholesky_fixed(
602 : const Word32 *x, /* i : vector x Q31 - exp_x*/
603 : const Word32 *A, /* i : Cholesky matrix A Q31 - exp_A*/
604 : const Word16 N /* i : vector & matrix size Q0*/
605 : )
606 : {
607 : Word16 i, j;
608 : Word64 suma, tmp_sum;
609 : Word32 tmp;
610 : const Word32 *pt_x, *pt_A;
611 0 : pt_A = A;
612 0 : suma = 0;
613 0 : move64();
614 :
615 0 : FOR( i = 0; i < N; i++ )
616 : {
617 0 : tmp_sum = 0;
618 0 : move32();
619 0 : pt_x = x;
620 :
621 0 : FOR( j = 0; j <= i; j++ )
622 : {
623 0 : tmp_sum = W_add( tmp_sum, Mpy_32_32( *pt_x++, *pt_A++ ) );
624 : }
625 :
626 0 : tmp = W_shl_sat_l( tmp_sum, -4 ); // to make sure that the tmp_sum will not overflow
627 0 : suma = W_mac_32_32( suma, tmp, tmp );
628 : }
629 :
630 0 : return suma;
631 : }
632 :
633 0 : void v_mult_mat_fixed(
634 : Word32 *y, /* o : the product x*A Qx - guardbits*/
635 : const Word32 *x, /* i : vector x Qx*/
636 : const Word32 *A, /* i : matrix A Q31*/
637 : const Word16 Nr, /* i : number of rows Q0*/
638 : const Word16 Nc, /* i : number of columns Q0*/
639 : Word16 guardbits )
640 : {
641 : Word16 i, j;
642 : const Word32 *pt_x, *pt_A;
643 : Word32 tmp_y[MAX_V_MULT_MAT];
644 : Word32 *pt_y;
645 :
646 0 : pt_y = tmp_y;
647 0 : pt_A = A; /*Q31*/
648 :
649 0 : FOR( i = 0; i < Nc; i++ )
650 : {
651 0 : pt_x = x; /*Qx*/
652 0 : *pt_y = 0;
653 0 : move32();
654 0 : FOR( j = 0; j < Nr; j++ )
655 : {
656 0 : *pt_y = L_add( *pt_y, L_shr( Mpy_32_32( ( *pt_x++ ), ( *pt_A++ ) ), guardbits ) ); /*Qx - guardbits*/
657 0 : move32();
658 : }
659 0 : pt_y++;
660 : }
661 :
662 0 : MVR2R_WORD32( tmp_y, y, Nc ); /*Qx - guardbits*/
663 0 : }
664 :
665 0 : Word32 dot_product_cholesky_fx(
666 : const Word32 *x, /* i : vector x Qx1*/
667 : const Word32 *A, /* i : Cholesky matrix A Qx2*/
668 : const Word16 N /* i : vector & matrix size Q0*/
669 : )
670 : {
671 : Word16 i, j;
672 : Word32 suma, tmp_sum;
673 : const Word32 *pt_x, *pt_A;
674 :
675 0 : pt_A = A;
676 0 : suma = 0;
677 0 : move32();
678 :
679 0 : FOR( i = 0; i < N; i++ )
680 : {
681 0 : tmp_sum = 0;
682 0 : move32();
683 0 : pt_x = x;
684 0 : FOR( j = 0; j <= i; j++ )
685 : {
686 0 : tmp_sum = L_add( tmp_sum, Mpy_32_32( *pt_x++, *pt_A++ ) ); /*Qx1 + Qx2 - 31*/
687 : }
688 :
689 0 : suma = L_add( suma, Mpy_32_32( tmp_sum, tmp_sum ) );
690 : }
691 :
692 0 : return suma;
693 : }
694 :
695 :
696 : /*---------------------------------------------------------------------*
697 : * v_mult_mat_fx()
698 : *
699 : * Multiplication of row vector x by matrix A, where x has size Nr and
700 : * A has size Nr x Nc ans it is stored column-wise in memory.
701 : * The resulting row vector y has size Nc
702 : *---------------------------------------------------------------------*/
703 :
704 0 : void v_mult_mat_fx(
705 : Word32 *y_fx, /* o : the product x*A y_q_fx*/
706 : Word16 *y_q_fx,
707 : const Word32 *x_fx, /* i : vector x x_q_fx*/
708 : Word16 *x_q_fx,
709 : const Word32 *A_fx, /* i : matrix A A_q_fx*/
710 : Word16 *A_q_fx,
711 : const Word16 Nr, /* i : number of rows Q0*/
712 : const Word16 Nc /* i : number of columns Q0*/
713 : )
714 : {
715 : Word16 i, j;
716 : const Word32 *pt_x_fx, *pt_A_fx;
717 : Word32 *pt_y_fx;
718 : Word32 tmp_y_fx[MAX_V_MULT_MAT];
719 : Word32 temp;
720 : Word16 temp_q;
721 :
722 0 : pt_y_fx = tmp_y_fx;
723 0 : pt_A_fx = A_fx; /*A_q_fx*/
724 0 : pt_x_fx = x_fx; /*x_q_fx*/
725 :
726 0 : FOR( i = 0; i < Nc; i++ )
727 : {
728 0 : pt_x_fx = x_fx; /*x_q_fx*/
729 0 : *pt_y_fx = 0;
730 0 : move32();
731 0 : y_q_fx[i] = 0;
732 0 : move32();
733 0 : FOR( j = 0; j < Nr; j++ )
734 : {
735 0 : temp = Mpy_32_32( *pt_x_fx++, *pt_A_fx++ ); /*x_q_fx + A_q_fx - 31*/
736 0 : temp_q = sub( add( x_q_fx[j], A_q_fx[Nr * i + j] ), 31 );
737 0 : IF( j == 0 )
738 : {
739 0 : *pt_y_fx = temp;
740 0 : move32();
741 0 : y_q_fx[i] = temp_q;
742 0 : move16();
743 : }
744 : ELSE
745 : {
746 0 : IF( GT_16( y_q_fx[i], temp_q ) )
747 : {
748 0 : *pt_y_fx = L_add( L_shr( *pt_y_fx, sub( y_q_fx[i], temp_q ) ), temp );
749 0 : move32();
750 0 : y_q_fx[i] = temp_q;
751 0 : move16();
752 : }
753 : ELSE
754 : {
755 0 : *pt_y_fx = L_add( *pt_y_fx, L_shr( temp, sub( temp_q, y_q_fx[i] ) ) );
756 0 : move32();
757 : }
758 : }
759 : }
760 0 : pt_y_fx++;
761 : }
762 :
763 0 : MVR2R_WORD32( tmp_y_fx, y_fx, Nc ); /*y_q_fx*/
764 :
765 0 : return;
766 : }
767 :
768 : /*---------------------------------------------------------------------*
769 : * logsumexp()
770 : *
771 : * Compute logarithm of the sum of exponentials of input vector in a numerically stable way
772 : *---------------------------------------------------------------------*/
773 :
774 : /*! r: log(sum(exp(x)) of the input array x */
775 0 : Word32 logsumexp_fx(
776 : const Word32 x[], /* i : input array x Q31 - x_e*/
777 : const Word16 x_e,
778 : const Word16 N /* i : number of elements in array x Q0*/
779 : )
780 : {
781 : Word32 max_exp, temp32_sub;
782 : Word32 sum, temp32, pow_temp;
783 0 : Word32 log2_e_fx = 1549082005; // Q30 of log2(e);
784 0 : Word16 log2_e_fx_e = 1;
785 0 : move16();
786 0 : move16();
787 : Word16 i;
788 0 : Word16 pow_e, sum_e = 0;
789 0 : move16();
790 0 : max_exp = x[0]; /*Q31 - x_e*/
791 0 : move32();
792 0 : sum = 0;
793 0 : move32();
794 0 : FOR( i = 1; i < N; i++ )
795 : {
796 0 : IF( GT_32( x[i], max_exp ) )
797 : {
798 0 : max_exp = x[i]; /*Q31 - x_e*/
799 0 : move32();
800 : }
801 : }
802 :
803 0 : FOR( i = 0; i < N; i++ )
804 : {
805 0 : temp32_sub = L_sub( x[i], max_exp ); /*Q31 - x_e*/
806 0 : pow_e = 0;
807 0 : move16();
808 0 : temp32 = Mpy_32_32( log2_e_fx, temp32_sub ); /*Q30 - x_e*/
809 0 : pow_temp = BASOP_util_Pow2( temp32, add( x_e, log2_e_fx_e ), &pow_e ); /*Q31 - pow_e*/
810 0 : sum = BASOP_Util_Add_Mant32Exp( sum, sum_e, pow_temp, pow_e, &sum_e ); /*Q31 - sum_e*/
811 : }
812 0 : temp32 = L_add( BASOP_Util_Log2( sum ), L_shl( sum_e, Q25 ) ); /*Q25*/
813 0 : temp32 = Mpy_32_32( temp32, 1488522239 ); /*logf(x) = log2(x)*logf(2) Q25*/
814 0 : temp32 = L_add( L_shr( temp32, sub( x_e, 6 ) ), max_exp ); // q = 31-x_e
815 0 : return temp32; /*31-x_e*/
816 : }
817 :
818 : /*---------------------------------------------------------------------*
819 : * lin_interp()
820 : *
821 : * Linearly maps x from source range <x1, x2> to the target range <y1, y2>
822 : *---------------------------------------------------------------------*/
823 : /*! r: mapped output value */
824 0 : Word32 lin_interp32_fx(
825 : const Word32 x, /* i : the value to be mapped Qx */
826 : const Word32 x1, /* i : source range interval: low end Qx */
827 : const Word32 y1, /* i : source range interval: high end Q31 */
828 : const Word32 x2, /* i : target range interval: low Qx */
829 : const Word32 y2, /* i : target range interval: high Q31 */
830 : const Word16 flag_sat /* i : flag to indicate whether to apply saturation */
831 : )
832 : {
833 : Word32 temp32;
834 : Word32 temp_div;
835 : Word16 exp_out;
836 :
837 0 : IF( L_sub( x2, x1 ) == 0 )
838 : {
839 0 : return y1;
840 : }
841 0 : ELSE IF( flag_sat )
842 : {
843 0 : IF( GE_32( x, L_max( x1, x2 ) ) )
844 : {
845 0 : return GT_32( x1, x2 ) ? y1 : y2;
846 : }
847 0 : ELSE IF( LE_32( x, L_min( x1, x2 ) ) )
848 : {
849 0 : return LT_32( x1, x2 ) ? y1 : y2;
850 : }
851 : }
852 :
853 0 : temp32 = Mpy_32_32( L_sub( x, x1 ), L_sub( y2, y1 ) ); /* Qx */
854 0 : temp_div = L_deposit_h( BASOP_Util_Divide3232_Scale( temp32, L_sub( x2, x1 ), &exp_out ) );
855 0 : temp32 = BASOP_Util_Add_Mant32Exp( y1, 0, temp_div, exp_out, &exp_out );
856 0 : temp32 = L_shl_sat( temp32, exp_out ); /* Q31 */
857 :
858 0 : return temp32;
859 : }
860 :
861 : /*-------------------------------------------------------------------*
862 : * check_bounds_s_fx()
863 : *
864 : * Ensure the input value is within the given limits
865 : *-------------------------------------------------------------------*/
866 :
867 : /*! r: Adjusted value */
868 851426 : Word16 check_bounds_s_fx(
869 : const Word16 value, /* i : Input value Q0*/
870 : const Word16 low, /* i : Low limit Q0*/
871 : const Word16 high /* i : High limit Q0*/
872 : )
873 : {
874 : Word16 value_adj;
875 :
876 851426 : value_adj = s_min( s_max( value, low ), high ); /*Q0*/
877 :
878 851426 : return value_adj; /*Q0*/
879 : }
880 :
881 : /*-------------------------------------------------------------------*
882 : * check_bounds_l()
883 : *
884 : * Ensure the input value is within the given limits
885 : *-------------------------------------------------------------------*/
886 :
887 : /*! r: Adjusted value */
888 531730 : Word32 check_bounds_l(
889 : const Word32 value, /* i : Input value Q0*/
890 : const Word32 low, /* i : Low limit Q0*/
891 : const Word32 high /* i : High limit Q0*/
892 : )
893 : {
894 : Word32 value_adj;
895 :
896 531730 : value_adj = L_min( L_max( value, low ), high ); /*Q0*/
897 :
898 531730 : return value_adj; /*Q0*/
899 : }
900 :
901 : /****************************************************************************/
902 : /* matrix functions */
903 : /* matrices are ordered column-wise in memory */
904 : /****************************************************************************/
905 :
906 : /*---------------------------------------------------------------------*
907 : * matrix product
908 : *
909 : * comput the matrix product of two matrices (Z=X*Y)
910 : *---------------------------------------------------------------------*/
911 :
912 2138353 : Word16 matrix_product_mant_exp_fx(
913 : const Word32 *X_fx, /* i : left hand matrix Q31 - X_fx_e*/
914 : const Word16 X_fx_e, /* i : left hand matrix */
915 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
916 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
917 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
918 : const Word32 *Y_fx, /* i : right hand matrix Q31 - Y_fx_e*/
919 : const Word16 Y_fx_e, /* i : right hand matrix */
920 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
921 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
922 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
923 : Word32 *Z_fx, /* o : resulting matrix after the matrix multiplication Q31 - Z_fx_e*/
924 : Word16 *Z_fx_e /* o : resulting matrix after the matrix multiplication */
925 : )
926 : {
927 : Word16 i, j, k;
928 2138353 : Word32 *Zp_fx = Z_fx;
929 : Word16 out_e[MAX_OUTPUT_CHANNELS * MAX_OUTPUT_CHANNELS];
930 2138353 : Word16 *Zp_fx_e = out_e;
931 : Word16 row, col;
932 : Word64 temp;
933 : Word16 temp_e;
934 2138353 : Word16 prod_e = add( X_fx_e, Y_fx_e );
935 :
936 2138353 : Word16 max_exp = -31;
937 2138353 : move16();
938 :
939 : /* Processing */
940 2138353 : test();
941 2138353 : test();
942 2138353 : test();
943 2138353 : IF( EQ_16( transpX, 1 ) && transpY == 0 ) /* We use X transpose */
944 : {
945 0 : IF( NE_16( rowsX, rowsY ) )
946 : {
947 0 : return EXIT_FAILURE;
948 : }
949 0 : FOR( j = 0; j < colsY; ++j )
950 : {
951 0 : FOR( i = 0; i < colsX; ++i )
952 : {
953 0 : temp = 0;
954 0 : move64();
955 :
956 0 : FOR( k = 0; k < rowsX; ++k )
957 : {
958 0 : temp = W_mac_32_32( temp, X_fx[k + i * rowsX], Y_fx[k + j * rowsY] ); // X_fx_e + Y_fx_e
959 : }
960 : /* Maximize accumulated value to 32-bit */
961 0 : temp_e = W_norm( temp );
962 0 : temp = W_shl( temp, temp_e );
963 0 : if ( 0 == temp )
964 : {
965 0 : temp_e = prod_e;
966 0 : move16();
967 : }
968 0 : *Zp_fx_e = sub( prod_e, temp_e );
969 0 : move16();
970 0 : ( *Zp_fx ) = W_extract_h( temp );
971 0 : move32();
972 0 : max_exp = s_max( max_exp, *Zp_fx_e ); // Find the max exp
973 0 : Zp_fx++;
974 0 : Zp_fx_e++;
975 : }
976 : }
977 0 : row = colsY; /*Q0*/
978 0 : move16();
979 0 : col = colsX; /*Q0*/
980 0 : move16();
981 : }
982 2138353 : ELSE IF( transpX == 0 && EQ_16( transpY, 1 ) ) /* We use Y transpose */
983 : {
984 869996 : IF( NE_16( colsX, colsY ) )
985 : {
986 0 : return EXIT_FAILURE;
987 : }
988 5193485 : FOR( j = 0; j < rowsY; ++j )
989 : {
990 38051404 : FOR( i = 0; i < rowsX; ++i )
991 : {
992 33727915 : temp = 0;
993 33727915 : move64();
994 110702540 : FOR( k = 0; k < colsX; ++k )
995 : {
996 76974625 : temp = W_mac_32_32( temp, X_fx[i + k * rowsX], Y_fx[j + k * rowsY] ); // X_fx_e + Y_fx_e
997 : }
998 : /* Maximize accumulated value to 32-bit */
999 33727915 : temp_e = W_norm( temp );
1000 33727915 : temp = W_shl( temp, temp_e );
1001 33727915 : if ( 0 == temp )
1002 : {
1003 15075146 : temp_e = prod_e;
1004 15075146 : move16();
1005 : }
1006 33727915 : *Zp_fx_e = sub( prod_e, temp_e );
1007 33727915 : move16();
1008 33727915 : ( *Zp_fx ) = W_extract_h( temp );
1009 33727915 : move32();
1010 33727915 : max_exp = s_max( max_exp, *Zp_fx_e ); // Find the max exp
1011 33727915 : Zp_fx++;
1012 33727915 : Zp_fx_e++;
1013 : }
1014 : }
1015 869996 : row = rowsY; /*Q0*/
1016 869996 : move16();
1017 869996 : col = rowsX; /*Q0*/
1018 869996 : move16();
1019 : }
1020 1268357 : ELSE IF( EQ_16( transpX, 1 ) && EQ_16( transpY, 1 ) ) /* We use both transpose */
1021 : {
1022 132787 : IF( NE_16( rowsX, colsY ) )
1023 : {
1024 0 : return EXIT_FAILURE;
1025 : }
1026 812472 : FOR( j = 0; j < rowsY; ++j )
1027 : {
1028 2052025 : FOR( i = 0; i < colsX; ++i )
1029 : {
1030 1372340 : temp = 0;
1031 1372340 : move64();
1032 4155930 : FOR( k = 0; k < colsX; ++k )
1033 : {
1034 2783590 : temp = W_mac_32_32( temp, X_fx[k + i * rowsX], Y_fx[j + k * rowsY] ); // X_fx_e + Y_fx_e
1035 : }
1036 : /* Maximize accumulated value to 32-bit */
1037 1372340 : temp_e = W_norm( temp );
1038 1372340 : temp = W_shl( temp, temp_e );
1039 1372340 : if ( 0 == temp )
1040 : {
1041 0 : temp_e = prod_e;
1042 0 : move16();
1043 : }
1044 1372340 : *Zp_fx_e = sub( prod_e, temp_e );
1045 1372340 : move16();
1046 1372340 : ( *Zp_fx ) = W_extract_h( temp );
1047 1372340 : move32();
1048 1372340 : max_exp = s_max( max_exp, *Zp_fx_e ); // Find the max exp
1049 1372340 : Zp_fx++;
1050 1372340 : Zp_fx_e++;
1051 : }
1052 : }
1053 132787 : row = rowsY; /*Q0*/
1054 132787 : move16();
1055 132787 : col = colsX; /*Q0*/
1056 132787 : move16();
1057 : }
1058 : ELSE /* Regular case */
1059 : {
1060 1135570 : IF( NE_16( colsX, rowsY ) )
1061 : {
1062 0 : return EXIT_FAILURE;
1063 : }
1064 :
1065 4164318 : FOR( j = 0; j < colsY; ++j )
1066 : {
1067 17023303 : FOR( i = 0; i < rowsX; ++i )
1068 : {
1069 13994555 : temp = 0;
1070 13994555 : move64();
1071 69572380 : FOR( k = 0; k < colsX; ++k )
1072 : {
1073 55577825 : temp = W_mac_32_32( temp, X_fx[i + k * rowsX], Y_fx[k + j * rowsY] ); // X_fx_e + Y_fx_e
1074 : }
1075 : /* Maximize accumulated value to 32-bit */
1076 13994555 : temp_e = W_norm( temp );
1077 13994555 : temp = W_shl( temp, temp_e );
1078 13994555 : if ( 0 == temp )
1079 : {
1080 2399484 : temp_e = prod_e;
1081 : }
1082 13994555 : *Zp_fx_e = sub( prod_e, temp_e );
1083 13994555 : move16();
1084 13994555 : ( *Zp_fx ) = W_extract_h( temp );
1085 13994555 : move32();
1086 13994555 : max_exp = s_max( max_exp, *Zp_fx_e ); // Find the max exp
1087 13994555 : Zp_fx++;
1088 13994555 : Zp_fx_e++;
1089 : }
1090 : }
1091 1135570 : row = colsY; /*Q0*/
1092 1135570 : move16();
1093 1135570 : col = rowsX; /*Q0*/
1094 1135570 : move16();
1095 : }
1096 2138353 : Zp_fx = Z_fx; /*Q31 - Zp_fx_e*/
1097 :
1098 2138353 : Zp_fx_e = out_e;
1099 2138353 : move16();
1100 :
1101 :
1102 2138353 : *Z_fx_e = max_exp;
1103 2138353 : move16();
1104 10170275 : FOR( j = 0; j < row; ++j )
1105 : {
1106 57126732 : FOR( i = 0; i < col; ++i )
1107 : {
1108 49094810 : *Zp_fx = L_shr_r( *Zp_fx, sub( *Z_fx_e, *Zp_fx_e ) ); /*Q31 - Zp_fx_e*/
1109 49094810 : move32();
1110 49094810 : Zp_fx++;
1111 49094810 : Zp_fx_e++;
1112 : }
1113 : }
1114 :
1115 2138353 : return EXIT_SUCCESS;
1116 : }
1117 :
1118 399444 : Word16 matrix_product_fx(
1119 : const Word32 *X_fx, /* i : left hand matrix Qx*/
1120 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1121 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1122 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1123 : const Word32 *Y_fx, /* i : right hand matrix Qy*/
1124 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1125 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1126 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1127 : Word32 *Z_fx /* o : resulting matrix after the matrix multiplication Qx + Qy - 31*/
1128 : )
1129 : {
1130 : Word16 i, j, k;
1131 399444 : Word32 *Zp_fx = Z_fx;
1132 :
1133 : /* Processing */
1134 399444 : test();
1135 399444 : test();
1136 399444 : test();
1137 399444 : IF( EQ_16( transpX, 1 ) && transpY == 0 ) /* We use X transpose */
1138 : {
1139 1080 : IF( NE_16( rowsX, rowsY ) )
1140 : {
1141 0 : return EXIT_FAILURE;
1142 : }
1143 5400 : FOR( j = 0; j < colsY; ++j )
1144 : {
1145 25920 : FOR( i = 0; i < colsX; ++i )
1146 : {
1147 21600 : ( *Zp_fx ) = 0;
1148 21600 : move32();
1149 129600 : FOR( k = 0; k < rowsX; ++k )
1150 : {
1151 108000 : ( *Zp_fx ) = Madd_32_32( *Zp_fx, X_fx[k + i * rowsX], Y_fx[k + j * rowsY] ); /*Qx + Qy - 31*/
1152 108000 : move32();
1153 : }
1154 21600 : Zp_fx++;
1155 : }
1156 : }
1157 : }
1158 398364 : ELSE IF( transpX == 0 && EQ_16( transpY, 1 ) ) /* We use Y transpose */
1159 : {
1160 132787 : IF( NE_16( colsX, colsY ) )
1161 : {
1162 0 : return EXIT_FAILURE;
1163 : }
1164 948409 : FOR( j = 0; j < rowsY; ++j )
1165 : {
1166 5880714 : FOR( i = 0; i < rowsX; ++i )
1167 : {
1168 5065092 : ( *Zp_fx ) = 0;
1169 5065092 : move32();
1170 15349516 : FOR( k = 0; k < colsX; ++k )
1171 : {
1172 10284424 : ( *Zp_fx ) = Madd_32_32( *Zp_fx, X_fx[i + k * rowsX], Y_fx[j + k * rowsY] ); /*Qx + Qy - 31*/
1173 10284424 : move32();
1174 : }
1175 5065092 : Zp_fx++;
1176 : }
1177 : }
1178 : }
1179 265577 : ELSE IF( EQ_16( transpX, 1 ) && EQ_16( transpY, 1 ) ) /* We use both transpose */
1180 : {
1181 0 : IF( NE_16( rowsX, colsY ) )
1182 : {
1183 0 : return EXIT_FAILURE;
1184 : }
1185 0 : FOR( j = 0; j < rowsY; ++j )
1186 : {
1187 0 : FOR( i = 0; i < colsX; ++i )
1188 : {
1189 0 : ( *Zp_fx ) = 0;
1190 0 : move32();
1191 0 : FOR( k = 0; k < colsX; ++k )
1192 : {
1193 0 : ( *Zp_fx ) = Madd_32_32( *Zp_fx, X_fx[k + i * rowsX], Y_fx[j + k * rowsY] ); /*Qx + Qy - 31*/
1194 0 : move32();
1195 : }
1196 :
1197 0 : Zp_fx++;
1198 : }
1199 : }
1200 : }
1201 : ELSE /* Regular case */
1202 : {
1203 265577 : IF( NE_16( colsX, rowsY ) )
1204 : {
1205 0 : return EXIT_FAILURE;
1206 : }
1207 :
1208 799453 : FOR( j = 0; j < colsY; ++j )
1209 : {
1210 3551724 : FOR( i = 0; i < rowsX; ++i )
1211 : {
1212 3017848 : ( *Zp_fx ) = 0;
1213 3017848 : move32();
1214 9135366 : FOR( k = 0; k < colsX; ++k )
1215 : {
1216 6117518 : ( *Zp_fx ) = L_add_sat( *Zp_fx, Mpy_32_32( X_fx[i + k * rowsX], Y_fx[k + j * rowsY] ) ); /*Qx + Qy - 31*/
1217 : // TODO: overflow of Z_fx to be checked
1218 6117518 : move32();
1219 : }
1220 3017848 : Zp_fx++;
1221 : }
1222 : }
1223 : }
1224 :
1225 399444 : return EXIT_SUCCESS;
1226 : }
1227 :
1228 1637 : Word16 matrix_product_q30_fx(
1229 : const Word32 *X_fx, /* i : left hand matrix Q31*/
1230 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1231 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1232 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1233 : const Word32 *Y_fx, /* i : right hand matrix Q25*/
1234 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1235 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1236 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1237 : Word32 *Z_fx /* o : resulting matrix after the matrix multiplication Q30*/
1238 : )
1239 : {
1240 : Word16 i, j, k;
1241 1637 : Word32 *Zp_fx = Z_fx;
1242 : Word64 W_tmp;
1243 :
1244 : /* Processing */
1245 1637 : test();
1246 1637 : test();
1247 1637 : test();
1248 1637 : IF( EQ_16( transpX, 1 ) && transpY == 0 ) /* We use X transpose */
1249 : {
1250 557 : IF( NE_16( rowsX, rowsY ) )
1251 : {
1252 0 : return EXIT_FAILURE;
1253 : }
1254 1114 : FOR( j = 0; j < colsY; ++j )
1255 : {
1256 5863 : FOR( i = 0; i < colsX; ++i )
1257 : {
1258 : //( *Zp_fx ) = 0;
1259 5306 : W_tmp = 0;
1260 5306 : move64();
1261 62248 : FOR( k = 0; k < rowsX; ++k )
1262 : {
1263 56942 : W_tmp = W_add( W_tmp, W_mult0_32_32( X_fx[k + i * rowsX], Y_fx[k + j * rowsY] ) ); // Q56
1264 : }
1265 5306 : W_tmp = W_shl( W_tmp, 6 ); /*Q62*/
1266 5306 : ( *Zp_fx ) = W_round64_L( W_tmp ); /*Q30*/
1267 5306 : move32();
1268 5306 : Zp_fx++;
1269 : }
1270 : }
1271 : }
1272 1080 : ELSE IF( transpX == 0 && EQ_16( transpY, 1 ) ) /* We use Y transpose */
1273 : {
1274 0 : IF( NE_16( colsX, colsY ) )
1275 : {
1276 0 : return EXIT_FAILURE;
1277 : }
1278 0 : FOR( j = 0; j < rowsY; ++j )
1279 : {
1280 0 : FOR( i = 0; i < rowsX; ++i )
1281 : {
1282 : //( *Zp_fx ) = 0;
1283 0 : W_tmp = 0;
1284 0 : move64();
1285 0 : FOR( k = 0; k < colsX; ++k )
1286 : {
1287 0 : W_tmp = W_add( W_tmp, W_mult0_32_32( X_fx[i + k * rowsX], Y_fx[j + k * rowsY] ) ); // Q56
1288 : }
1289 0 : W_tmp = W_shl( W_tmp, 6 ); /*Q62*/
1290 0 : ( *Zp_fx ) = W_round64_L( W_tmp ); /*Q30*/
1291 0 : move32();
1292 0 : Zp_fx++;
1293 : }
1294 : }
1295 : }
1296 1080 : ELSE IF( EQ_16( transpX, 1 ) && EQ_16( transpY, 1 ) ) /* We use both transpose */
1297 : {
1298 0 : IF( NE_16( rowsX, colsY ) )
1299 : {
1300 0 : return EXIT_FAILURE;
1301 : }
1302 0 : FOR( j = 0; j < rowsY; ++j )
1303 : {
1304 0 : FOR( i = 0; i < colsX; ++i )
1305 : {
1306 : //( *Zp_fx ) = 0;
1307 0 : W_tmp = 0;
1308 0 : move64();
1309 0 : FOR( k = 0; k < colsX; ++k )
1310 : {
1311 0 : W_tmp = W_add( W_tmp, W_mult0_32_32( X_fx[k + i * rowsX], Y_fx[j + k * rowsY] ) ); // Q56
1312 : }
1313 :
1314 0 : W_tmp = W_shl( W_tmp, 6 ); /*Q62*/
1315 0 : ( *Zp_fx ) = W_round64_L( W_tmp ); /*Q30*/
1316 0 : move32();
1317 0 : Zp_fx++;
1318 : }
1319 : }
1320 : }
1321 : ELSE /* Regular case */
1322 : {
1323 1080 : IF( NE_16( colsX, rowsY ) )
1324 : {
1325 0 : return EXIT_FAILURE;
1326 : }
1327 :
1328 5400 : FOR( j = 0; j < colsY; ++j )
1329 : {
1330 25920 : FOR( i = 0; i < rowsX; ++i )
1331 : {
1332 : //( *Zp_fx ) = 0;
1333 21600 : W_tmp = 0;
1334 21600 : move64();
1335 108000 : FOR( k = 0; k < colsX; ++k )
1336 : {
1337 86400 : W_tmp = W_add( W_tmp, W_mult0_32_32( X_fx[i + k * rowsX], Y_fx[k + j * rowsY] ) ); // Q56
1338 : }
1339 21600 : W_tmp = W_shl( W_tmp, 6 ); /*Q62*/
1340 21600 : ( *Zp_fx ) = W_round64_L( W_tmp ); /*Q30*/
1341 21600 : move32();
1342 21600 : Zp_fx++;
1343 : }
1344 : }
1345 : }
1346 :
1347 1637 : return EXIT_SUCCESS;
1348 : }
1349 : /*takes input matrices in mantissa and exponent forms*/
1350 402561 : Word16 matrix_product_mant_exp(
1351 : const Word32 *X_fx, /* i : left hand matrix Q31 - X_e*/
1352 : const Word16 *X_e, /* i : left hand matrix */
1353 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1354 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1355 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1356 : const Word32 *Y_fx, /* i : right hand matrix Q31 - Y_e*/
1357 : const Word16 *Y_e, /* i : right hand matrix */
1358 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1359 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1360 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1361 : Word32 *Z_fx, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1362 : Word16 *Z_e /* o : resulting matrix after the matrix multiplication */
1363 : )
1364 : {
1365 : Word16 i, j, k;
1366 402561 : Word32 *Zp = Z_fx;
1367 402561 : Word16 *Zp_e = Z_e;
1368 : Word32 L_tmp;
1369 : Word16 tmp_e;
1370 :
1371 : /* Processing */
1372 402561 : test();
1373 402561 : test();
1374 402561 : test();
1375 402561 : IF( EQ_16( transpX, 1 ) && transpY == 0 ) /* We use X transpose */
1376 : {
1377 0 : IF( NE_16( rowsX, rowsY ) )
1378 : {
1379 0 : return EXIT_FAILURE;
1380 : }
1381 0 : FOR( j = 0; j < colsY; ++j )
1382 : {
1383 0 : FOR( i = 0; i < colsX; ++i )
1384 : {
1385 0 : ( *Zp ) = 0;
1386 0 : move32();
1387 0 : ( *Zp_e ) = 0;
1388 0 : move16();
1389 0 : FOR( k = 0; k < rowsX; ++k )
1390 : {
1391 0 : L_tmp = Mpy_32_32( X_fx[k + i * rowsX], Y_fx[k + j * rowsY] ); /*Q31 - (X_e + Y_e)*/
1392 0 : tmp_e = add( X_e[k + i * rowsX], Y_e[k + j * rowsY] );
1393 :
1394 0 : ( *Zp ) = BASOP_Util_Add_Mant32Exp( *Zp, *Zp_e, L_tmp, tmp_e, &tmp_e );
1395 0 : move32();
1396 0 : ( *Zp_e ) = tmp_e;
1397 0 : move16();
1398 : }
1399 0 : Zp++;
1400 0 : Zp_e++;
1401 : }
1402 : }
1403 : }
1404 402561 : ELSE IF( transpX == 0 && EQ_16( transpY, 1 ) ) /* We use Y transpose */
1405 : {
1406 134887 : IF( NE_16( colsX, colsY ) )
1407 : {
1408 0 : return EXIT_FAILURE;
1409 : }
1410 827172 : FOR( j = 0; j < rowsY; ++j )
1411 : {
1412 4297870 : FOR( i = 0; i < rowsX; ++i )
1413 : {
1414 3605585 : ( *Zp ) = 0;
1415 3605585 : move32();
1416 3605585 : ( *Zp_e ) = 0;
1417 3605585 : move16();
1418 11399305 : FOR( k = 0; k < colsX; ++k )
1419 : {
1420 7793720 : L_tmp = Mpy_32_32( X_fx[i + k * rowsX], Y_fx[j + k * rowsY] ); /*Q31 - (X_e + Y_e)*/
1421 7793720 : tmp_e = add( X_e[i + k * rowsX], Y_e[j + k * rowsY] );
1422 :
1423 7793720 : ( *Zp ) = BASOP_Util_Add_Mant32Exp( *Zp, *Zp_e, L_tmp, tmp_e, &tmp_e );
1424 7793720 : ( *Zp_e ) = tmp_e;
1425 7793720 : move16();
1426 : }
1427 3605585 : Zp++;
1428 3605585 : Zp_e++;
1429 : }
1430 : }
1431 : }
1432 267674 : ELSE IF( EQ_16( transpX, 1 ) && EQ_16( transpY, 1 ) ) /* We use both transpose */
1433 : {
1434 0 : IF( NE_16( rowsX, colsY ) )
1435 : {
1436 0 : return EXIT_FAILURE;
1437 : }
1438 0 : FOR( j = 0; j < rowsY; ++j )
1439 : {
1440 0 : FOR( i = 0; i < colsX; ++i )
1441 : {
1442 0 : ( *Zp ) = 0;
1443 0 : move32();
1444 0 : ( *Zp_e ) = 0;
1445 0 : move16();
1446 0 : FOR( k = 0; k < colsX; ++k )
1447 : {
1448 0 : L_tmp = Mpy_32_32( X_fx[k + i * rowsX], Y_fx[j + k * rowsY] ); /*Q31 - (X_e + Y_e)*/
1449 0 : tmp_e = add( X_e[k + i * rowsX], Y_e[j + k * rowsY] );
1450 :
1451 0 : ( *Zp ) = BASOP_Util_Add_Mant32Exp( *Zp, *Zp_e, L_tmp, tmp_e, &tmp_e );
1452 0 : move32();
1453 0 : ( *Zp_e ) = tmp_e;
1454 0 : move16();
1455 : }
1456 :
1457 0 : Zp++;
1458 0 : Zp_e++;
1459 : }
1460 : }
1461 : }
1462 : ELSE /* Regular case */
1463 : {
1464 267674 : IF( NE_16( colsX, rowsY ) )
1465 : {
1466 0 : return EXIT_FAILURE;
1467 : }
1468 :
1469 818342 : FOR( j = 0; j < colsY; ++j )
1470 : {
1471 3396148 : FOR( i = 0; i < rowsX; ++i )
1472 : {
1473 2845480 : ( *Zp ) = 0;
1474 2845480 : move32();
1475 2845480 : ( *Zp_e ) = 0;
1476 2845480 : move16();
1477 9219060 : FOR( k = 0; k < colsX; ++k )
1478 : {
1479 6373580 : L_tmp = Mpy_32_32( X_fx[i + k * rowsX], Y_fx[k + j * rowsY] ); /*Q31 - (X_e + Y_e)*/
1480 6373580 : tmp_e = add( X_e[i + k * rowsX], Y_e[k + j * rowsY] );
1481 :
1482 6373580 : ( *Zp ) = BASOP_Util_Add_Mant32Exp( *Zp, *Zp_e, L_tmp, tmp_e, &tmp_e );
1483 6373580 : move32();
1484 6373580 : ( *Zp_e ) = tmp_e;
1485 6373580 : move16();
1486 : }
1487 2845480 : Zp++;
1488 2845480 : Zp_e++;
1489 : }
1490 : }
1491 : }
1492 :
1493 402561 : return EXIT_SUCCESS;
1494 : }
1495 :
1496 :
1497 1575900 : Word16 matrix_diag_product_fx(
1498 : const Word32 *X, /* i : left hand matrix Q31 - X_e*/
1499 : Word16 X_e,
1500 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1501 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1502 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1503 : const Word32 *Y, /* i : right hand diagonal matrix as vector containing the diagonal elements Q31 - Y_e*/
1504 : Word16 Y_e,
1505 : const Word16 entriesY, /* i : number of entries in the diagonal Q0*/
1506 : Word32 *Z, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1507 : Word16 *Z_e )
1508 : {
1509 : Word16 i, j;
1510 1575900 : Word32 *Zp = Z;
1511 :
1512 : /* Processing */
1513 1575900 : IF( EQ_16( transpX, 1 ) ) /* We use X transpose */
1514 : {
1515 0 : IF( NE_16( rowsX, entriesY ) )
1516 : {
1517 0 : return EXIT_FAILURE;
1518 : }
1519 0 : FOR( j = 0; j < entriesY; ++j )
1520 : {
1521 0 : FOR( i = 0; i < colsX; ++i )
1522 : {
1523 0 : *( Zp ) = Mpy_32_32( X[j + i * rowsX], Y[j] ); /*Q31 - (X_e + Y_e)*/
1524 0 : move32();
1525 0 : Zp++;
1526 : }
1527 : }
1528 : }
1529 : ELSE /* Regular case */
1530 : {
1531 1575900 : IF( NE_16( colsX, entriesY ) )
1532 : {
1533 0 : return EXIT_FAILURE;
1534 : }
1535 :
1536 6983460 : FOR( j = 0; j < entriesY; ++j )
1537 : {
1538 34250760 : FOR( i = 0; i < rowsX; ++i )
1539 : {
1540 28843200 : *( Zp ) = Mpy_32_32( *( X ), Y[j] ); /*Q31 - (X_e + Y_e)*/
1541 28843200 : move32();
1542 28843200 : Zp++;
1543 28843200 : X++;
1544 : }
1545 : }
1546 : }
1547 :
1548 1575900 : *Z_e = add( X_e, Y_e );
1549 1575900 : move16();
1550 :
1551 1575900 : return EXIT_SUCCESS;
1552 : }
1553 :
1554 132787 : Word16 matrix_diag_product_fx_2(
1555 : const Word32 *X, /* i : left hand matrix Q31 - X_e*/
1556 : const Word16 X_e,
1557 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1558 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1559 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1560 : const Word32 *Y, /* i : right hand diagonal matrix as vector containing the diagonal elements Q31 - Y_e*/
1561 : const Word16 *Y_e,
1562 : const Word16 entriesY, /* i : number of entries in the diagonal Q0*/
1563 : Word32 *Z, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1564 : Word16 *Z_e )
1565 : {
1566 : Word16 i, j;
1567 132787 : Word32 *Zp = Z;
1568 132787 : Word16 *Z_ep = Z_e;
1569 : Word16 tmp;
1570 132787 : Word16 max_exp = -31;
1571 132787 : move16();
1572 :
1573 : /* Processing */
1574 132787 : IF( EQ_16( transpX, 1 ) ) /* We use X transpose */
1575 : {
1576 0 : IF( NE_16( rowsX, entriesY ) )
1577 : {
1578 0 : return EXIT_FAILURE;
1579 : }
1580 0 : FOR( j = 0; j < entriesY; ++j )
1581 : {
1582 0 : FOR( i = 0; i < colsX; ++i )
1583 : {
1584 0 : tmp = j + i * rowsX; /*Q0*/
1585 0 : *( Zp ) = Mpy_32_32( X[tmp], Y[j] ); /*Q31 - (X_e + Y_e)*/
1586 0 : move32();
1587 0 : Zp++;
1588 0 : *( Z_ep ) = add( X_e, Y_e[j] );
1589 0 : move16();
1590 0 : max_exp = s_max( max_exp, *Z_ep ); // Find the max exp
1591 0 : Z_ep++;
1592 : }
1593 : }
1594 :
1595 0 : Zp = Z;
1596 0 : Z_ep = Z_e;
1597 0 : FOR( j = 0; j < entriesY; ++j )
1598 : {
1599 0 : FOR( i = 0; i < colsX; ++i )
1600 : {
1601 0 : *Zp = L_shr( *Zp, sub( max_exp, *Z_ep ) );
1602 0 : *Z_ep = max_exp;
1603 0 : Zp++;
1604 0 : Z_ep++;
1605 : }
1606 : }
1607 : }
1608 : ELSE /* Regular case */
1609 : {
1610 132787 : IF( NE_16( colsX, entriesY ) )
1611 : {
1612 0 : return EXIT_FAILURE;
1613 : }
1614 :
1615 812472 : FOR( j = 0; j < entriesY; ++j )
1616 : {
1617 2052025 : FOR( i = 0; i < rowsX; ++i )
1618 : {
1619 1372340 : *( Zp ) = Mpy_32_32( *( X ), Y[j] ); /*Q31 - (X_e + Y_e)*/
1620 1372340 : move32();
1621 1372340 : Zp++;
1622 1372340 : *( Z_ep ) = add( X_e, Y_e[j] );
1623 1372340 : move16();
1624 1372340 : max_exp = s_max( max_exp, *Z_ep ); // Find the max exp
1625 1372340 : Z_ep++;
1626 1372340 : X++;
1627 : }
1628 : }
1629 132787 : Zp = Z;
1630 132787 : Z_ep = Z_e;
1631 812472 : FOR( j = 0; j < entriesY; ++j )
1632 : {
1633 2052025 : FOR( i = 0; i < rowsX; ++i )
1634 : {
1635 1372340 : *Zp = L_shr( *Zp, sub( max_exp, *Z_ep ) );
1636 1372340 : *Z_ep = max_exp;
1637 1372340 : Zp++;
1638 1372340 : Z_ep++;
1639 : }
1640 : }
1641 : }
1642 :
1643 132787 : return EXIT_SUCCESS;
1644 : }
1645 :
1646 106849 : Word16 matrix_diag_product_fx_1(
1647 : const Word32 *X, /* i : left hand matrix Q31 - X_e*/
1648 : const Word16 *X_e,
1649 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1650 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1651 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1652 : const Word32 *Y, /* i : right hand diagonal matrix as vector containing the diagonal elements Q31 - Y_e*/
1653 : const Word16 *Y_e,
1654 : const Word16 entriesY, /* i : number of entries in the diagonal Q0*/
1655 : Word32 *Z, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1656 : Word16 *Z_e )
1657 : {
1658 : Word16 i, j;
1659 106849 : Word32 *Zp = Z;
1660 106849 : Word16 *Z_ep = Z_e;
1661 :
1662 : /* Processing */
1663 106849 : IF( EQ_16( transpX, 1 ) ) /* We use X transpose */
1664 : {
1665 0 : IF( NE_16( rowsX, entriesY ) )
1666 : {
1667 0 : return EXIT_FAILURE;
1668 : }
1669 0 : FOR( j = 0; j < entriesY; ++j )
1670 : {
1671 0 : FOR( i = 0; i < colsX; ++i )
1672 : {
1673 0 : *( Zp ) = Mpy_32_32( X[j + i * rowsX], Y[j] ); /*Q31 - (X_e + Y_e)*/
1674 0 : move32();
1675 0 : Zp++;
1676 0 : *( Z_ep ) = add( X_e[j + i * rowsX], Y_e[j] );
1677 0 : move16();
1678 0 : Z_ep++;
1679 : }
1680 : }
1681 : }
1682 : ELSE /* Regular case */
1683 : {
1684 106849 : IF( NE_16( colsX, entriesY ) )
1685 : {
1686 0 : return EXIT_FAILURE;
1687 : }
1688 :
1689 654124 : FOR( j = 0; j < entriesY; ++j )
1690 : {
1691 3391850 : FOR( i = 0; i < rowsX; ++i )
1692 : {
1693 2844575 : *( Zp ) = Mpy_32_32( *( X ), Y[j] ); /*Q31 - (X_e + Y_e)*/
1694 2844575 : move32();
1695 2844575 : Zp++;
1696 2844575 : *( Z_ep ) = add( *( X_e ), Y_e[j] );
1697 2844575 : move16();
1698 2844575 : Z_ep++;
1699 2844575 : X++;
1700 2844575 : X_e++;
1701 : }
1702 : }
1703 : }
1704 :
1705 106849 : return EXIT_SUCCESS;
1706 : }
1707 :
1708 763147 : Word16 diag_matrix_product_fx(
1709 : const Word32 *Y, /* i : left hand diagonal matrix as vector containing the diagonal elements Q31 - Y_e*/
1710 : Word16 Y_e,
1711 : const Word16 entriesY, /* i : length of the diagonal of the left hand matrix Q0*/
1712 : const Word32 *X, /* i : right hand matrix Q31 - X_e*/
1713 : Word16 X_e,
1714 : const Word16 rowsX, /* i : number of rows of the right hand matrix Q0*/
1715 : const Word16 colsX, /* i : number of columns of the right hand matrix Q0*/
1716 : const Word16 transpX, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1717 : Word32 *Z, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1718 : Word16 *Z_e )
1719 : {
1720 : Word16 i, j;
1721 763147 : Word32 *Zp = Z;
1722 :
1723 : /* Processing */
1724 763147 : IF( EQ_16( transpX, 1 ) ) /* We use X transpose */
1725 : {
1726 315180 : IF( NE_16( colsX, entriesY ) )
1727 : {
1728 0 : return EXIT_FAILURE;
1729 : }
1730 3194100 : FOR( i = 0; i < rowsX; ++i )
1731 : {
1732 8636760 : FOR( j = 0; j < entriesY; ++j )
1733 : {
1734 5757840 : *( Zp ) = Mpy_32_32( X[i + j * rowsX], Y[j] ); /*Q31 - (X_e + Y_e)*/
1735 5757840 : move32();
1736 5757840 : Zp++;
1737 : }
1738 : }
1739 : }
1740 : ELSE /* Regular case */
1741 : {
1742 447967 : IF( NE_16( rowsX, entriesY ) )
1743 : {
1744 0 : return EXIT_FAILURE;
1745 : }
1746 1677758 : FOR( i = 0; i < colsX; ++i )
1747 : {
1748 10099706 : FOR( j = 0; j < entriesY; ++j )
1749 : {
1750 8869915 : *( Zp ) = Mpy_32_32( *( X ), Y[j] ); /*Q31 - (X_e + Y_e)*/
1751 8869915 : move32();
1752 8869915 : Zp++;
1753 8869915 : X++;
1754 : }
1755 : }
1756 : }
1757 :
1758 763147 : *Z_e = add( Y_e, X_e );
1759 763147 : move16();
1760 :
1761 763147 : return EXIT_SUCCESS;
1762 : }
1763 :
1764 804489 : Word16 matrix_product_diag_fx(
1765 : const Word32 *X, /* i : left hand matrix Q31 - X_e*/
1766 : Word16 X_e,
1767 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1768 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1769 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1770 : const Word32 *Y, /* i : right hand matrix Q31 - Y_e*/
1771 : Word16 Y_e,
1772 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1773 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1774 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1775 : Word32 *Z, /* o : resulting matrix after the matrix multiplication Q31 - Z_e*/
1776 : Word16 *Z_e )
1777 : {
1778 : Word16 j, k;
1779 804489 : Word32 *Zp = Z;
1780 :
1781 : /* Processing */
1782 804489 : test();
1783 804489 : test();
1784 804489 : test();
1785 804489 : IF( EQ_16( transpX, 1 ) && transpY == 0 ) /* We use X transpose */
1786 : {
1787 0 : IF( NE_16( rowsX, rowsY ) )
1788 : {
1789 0 : return EXIT_FAILURE;
1790 : }
1791 :
1792 0 : FOR( j = 0; j < colsY; ++j )
1793 : {
1794 0 : ( *Zp ) = 0;
1795 0 : move32();
1796 0 : FOR( k = 0; k < rowsX; ++k )
1797 : {
1798 0 : ( *Zp ) = Madd_32_32( ( *Zp ), X[k + j * rowsX], Y[k + j * rowsY] ); /*Q31 - (X_e + Y_e)*/
1799 0 : move32();
1800 : }
1801 0 : Zp++;
1802 : }
1803 : }
1804 804489 : ELSE IF( transpX == 0 && EQ_16( transpY, 1 ) ) /* We use Y transpose */
1805 : {
1806 687603 : IF( NE_16( colsX, colsY ) )
1807 : {
1808 0 : return EXIT_FAILURE;
1809 : }
1810 5473168 : FOR( j = 0; j < rowsY; ++j )
1811 : {
1812 4785565 : ( *Zp ) = 0;
1813 4785565 : move32();
1814 16132660 : FOR( k = 0; k < colsX; ++k )
1815 : {
1816 11347095 : ( *Zp ) = Madd_32_32( ( *Zp ), X[j + k * rowsX], Y[j + k * rowsY] ); /*Q31 - (X_e + Y_e)*/
1817 11347095 : move32();
1818 : }
1819 4785565 : Zp++;
1820 : }
1821 : }
1822 116886 : ELSE IF( EQ_16( transpX, 1 ) && EQ_16( transpY, 1 ) ) /* We use both transpose */
1823 : {
1824 0 : IF( NE_16( rowsX, colsY ) )
1825 : {
1826 0 : return EXIT_FAILURE;
1827 : }
1828 :
1829 0 : FOR( j = 0; j < rowsY; ++j )
1830 : {
1831 :
1832 0 : ( *Zp ) = 0;
1833 0 : move32();
1834 0 : FOR( k = 0; k < colsX; ++k )
1835 : {
1836 0 : ( *Zp ) = Madd_32_32( ( *Zp ), X[k + j * rowsX], Y[j + k * rowsY] ); /*Q31 - (X_e + Y_e)*/
1837 0 : move32();
1838 : }
1839 :
1840 0 : Zp++;
1841 : }
1842 : }
1843 : ELSE /* Regular case */
1844 : {
1845 116886 : IF( NE_16( colsX, rowsY ) )
1846 : {
1847 0 : return EXIT_FAILURE;
1848 : }
1849 :
1850 1182390 : FOR( j = 0; j < colsY; ++j )
1851 : {
1852 1065504 : ( *Zp ) = 0;
1853 1065504 : move32();
1854 2131008 : FOR( k = 0; k < colsX; ++k )
1855 : {
1856 1065504 : ( *Zp ) = Madd_32_32( ( *Zp ), X[j + k * rowsX], Y[k + j * rowsY] ); /*Q31 - (X_e + Y_e)*/
1857 1065504 : move32();
1858 : }
1859 1065504 : Zp++;
1860 : }
1861 : }
1862 :
1863 804489 : *Z_e = add( X_e, Y_e );
1864 804489 : move16();
1865 :
1866 804489 : return EXIT_SUCCESS;
1867 : }
1868 :
1869 : /*---------------------------------------------------------------------*
1870 : * matrix_product_diag()
1871 : *
1872 : * compute only the main diagonal of X*Y (Z=diag(X*Y))
1873 : *---------------------------------------------------------------------*/
1874 :
1875 2084371 : void cmplx_matrix_square_fx(
1876 : const Word32 *realX, /* i : real part of the matrix Q31 - input_exp*/
1877 : const Word32 *imagX, /* i : imaginary part of the matrix Q31 - input_exp*/
1878 : const Word16 mRows, /* i : number of rows of the matrix Q0*/
1879 : const Word16 nCols, /* i : number of columns of the matrix Q0*/
1880 : Word32 *realZ, /* o : real part of the resulting matrix Q31 - output_exp*/
1881 : Word32 *imagZ, /* o : imaginary part of the resulting matrix Q31 - output_exp*/
1882 : Word16 input_exp,
1883 : Word16 *output_exp )
1884 : {
1885 : Word16 i, j, k;
1886 : Word32 *realZp, *imagZp;
1887 : const Word32 *p_real1, *p_real2, *p_imag1, *p_imag2;
1888 : Word16 tmp1, tmp2;
1889 :
1890 : /* resulting matrix is hermitean, we only need to calc the upper triangle */
1891 : /* we assume transposition needed */
1892 :
1893 : /* column*column = column/column */
1894 6272881 : FOR( i = 0; i < nCols; i++ )
1895 : {
1896 10500927 : FOR( j = i; j < nCols; j++ )
1897 : {
1898 6312417 : p_real1 = realX + imult1616( i, mRows ); /*Q31 - input_exp*/
1899 6312417 : p_imag1 = imagX + imult1616( i, mRows ); /*Q31 - input_exp*/
1900 6312417 : p_real2 = realX + imult1616( j, mRows ); /*Q31 - input_exp*/
1901 6312417 : p_imag2 = imagX + imult1616( j, mRows ); /*Q31 - input_exp*/
1902 6312417 : realZp = realZ + add( i, imult1616( nCols, j ) ); /*Q31 - output_exp*/
1903 6312417 : imagZp = imagZ + add( i, imult1616( nCols, j ) ); /*Q31 - output_exp*/
1904 6312417 : *( realZp ) = 0;
1905 6312417 : move32();
1906 6312417 : *( imagZp ) = 0;
1907 6312417 : move32();
1908 :
1909 33082257 : FOR( k = 0; k < mRows; k++ )
1910 : {
1911 26769840 : *( imagZp ) = L_add( *( imagZp ), L_sub( Mpy_32_32( *( p_real1 ), *( p_imag2 ) ), Mpy_32_32( *( p_real2 ), *( p_imag1 ) ) ) ); /* Q31 - 2*input_exp */
1912 26769840 : move32();
1913 26769840 : *( realZp ) = L_add( *( realZp ), L_add( Mpy_32_32( *( p_real1 ), *( p_real2 ) ), Mpy_32_32( *( p_imag1 ), *( p_imag2 ) ) ) ); /* Q31 - 2*input_exp */
1914 26769840 : move32();
1915 26769840 : p_real1++;
1916 26769840 : p_real2++;
1917 26769840 : p_imag1++;
1918 26769840 : p_imag2++;
1919 : }
1920 : }
1921 : }
1922 :
1923 : /* fill lower triangle */
1924 4188510 : FOR( i = 1; i < nCols; i++ )
1925 : {
1926 4228046 : FOR( j = 0; j < i; j++ )
1927 : {
1928 2123907 : tmp1 = add( i, imult1616( nCols, j ) ); /*Q0*/
1929 2123907 : tmp2 = add( j, imult1616( nCols, i ) ); /*Q0*/
1930 2123907 : realZ[tmp1] = realZ[tmp2]; /*Q31 - output_exp*/
1931 2123907 : move32();
1932 2123907 : imagZ[tmp1] = imagZ[tmp2]; /*Q31 - output_exp*/
1933 2123907 : move32();
1934 : }
1935 : }
1936 :
1937 2084371 : *output_exp = shl( input_exp, 1 );
1938 2084371 : move16();
1939 :
1940 2084371 : return;
1941 : }
1942 :
1943 :
1944 731303 : void v_multc_acc_32_16(
1945 : const Word32 x[], /* i : Input vector Qx*/
1946 : const Word16 c, /* i : Constant Q31*/
1947 : Word32 y[], /* o : Output vector that contains y + c*x Qx*/
1948 : const Word16 N /* i : Vector length Q0*/
1949 : )
1950 : {
1951 : Word16 i;
1952 :
1953 39214143 : FOR( i = 0; i < N; i++ )
1954 : {
1955 38482840 : y[i] = Madd_32_16( y[i], x[i], c );
1956 38482840 : move32();
1957 : }
1958 :
1959 731303 : return;
1960 : }
1961 283200 : void v_multc_acc_32_32(
1962 : const Word32 x[], /* i : Input vector Qx*/
1963 : const Word32 c, /* i : Constant Q31*/
1964 : Word32 y[], /* o : Output vector that contains y + c*x Qx*/
1965 : const Word16 N /* i : Vector length Q0*/
1966 : )
1967 : {
1968 : Word16 i;
1969 :
1970 17275200 : FOR( i = 0; i < N; i++ )
1971 : {
1972 16992000 : y[i] = Madd_32_32( y[i], x[i], c ); /*Qx*/
1973 16992000 : move32();
1974 : }
1975 :
1976 283200 : return;
1977 : }
1978 :
1979 : /*---------------------------------------------------------------------*
1980 : * lls_interp_n()
1981 : *
1982 : * Linear least squares interpolation of an input vector
1983 : * The function calculates the slope 'a' and the offset 'b' of a LLS estimate for an input vector x such that
1984 : * y_i = a*i + b where i=1,..,N and (a,b) = min(a,b) [sum_N[(y_i - x_i)^2]]
1985 : * the interpolated vector is return as x[], if requested
1986 : *---------------------------------------------------------------------*/
1987 :
1988 4067 : void lls_interp_n_fx(
1989 : Word16 x_fx[], /* i/o: input/output vector Q15*/
1990 : const Word16 N, /* i : length of the input vector Q0*/
1991 : Word16 *a_fx, /* o : calculated slope Q15*/
1992 : Word16 *b_fx, /* o : calculated offset Q15*/
1993 : const Word16 upd /* i : use 1 to update x[] with the interpolated output Q0*/
1994 : )
1995 : {
1996 : Word16 i;
1997 4067 : const Word16 n_i_fx[11] = { 0, 2048, 4096, 6144, 8192, 10240, 12288, 14336, 16384, 18432, 20480 }; // Q11
1998 4067 : move16();
1999 4067 : move16();
2000 4067 : move16();
2001 4067 : move16();
2002 4067 : move16();
2003 4067 : move16();
2004 4067 : move16();
2005 4067 : move16();
2006 4067 : move16();
2007 4067 : move16();
2008 4067 : move16();
2009 :
2010 4067 : const Word16 one_by_n_fx[11] = { 0, 32767, 16384, 10911, 8192, 6553, 5459, 4681, 4096, 3640, 3276 }; /*Q15*/
2011 4067 : move16();
2012 4067 : move16();
2013 4067 : move16();
2014 4067 : move16();
2015 4067 : move16();
2016 4067 : move16();
2017 4067 : move16();
2018 4067 : move16();
2019 4067 : move16();
2020 4067 : move16();
2021 4067 : move16();
2022 :
2023 4067 : const Word16 sum_i_fx[12] = { 0, 0, 1, 3, 6, 10, 15, 21, 28, 36, 45, 55 }; /*Q0*/
2024 4067 : move16();
2025 4067 : move16();
2026 4067 : move16();
2027 4067 : move16();
2028 4067 : move16();
2029 4067 : move16();
2030 4067 : move16();
2031 4067 : move16();
2032 4067 : move16();
2033 4067 : move16();
2034 4067 : move16();
2035 4067 : move16();
2036 :
2037 : // 1.0f/ ( N * sum_ii[N] - sum_i[N] * sum_i[N] )
2038 4067 : const Word32 res_table[12] = { 0, 0, 0, 357913952, 107374184, 42949672, 20452226, 10956549, 6391320, 3976821, 2603010, 385 }; /*Q31*/
2039 4067 : move16();
2040 4067 : move16();
2041 4067 : move16();
2042 4067 : move16();
2043 4067 : move16();
2044 4067 : move16();
2045 4067 : move16();
2046 4067 : move16();
2047 4067 : move16();
2048 4067 : move16();
2049 4067 : move16();
2050 4067 : move16();
2051 :
2052 : Word32 sum_x_fx, sum_ix_fx, slope_fx, offset_fx;
2053 4067 : Word16 dot_exp = 0, sum_ix_q = 0;
2054 4067 : move16();
2055 4067 : move16();
2056 :
2057 : Word32 num;
2058 4067 : assert( N > 0 && LE_16( N, 10 ) );
2059 :
2060 4067 : sum_x_fx = 0;
2061 4067 : move32();
2062 : Word16 idx;
2063 20335 : FOR( idx = 0; idx < N; idx++ )
2064 : {
2065 16268 : sum_x_fx = L_add( sum_x_fx, x_fx[idx] ); /*Q15*/
2066 : }
2067 4067 : sum_ix_fx = dotp_fx( x_fx, n_i_fx, N, &dot_exp ); /*sum_ix_q*/
2068 4067 : sum_ix_q = sub( 30, sub( dot_exp, ( 11 + 15 ) ) );
2069 :
2070 4067 : sum_ix_fx = L_shr( sum_ix_fx, sub( sum_ix_q, 15 ) ); /*Q15*/
2071 4067 : num = L_sub( imult3216( sum_ix_fx, N ), imult3216( sum_x_fx, sum_i_fx[N] ) ); /*Q15*/
2072 4067 : slope_fx = Mpy_32_32( num, res_table[N] ); /*Q15*/
2073 4067 : offset_fx = Mpy_32_16_1( L_sub( sum_x_fx, imult3216( slope_fx, sum_i_fx[N] ) ), one_by_n_fx[N] ); /*Q15*/
2074 :
2075 4067 : IF( upd )
2076 : {
2077 20335 : FOR( i = 0; i < N; i++ )
2078 : {
2079 16268 : IF( GT_32( imult3216( slope_fx, i ), MAX_WORD16 ) )
2080 : {
2081 0 : x_fx[i] = MAX_WORD16; /*Q15*/
2082 0 : move16();
2083 : }
2084 : ELSE
2085 : {
2086 16268 : x_fx[i] = extract_l( L_add_sat( imult3216( slope_fx, i ), offset_fx ) ); /*Q15*/
2087 16268 : move16();
2088 : }
2089 : }
2090 : }
2091 :
2092 4067 : IF( a_fx != NULL )
2093 : {
2094 4067 : *a_fx = extract_l( slope_fx ); /*Q15*/
2095 4067 : move16();
2096 : }
2097 :
2098 4067 : IF( b_fx != NULL )
2099 : {
2100 4067 : *b_fx = extract_l( offset_fx ); /*Q15*/
2101 4067 : move16();
2102 : }
2103 :
2104 4067 : return;
2105 : }
2106 :
2107 :
2108 : /* helper function for panning_wrap_angles */
2109 1872 : static float wrap_azi(
2110 : const float azi_deg )
2111 : {
2112 1872 : float azi = azi_deg;
2113 :
2114 : /* Wrap azimuth value */
2115 1872 : while ( azi > 180 )
2116 : {
2117 0 : azi -= 360.0f;
2118 : }
2119 :
2120 1927 : while ( azi <= -180 )
2121 : {
2122 55 : azi += 360;
2123 : }
2124 :
2125 1872 : return azi;
2126 : }
2127 : /* helper function for panning_wrap_angles */
2128 5967632 : static Word32 wrap_azi_fx(
2129 : const Word32 azi_deg /* Q22 */ )
2130 : {
2131 5967632 : Word32 azi = azi_deg; /*Q22*/
2132 5967632 : move32();
2133 :
2134 : /* Wrap azimuth value */
2135 7420547 : WHILE( GT_32( azi, ANGLE_180_DEG_Q22 ) )
2136 : {
2137 1452915 : azi = L_sub( azi, ANGLE_360_DEG_Q22 ); /*Q22*/
2138 : }
2139 :
2140 6007331 : WHILE( LE_32( azi, -ANGLE_180_DEG_Q22 ) )
2141 : {
2142 39699 : azi = L_add( azi, ANGLE_360_DEG_Q22 ); /*Q22*/
2143 : }
2144 :
2145 5967632 : return azi; /*Q22*/
2146 : }
2147 : /*-------------------------------------------------------------------*
2148 : * panning_wrap_angles()
2149 : *
2150 : * Wrap angles for amplitude panning to the range:
2151 : * azimuth = (-180, 180]
2152 : * elevation = [-90, 90]
2153 : * Considers direction changes from large elevation values
2154 : *-------------------------------------------------------------------*/
2155 1872 : void panning_wrap_angles(
2156 : const float azi_deg, /* i : azimuth in degrees for panning direction (positive left) */
2157 : const float ele_deg, /* i : elevation in degrees for panning direction (positive up) */
2158 : float *azi_wrapped, /* o : wrapped azimuth component */
2159 : float *ele_wrapped /* o : wrapped elevation component */
2160 : )
2161 : {
2162 : float azi, ele;
2163 :
2164 1872 : azi = azi_deg;
2165 1872 : ele = ele_deg;
2166 :
2167 1872 : if ( fabsf( ele ) < 90 )
2168 : {
2169 1872 : *ele_wrapped = ele;
2170 1872 : *azi_wrapped = wrap_azi( azi );
2171 1872 : return;
2172 : }
2173 : else
2174 : {
2175 : /* Special case when elevation is a multiple of 90; azimuth is irrelevant */
2176 0 : if ( ( fmodf( ele, 90 ) == 0 ) && ( fmodf( ele, 180 ) != 0 ) )
2177 : {
2178 0 : *azi_wrapped = 0;
2179 0 : while ( ele > 90 )
2180 : {
2181 0 : ele -= 360;
2182 : }
2183 0 : while ( ele < -90 )
2184 : {
2185 0 : ele += 360;
2186 : }
2187 0 : *ele_wrapped = ele;
2188 : }
2189 : else
2190 : {
2191 : /* Wrap elevation and adjust azimuth accordingly */
2192 0 : while ( fabsf( ele ) > 90 )
2193 : {
2194 : /* Flip to other hemisphere */
2195 0 : azi += 180;
2196 :
2197 : /* Compensate elevation accordingly */
2198 0 : if ( ele > 90 )
2199 : {
2200 0 : ele = 180 - ele;
2201 : }
2202 0 : else if ( ele < -90 )
2203 : {
2204 0 : ele = -180 - ele;
2205 : }
2206 : }
2207 0 : *azi_wrapped = wrap_azi( azi );
2208 0 : *ele_wrapped = ele;
2209 : }
2210 :
2211 0 : return;
2212 : }
2213 : }
2214 : /*-------------------------------------------------------------------*
2215 : * panning_wrap_angles_fx()
2216 : *
2217 : * Wrap angles for amplitude panning to the range:
2218 : * azimuth = (-180, 180]
2219 : * elevation = [-90, 90]
2220 : * Considers direction changes from large elevation values
2221 : *-------------------------------------------------------------------*/
2222 5971102 : void panning_wrap_angles_fx(
2223 : const Word32 azi_deg, /* i : azimuth in degrees for panning direction (positive left) Q22 */
2224 : const Word32 ele_deg, /* i : elevation in degrees for panning direction (positive up) Q22 */
2225 : Word32 *azi_wrapped, /* o : wrapped azimuth component Q22 */
2226 : Word32 *ele_wrapped /* o : wrapped elevation component Q22 */
2227 : )
2228 : {
2229 : Word32 azi, ele;
2230 :
2231 5971102 : azi = azi_deg; /*Q22*/
2232 5971102 : move32();
2233 5971102 : ele = ele_deg; /*Q22*/
2234 5971102 : move32();
2235 :
2236 5971102 : IF( LT_32( L_abs( ele ), ANGLE_90_DEG_Q22 ) )
2237 : {
2238 5967632 : *ele_wrapped = ele; /*Q22*/
2239 5967632 : move32();
2240 5967632 : *azi_wrapped = wrap_azi_fx( azi ); /*Q22*/
2241 5967632 : move32();
2242 5967632 : return;
2243 : }
2244 : ELSE
2245 : {
2246 : /* Special case when elevation is a multiple of 90; azimuth is irrelevant */
2247 3470 : IF( ( ( ele % ANGLE_90_DEG_Q22 ) == 0 ) && ( ( ele % ANGLE_180_DEG_Q22 ) != 0 ) )
2248 : {
2249 3470 : *azi_wrapped = 0;
2250 3470 : move32();
2251 3470 : WHILE( GT_32( ele, ANGLE_90_DEG_Q22 ) )
2252 : {
2253 0 : ele = L_sub( ele, ANGLE_360_DEG_Q22 );
2254 : }
2255 3470 : WHILE( LT_32( ele, -ANGLE_90_DEG_Q22 ) )
2256 : {
2257 0 : ele = L_add( ele, ANGLE_360_DEG_Q22 );
2258 : }
2259 3470 : *ele_wrapped = ele;
2260 3470 : move32();
2261 : }
2262 : ELSE
2263 : {
2264 : /* Wrap elevation and adjust azimuth accordingly */
2265 0 : WHILE( GT_32( L_abs( ele ), ANGLE_90_DEG_Q22 ) )
2266 : {
2267 : /* Flip to other hemisphere */
2268 0 : azi = L_add( azi, ANGLE_180_DEG_Q22 );
2269 :
2270 : /* Compensate elevation accordingly */
2271 0 : IF( GT_32( ele, ANGLE_90_DEG_Q22 ) )
2272 : {
2273 0 : ele = L_sub( ANGLE_180_DEG_Q22, ele );
2274 : }
2275 0 : ELSE IF( LT_32( ele, -ANGLE_90_DEG_Q22 ) )
2276 : {
2277 0 : ele = L_sub( -ANGLE_180_DEG_Q22, ele );
2278 : }
2279 : }
2280 0 : *azi_wrapped = wrap_azi_fx( azi ); /*Q22*/
2281 0 : move32();
2282 0 : *ele_wrapped = ele; /*Q22*/
2283 0 : move32();
2284 : }
2285 :
2286 3470 : return;
2287 : }
2288 : }
2289 :
2290 : /*-------------------------------------------------------------------------*
2291 : * v_sort_ind_fixed()
2292 : *
2293 : * Sort a float array
2294 : * (modified version of v_sort() to return an index array)
2295 : *-------------------------------------------------------------------------*/
2296 :
2297 16639 : void v_sort_ind_fixed(
2298 : Word32 *x, /* i/o: Vector to be sorted Qx*/
2299 : Word16 *idx, /* o : Original index positions Q0*/
2300 : const Word16 len /* i : vector length Q0*/
2301 : )
2302 : {
2303 : Word16 i, j;
2304 : Word32 tempr;
2305 : Word16 tempi;
2306 :
2307 76505 : FOR( i = 0; i < len; i++ )
2308 : {
2309 59866 : idx[i] = i;
2310 59866 : move16();
2311 : }
2312 :
2313 59866 : FOR( i = len - 2; i >= 0; i-- )
2314 : {
2315 43227 : tempr = x[i]; /*Qx*/
2316 43227 : move32();
2317 43227 : tempi = idx[i]; /*Qx*/
2318 43227 : move16();
2319 43227 : test();
2320 100197 : FOR( j = ( i + 1 ); LT_16( j, len ) && GT_32( tempr, x[j] ); j++ )
2321 : {
2322 56970 : test();
2323 56970 : x[j - 1] = x[j]; /*Qx*/
2324 56970 : move32();
2325 56970 : idx[j - 1] = idx[j]; /*Qx*/
2326 56970 : move16();
2327 : }
2328 43227 : x[j - 1] = tempr; /*Qx*/
2329 43227 : move32();
2330 43227 : idx[j - 1] = tempi; /*Qx*/
2331 43227 : move16();
2332 : }
2333 :
2334 16639 : return;
2335 : }
2336 :
2337 : /*-------------------------------------------------------------------*
2338 : * is_IVAS_bitrate()
2339 : *
2340 : * check if the bitrate is IVAS supported active bitrate
2341 : *-------------------------------------------------------------------*/
2342 :
2343 : /*! r: flag indicating a valid bitrate */
2344 0 : Word16 is_IVAS_bitrate_fx(
2345 : const Word32 ivas_total_brate /* i : IVAS total bitrate Q0*/
2346 : )
2347 : {
2348 : Word16 j;
2349 :
2350 0 : j = SIZE_IVAS_BRATE_TBL - IVAS_NUM_ACTIVE_BRATES; /* skip NO_DATA and SID bitrates */
2351 0 : move16();
2352 :
2353 0 : test();
2354 0 : WHILE( LE_16( j, SIZE_IVAS_BRATE_TBL ) && NE_32( ivas_total_brate, ivas_brate_tbl[j] ) )
2355 : {
2356 0 : test();
2357 0 : j = add( j, 1 );
2358 : }
2359 :
2360 0 : IF( GE_16( j, SIZE_IVAS_BRATE_TBL ) )
2361 : {
2362 0 : return 0;
2363 : }
2364 :
2365 0 : return 1;
2366 : }
2367 :
2368 : /*-------------------------------------------------------------------*
2369 : * is_DTXrate()
2370 : *
2371 : * identify DTX frame bitrates
2372 : *-------------------------------------------------------------------*/
2373 :
2374 4286172 : Word16 is_DTXrate(
2375 : const Word32 ivas_total_brate /* i : IVAS total bitrate Q0*/
2376 : )
2377 : {
2378 4286172 : Word16 dtx_rate_flag = 0;
2379 4286172 : move16();
2380 :
2381 4286172 : test();
2382 4286172 : IF( is_SIDrate( ivas_total_brate ) || ( EQ_32( ivas_total_brate, FRAME_NO_DATA ) ) )
2383 : {
2384 169620 : dtx_rate_flag = 1;
2385 169620 : move16();
2386 : }
2387 :
2388 4286172 : return dtx_rate_flag; /*Q0*/
2389 : }
2390 :
2391 : /*-------------------------------------------------------------------*
2392 : * is_SIDrate()
2393 : *
2394 : * identify SID frame bitrates
2395 : *-------------------------------------------------------------------*/
2396 :
2397 4698399 : Word16 is_SIDrate(
2398 : const Word32 ivas_total_brate /* i : IVAS total bitrate Q0*/
2399 : )
2400 : {
2401 4698399 : Word16 sid_rate_flag = 0;
2402 4698399 : move16();
2403 :
2404 4698399 : test();
2405 4698399 : test();
2406 9396798 : if ( EQ_32( ivas_total_brate, SID_1k75 ) ||
2407 9396798 : EQ_32( ivas_total_brate, SID_2k40 ) ||
2408 4698399 : EQ_32( ivas_total_brate, IVAS_SID_5k2 ) )
2409 : {
2410 29148 : sid_rate_flag = 1;
2411 29148 : move16();
2412 : }
2413 :
2414 4698399 : return sid_rate_flag; /*Q0*/
2415 : }
2416 :
2417 :
2418 : /*-------------------------------------------------------------------*
2419 : * rand_triangular_signed()
2420 : *
2421 : * generate a random value with a triangular pdf in [-0.5, 0.5]
2422 : *-------------------------------------------------------------------*/
2423 :
2424 25706776 : Word16 rand_triangular_signed_fx(
2425 : Word16 *seed, /*Q0*/
2426 : Word16 *exp_fac )
2427 : {
2428 : Word16 rand_val;
2429 25706776 : rand_val = Random( seed ); // q15
2430 : Word16 tmp1, tmp2;
2431 25706776 : Word16 exp1, exp = 1;
2432 25706776 : move16();
2433 25706776 : IF( rand_val <= 0 )
2434 : {
2435 : /* rand_val in [-1, 0] */
2436 : /*0.5f * (sqrtf(rand_val + 1.0f) - 1)*/
2437 12849459 : tmp1 = Sqrt16( add( shr( rand_val, 1 ), ONE_IN_Q14 ), &exp ); /*Q15 - exp*/
2438 12849459 : exp1 = BASOP_Util_Add_MantExp( tmp1, exp, negate( ONE_IN_Q14 ), 1, &tmp2 ); /*Q15 - exp1*/
2439 12849459 : tmp2 = shr( tmp2, 1 ); /*Q15 - exp1*/
2440 12849459 : *exp_fac = exp1;
2441 12849459 : move16();
2442 12849459 : return tmp2;
2443 : }
2444 : ELSE
2445 : {
2446 : /* rand_val in (0, 1) */
2447 : /*0.5f * ( 1 - sqrtf(1.0f - rand_val))*/
2448 12857317 : Word16 one_minus_rand = sub( MAX16B, rand_val ); /*Q15*/
2449 12857317 : exp = 0;
2450 12857317 : move16();
2451 12857317 : tmp1 = Sqrt16( one_minus_rand, &exp ); // q15 - exp
2452 12857317 : exp1 = BASOP_Util_Add_MantExp( ONE_IN_Q14, 1, negate( tmp1 ), exp, &tmp2 ); /*Q15 - exp1*/
2453 12857317 : tmp2 = shr( tmp2, 1 ); /*Q15 - exp1*/
2454 12857317 : *exp_fac = exp1;
2455 12857317 : move16();
2456 :
2457 12857317 : return tmp2; /*Q15 - exp_fac*/
2458 : }
2459 : }
2460 :
2461 : /*-------------------------------------------------------------------*
2462 : * ceil_log_2()
2463 : *
2464 : * calculates ceil(log2(val))
2465 : *-------------------------------------------------------------------*/
2466 54775 : Word16 ceil_log_2(
2467 : UWord64 val /*Q0*/ )
2468 : {
2469 :
2470 54775 : IF( val <= 0 )
2471 : {
2472 0 : assert( 0 );
2473 : }
2474 54775 : ELSE IF( LE_64( val, 1 ) )
2475 : {
2476 285 : return 0;
2477 : }
2478 54490 : ELSE IF( LE_64( val, 2 ) )
2479 : {
2480 2973 : return 1;
2481 : }
2482 51517 : ELSE IF( LE_64( val, 4 ) )
2483 : {
2484 4214 : return 2;
2485 : }
2486 47303 : ELSE IF( LE_64( val, 8 ) )
2487 : {
2488 2743 : return 3;
2489 : }
2490 44560 : ELSE IF( LE_64( val, 16 ) )
2491 : {
2492 1319 : return 4;
2493 : }
2494 43241 : ELSE IF( LE_64( val, 32 ) )
2495 : {
2496 1669 : return 5;
2497 : }
2498 41572 : ELSE IF( LE_64( val, 64 ) )
2499 : {
2500 1718 : return 6;
2501 : }
2502 39854 : ELSE IF( LE_64( val, 128 ) )
2503 : {
2504 4063 : return 7;
2505 : }
2506 35791 : ELSE IF( LE_64( val, 256 ) )
2507 : {
2508 6036 : return 8;
2509 : }
2510 29755 : ELSE IF( LE_64( val, 512 ) )
2511 : {
2512 5363 : return 9;
2513 : }
2514 24392 : ELSE IF( LE_64( val, 1024 ) )
2515 : {
2516 7298 : return 10;
2517 : }
2518 17094 : ELSE IF( LE_64( val, 2048 ) )
2519 : {
2520 2102 : return 11;
2521 : }
2522 14992 : ELSE IF( LE_64( val, 4096 ) )
2523 : {
2524 1669 : return 12;
2525 : }
2526 13323 : ELSE IF( LE_64( val, 8192 ) )
2527 : {
2528 1254 : return 13;
2529 : }
2530 12069 : ELSE IF( LE_64( val, 16384 ) )
2531 : {
2532 1705 : return 14;
2533 : }
2534 10364 : ELSE IF( LE_64( val, 32768 ) )
2535 : {
2536 3506 : return 15;
2537 : }
2538 6858 : ELSE IF( LE_64( val, 65536 ) )
2539 : {
2540 472 : return 16;
2541 : }
2542 6386 : ELSE IF( LE_64( val, 131072 ) )
2543 : {
2544 192 : return 17;
2545 : }
2546 6194 : ELSE IF( LE_64( val, 262144 ) )
2547 : {
2548 259 : return 18;
2549 : }
2550 5935 : ELSE IF( LE_64( val, 524288 ) )
2551 : {
2552 248 : return 19;
2553 : }
2554 5687 : ELSE IF( LE_64( val, 1048576 ) )
2555 : {
2556 92 : return 20;
2557 : }
2558 5595 : ELSE IF( LE_64( val, 2097152 ) )
2559 : {
2560 115 : return 21;
2561 : }
2562 5480 : ELSE IF( LE_64( val, 4194304 ) )
2563 : {
2564 349 : return 22;
2565 : }
2566 5131 : ELSE IF( LE_64( val, 8388608 ) )
2567 : {
2568 456 : return 23;
2569 : }
2570 4675 : ELSE IF( LE_64( val, 16777216 ) )
2571 : {
2572 810 : return 24;
2573 : }
2574 3865 : ELSE IF( LE_64( val, 33554432 ) )
2575 : {
2576 493 : return 25;
2577 : }
2578 3372 : ELSE IF( LE_64( val, 67108864 ) )
2579 : {
2580 337 : return 26;
2581 : }
2582 3035 : ELSE IF( LE_64( val, 134217728 ) )
2583 : {
2584 153 : return 27;
2585 : }
2586 2882 : ELSE IF( LE_64( val, 268435456 ) )
2587 : {
2588 139 : return 28;
2589 : }
2590 2743 : ELSE IF( LE_64( val, 536870912 ) )
2591 : {
2592 57 : return 29;
2593 : }
2594 2686 : ELSE IF( LE_64( val, 1073741824 ) )
2595 : {
2596 60 : return 30;
2597 : }
2598 2626 : ELSE IF( LE_64( val, 2147483648 ) )
2599 : {
2600 93 : return 31;
2601 : }
2602 2533 : ELSE IF( LE_64( val, 4294967296 ) )
2603 : {
2604 147 : return 32;
2605 : }
2606 2386 : ELSE IF( LE_64( val, 8589934592 ) )
2607 : {
2608 61 : return 33;
2609 : }
2610 2325 : ELSE IF( LE_64( val, 17179869184 ) )
2611 : {
2612 208 : return 34;
2613 : }
2614 2117 : ELSE IF( LE_64( val, 34359738368 ) )
2615 : {
2616 226 : return 35;
2617 : }
2618 1891 : ELSE IF( LE_64( val, 68719476736 ) )
2619 : {
2620 176 : return 36;
2621 : }
2622 1715 : ELSE IF( LE_64( val, 137438953472 ) )
2623 : {
2624 172 : return 37;
2625 : }
2626 1543 : ELSE IF( LE_64( val, 274877906944 ) )
2627 : {
2628 204 : return 38;
2629 : }
2630 1339 : ELSE IF( LE_64( val, 549755813888 ) )
2631 : {
2632 165 : return 39;
2633 : }
2634 1174 : ELSE IF( LE_64( val, 1099511627776 ) )
2635 : {
2636 158 : return 40;
2637 : }
2638 1016 : ELSE IF( LE_64( val, 2199023255552 ) )
2639 : {
2640 142 : return 41;
2641 : }
2642 874 : ELSE IF( LE_64( val, 4398046511104 ) )
2643 : {
2644 211 : return 42;
2645 : }
2646 663 : ELSE IF( LE_64( val, 8796093022208 ) )
2647 : {
2648 122 : return 43;
2649 : }
2650 541 : ELSE IF( LE_64( val, 17592186044416 ) )
2651 : {
2652 90 : return 44;
2653 : }
2654 451 : ELSE IF( LE_64( val, 35184372088832 ) )
2655 : {
2656 166 : return 45;
2657 : }
2658 285 : ELSE IF( LE_64( val, 70368744177664 ) )
2659 : {
2660 134 : return 46;
2661 : }
2662 151 : ELSE IF( LE_64( val, 140737488355328 ) )
2663 : {
2664 92 : return 47;
2665 : }
2666 59 : ELSE IF( LE_64( val, 281474976710656 ) )
2667 : {
2668 59 : return 48;
2669 : }
2670 0 : ELSE IF( LE_64( val, 562949953421312 ) )
2671 : {
2672 0 : return 49;
2673 : }
2674 0 : ELSE IF( LE_64( val, 1125899906842624 ) )
2675 : {
2676 0 : return 50;
2677 : }
2678 0 : ELSE IF( LE_64( val, 2251799813685248 ) )
2679 : {
2680 0 : return 51;
2681 : }
2682 0 : ELSE IF( LE_64( val, 4503599627370496 ) )
2683 : {
2684 0 : return 52;
2685 : }
2686 0 : ELSE IF( LE_64( val, 9007199254740992 ) )
2687 : {
2688 0 : return 53;
2689 : }
2690 0 : ELSE IF( LE_64( val, 18014398509481984 ) )
2691 : {
2692 0 : return 54;
2693 : }
2694 0 : ELSE IF( LE_64( val, 36028797018963968 ) )
2695 : {
2696 0 : return 55;
2697 : }
2698 0 : ELSE IF( LE_64( val, 72057594037927936 ) )
2699 : {
2700 0 : return 56;
2701 : }
2702 0 : ELSE IF( LE_64( val, 144115188075855872 ) )
2703 : {
2704 0 : return 57;
2705 : }
2706 0 : ELSE IF( LE_64( val, 288230376151711744 ) )
2707 : {
2708 0 : return 58;
2709 : }
2710 0 : ELSE IF( LE_64( val, 576460752303423488 ) )
2711 : {
2712 0 : return 59;
2713 : }
2714 0 : ELSE IF( LE_64( val, 1152921504606846976 ) )
2715 : {
2716 0 : return 60;
2717 : }
2718 0 : ELSE IF( LE_64( val, 2305843009213693952 ) )
2719 : {
2720 0 : return 61;
2721 : }
2722 0 : ELSE IF( LE_64( val, 4611686018427387904 ) )
2723 : {
2724 0 : return 62;
2725 : }
2726 0 : ELSE IF( LE_64( val, 9223372036854775807 ) )
2727 : {
2728 0 : return 63;
2729 : }
2730 0 : return 64;
2731 : }
2732 :
2733 :
2734 : /*-------------------------------------------------------------------*
2735 : * var_32_fx()
2736 : *
2737 : * calculates variance of 32-bit array
2738 : * Currently using direct division.
2739 : * Needs update once required basops are implemented.
2740 : *-------------------------------------------------------------------*/
2741 :
2742 142922 : Word64 var_32_fx(
2743 : const Word32 *x, /* i : input vector q*/
2744 : const Word16 len, /* i : length of inputvector Q0*/
2745 : Word16 q /* q : q-factor for the array */
2746 : )
2747 : {
2748 : Word16 i;
2749 : Word64 mean, var;
2750 :
2751 142922 : mean = 0;
2752 142922 : move64();
2753 142922 : var = 0;
2754 142922 : move64();
2755 :
2756 714610 : FOR( i = 0; i < len; i++ )
2757 : {
2758 571688 : mean = W_add( mean, x[i] ); /*q*/
2759 : }
2760 :
2761 142922 : mean = mean / len; /* NOTE: No BASOP for 64 bit division q*/
2762 :
2763 714610 : FOR( i = 0; i < len; i++ )
2764 : {
2765 571688 : var = W_add( var, Mpy_32_32( L_sub( x[i], W_extract_l( mean ) ), L_sub( x[i], W_extract_l( mean ) ) ) ); /*q + q - 31*/
2766 : }
2767 :
2768 142922 : var = W_shl( var, sub( 31, q ) ); /*q*/
2769 :
2770 142922 : var = var / len; /* NOTE: No BASOP for 64 bit division q*/
2771 :
2772 142922 : return var; /*q*/
2773 : }
|
