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