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