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 : #ifdef DEBUGGING
37 : #include "debug.h"
38 : #endif
39 : #include <math.h>
40 : #include "prot.h"
41 : #include "ivas_prot.h"
42 : #include "wmc_auto.h"
43 : #include "ivas_rom_com.h"
44 : #include "basop_settings.h"
45 :
46 : /*---------------------------------------------------------------
47 : * sumAbs()
48 : *
49 : * sum of absolute values
50 : * ---------------------------------------------------------------*/
51 :
52 : /*! r: sum abs of all vector elements */
53 15164 : float sumAbs(
54 : const float *vec, /* i : input vector */
55 : const int16_t lvec /* i : length of input vector */
56 : )
57 : {
58 : int16_t i;
59 : float tmp;
60 :
61 15164 : tmp = 0.0f;
62 1228284 : for ( i = 0; i < lvec; i++ )
63 : {
64 1213120 : tmp += fabsf( vec[i] );
65 : }
66 :
67 15164 : return tmp;
68 : }
69 :
70 : /*---------------------------------------------------------------------*
71 : * mvc2c()
72 : *
73 : * Transfer the contents of vector x[] to vector y[] in char format
74 : *---------------------------------------------------------------------*/
75 :
76 11452818 : void mvc2c(
77 : const uint8_t x[], /* i : input vector */
78 : uint8_t y[], /* o : output vector */
79 : const int16_t n /* i : vector size */
80 : )
81 : {
82 : int16_t i;
83 :
84 11452818 : if ( n <= 0 )
85 : {
86 : /* no need to transfer vectors with size 0 */
87 0 : return;
88 : }
89 :
90 11452818 : if ( y < x )
91 : {
92 161892067 : for ( i = 0; i < n; i++ )
93 : {
94 153326501 : y[i] = x[i];
95 : }
96 : }
97 : else
98 : {
99 48156527 : for ( i = n - 1; i >= 0; i-- )
100 : {
101 45269275 : y[i] = x[i];
102 : }
103 : }
104 :
105 11452818 : return;
106 : }
107 :
108 :
109 : /*-------------------------------------------------------------------*
110 : * ivas_syn_output()
111 : *
112 : * Output ivas synthesis signal with compensation for saturation
113 : * returns number of clipped samples
114 : *-------------------------------------------------------------------*/
115 :
116 : /*! r: number of clipped samples */
117 2881812 : uint32_t ivas_syn_output(
118 : float *synth[], /* i/o: float synthesis signal */
119 : const int16_t output_frame, /* i : output frame length (one channel) */
120 : const int16_t n_channels, /* i : number of output channels */
121 : int16_t *synth_out /* o : integer 16 bits synthesis signal */
122 : )
123 : {
124 : int16_t i, n;
125 : int16_t synth_loc[MAX_JBM_L_FRAME48k];
126 2881812 : uint32_t noClipping = 0;
127 :
128 : /*-----------------------------------------------------------------*
129 : * float to integer conversion with saturation control
130 : *-----------------------------------------------------------------*/
131 :
132 13020626 : for ( n = 0; n < n_channels; n++ )
133 : {
134 10138814 : noClipping += mvr2s( synth[n], synth_loc, output_frame );
135 :
136 : #ifdef DEBUG_MODE_LFE
137 : if ( n == LFE_CHANNEL )
138 : {
139 : dbgwrite( synth_loc, sizeof( int16_t ), output_frame, 1, "./lfe_out.raw" );
140 : }
141 : #endif
142 3784948634 : for ( i = 0; i < output_frame; i++ )
143 : {
144 3774809820 : synth_out[i * n_channels + n] = synth_loc[i];
145 : }
146 : }
147 :
148 2881812 : return noClipping;
149 : }
150 :
151 :
152 : /*-------------------------------------------------------------------*
153 : * ivas_syn_output_f()
154 : *
155 : * Output ivas synthesis signal with compensation for saturation
156 : * returns number of clipped samples
157 : *-------------------------------------------------------------------*/
158 :
159 : /*! r: number of clipped samples */
160 76596 : void ivas_syn_output_f(
161 : float *synth[], /* i/o: float synthesis signal */
162 : const int16_t output_frame, /* i : output frame length (one channel) */
163 : const int16_t n_channels, /* i : number of output channels */
164 : float *synth_out /* o : integer 16 bits synthesis signal */
165 : )
166 : {
167 : int16_t i, n;
168 :
169 : /*-----------------------------------------------------------------*
170 : * float to integer conversion with saturation control
171 : *-----------------------------------------------------------------*/
172 :
173 275790 : for ( n = 0; n < n_channels; n++ )
174 : {
175 169510554 : for ( i = 0; i < output_frame; i++ )
176 : {
177 169311360 : synth_out[i * n_channels + n] = synth[n][i];
178 : }
179 : }
180 :
181 76596 : return;
182 : }
183 :
184 :
185 : /*-------------------------------------------------------------------*
186 : * mvr2r_inc()
187 : *
188 : *
189 : *-------------------------------------------------------------------*/
190 :
191 249719817 : void mvr2r_inc(
192 : const float x[], /* i : input vector */
193 : const int16_t x_inc, /* i : increment for vector x[] */
194 : float y[], /* o : output vector */
195 : const int16_t y_inc, /* i : increment for vector y[] */
196 : const int16_t n /* i : vector size */
197 : )
198 : {
199 : int16_t i;
200 : int16_t ix;
201 : int16_t iy;
202 :
203 249719817 : if ( n <= 0 )
204 : {
205 : /* cannot transfer vectors with size 0 */
206 0 : return;
207 : }
208 :
209 249719817 : if ( y < x )
210 : {
211 227573694 : ix = 0;
212 227573694 : iy = 0;
213 3532860639 : for ( i = 0; i < n; i++ )
214 : {
215 3305286945 : y[iy] = x[ix];
216 3305286945 : ix += x_inc;
217 3305286945 : iy += y_inc;
218 : }
219 : }
220 : else
221 : {
222 22146123 : ix = ( n - 1 ) * x_inc;
223 22146123 : iy = ( n - 1 ) * y_inc;
224 165517347 : for ( i = n - 1; i >= 0; i-- )
225 : {
226 143371224 : y[iy] = x[ix];
227 143371224 : ix -= x_inc;
228 143371224 : iy -= y_inc;
229 : }
230 : }
231 :
232 249719817 : return;
233 : }
234 :
235 : /*-------------------------------------------------------------------*
236 : * v_add_inc()
237 : *
238 : * Addition of two vectors sample by sample with explicit increments
239 : *-------------------------------------------------------------------*/
240 :
241 24814896 : void v_add_inc(
242 : const float x1[], /* i : Input vector 1 */
243 : const int16_t x_inc, /* i : Increment for input vector 1 */
244 : const float x2[], /* i : Input vector 2 */
245 : const int16_t x2_inc, /* i : Increment for input vector 2 */
246 : float y[], /* o : Output vector that contains vector 1 + vector 2 */
247 : const int16_t y_inc, /* i : increment for vector y[] */
248 : const int16_t N /* i : Vector length */
249 : )
250 : {
251 : int16_t i;
252 24814896 : int16_t ix1 = 0;
253 24814896 : int16_t ix2 = 0;
254 24814896 : int16_t iy = 0;
255 :
256 592564473 : for ( i = 0; i < N; i++ )
257 : {
258 567749577 : y[iy] = x1[ix1] + x2[ix2];
259 567749577 : ix1 += x_inc;
260 567749577 : ix2 += x2_inc;
261 567749577 : iy += y_inc;
262 : }
263 :
264 24814896 : return;
265 : }
266 :
267 :
268 : /*-------------------------------------------------------------------*
269 : * v_mult_inc()
270 : *
271 : * Multiplication of two vectors with explicit increments
272 : *-------------------------------------------------------------------*/
273 :
274 13148676 : void v_mult_inc(
275 : const float x1[], /* i : Input vector 1 */
276 : const int16_t x1_inc, /* i : Increment for input vector 1 */
277 : const float x2[], /* i : Input vector 2 */
278 : const int16_t x2_inc, /* i : Increment for input vector 1 */
279 : float y[], /* o : Output vector that contains vector 1 .* vector 2 */
280 : const int16_t y_inc, /* i : increment for vector y[i] */
281 : const int16_t N /* i : Vector length */
282 : )
283 : {
284 : int16_t i;
285 13148676 : int16_t ix1 = 0;
286 13148676 : int16_t ix2 = 0;
287 13148676 : int16_t iy = 0;
288 :
289 227114502 : for ( i = 0; i < N; i++ )
290 : {
291 213965826 : y[iy] = x1[ix1] * x2[ix2];
292 213965826 : ix1 += x1_inc;
293 213965826 : ix2 += x2_inc;
294 213965826 : iy += y_inc;
295 : }
296 :
297 13148676 : return;
298 : }
299 :
300 : /*-------------------------------------------------------------------*
301 : * v_addc()
302 : *
303 : * Addition of constant to vector
304 : *-------------------------------------------------------------------*/
305 :
306 5827887 : void v_addc(
307 : const float x[], /* i : Input vector */
308 : const float c, /* i : Constant */
309 : float y[], /* o : Output vector that contains c*x */
310 : const int16_t N /* i : Vector length */
311 : )
312 : {
313 : int16_t i;
314 :
315 167530362 : for ( i = 0; i < N; i++ )
316 : {
317 161702475 : y[i] = c + x[i];
318 : }
319 :
320 5827887 : return;
321 : }
322 :
323 : /*-------------------------------------------------------------------*
324 : * v_min()
325 : *
326 : * minimum of two vectors
327 : *-------------------------------------------------------------------*/
328 :
329 10920 : void v_min(
330 : const float x1[], /* i : Input vector 1 */
331 : const float x2[], /* i : Input vector 2 */
332 : float y[], /* o : Output vector that contains vector 1 .* vector 2 */
333 : const int16_t N /* i : Vector length */
334 : )
335 : {
336 : int16_t i;
337 :
338 156720 : for ( i = 0; i < N; i++ )
339 : {
340 145800 : y[i] = ( x1[i] < x2[i] ) ? x1[i] : x2[i];
341 : }
342 :
343 10920 : return;
344 : }
345 :
346 :
347 : /*-------------------------------------------------------------------*
348 : * v_sqrt()
349 : *
350 : * square root of vector
351 : *-------------------------------------------------------------------*/
352 :
353 1216440 : void v_sqrt(
354 : const float x[], /* i : Input vector */
355 : float y[], /* o : Output vector that contains sqrt(x) */
356 : const int16_t N /* i : Vector length */
357 : )
358 : {
359 : int16_t i;
360 :
361 3649320 : for ( i = 0; i < N; i++ )
362 : {
363 2432880 : y[i] = sqrtf( x[i] );
364 : }
365 :
366 1216440 : return;
367 : }
368 :
369 :
370 : /*-------------------------------------------------------------------*
371 : * v_sub_s()
372 : *
373 : * Subtraction of two vectors
374 : *-------------------------------------------------------------------*/
375 :
376 484302 : void v_sub_s(
377 : const int16_t x1[], /* i : Input vector 1 */
378 : const int16_t x2[], /* i : Input vector 2 */
379 : int16_t y[], /* o : Output vector that contains vector 1 - vector 2 */
380 : const int16_t N /* i : Vector length */
381 : )
382 : {
383 : int16_t i;
384 :
385 1788483 : for ( i = 0; i < N; i++ )
386 : {
387 1304181 : y[i] = x1[i] - x2[i];
388 : }
389 :
390 484302 : return;
391 : }
392 :
393 :
394 : /*---------------------------------------------------------------------*
395 : * dot_product_cholesky()
396 : *
397 : * Calculates dot product of type x'*A*A'*x, where x is column vector of size m,
398 : * and A is a Cholesky decomposition of some Hermitian matrix S whose size is m*m.
399 : * Therefore, S=A*A' where A is upper triangular matrix of size (m*m+m)/2 (zeros ommitted, column-wise)
400 : *---------------------------------------------------------------------*/
401 :
402 : /*! r: the dot product x'*A*A'*x */
403 20713212 : float dot_product_cholesky(
404 : const float *x, /* i : vector x */
405 : const float *A, /* i : Cholesky matrix A */
406 : const int16_t N /* i : vector & matrix size */
407 : )
408 : {
409 : int16_t i, j;
410 : float suma, tmp_sum;
411 : const float *pt_x, *pt_A;
412 :
413 20713212 : pt_A = A;
414 20713212 : suma = 0;
415 :
416 269271756 : for ( i = 0; i < N; i++ )
417 : {
418 248558544 : tmp_sum = 0;
419 248558544 : pt_x = x;
420 1864189080 : for ( j = 0; j <= i; j++ )
421 : {
422 1615630536 : tmp_sum += *pt_x++ * *pt_A++;
423 : }
424 :
425 248558544 : suma += tmp_sum * tmp_sum;
426 : }
427 :
428 20713212 : return suma;
429 : }
430 :
431 : /*---------------------------------------------------------------------*
432 : * v_mult_mat()
433 : *
434 : * Multiplication of row vector x by matrix A, where x has size Nr and
435 : * A has size Nr x Nc ans it is stored column-wise in memory.
436 : * The resulting row vector y has size Nc
437 : *---------------------------------------------------------------------*/
438 :
439 2301468 : void v_mult_mat(
440 : float *y, /* o : the product x*A */
441 : const float *x, /* i : vector x */
442 : const float *A, /* i : matrix A */
443 : const int16_t Nr, /* i : number of rows */
444 : const int16_t Nc /* i : number of columns */
445 : )
446 : {
447 : int16_t i, j;
448 : const float *pt_x, *pt_A;
449 : float tmp_y[MAX_V_MULT_MAT];
450 : float *pt_y;
451 :
452 2301468 : pt_y = tmp_y;
453 2301468 : pt_A = A;
454 31069818 : for ( i = 0; i < Nc; i++ )
455 : {
456 28768350 : pt_x = x;
457 28768350 : *pt_y = 0;
458 834282150 : for ( j = 0; j < Nr; j++ )
459 : {
460 805513800 : *pt_y += ( *pt_x++ ) * ( *pt_A++ );
461 : }
462 28768350 : pt_y++;
463 : }
464 :
465 2301468 : mvr2r( tmp_y, y, Nc );
466 :
467 2301468 : return;
468 : }
469 :
470 :
471 : /*---------------------------------------------------------------------*
472 : * logsumexp()
473 : *
474 : * Compute logarithm of the sum of exponentials of input vector in a numerically stable way
475 : *---------------------------------------------------------------------*/
476 :
477 : /*! r: log(sum(exp(x)) of the input array x */
478 3452202 : float logsumexp(
479 : const float x[], /* i : input array x */
480 : const int16_t N /* i : number of elements in array x */
481 : )
482 : {
483 : float max_exp;
484 : float sum;
485 : int16_t i;
486 :
487 3452202 : max_exp = x[0];
488 3452202 : sum = 0;
489 20713212 : for ( i = 1; i < N; i++ )
490 : {
491 17261010 : if ( x[i] > max_exp )
492 : {
493 5935576 : max_exp = x[i];
494 : }
495 : }
496 :
497 24165414 : for ( i = 0; i < N; i++ )
498 : {
499 20713212 : sum += expf( x[i] - max_exp );
500 : }
501 :
502 3452202 : return logf( sum ) + max_exp;
503 : }
504 :
505 :
506 : /*---------------------------------------------------------------------*
507 : * lin_interp()
508 : *
509 : * Linearly maps x from source range <x1, x2> to the target range <y1, y2>
510 : *---------------------------------------------------------------------*/
511 :
512 : /*! r: mapped output value */
513 3683995 : float lin_interp(
514 : const float x, /* i : the value to be mapped */
515 : const float x1, /* i : source range interval: low end */
516 : const float y1, /* i : source range interval: high end */
517 : const float x2, /* i : target range interval: low */
518 : const float y2, /* i : target range interval: high */
519 : const int16_t flag_sat /* i : flag to indicate whether to apply saturation */
520 : )
521 : {
522 3683995 : if ( x2 - x1 == 0 )
523 : {
524 0 : return y1;
525 : }
526 3683995 : else if ( flag_sat )
527 : {
528 3680603 : if ( x >= max( x1, x2 ) )
529 : {
530 56822 : return x1 > x2 ? y1 : y2;
531 : }
532 3623781 : else if ( x <= min( x1, x2 ) )
533 : {
534 2550451 : return x1 < x2 ? y1 : y2;
535 : }
536 : }
537 :
538 1076722 : return y1 + ( x - x1 ) * ( y2 - y1 ) / ( x2 - x1 );
539 : }
540 :
541 :
542 : /*-------------------------------------------------------------------*
543 : * check_bounds()
544 : *
545 : * Ensure the input value is within the given limits
546 : *-------------------------------------------------------------------*/
547 :
548 : /*! r: Adjusted value */
549 1702658 : float check_bounds(
550 : const float value, /* i : Input value */
551 : const float low, /* i : Low limit */
552 : const float high /* i : High limit */
553 : )
554 : {
555 : float value_adj;
556 :
557 1702658 : value_adj = min( max( value, low ), high );
558 :
559 1702658 : return value_adj;
560 : }
561 :
562 :
563 : /*-------------------------------------------------------------------*
564 : * check_bounds_s()
565 : *
566 : * Ensure the input value is within the given limits
567 : *-------------------------------------------------------------------*/
568 :
569 : /*! r: Adjusted value */
570 2619780 : int16_t check_bounds_s(
571 : const int16_t value, /* i : Input value */
572 : const int16_t low, /* i : Low limit */
573 : const int16_t high /* i : High limit */
574 : )
575 : {
576 : int16_t value_adj;
577 :
578 2619780 : value_adj = min( max( value, low ), high );
579 :
580 2619780 : return value_adj;
581 : }
582 :
583 :
584 : /*---------------------------------------------------------------------*
585 : * set_zero_l()
586 : *
587 : * Set a vector vec[] of dimension lvec to zero
588 : *---------------------------------------------------------------------*/
589 :
590 29733 : void set_zero_l(
591 : float *vec, /* i/o: input/output vector */
592 : const uint32_t lvec /* i : length of the vector */
593 : )
594 : {
595 : uint32_t i;
596 :
597 99016836 : for ( i = 0; i < lvec; i++ )
598 : {
599 98987103 : *vec++ = 0.0f;
600 : }
601 :
602 29733 : return;
603 : }
604 :
605 :
606 : /****************************************************************************/
607 : /* matrix functions */
608 : /* matrices are ordered column-wise in memory */
609 : /****************************************************************************/
610 :
611 : /*---------------------------------------------------------------------*
612 : * matrix product
613 : *
614 : * comput the matrix product of two matrices (Z=X*Y)
615 : *---------------------------------------------------------------------*/
616 :
617 : /*! r: success or failure */
618 83570501 : int16_t matrix_product(
619 : const float *X, /* i : left hand matrix */
620 : const int16_t rowsX, /* i : number of rows of the left hand matrix */
621 : const int16_t colsX, /* i : number of columns of the left hand matrix */
622 : const int16_t transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication */
623 : const float *Y, /* i : right hand matrix */
624 : const int16_t rowsY, /* i : number of rows of the right hand matrix */
625 : const int16_t colsY, /* i : number of columns of the right hand matrix */
626 : const int16_t transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication */
627 : float *Z /* o : resulting matrix after the matrix multiplication */
628 : )
629 : {
630 : int16_t i, j, k;
631 83570501 : float *Zp = Z;
632 :
633 : /* Processing */
634 83570501 : if ( transpX == 1 && transpY == 0 ) /* We use X transpose */
635 : {
636 0 : if ( rowsX != rowsY )
637 : {
638 0 : return EXIT_FAILURE;
639 : }
640 0 : for ( j = 0; j < colsY; ++j )
641 : {
642 0 : for ( i = 0; i < colsX; ++i )
643 : {
644 0 : ( *Zp ) = 0.0f;
645 0 : for ( k = 0; k < rowsX; ++k )
646 : {
647 0 : ( *Zp ) += X[k + i * rowsX] * Y[k + j * rowsY];
648 : }
649 0 : Zp++;
650 : }
651 : }
652 : }
653 83570501 : else if ( transpX == 0 && transpY == 1 ) /* We use Y transpose */
654 : {
655 3958434 : if ( colsX != colsY )
656 : {
657 0 : return EXIT_FAILURE;
658 : }
659 24987186 : for ( j = 0; j < rowsY; ++j )
660 : {
661 182154738 : for ( i = 0; i < rowsX; ++i )
662 : {
663 161125986 : ( *Zp ) = 0.0f;
664 514326073 : for ( k = 0; k < colsX; ++k )
665 : {
666 353200087 : ( *Zp ) += X[i + k * rowsX] * Y[j + k * rowsY];
667 : }
668 161125986 : Zp++;
669 : }
670 : }
671 : }
672 79612067 : else if ( transpX == 1 && transpY == 1 ) /* We use both transpose */
673 : {
674 398496 : if ( rowsX != colsY )
675 : {
676 0 : return EXIT_FAILURE;
677 : }
678 2438223 : for ( j = 0; j < rowsY; ++j )
679 : {
680 6158091 : for ( i = 0; i < colsX; ++i )
681 : {
682 4118364 : ( *Zp ) = 0.0f;
683 12471822 : for ( k = 0; k < colsX; ++k )
684 : {
685 8353458 : ( *Zp ) += X[k + i * rowsX] * Y[j + k * rowsY];
686 : }
687 :
688 4118364 : Zp++;
689 : }
690 : }
691 : }
692 : else /* Regular case */
693 : {
694 79213571 : if ( colsX != rowsY )
695 : {
696 0 : return EXIT_FAILURE;
697 : }
698 :
699 166409191 : for ( j = 0; j < colsY; ++j )
700 : {
701 603766195 : for ( i = 0; i < rowsX; ++i )
702 : {
703 516570575 : ( *Zp ) = 0.0f;
704 2150392126 : for ( k = 0; k < colsX; ++k )
705 : {
706 1633821551 : ( *Zp ) += X[i + k * rowsX] * Y[k + j * rowsY];
707 : }
708 516570575 : Zp++;
709 : }
710 : }
711 : }
712 :
713 83570501 : return EXIT_SUCCESS;
714 : }
715 :
716 :
717 : /*---------------------------------------------------------------------*
718 : * matrix_diag_product
719 : *
720 : * compute the product of a matrix with a diagonal of a matrix (Z=X*diag(Y))
721 : *---------------------------------------------------------------------*/
722 :
723 : /*! r: success or failure */
724 6801366 : int16_t matrix_diag_product(
725 : const float *X, /* i : left hand matrix */
726 : const int16_t rowsX, /* i : number of rows of the left hand matrix */
727 : const int16_t colsX, /* i : number of columns of the left hand matrix */
728 : const int16_t transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication */
729 : const float *Y, /* i : right hand diagonal matrix as vector containing the diagonal elements */
730 : const int16_t entriesY, /* i : number of entries in the diagonal */
731 : float *Z /* o : resulting matrix after the matrix multiplication */
732 : )
733 : {
734 : int16_t i, j;
735 6801366 : float *Zp = Z;
736 :
737 : /* Processing */
738 6801366 : if ( transpX == 1 ) /* We use X transpose */
739 : {
740 0 : if ( rowsX != entriesY )
741 : {
742 0 : return EXIT_FAILURE;
743 : }
744 0 : for ( j = 0; j < entriesY; ++j )
745 : {
746 0 : for ( i = 0; i < colsX; ++i )
747 : {
748 0 : *( Zp++ ) = X[j + i * rowsX] * Y[j];
749 : }
750 : }
751 : }
752 : else /* Regular case */
753 : {
754 6801366 : if ( colsX != entriesY )
755 : {
756 0 : return EXIT_FAILURE;
757 : }
758 :
759 31853310 : for ( j = 0; j < entriesY; ++j )
760 : {
761 154035675 : for ( i = 0; i < rowsX; ++i )
762 : {
763 128983731 : *( Zp++ ) = *( X++ ) * Y[j];
764 : }
765 : }
766 : }
767 :
768 6801366 : return EXIT_SUCCESS;
769 : }
770 :
771 : /*---------------------------------------------------------------------*
772 : * diag_matrix_product()
773 : *
774 : * compute the matrix product of a diagonal matrix X with a full matrix Y (Z=diag(Y)*X)
775 : *---------------------------------------------------------------------*/
776 :
777 : /*! r: success or failure */
778 2831376 : int16_t diag_matrix_product(
779 : const float *Y, /* i : left hand diagonal matrix as vector containing the diagonal elements */
780 : const int16_t entriesY, /* i : length of the diagonal of the left hand matrix */
781 : const float *X, /* i : right hand matrix */
782 : const int16_t rowsX, /* i : number of rows of the right hand matrix */
783 : const int16_t colsX, /* i : number of columns of the right hand matrix */
784 : const int16_t transpX, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication */
785 : float *Z /* o : resulting matrix after the matrix multiplication */
786 : )
787 : {
788 : int16_t i, j;
789 2831376 : float *Zp = Z;
790 :
791 : /* Processing */
792 2831376 : if ( transpX == 1 ) /* We use X transpose */
793 : {
794 1216440 : if ( colsX != entriesY )
795 : {
796 0 : return EXIT_FAILURE;
797 : }
798 12833100 : for ( i = 0; i < rowsX; ++i )
799 : {
800 34849980 : for ( j = 0; j < entriesY; ++j )
801 : {
802 23233320 : *( Zp++ ) = X[i + j * rowsX] * Y[j];
803 : }
804 : }
805 : }
806 : else /* Regular case */
807 : {
808 1614936 : if ( rowsX != entriesY )
809 : {
810 0 : return EXIT_FAILURE;
811 : }
812 5846745 : for ( i = 0; i < colsX; ++i )
813 : {
814 36804516 : for ( j = 0; j < entriesY; ++j )
815 : {
816 32572707 : *( Zp++ ) = *( X++ ) * Y[j];
817 : }
818 : }
819 : }
820 :
821 2831376 : return EXIT_SUCCESS;
822 : }
823 :
824 :
825 : /*---------------------------------------------------------------------*
826 : * matrix_product_diag()
827 : *
828 : * compute only the main diagonal of X*Y (Z=diag(X*Y))
829 : *---------------------------------------------------------------------*/
830 :
831 : /*! r: success or failure */
832 2784090 : int16_t matrix_product_diag(
833 : const float *X, /* i : left hand matrix */
834 : const int16_t rowsX, /* i : number of rows of the left hand matrix */
835 : const int16_t colsX, /* i : number of columns of the left hand matrix */
836 : const int16_t transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication */
837 : const float *Y, /* i : right hand matrix */
838 : const int16_t rowsY, /* i : number of rows of the right hand matrix */
839 : const int16_t colsY, /* i : number of columns of the right hand matrix */
840 : const int16_t transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication */
841 : float *Z /* o : resulting matrix after the matrix multiplication */
842 : )
843 : {
844 : int16_t j, k;
845 2784090 : float *Zp = Z;
846 :
847 : /* Processing */
848 2784090 : if ( transpX == 1 && transpY == 0 ) /* We use X transpose */
849 : {
850 0 : if ( rowsX != rowsY )
851 : {
852 0 : return EXIT_FAILURE;
853 : }
854 :
855 0 : for ( j = 0; j < colsY; ++j )
856 : {
857 0 : ( *Zp ) = 0.0f;
858 0 : for ( k = 0; k < rowsX; ++k )
859 : {
860 0 : ( *Zp ) += X[k + j * rowsX] * Y[k + j * rowsY];
861 : }
862 0 : Zp++;
863 : }
864 : }
865 2784090 : else if ( transpX == 0 && transpY == 1 ) /* We use Y transpose */
866 : {
867 2334102 : if ( colsX != colsY )
868 : {
869 0 : return EXIT_FAILURE;
870 : }
871 19672653 : for ( j = 0; j < rowsY; ++j )
872 : {
873 17338551 : ( *Zp ) = 0.0f;
874 57345366 : for ( k = 0; k < colsX; ++k )
875 : {
876 40006815 : ( *Zp ) += X[j + k * rowsX] * Y[j + k * rowsY];
877 : }
878 17338551 : Zp++;
879 : }
880 : }
881 449988 : else if ( transpX == 1 && transpY == 1 ) /* We use both transpose */
882 : {
883 0 : if ( rowsX != colsY )
884 : {
885 0 : return EXIT_FAILURE;
886 : }
887 :
888 0 : for ( j = 0; j < rowsY; ++j )
889 : {
890 :
891 0 : ( *Zp ) = 0.0f;
892 0 : for ( k = 0; k < colsX; ++k )
893 : {
894 0 : ( *Zp ) += X[k + j * rowsX] * Y[j + k * rowsY];
895 : }
896 :
897 0 : Zp++;
898 : }
899 : }
900 : else /* Regular case */
901 : {
902 449988 : if ( colsX != rowsY )
903 : {
904 0 : return EXIT_FAILURE;
905 : }
906 :
907 4739130 : for ( j = 0; j < colsY; ++j )
908 : {
909 4289142 : ( *Zp ) = 0.0f;
910 8578284 : for ( k = 0; k < colsX; ++k )
911 : {
912 4289142 : ( *Zp ) += X[j + k * rowsX] * Y[k + j * rowsY];
913 : }
914 4289142 : Zp++;
915 : }
916 : }
917 :
918 2784090 : return EXIT_SUCCESS;
919 : }
920 :
921 :
922 : /*---------------------------------------------------------------------*
923 : * cmplx_matrix_square()
924 : *
925 : * compute the square of a complex matrix (Z=X*X)
926 : *---------------------------------------------------------------------*/
927 :
928 : /*! r: success or failure */
929 6256065 : void cmplx_matrix_square(
930 : const float *realX, /* i : real part of the matrix */
931 : const float *imagX, /* i : imaginary part of the matrix */
932 : const int16_t mRows, /* i : number of rows of the matrix */
933 : const int16_t nCols, /* i : number of columns of the matrix */
934 : float *realZ, /* o : real part of the resulting matrix */
935 : float *imagZ /* o : imaginary part of the resulting matrix */
936 : )
937 : {
938 : int16_t i, j, k;
939 : float *realZp, *imagZp;
940 : const float *p_real1, *p_real2, *p_imag1, *p_imag2;
941 :
942 : /* resulting matrix is hermitean, we only need to calc the upper triangle */
943 : /* we assume transposition needed */
944 :
945 : /* column*column = column/column */
946 18827499 : for ( i = 0; i < nCols; i++ )
947 : {
948 31517541 : for ( j = i; j < nCols; j++ )
949 : {
950 18946107 : p_real1 = realX + i * mRows;
951 18946107 : p_imag1 = imagX + i * mRows;
952 18946107 : p_real2 = realX + j * mRows;
953 18946107 : p_imag2 = imagX + j * mRows;
954 18946107 : realZp = realZ + ( i + nCols * j );
955 18946107 : imagZp = imagZ + ( i + nCols * j );
956 18946107 : *( realZp ) = 0.0f;
957 18946107 : *( imagZp ) = 0.0f;
958 :
959 99290907 : for ( k = 0; k < mRows; k++ )
960 : {
961 80344800 : *( imagZp ) += *( p_real1 ) * ( *( p_imag2 ) ) - ( *( p_real2 ) ) * ( *( p_imag1 ) );
962 80344800 : *( realZp ) += *( p_real1++ ) * ( *( p_real2++ ) ) + *( p_imag1++ ) * ( *( p_imag2++ ) );
963 : }
964 : }
965 : }
966 :
967 : /* fill lower triangle */
968 12571434 : for ( i = 1; i < nCols; i++ )
969 : {
970 12690042 : for ( j = 0; j < i; j++ )
971 : {
972 6374673 : realZ[i + nCols * j] = realZ[j + nCols * i];
973 6374673 : imagZ[i + nCols * j] = imagZ[j + nCols * i];
974 : }
975 : }
976 :
977 6256065 : return;
978 : }
979 :
980 : /*-------------------------------------------------------------------*
981 : * v_multc_acc()
982 : *
983 : * Multiplication of vector by constant, accumulate to the output
984 : *-------------------------------------------------------------------*/
985 :
986 5677419 : void v_multc_acc(
987 : const float x[], /* i : Input vector */
988 : const float c, /* i : Constant */
989 : float y[], /* o : Output vector that contains y + c*x */
990 : const int16_t N /* i : Vector length */
991 : )
992 : {
993 : int16_t i;
994 :
995 656833699 : for ( i = 0; i < N; i++ )
996 : {
997 651156280 : y[i] += c * x[i];
998 : }
999 :
1000 5677419 : return;
1001 : }
1002 :
1003 : /*---------------------------------------------------------------------*
1004 : * lls_interp_n()
1005 : *
1006 : * Linear least squares interpolation of an input vector
1007 : * The function calculates the slope 'a' and the offset 'b' of a LLS estimate for an input vector x such that
1008 : * y_i = a*i + b where i=1,..,N and (a,b) = min(a,b) [sum_N[(y_i - x_i)^2]]
1009 : * the interpolated vector is return as x[], if requested
1010 : *---------------------------------------------------------------------*/
1011 :
1012 :
1013 137886 : void lls_interp_n(
1014 : float x[], /* i/o: input/output vector */
1015 : const int16_t N, /* i : length of the input vector */
1016 : float *a, /* o : calculated slope */
1017 : float *b, /* o : calculated offset */
1018 : const int16_t upd /* i : use 1 to update x[] with the interpolated output */
1019 : )
1020 : {
1021 : int16_t i;
1022 137886 : const float n_i[11] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
1023 137886 : const float one_by_n[11] = { 0, 1, 0.5f, 0.333f, 0.25f, 0.2f, 0.1666f, 0.14286f, 0.125f, 0.111111f, 0.1f };
1024 137886 : const float sum_i[12] = { 0, 0, 1, 3, 6, 10, 15, 21, 28, 36, 45, 55 };
1025 137886 : const float sum_ii[12] = { 0, 0, 1, 5, 14, 30, 55, 91, 140, 204, 285, 385 };
1026 : float sum_x, sum_ix, slope, offset;
1027 :
1028 137886 : assert( N > 0 && N <= 10 );
1029 :
1030 137886 : sum_x = sum_f( x, N );
1031 137886 : sum_ix = dotp( x, n_i, N );
1032 :
1033 137886 : slope = ( N * sum_ix - sum_i[N] * sum_x ) / ( N * sum_ii[N] - sum_i[N] * sum_i[N] );
1034 137886 : offset = ( sum_x - slope * sum_i[N] ) * one_by_n[N];
1035 :
1036 137886 : if ( upd )
1037 : {
1038 668250 : for ( i = 0; i < N; i++ )
1039 : {
1040 534600 : x[i] = slope * i + offset;
1041 : }
1042 : }
1043 :
1044 137886 : if ( a != NULL )
1045 : {
1046 137886 : *a = slope;
1047 : }
1048 :
1049 137886 : if ( b != NULL )
1050 : {
1051 137886 : *b = offset;
1052 : }
1053 :
1054 137886 : return;
1055 : }
1056 :
1057 : /* helper function for panning_wrap_angles */
1058 16213522 : static float wrap_azi(
1059 : const float azi_deg )
1060 : {
1061 16213522 : float azi = azi_deg;
1062 :
1063 : /* Wrap azimuth value */
1064 20390284 : while ( azi > 180 )
1065 : {
1066 4176762 : azi -= 360.0f;
1067 : }
1068 :
1069 16329875 : while ( azi <= -180 )
1070 : {
1071 116353 : azi += 360;
1072 : }
1073 :
1074 16213522 : return azi;
1075 : }
1076 :
1077 : /*-------------------------------------------------------------------*
1078 : * panning_wrap_angles()
1079 : *
1080 : * Wrap angles for amplitude panning to the range:
1081 : * azimuth = (-180, 180]
1082 : * elevation = [-90, 90]
1083 : * Considers direction changes from large elevation values
1084 : *-------------------------------------------------------------------*/
1085 16224373 : void panning_wrap_angles(
1086 : const float azi_deg, /* i : azimuth in degrees for panning direction (positive left) */
1087 : const float ele_deg, /* i : elevation in degrees for panning direction (positive up) */
1088 : float *azi_wrapped, /* o : wrapped azimuth component */
1089 : float *ele_wrapped /* o : wrapped elevation component */
1090 : )
1091 : {
1092 : float azi, ele;
1093 :
1094 16224373 : azi = azi_deg;
1095 16224373 : ele = ele_deg;
1096 :
1097 16224373 : if ( fabsf( ele ) < 90 )
1098 : {
1099 16213522 : *ele_wrapped = ele;
1100 16213522 : *azi_wrapped = wrap_azi( azi );
1101 16213522 : return;
1102 : }
1103 : else
1104 : {
1105 : /* Special case when elevation is a multiple of 90; azimuth is irrelevant */
1106 10851 : if ( ( fmodf( ele, 90 ) == 0 ) && ( fmodf( ele, 180 ) != 0 ) )
1107 : {
1108 10851 : *azi_wrapped = 0;
1109 10851 : while ( ele > 90 )
1110 : {
1111 0 : ele -= 360;
1112 : }
1113 10851 : while ( ele < -90 )
1114 : {
1115 0 : ele += 360;
1116 : }
1117 10851 : *ele_wrapped = ele;
1118 : }
1119 : else
1120 : {
1121 : /* Wrap elevation and adjust azimuth accordingly */
1122 0 : while ( fabsf( ele ) > 90 )
1123 : {
1124 : /* Flip to other hemisphere */
1125 0 : azi += 180;
1126 :
1127 : /* Compensate elevation accordingly */
1128 0 : if ( ele > 90 )
1129 : {
1130 0 : ele = 180 - ele;
1131 : }
1132 0 : else if ( ele < -90 )
1133 : {
1134 0 : ele = -180 - ele;
1135 : }
1136 : }
1137 0 : *azi_wrapped = wrap_azi( azi );
1138 0 : *ele_wrapped = ele;
1139 : }
1140 :
1141 10851 : return;
1142 : }
1143 : }
1144 :
1145 : /*-------------------------------------------------------------------------*
1146 : * v_sort_ind()
1147 : *
1148 : * Sort a float array
1149 : * (modified version of v_sort() to return an index array)
1150 : *-------------------------------------------------------------------------*/
1151 :
1152 42055 : void v_sort_ind(
1153 : float *x, /* i/o: Vector to be sorted */
1154 : int16_t *idx, /* o : Original index positions */
1155 : const int16_t len /* i : vector length */
1156 : )
1157 : {
1158 : int16_t i, j;
1159 : float tempr;
1160 : int16_t tempi;
1161 :
1162 197459 : for ( i = 0; i < len; i++ )
1163 : {
1164 155404 : idx[i] = i;
1165 : }
1166 :
1167 155404 : for ( i = len - 2; i >= 0; i-- )
1168 : {
1169 113349 : tempr = x[i];
1170 113349 : tempi = idx[i];
1171 278841 : for ( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1172 : {
1173 165492 : x[j - 1] = x[j];
1174 165492 : idx[j - 1] = idx[j];
1175 : }
1176 113349 : x[j - 1] = tempr;
1177 113349 : idx[j - 1] = tempi;
1178 : }
1179 :
1180 42055 : return;
1181 : }
1182 :
1183 :
1184 : /*-------------------------------------------------------------------*
1185 : * is_IVAS_bitrate()
1186 : *
1187 : * check if the bitrate is IVAS supported active bitrate
1188 : *-------------------------------------------------------------------*/
1189 :
1190 : /*! r: flag indicating a valid bitrate */
1191 58762 : int16_t is_IVAS_bitrate(
1192 : const int32_t ivas_total_brate /* i : IVAS total bitrate */
1193 : )
1194 : {
1195 : int16_t j;
1196 :
1197 58762 : j = SIZE_IVAS_BRATE_TBL - IVAS_NUM_ACTIVE_BRATES; /* skip NO_DATA and SID bitrates */
1198 370764 : while ( j < SIZE_IVAS_BRATE_TBL && ivas_total_brate != ivas_brate_tbl[j] )
1199 : {
1200 312002 : j++;
1201 : }
1202 :
1203 58762 : if ( j >= SIZE_IVAS_BRATE_TBL )
1204 : {
1205 0 : return 0;
1206 : }
1207 :
1208 58762 : return 1;
1209 : }
1210 :
1211 :
1212 : /*-------------------------------------------------------------------*
1213 : * is_DTXrate()
1214 : *
1215 : * identify DTX frame bitrates
1216 : *-------------------------------------------------------------------*/
1217 :
1218 10789062 : int16_t is_DTXrate(
1219 : const int32_t ivas_total_brate /* i : IVAS total bitrate */
1220 : )
1221 : {
1222 10789062 : int16_t dtx_rate_flag = 0;
1223 :
1224 10789062 : if ( is_SIDrate( ivas_total_brate ) || ( ivas_total_brate == FRAME_NO_DATA ) )
1225 : {
1226 390705 : dtx_rate_flag = 1;
1227 : }
1228 :
1229 10789062 : return dtx_rate_flag;
1230 : }
1231 :
1232 : /*-------------------------------------------------------------------*
1233 : * is_SIDrate()
1234 : *
1235 : * identify SID frame bitrates
1236 : *-------------------------------------------------------------------*/
1237 :
1238 12064080 : int16_t is_SIDrate(
1239 : const int32_t ivas_total_brate /* i : IVAS total bitrate */
1240 : )
1241 : {
1242 12064080 : int16_t sid_rate_flag = 0;
1243 :
1244 12064080 : if ( ( ivas_total_brate == SID_1k75 ) ||
1245 12064080 : ( ivas_total_brate == SID_2k40 ) ||
1246 : ( ivas_total_brate == IVAS_SID_5k2 ) )
1247 : {
1248 68613 : sid_rate_flag = 1;
1249 : }
1250 :
1251 12064080 : return sid_rate_flag;
1252 : }
1253 :
1254 :
1255 : /*-------------------------------------------------------------------*
1256 : * rand_triangular_signed()
1257 : *
1258 : * generate a random value with a triangular pdf in [-0.5, 0.5]
1259 : *-------------------------------------------------------------------*/
1260 :
1261 79261656 : float rand_triangular_signed(
1262 : int16_t *seed )
1263 : {
1264 : float rand_val;
1265 :
1266 79261656 : rand_val = own_random( seed ) * OUTMAX_INV;
1267 79261656 : if ( rand_val <= 0.0f )
1268 : {
1269 : /* rand_val in [-1, 0] */
1270 39619152 : return 0.5f * sqrtf( rand_val + 1.0f ) - 0.5f;
1271 : }
1272 : else
1273 : {
1274 : /* rand_val in (0, 1) */
1275 39642504 : return 0.5f - 0.5f * sqrtf( 1.0f - rand_val );
1276 : }
1277 : }
1278 4924 : Word16 matrix_product_fx(
1279 : const Word32 *X_fx, /* i : left hand matrix Qx*/
1280 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1281 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1282 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1283 : const Word32 *Y_fx, /* i : right hand matrix Qy*/
1284 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1285 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1286 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1287 : Word32 *Z_fx /* o : resulting matrix after the matrix multiplication Qx + Qy - 31*/
1288 : )
1289 : {
1290 : Word16 i, j, k;
1291 : Word16 x_idx, y_idx;
1292 4924 : Word32 *Zp_fx = Z_fx;
1293 :
1294 : /* Processing */
1295 :
1296 4924 : if ( transpX == 1 && transpY == 0 ) /* We use X transpose */
1297 : {
1298 4924 : if ( rowsX != rowsY )
1299 : {
1300 0 : return EXIT_FAILURE;
1301 : }
1302 24620 : for ( j = 0; j < colsY; ++j )
1303 : {
1304 118176 : for ( i = 0; i < colsX; ++i )
1305 : {
1306 98480 : ( *Zp_fx ) = 0;
1307 :
1308 590880 : for ( k = 0; k < rowsX; ++k )
1309 : {
1310 492400 : x_idx = k + i * rowsX; /*Q0*/
1311 492400 : y_idx = k + j * rowsY; /*Q0*/
1312 492400 : ( *Zp_fx ) = *Zp_fx + Mpy_32_32( X_fx[x_idx], Y_fx[y_idx] ); /*Qx + Qy - 31*/
1313 : }
1314 98480 : Zp_fx++;
1315 : }
1316 : }
1317 : }
1318 0 : else if ( transpX == 0 && transpY == 1 ) /* We use Y transpose */
1319 : {
1320 0 : if ( colsX != colsY )
1321 : {
1322 0 : return EXIT_FAILURE;
1323 : }
1324 0 : for ( j = 0; j < rowsY; ++j )
1325 : {
1326 0 : for ( i = 0; i < rowsX; ++i )
1327 : {
1328 0 : ( *Zp_fx ) = 0;
1329 0 : for ( k = 0; k < colsX; ++k )
1330 : {
1331 0 : x_idx = i + k * rowsX; /*Q0*/
1332 0 : y_idx = j + k * rowsY; /*Q0*/
1333 0 : ( *Zp_fx ) = ( *Zp_fx ) + Mpy_32_32( X_fx[x_idx], Y_fx[y_idx] ); /*Qx + Qy - 31*/
1334 : }
1335 0 : Zp_fx++;
1336 : }
1337 : }
1338 : }
1339 0 : else if ( transpX == 1 && transpY == 1 ) /* We use both transpose */
1340 : {
1341 0 : if ( rowsX != colsY )
1342 : {
1343 0 : return EXIT_FAILURE;
1344 : }
1345 0 : for ( j = 0; j < rowsY; ++j )
1346 : {
1347 0 : for ( i = 0; i < colsX; ++i )
1348 : {
1349 0 : ( *Zp_fx ) = 0;
1350 :
1351 0 : for ( k = 0; k < colsX; ++k )
1352 : {
1353 0 : x_idx = k + i * rowsX; /*Q0*/
1354 0 : y_idx = j + k * rowsY; /*Q0*/
1355 0 : ( *Zp_fx ) = ( *Zp_fx ) + Mpy_32_32( X_fx[x_idx], Y_fx[y_idx] ); /*Qx + Qy - 31*/
1356 : }
1357 :
1358 0 : Zp_fx++;
1359 : }
1360 : }
1361 : }
1362 : else /* Regular case */
1363 : {
1364 0 : if ( colsX != rowsY )
1365 : {
1366 0 : return EXIT_FAILURE;
1367 : }
1368 :
1369 0 : for ( j = 0; j < colsY; ++j )
1370 : {
1371 0 : for ( i = 0; i < rowsX; ++i )
1372 : {
1373 0 : ( *Zp_fx ) = 0;
1374 :
1375 0 : for ( k = 0; k < colsX; ++k )
1376 : {
1377 0 : x_idx = i + k * rowsX; /*Q0*/
1378 0 : y_idx = k + j * rowsY; /*Q0*/
1379 0 : ( *Zp_fx ) = ( *Zp_fx ) + Mpy_32_32( X_fx[x_idx], Y_fx[y_idx] ); /*Qx + Qy - 31 L_sat_add() */
1380 : /* TODO: overflow of Z_fx to be checked */
1381 0 : move32();
1382 : }
1383 0 : Zp_fx++;
1384 : }
1385 : }
1386 : }
1387 :
1388 4924 : return EXIT_SUCCESS;
1389 : }
1390 :
1391 7364 : Word16 matrix_product_q30_fx(
1392 : const Word32 *X_fx, /* i : left hand matrix Q31*/
1393 : const Word16 rowsX, /* i : number of rows of the left hand matrix Q0*/
1394 : const Word16 colsX, /* i : number of columns of the left hand matrix Q0*/
1395 : const Word16 transpX, /* i : flag indicating the transposition of the left hand matrix prior to the multiplication Q0*/
1396 : const Word32 *Y_fx, /* i : right hand matrix Q25*/
1397 : const Word16 rowsY, /* i : number of rows of the right hand matrix Q0*/
1398 : const Word16 colsY, /* i : number of columns of the right hand matrix Q0*/
1399 : const Word16 transpY, /* i : flag indicating the transposition of the right hand matrix prior to the multiplication Q0*/
1400 : Word32 *Z_fx /* o : resulting matrix after the matrix multiplication Q30*/
1401 : )
1402 : {
1403 : Word16 i, j, k;
1404 : Word16 x_idx, y_idx;
1405 7364 : Word32 *Zp_fx = Z_fx;
1406 : int64_t W_tmp;
1407 :
1408 : /* Processing */
1409 7364 : test();
1410 7364 : test();
1411 7364 : test();
1412 7364 : if ( transpX == 1 && transpY == 0 ) /* We use X transpose */
1413 : {
1414 2440 : if ( rowsX != rowsY )
1415 : {
1416 0 : return EXIT_FAILURE;
1417 : }
1418 4880 : for ( j = 0; j < colsY; ++j )
1419 : {
1420 24753 : for ( i = 0; i < colsX; ++i )
1421 : {
1422 22313 : W_tmp = 0;
1423 255978 : for ( k = 0; k < rowsX; ++k )
1424 : {
1425 : /*( *Zp_fx ) = L_add( *Zp_fx, Mpy_32_32( X_fx[k + i * rowsX], Y_fx[k + j * rowsY] ) ); */
1426 233665 : x_idx = k + i * rowsX; /* Q0 */
1427 233665 : y_idx = k + j * rowsY; /* Q0 */
1428 233665 : W_tmp += ( (int64_t) X_fx[x_idx] * (int64_t) Y_fx[y_idx] ); /* Q56 */
1429 : }
1430 22313 : W_tmp = W_tmp * 64; /* W_shl( W_tmp, 6 ); */ /*Q62*/
1431 22313 : ( *Zp_fx ) = ( W_tmp + 0x80000000 ) >> 32; /* W_round64_L( W_tmp ); */ /*Q30*/
1432 22313 : Zp_fx++;
1433 : }
1434 : }
1435 : }
1436 4924 : else if ( transpX == 0 && transpY == 1 ) /* We use Y transpose */
1437 : {
1438 0 : if ( colsX != colsY )
1439 : {
1440 0 : return EXIT_FAILURE;
1441 : }
1442 0 : for ( j = 0; j < rowsY; ++j )
1443 : {
1444 0 : for ( i = 0; i < rowsX; ++i )
1445 : {
1446 0 : W_tmp = 0;
1447 0 : for ( k = 0; k < colsX; ++k )
1448 : {
1449 : /* ( *Zp_fx ) = L_add( *Zp_fx, Mpy_32_32( X_fx[i + k * rowsX], Y_fx[j + k * rowsY] ) ); */
1450 0 : x_idx = i + k * rowsX; /*Q0*/
1451 0 : y_idx = j + k * rowsY; /*Q0*/
1452 0 : W_tmp += ( (int64_t) X_fx[x_idx] * (int64_t) Y_fx[y_idx] ); /* Q56 */
1453 : }
1454 0 : W_tmp = W_tmp * 64; /*Q62*/
1455 0 : ( *Zp_fx ) = ( W_tmp + 0x80000000 ) >> 32; /*Q30*/
1456 0 : Zp_fx++;
1457 : }
1458 : }
1459 : }
1460 4924 : else if ( transpX == 1 && transpY == 1 ) /* We use both transpose */
1461 : {
1462 0 : if ( rowsX != colsY )
1463 : {
1464 0 : return EXIT_FAILURE;
1465 : }
1466 0 : for ( j = 0; j < rowsY; ++j )
1467 : {
1468 0 : for ( i = 0; i < colsX; ++i )
1469 : {
1470 0 : W_tmp = 0;
1471 0 : for ( k = 0; k < colsX; ++k )
1472 : {
1473 : /* ( *Zp_fx ) = L_add( *Zp_fx, Mpy_32_32( X_fx[k + i * rowsX], Y_fx[j + k * rowsY] ) ); */
1474 0 : x_idx = k + i * rowsX; /*Q0*/
1475 0 : y_idx = j + k * rowsY; /*Q0*/
1476 0 : W_tmp += ( (int64_t) X_fx[x_idx] * (int64_t) Y_fx[y_idx] ); /* Q56*/
1477 : }
1478 0 : W_tmp = W_tmp * 64; /*Q62*/
1479 0 : ( *Zp_fx ) = ( W_tmp + 0x80000000 ) >> 32; /*Q30*/
1480 :
1481 0 : Zp_fx++;
1482 : }
1483 : }
1484 : }
1485 : else /* Regular case */
1486 : {
1487 4924 : if ( colsX != rowsY )
1488 : {
1489 0 : return EXIT_FAILURE;
1490 : }
1491 :
1492 24620 : for ( j = 0; j < colsY; ++j )
1493 : {
1494 118176 : for ( i = 0; i < rowsX; ++i )
1495 : {
1496 98480 : W_tmp = 0;
1497 :
1498 492400 : for ( k = 0; k < colsX; ++k )
1499 : {
1500 : /* ( *Zp_fx ) = L_add( *Zp_fx, Mpy_32_32( X_fx[i + k * rowsX], Y_fx[k + j * rowsY] ) ); */
1501 393920 : x_idx = i + k * rowsX; /*Q0*/
1502 393920 : y_idx = k + j * rowsY; /*Q0*/
1503 393920 : W_tmp += ( (int64_t) X_fx[x_idx] * (int64_t) Y_fx[y_idx] ); /* Q56*/
1504 : }
1505 98480 : W_tmp = W_tmp * 64; /*Q62*/
1506 98480 : ( *Zp_fx ) = ( W_tmp + 0x80000000 ) >> 32; /*Q30*/
1507 :
1508 98480 : Zp_fx++;
1509 : }
1510 : }
1511 : }
1512 :
1513 7364 : return EXIT_SUCCESS;
1514 : }
|
