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 <stdint.h>
34 : #include "options.h"
35 : #include "cnst.h"
36 : #include "prot_fx.h"
37 : #include "ivas_cnst.h"
38 : #include "ivas_prot_fx.h"
39 : #include "math.h"
40 : #include "wmc_auto.h"
41 : #include "basop_util.h"
42 :
43 : /*---------------------------------------------------------------
44 : * Local constants
45 : * ---------------------------------------------------------------*/
46 :
47 : #define ZP8k 15 /* zero padding in 8kHz DFT analysis */
48 : #define OFFSET8k 55 /* offset in 8 kHz */
49 : #define STEREO_DFT_PLC_STEP21 ( L_FRAME8k - OFFSET8k ) /* Step from subframe 2 in frame n to subframe 1 in frame n+1 */
50 : #define STEREO_DFT_PLC_PH_C_FX ( Word16 )( 0x0A8D ) /* Phase estimation constant, for estimating phase of fractional frequency */
51 :
52 :
53 : /*---------------------------------------------------------------
54 : * stereo_dft_res_ecu_fx()
55 : *
56 : * Error concealment of DFT Stereo residual, including memory
57 : * updates of DFT analysis memory and IMDCT OLA
58 : * ---------------------------------------------------------------*/
59 :
60 938 : void stereo_dft_res_ecu_fx(
61 : STEREO_DFT_DEC_DATA_HANDLE hStereoDft, /* i/o: Decoder DFT stereo handle */
62 : Word32 *pDFT_RES, /* i/o: residual signal hStereoDft->q_dft*/
63 : Word32 *const DFT_PRED_RES, /* i/o: residual prediction signal hStereoDft->q_dft*/
64 : const Word16 k, /* i : Subframe index Q0*/
65 : const Word16 output_frame, /* i : Output frame length Q0*/
66 : const Word16 prev_bfi, /* i : Previous BFI Q0*/
67 : const Word32 dmx_nrg, /* i : Down-mix energy Qx*/
68 : Word16 *num_plocs, /* i/o: Number of peak locations Q0*/
69 : Word16 *plocs, /* i/o: Peak locations (bin) Q0*/
70 : Word32 *plocsi, /* i/o: Peak locations (fractional) Qx*/
71 : Word32 *input_mem /* o : Residual DFT buffer input mem Q11*/
72 : )
73 : {
74 938 : Word32 res_buf[L_FRAME8k] = { 0 };
75 : Word16 i;
76 : Word16 L_res;
77 : Word16 step;
78 : Word16 fac;
79 : Word16 trigo_dec[STEREO_DFT32MS_N_8k / 2 + 1];
80 : Word16 trigo_step;
81 : Word16 q_fac;
82 : Word16 time_offs;
83 :
84 938 : set32_fx( pDFT_RES, 0, L_FRAME8k );
85 :
86 938 : L_res = hStereoDft->band_limits[hStereoDft->res_cod_band_max]; /* Q0 */
87 938 : move16();
88 :
89 938 : stereo_dft_res_subst_spec_fx( hStereoDft, pDFT_RES, DFT_PRED_RES, hStereoDft->time_offs, L_res, L_FRAME8k, k, num_plocs, plocs, plocsi, k == 0 );
90 :
91 938 : fac = BASOP_Util_Divide3232_Scale( L_FRAME8k, hStereoDft->NFFT, &q_fac );
92 938 : IF( q_fac > 0 )
93 : {
94 0 : assert( 0 );
95 : }
96 : ELSE
97 : {
98 938 : fac = shl( fac, q_fac );
99 : }
100 :
101 938 : IF( hStereoDft->core_hist[0] == ACELP_CORE )
102 : {
103 424 : fac = extract_l( L_shr( L_mult0( fac, 0x2000 ), 15 ) ); /* Q15 */
104 : }
105 :
106 938 : trigo_step = STEREO_DFT_TRIGO_SRATE_8k_STEP * STEREO_DFT_TRIGO_DEC_STEP; /* Q0 */
107 938 : move16();
108 38458 : FOR( i = 0; i < STEREO_DFT32MS_N_8k / 4; i++ )
109 : {
110 37520 : trigo_dec[i] = hStereoDft->dft_trigo_8k_fx[i * trigo_step]; /* Q15 */
111 37520 : move16();
112 37520 : trigo_dec[STEREO_DFT32MS_N_8k / 2 - i] = hStereoDft->dft_trigo_8k_fx[i * trigo_step]; /* Q15 */
113 37520 : move16();
114 : }
115 938 : trigo_dec[STEREO_DFT32MS_N_8k / 4] = hStereoDft->dft_trigo_8k_fx[STEREO_DFT32MS_N_8k / 4 * trigo_step]; /* Q15 */
116 938 : move16();
117 :
118 : /* estimation of res_cod_mem (ola part in imdct residual signal) and input_mem (memory for buffer in DFT analysis)*/
119 938 : IF( k == 0 )
120 : {
121 469 : Copy32( pDFT_RES, res_buf, L_FRAME8k ); /* hStereoDft->q_dft */
122 469 : time_offs = add_sat( hStereoDft->time_offs, output_frame );
123 469 : stereo_dft_res_subst_spec_fx( hStereoDft, res_buf, DFT_PRED_RES, time_offs, L_res, L_FRAME8k, k, num_plocs, plocs, plocsi, FALSE );
124 :
125 469 : rfft_fx( res_buf, trigo_dec, L_FRAME8k, +1 );
126 :
127 469 : v_multc_fixed_16( res_buf, fac, res_buf, L_FRAME8k ); /* hStereoDft->q_dft */
128 469 : Copy32( res_buf + ( OFFSET8k - ZP8k ), &hStereoDft->res_cod_mem_fx[0], STEREO_DFT_OVL_8k ); /* hStereoDft->q_dft */
129 469 : hStereoDft->q_res_cod_mem_fx = hStereoDft->q_dft;
130 469 : move16();
131 :
132 469 : Copy32( res_buf + ZP8k, input_mem, NS2SA( 8000, STEREO_DFT32MS_OVL_NS ) ); /* Store memory for cross-fade to next frame, in case of good frame */ /* hStereoDft->q_dft */
133 469 : hStereoDft->q_ip_mem = hStereoDft->q_dft;
134 469 : move16();
135 : }
136 : ELSE
137 : {
138 469 : Word16 scale_fac = getScaleFactor32( res_buf, L_FRAME8k );
139 469 : Word16 q_shift = sub( 8, scale_fac );
140 469 : move16();
141 : // Copy32( pDFT_RES, res_buf, L_FRAME8k );
142 469 : v_shr_32( pDFT_RES, res_buf, L_FRAME8k, q_shift ); /* hStereoDft->q_dft - q_shift */
143 :
144 469 : rfft_fx( res_buf, trigo_dec, L_FRAME8k, +1 );
145 :
146 469 : v_shr_32( res_buf, res_buf, L_FRAME8k, negate( q_shift ) ); /* hStereoDft->q_dft */
147 :
148 469 : v_multc_fixed( res_buf, fac, res_buf, L_FRAME8k ); /* hStereoDft->q_dft */
149 :
150 : /* Cross-fade memory */
151 469 : fac = 0;
152 469 : move16();
153 469 : step = ONE_BY_NS2A_8K_ST_DFT32MS_OVL_NS;
154 469 : move16();
155 12194 : FOR( i = 0; i < NS2SA( 8000, STEREO_DFT32MS_OVL_NS ); i++ )
156 : {
157 11725 : input_mem[i] = Madd_32_16( Mpy_32_16_1( res_buf[i + L_FRAME8k - NS2SA( 8000, STEREO_DFT32MS_OVL_NS ) - ZP8k], sub( MAX_16, fac ) ), input_mem[i], fac ); /* hStereoDft->q_dft */
158 11725 : move32();
159 11725 : fac = add( fac, step );
160 : }
161 :
162 : /*in case of burst error*/
163 469 : hStereoDft->time_offs = add_sat( hStereoDft->time_offs, L_FRAME8k ); /* Q0 */
164 469 : move16();
165 : }
166 :
167 938 : set32_fx( DFT_PRED_RES, 0, shl( L_res, 1 ) );
168 :
169 938 : IF( prev_bfi )
170 : {
171 368 : stereo_dft_res_ecu_burst_att_fx( hStereoDft, pDFT_RES, dmx_nrg, L_res, L_FRAME8k );
172 : }
173 :
174 938 : return;
175 : }
176 :
177 : /*-------------------------------------------------------------------*
178 : * imax_pos()
179 : *
180 : * Get interpolated maximum position
181 : *-------------------------------------------------------------------*/
182 :
183 : /*! r: interpolated maximum position */
184 1165 : Word32 imax_pos_fx(
185 : const Word32 *y /* i : Input vector for peak interpolation Qx*/
186 : )
187 : {
188 : Word32 posi, y1, y2, y3, y3_y1, y2i;
189 : Word32 ftmp_den1, ftmp_den2;
190 : Word16 q_div_2i, q_div_posi;
191 : Word64 W_tmp;
192 : Word16 shift1, shift2;
193 : /* Seek the extrema of the parabola P(x) defined by 3 consecutive points so that P([-1 0 1]) = [y1 y2 y3] */
194 1165 : y1 = y[0]; /* Qx */
195 1165 : move32();
196 1165 : y2 = y[1]; /* Qx */
197 1165 : move32();
198 1165 : y3 = y[2]; /* Qx */
199 1165 : move32();
200 1165 : y3_y1 = L_sub( y3, y1 ); /* Qx */
201 1165 : W_tmp = W_sub( W_add( y1, y3 ), W_shl( y2, 1 ) ); /* Qx */
202 1165 : shift1 = W_norm( W_tmp );
203 1165 : ftmp_den1 = W_extract_h( W_shl( W_tmp, shift1 ) ); /* Qx + shift1 - 32 */
204 1165 : W_tmp = W_shl( W_sub( W_sub( W_shl( y2, 1 ), y1 ), y3 ), 1 ); /* Qx */
205 1165 : shift2 = W_norm( W_tmp );
206 1165 : ftmp_den2 = W_extract_h( W_shl( W_tmp, shift2 ) ); /* Qx + shift2 - 32 */
207 :
208 1165 : test();
209 1165 : IF( ( ftmp_den2 == 0 ) || ( ftmp_den1 == 0 ) )
210 : {
211 0 : return ( 0 ); /* early exit with left-most value */
212 : }
213 :
214 1165 : y2i = Mpy_32_16_1( Mpy_32_16_1( y3_y1, BASOP_Util_Divide3232_Scale( y3_y1, ftmp_den1, &q_div_2i ) ), (Word16) ( 0xF000 ) ); /* q_div_2i */
215 1165 : q_div_2i = sub( add( q_div_2i, shift1 ), 32 );
216 1165 : IF( q_div_2i < 0 )
217 : {
218 838 : y2i = L_shl( y2i, q_div_2i );
219 838 : q_div_2i = 0;
220 838 : move16();
221 838 : y2i = L_add( y2i, y2 ); /* Qx */
222 : }
223 : ELSE
224 : {
225 327 : y2i = L_add( y2i, L_shr( y2, q_div_2i ) ); /* q_div_2i */
226 : }
227 : /* their corresponding normalized locations */
228 1165 : posi = BASOP_Util_Divide3232_Scale( y3_y1, ( ftmp_den2 ), &q_div_posi ); /* q_div_posi */
229 1165 : q_div_posi = sub( add( q_div_posi, shift2 ), 32 );
230 1165 : IF( ( q_div_posi != 0 ) )
231 : {
232 1084 : posi = L_shl( posi, q_div_posi );
233 1084 : q_div_posi = 0;
234 1084 : move16();
235 : }
236 : /* Interpolated maxima if locations are not within [-1,1], calculated extrema are ignored */
237 1165 : test();
238 1165 : IF( GE_32( posi, L_shr( ONE_IN_Q15, q_div_posi ) ) || LE_32( posi, L_shr( MIN_16, q_div_posi ) ) )
239 : {
240 : // posi = GT_32( y3, y1 ) ? ONE_IN_Q15 : MIN_16;
241 0 : IF( GT_32( y3, y1 ) )
242 : {
243 0 : posi = ONE_IN_Q15; /* Q15 */
244 0 : move32();
245 : }
246 : ELSE
247 : {
248 0 : posi = MIN_16; /* Q15 */
249 0 : move32();
250 : }
251 : }
252 : ELSE
253 : {
254 1165 : IF( GE_32( L_shr( y1, q_div_2i ), y2i ) )
255 : {
256 : // posi = GT_32( y1, y3 ) ? MIN_16 : ONE_IN_Q15;
257 0 : IF( GT_32( y1, y3 ) )
258 : {
259 0 : posi = MIN_16; /* Q15 */
260 0 : move32();
261 : }
262 : ELSE
263 : {
264 0 : posi = ONE_IN_Q15; /* Q15 */
265 0 : move32();
266 : }
267 0 : move16();
268 : }
269 1165 : ELSE IF( GE_32( L_shr( y3, q_div_2i ), y2i ) )
270 : {
271 0 : posi = ONE_IN_Q15; /* Q15 */
272 0 : move16();
273 : }
274 : }
275 :
276 1165 : return L_add( posi, ONE_IN_Q15 );
277 : }
278 :
279 469 : static void ivas_peakfinder_fx(
280 : const Word32 *x0, /* i : vector from which the maxima will be found Qx*/
281 : const Word16 len0, /* i : length of input vector Q0*/
282 : Word16 *plocs, /* o : the indicies of the identified peaks in x0 Q0*/
283 : Word16 *cInd, /* o : number of identified peaks Q0*/
284 : const Word32 sel, /* i : The amount above surrounding data for a peak to be identified Q0*/
285 : const Word16 endpoints /* i : Flag to include endpoints in peak search Q0*/
286 : )
287 : {
288 : Word32 minMag, tempMag, leftMin;
289 : Word32 dx0[L_PROT48k_2], x[L_PROT48k_2 + 1], peakMag[MAX_PLOCS];
290 469 : Word16 k, i, len, tempLoc = 0, foundPeak, ii, xInd;
291 : Word16 *ind, indarr[L_PROT48k_2 + 1], peakLoc[MAX_PLOCS];
292 469 : move16();
293 :
294 469 : ind = indarr;
295 469 : move16();
296 :
297 : /* Find derivative */
298 469 : v_sub_32( x0 + 1, x0, dx0, sub( len0, 1 ) );
299 :
300 : /* This is so we find the first of repeated values */
301 9849 : FOR( i = 0; i < len0 - 1; i++ )
302 : {
303 9380 : IF( dx0[i] == 0 )
304 : {
305 605 : dx0[i] = -EPSILON_FX;
306 605 : move32();
307 : }
308 : }
309 :
310 : /* Find where the derivative changes sign
311 : Include endpoints in potential peaks and valleys */
312 469 : k = 0;
313 469 : move16();
314 :
315 469 : IF( endpoints )
316 : {
317 0 : x[k] = x0[0]; /* Qx */
318 0 : move32();
319 0 : ind[k] = 0;
320 0 : move16();
321 0 : k = add( k, 1 );
322 : }
323 :
324 9380 : FOR( i = 1; i < len0 - 1; i++ )
325 : {
326 8911 : IF( L_xor( dx0[i - 1], dx0[i] ) < 0 )
327 : {
328 3974 : ind[k] = i; /* Q0 */
329 3974 : move16();
330 3974 : x[k++] = x0[i]; /* Qx */
331 3974 : move32();
332 : }
333 : }
334 :
335 469 : IF( endpoints )
336 : {
337 0 : ind[k] = sub( len0, 1 ); /* Q0 */
338 0 : move16();
339 0 : x[k] = x0[len0 - 1]; /* Qx */
340 0 : move32();
341 0 : k = add( k, 1 );
342 : }
343 : /* x only has the peaks, valleys, and endpoints */
344 469 : len = k;
345 469 : move16();
346 469 : minimum_l( x, len, &minMag );
347 :
348 469 : test();
349 469 : test();
350 469 : IF( GT_16( len, 2 ) || ( !endpoints && ( len > 0 ) ) )
351 : {
352 : /* Set initial parameters for loop */
353 439 : tempMag = minMag; /* Q0 */
354 439 : move32();
355 439 : foundPeak = 0;
356 439 : move16();
357 439 : leftMin = minMag; /* Q0 */
358 439 : move32();
359 :
360 439 : IF( endpoints )
361 : {
362 : /* Deal with first point a little differently since tacked it on
363 : Calculate the sign of the derivative since we taked the first point
364 : on it does not necessarily alternate like the rest. */
365 :
366 : /* The first point is larger or equal to the second */
367 0 : IF( GE_32( x[0], x[1] ) )
368 : {
369 0 : ii = -1;
370 0 : move16();
371 0 : IF( GE_32( x[1], x[2] ) ) /* x[1] is not extremum -> overwrite with x[0] */
372 : {
373 0 : x[1] = x[0]; /* Qx */
374 0 : move32();
375 0 : ind[1] = ind[0]; /* Q0 */
376 0 : move16();
377 0 : ind++;
378 0 : len--;
379 : }
380 : }
381 : ELSE /* First point is smaller than the second */
382 : {
383 0 : ii = 0;
384 0 : move16();
385 0 : IF( LT_32( x[1], x[2] ) ) /* x[1] is not extremum -> overwrite with x[0] */
386 : {
387 0 : x[1] = x[0]; /* Qx */
388 0 : move32();
389 0 : ind[1] = ind[0]; /* Q0 */
390 0 : move16();
391 0 : ind++;
392 0 : len--;
393 : }
394 : }
395 : }
396 : ELSE
397 : {
398 439 : ii = -1; /* First point is a peak */
399 439 : move16();
400 439 : IF( GE_16( len, 2 ) )
401 : {
402 439 : IF( GE_32( x[1], x[0] ) )
403 : {
404 81 : ii = 0; /* First point is a valley, skip it */
405 81 : move16();
406 : }
407 : }
408 : }
409 439 : *cInd = 0;
410 439 : move16();
411 :
412 : /* Loop through extrema which should be peaks and then valleys */
413 2243 : WHILE( LT_16( ii, sub( len, 1 ) ) )
414 : {
415 2089 : ii++; /* This is a peak */
416 :
417 : /*Reset peak finding if we had a peak and the next peak is bigger
418 : than the last or the left min was small enough to reset.*/
419 2089 : IF( foundPeak )
420 : {
421 942 : tempMag = minMag; /* Q0 */
422 942 : move32();
423 942 : foundPeak = 0;
424 942 : move16();
425 : }
426 :
427 : /* Make sure we don't iterate past the length of our vector */
428 2089 : IF( EQ_16( ii, sub( len, 1 ) ) )
429 : {
430 285 : BREAK; /* We assign the last point differently out of the loop */
431 : }
432 :
433 : /* Found new peak that was larger than temp mag and selectivity larger
434 : than the minimum to its left. */
435 1804 : test();
436 1804 : IF( GT_32( x[ii], tempMag ) && GT_32( x[ii], L_add( leftMin, sel ) ) )
437 : {
438 1091 : tempLoc = ii; /* Q0 */
439 1091 : move16();
440 1091 : tempMag = x[ii]; /* Q0 */
441 1091 : move32();
442 : }
443 :
444 1804 : ii++; /* Move onto the valley */
445 :
446 : /* Come down at least sel from peak */
447 1804 : test();
448 1804 : IF( !foundPeak && GT_32( tempMag, L_add( sel, x[ii] ) ) )
449 : {
450 1075 : foundPeak = 1; /* We have found a peak */
451 1075 : move16();
452 1075 : leftMin = x[ii]; /* Qx */
453 1075 : move32();
454 1075 : peakLoc[*cInd] = tempLoc; /* Add peak to index Q0*/
455 1075 : move16();
456 1075 : peakMag[*cInd] = tempMag; /* Q0 */
457 1075 : move32();
458 1075 : ( *cInd ) = add( *( cInd ), 1 );
459 1075 : move16();
460 : }
461 729 : ELSE IF( LT_32( x[ii], leftMin ) ) /* New left minimum */
462 : {
463 267 : leftMin = x[ii]; /* Qx */
464 267 : move32();
465 : }
466 : }
467 :
468 : /* Check end point */
469 439 : test();
470 439 : test();
471 439 : IF( GT_32( x[len - 1], tempMag ) && GT_32( x[len - 1], L_add( leftMin, sel ) ) )
472 : {
473 85 : peakLoc[*cInd] = sub( len, 1 ); /* Q0 */
474 85 : move16();
475 85 : peakMag[*cInd] = x[len - 1]; /* Q0 */
476 85 : move32();
477 85 : ( *cInd )++;
478 : }
479 354 : ELSE IF( !foundPeak && GT_32( tempMag, minMag ) ) /* Check if we still need to add the last point */
480 : {
481 1 : peakLoc[*cInd] = tempLoc; /* Q0 */
482 1 : move16();
483 1 : peakMag[*cInd] = tempMag; /* Q0 */
484 1 : move32();
485 1 : ( *cInd ) = add( *( cInd ), 1 );
486 1 : move16();
487 : }
488 :
489 : /* Create output */
490 1600 : FOR( i = 0; i < *cInd; i++ )
491 : {
492 1161 : plocs[i] = ind[peakLoc[i]]; /* Q0 */
493 1161 : move16();
494 : }
495 : }
496 : ELSE
497 : {
498 30 : IF( endpoints )
499 : {
500 : /* This is a monotone function where an endpoint is the only peak */
501 : // xInd = GT_32( x[0], x[1] ) ? 0 : 1;
502 0 : IF( GT_32( x[0], x[1] ) )
503 : {
504 0 : xInd = 0;
505 0 : move16();
506 : }
507 : ELSE
508 : {
509 0 : xInd = 1;
510 0 : move16();
511 : }
512 0 : move16();
513 0 : peakMag[0] = x[xInd];
514 0 : move32();
515 0 : IF( GT_32( peakMag[0], L_add( minMag, sel ) ) )
516 : {
517 0 : plocs[0] = ind[xInd]; /* Q0 */
518 0 : move16();
519 0 : *cInd = 1;
520 0 : move16();
521 : }
522 : ELSE
523 : {
524 0 : *cInd = 0;
525 0 : move16();
526 : }
527 : }
528 : ELSE
529 : {
530 : /* Input constant or all zeros -- no peaks found */
531 30 : *cInd = 0;
532 30 : move16();
533 : }
534 : }
535 :
536 469 : return;
537 : }
538 :
539 :
540 : /*---------------------------------------------------------------
541 : * stereo_dft_res_subst_spec_fx()
542 : *
543 : * Generate error concealment frame in DFT domain
544 : * ---------------------------------------------------------------*/
545 :
546 1407 : void stereo_dft_res_subst_spec_fx(
547 : STEREO_DFT_DEC_DATA_HANDLE hStereoDft, /* i/o: Decoder DFT stereo handle */
548 : Word32 *pDFT_RES, /* i/o: residual signal qDFT*/
549 : const Word32 *const DFT_PRED_RES, /* i : residual prediction signal qDFT*/
550 : const Word16 time_offs, /* i : Time offset for phase adjustment Q0*/
551 : const Word16 L_res, /* i : bandwidth of residual signal Q0*/
552 : const Word16 L_ana, /* i : Length of FFT analysis Q0*/
553 : const Word16 k, /* i : Subframe index Q0*/
554 : Word16 *num_plocs, /* i/o: Number of peak locations Q0*/
555 : Word16 *plocs, /* i/o: Peak locations (bin) Q0*/
556 : Word32 *plocsi, /* i/o: Peak locations (fractional) Qx*/
557 : const Word16 analysis_flag /* i : Flag for running peak analysis Q0*/
558 : )
559 : {
560 : Word16 i, idx;
561 : // Word16 fac;
562 : Word32 s1, s2, abs1, abs2, abs3, abs4;
563 : Word32 abs_res[( STEREO_DFT_RES_BW_MAX ) / 2];
564 : Word32 Xmax, Xmin;
565 : Word32 sel;
566 : Word32 corr_phase;
567 : Word32 *p_mem;
568 : Word16 f_frac;
569 : Word32 peak_phase;
570 : Word32 phase_tmp;
571 : Word32 phase;
572 : Word32 conj_sign;
573 : Word16 Np;
574 : Word16 cos_F, sin_F;
575 :
576 : /* initialization */
577 1407 : Copy32( DFT_PRED_RES, pDFT_RES, 2 * L_res ); /* qDFT */
578 1407 : p_mem = hStereoDft->res_mem_fx; /* q_res_mem */
579 1407 : Np = 1;
580 1407 : move16();
581 :
582 1407 : IF( analysis_flag )
583 : {
584 469 : Word16 q_res = sub( getScaleFactor32( p_mem, shl( L_res, 1 ) ), 2 );
585 : /* Perform spectral analysis on 2nd subframe of last good frame */
586 469 : abs_res[0] = L_shr( Mpy_32_32( L_shl( p_mem[0], q_res ), L_shl( p_mem[0], q_res ) ), 1 ); /* DC */ /* 2 * q_res - 31 */
587 469 : move32();
588 9849 : FOR( i = 1; i < L_res; i++ )
589 : {
590 9380 : Word32 r = L_shl( p_mem[2 * i], q_res );
591 9380 : Word32 l = L_shl( p_mem[2 * i + 1], q_res );
592 9380 : abs_res[i] = Madd_32_32( Mpy_32_32( r, r ), l, l ); /* 2 * q_res - 31 */
593 9380 : move32();
594 : }
595 :
596 : /* Find maxima */
597 469 : maximum_l( abs_res, L_res, &Xmax );
598 469 : minimum_l( abs_res, L_res, &Xmin );
599 469 : sel = Mpy_32_16_1( L_sub( Xmax, Xmin ), (Word16) ( 0x03D7 ) /* Q15 */ ); /* 2 * q_res - 31 */
600 :
601 469 : ivas_peakfinder_fx( abs_res, L_res, plocs, num_plocs, sel, FALSE );
602 : /* Refine peaks */
603 1630 : FOR( i = 0; i < *num_plocs; i++ )
604 : {
605 1161 : IF( plocs[i] == 0 )
606 : {
607 0 : plocsi[i] = L_add( L_deposit_h( plocs[i] ), L_shl( imax_pos_fx( &abs_res[plocs[i]] ), Q1 ) ); /* Q16 */
608 0 : move32();
609 : }
610 1161 : ELSE IF( EQ_16( plocs[i], L_res ) )
611 : {
612 0 : plocsi[i] = L_add( L_deposit_h( sub( plocs[i], 2 ) ), L_shl( imax_pos_fx( &abs_res[plocs[i] - 2] ), Q1 ) ); /* Q16 */
613 0 : move32();
614 : }
615 : ELSE
616 : {
617 1161 : plocsi[i] = L_add( L_deposit_h( sub( plocs[i], 1 ) ), L_shl( imax_pos_fx( &abs_res[plocs[i] - 1] ), Q1 ) ); /* Q16 */
618 1161 : move16();
619 : }
620 : }
621 : }
622 :
623 : /* Apply phase of stereo filling on noise spectrum */
624 29547 : FOR( i = 1; i < L_res; i++ )
625 : {
626 28140 : s1 = sign_l( pDFT_RES[2 * i] );
627 28140 : s2 = sign_l( pDFT_RES[2 * i + 1] );
628 28140 : abs1 = L_abs( pDFT_RES[2 * i] ); /* qDFt */
629 28140 : abs2 = L_abs( pDFT_RES[2 * i + 1] ); /* qDFT */
630 28140 : abs3 = L_abs( p_mem[2 * i] ); /* q_res_mem */
631 28140 : abs4 = L_abs( p_mem[2 * i + 1] ); /* q_res_mem*/
632 :
633 : // fac = MAX_16;
634 :
635 : /* Low-complex phase matching that brings the angle within pi/4 of the target angle */
636 28140 : test();
637 28140 : test();
638 28140 : test();
639 28140 : IF( ( GT_32( abs1, abs2 ) && LT_32( abs3, abs4 ) ) || ( LE_32( abs1, abs2 ) && GE_32( abs3, abs4 ) ) )
640 : {
641 14879 : pDFT_RES[2 * i] = Mpy_32_32( s1, abs4 ); /* q_res - 31 */
642 14879 : move32();
643 14879 : pDFT_RES[2 * i + 1] = Mpy_32_32( s2, abs3 ); /* q_res - 31 */
644 14879 : move32();
645 : }
646 : ELSE
647 : {
648 13261 : pDFT_RES[2 * i] = Mpy_32_32( s1, abs3 ); /* q_res - 31 */
649 13261 : move32();
650 13261 : pDFT_RES[2 * i + 1] = Mpy_32_32( s2, abs4 ); /* q_res - 31 */
651 13261 : move32();
652 : }
653 : }
654 :
655 : /* Apply phase adjustment of identified peaks, including Np=1 peak neighbors on each side */
656 4890 : FOR( i = *num_plocs - 1; i >= 0; i-- )
657 : {
658 : Flag flg_ov;
659 3483 : IF( k == 0 )
660 : {
661 : Word32 op;
662 : Word16 q_div, q_shift;
663 : /* For 1st subframe, apply reversed time ECU to get correct analysis window */
664 2322 : f_frac = extract_l( L_shr( L_sub( plocsi[i], L_shl( plocs[i], Q16 ) ), 1 ) ); // Q15
665 2322 : peak_phase = BASOP_util_atan2( p_mem[2 * plocs[i] + 1], p_mem[2 * plocs[i]], 0 ); // Q13
666 2322 : phase_tmp = L_sub( peak_phase, mult( f_frac, STEREO_DFT_PLC_PH_C_FX ) ); // Q13
667 2322 : phase = L_sub( phase_tmp, mult( f_frac, EVS_PI_FX ) ); // Q13
668 2322 : op = L_negate( L_shl( phase, 1 ) ); // Q13
669 2322 : corr_phase = L_add( L_add( STEREO_DFT_PLC_STEP21, L_ana ), time_offs ); // Q0
670 2322 : q_shift = norm_l( corr_phase );
671 2322 : corr_phase = Mpy_32_32( L_shl( corr_phase, q_shift ), plocsi[i] ); // q_shift + Q16 - 31
672 2322 : corr_phase = L_deposit_l( BASOP_Util_Divide3232_Scale( corr_phase, L_ana, &q_div ) );
673 2322 : corr_phase = L_shl( corr_phase, sub( q_div, sub( q_shift, Q15 ) ) ); /* q_div */
674 2322 : corr_phase = L_negate( L_shl( Mpy_32_16_1( corr_phase, EVS_PI_FX ), Q1 ) ); /* Q13 */
675 2322 : corr_phase = L_add( op, corr_phase ); /* Q13 */
676 255360 : WHILE( LT_32( corr_phase, -EVS_PI_FX ) )
677 : {
678 253038 : corr_phase = L_add( L_add( corr_phase, EVS_PI_FX ), EVS_PI_FX ); /* Q13 */
679 : }
680 2322 : WHILE( GT_32( corr_phase, EVS_PI_FX ) )
681 : {
682 0 : corr_phase = L_sub( L_sub( corr_phase, EVS_PI_FX ), EVS_PI_FX ); /* Q13 */
683 : }
684 2322 : conj_sign = MIN_32;
685 2322 : move32();
686 : }
687 : ELSE
688 : {
689 : /* corr_phase = PI2 * ( L_ana + time_offs ) * ( plocsi[i] / L_ana ); */
690 1161 : Word32 op = L_add( L_ana, time_offs ); /* Q0 */
691 : Word16 q_div;
692 1161 : op = Mpy_32_32( L_shl( op, Q15 ), plocsi[i] ); /* Q0 */
693 1161 : corr_phase = BASOP_Util_Divide3232_Scale( op, L_ana, &q_div );
694 1161 : corr_phase = extract_l( L_and( L_shl( corr_phase, q_div ), MAX_16 ) ); /* Q13 */
695 : /* For 2nd subframe, do regular phase shift */
696 : /* Multiplying only with EVS_PI_Fx and taking care of 2 in shift */
697 1161 : corr_phase = ( L_shr( L_mult0( extract_l( corr_phase ), EVS_PI_FX ), Q14 /* Q13 + Q1*/ ) ); /* Q13 */
698 1161 : WHILE( LT_32( corr_phase, -EVS_PI_FX ) )
699 : {
700 0 : corr_phase = L_add( L_add( corr_phase, EVS_PI_FX ), EVS_PI_FX ); /* Q13 */
701 : }
702 1800 : WHILE( GT_32( corr_phase, EVS_PI_FX ) )
703 : {
704 639 : corr_phase = L_sub( L_sub( corr_phase, EVS_PI_FX ), EVS_PI_FX ); /* Q13 */
705 : }
706 1161 : conj_sign = MAX_32;
707 1161 : move32();
708 : }
709 :
710 3483 : cos_F = shl_o( getCosWord16( extract_l( corr_phase ) ), 1, &flg_ov );
711 3483 : sin_F = getSinWord16( extract_l( corr_phase ) );
712 :
713 3483 : idx = s_max( 0, sub( plocs[i], Np ) ); /* Iterate over plocs[i]-1:plocs[i]+1, considering the edges of the spectrum */
714 13932 : WHILE( ( idx < add( add( plocs[i], Np ), 1 ) ) && LT_16( idx, L_res ) )
715 : {
716 10449 : pDFT_RES[2 * idx] = Msub_32_16( Mpy_32_16_1( p_mem[2 * idx], cos_F ), p_mem[2 * idx + 1], sin_F ); /* q_res - 1 */
717 10449 : move32();
718 10449 : pDFT_RES[2 * idx + 1] = Mpy_32_32( conj_sign, Madd_32_16( Mpy_32_16_1( p_mem[2 * idx], sin_F ), p_mem[2 * idx + 1], cos_F ) ); /* q_res - 1 */
719 10449 : move32();
720 10449 : idx++;
721 : }
722 : }
723 1407 : return;
724 : }
725 :
726 :
727 : /*---------------------------------------------------------------
728 : * stereo_dft_res_ecu_burst_att_fx()
729 : *
730 : * scaling residual PLC in burst error, considering DMX PLC attenuation
731 : * ---------------------------------------------------------------*/
732 :
733 368 : void stereo_dft_res_ecu_burst_att_fx(
734 : STEREO_DFT_DEC_DATA_HANDLE hStereoDft, /* i/o: Decoder DFT stereo handle */
735 : Word32 *pDFT_RES, /* i/o: residual signal /att. residual qDFT*/
736 : const Word32 dmx_nrg, /* i : dmx energy of current frame Qx*/
737 : const Word16 L_res, /* i : Bandwidth of residual Q0*/
738 : const Word16 L_ana /* i : Length of FFT analysis Q0*/
739 : )
740 : {
741 : Word32 fac;
742 : Word16 q_fac;
743 : Word16 exponent;
744 :
745 368 : q_fac = 0;
746 368 : move16();
747 368 : exponent = 0;
748 368 : move16();
749 :
750 : /* attenuation of residual; follow attenuation of DMX */
751 368 : IF( hStereoDft->core_hist[0] == ACELP_CORE )
752 : {
753 90 : fac = Mpy_32_16_1( Sqrt32( L_deposit_h( BASOP_Util_Divide3232_Scale( dmx_nrg, hStereoDft->past_dmx_nrg_fx, &q_fac ) ), &exponent ), (Word16) ( 0x0CCD ) ); /* Q0 */
754 : }
755 : ELSE
756 : {
757 278 : fac = L_sub( MAX_32, L_deposit_h( BASOP_Util_Divide3232_Scale( L_sub( hStereoDft->time_offs, L_ana ), L_add( hStereoDft->time_offs, L_ana ), &q_fac ) ) ); /* Q0 */
758 : }
759 :
760 368 : v_multc_fixed( pDFT_RES, fac, pDFT_RES, shl( L_res, 1 ) );
761 :
762 368 : return;
763 : }
764 :
765 : /*---------------------------------------------------------------
766 : * stereo_dft_dmx_swb_nrg_fx()
767 : *
768 : * Calculate DMX energy
769 : * ---------------------------------------------------------------*/
770 :
771 : /*! r: total energy of downmix with maximum swb bandwidth max */
772 1367 : Word32 stereo_dft_dmx_swb_nrg_fx(
773 : const Word32 *dmx_k0, /* i : first subframe spectrum q0*/
774 : const Word32 *dmx_k1, /* i : second subframe spectrum q1*/
775 : const Word16 frame_length, /* i : frame lanegth Q0*/
776 : const Word16 q0,
777 : const Word16 q1 )
778 : {
779 : Word16 i;
780 : Word32 dmx_nrg;
781 :
782 1367 : dmx_nrg = EPSILON_FIX;
783 1367 : move32();
784 1367 : test();
785 1367 : IF( q0 == 0 && q1 == 0 )
786 : {
787 366106 : FOR( i = 0; i < frame_length / 2; i++ )
788 : {
789 729600 : dmx_nrg = L_add( dmx_nrg,
790 364800 : L_add( Madd_32_32( Mpy_32_32( dmx_k0[2 * i], dmx_k0[2 * i] ), dmx_k0[2 * i + 1], dmx_k0[2 * i + 1] ),
791 364800 : Madd_32_32( Mpy_32_32( dmx_k1[2 * i], dmx_k1[2 * i] ), dmx_k1[2 * i + 1], dmx_k1[2 * i + 1] ) ) ); /* Q0 */
792 : }
793 : }
794 : ELSE
795 : {
796 17661 : FOR( i = 0; i < frame_length / 2; i++ )
797 : {
798 35200 : dmx_nrg = L_add( dmx_nrg,
799 17600 : L_add( L_shl( Madd_32_32( Mpy_32_32( dmx_k0[2 * i], dmx_k0[2 * i] ), dmx_k0[2 * i + 1], dmx_k0[2 * i + 1] ), q0 ),
800 17600 : L_shl( Madd_32_32( Mpy_32_32( dmx_k1[2 * i], dmx_k1[2 * i] ), dmx_k1[2 * i + 1], dmx_k1[2 * i + 1] ), q1 ) ) ); /*3*q0 - 31*/
801 : }
802 : }
803 :
804 1367 : return dmx_nrg;
805 : }
806 :
807 : /*---------------------------------------------------------------
808 : * stereo_dft_sg_recovery_fx()
809 : *
810 : * estimates panning measure
811 : * updates recovery flag that might enbale recovery of side gain
812 : * ---------------------------------------------------------------*/
813 :
814 42382 : Word16 stereo_dft_sg_recovery_fx(
815 : STEREO_DFT_DEC_DATA_HANDLE hStereoDft /* i/o: Decoder DFT stereo handle */
816 : )
817 : {
818 : Word16 b;
819 : Word32 *pSideGain;
820 : Word32 sg_m;
821 : Word16 beta;
822 : Word16 exp_sg_m;
823 :
824 42382 : test();
825 42382 : IF( !hStereoDft->sg_mem_corrupt )
826 : {
827 41728 : pSideGain = hStereoDft->side_gain_fx + 2 * STEREO_DFT_BAND_MAX;
828 41728 : beta = (Word16) ( 0x3666 ); /* Q15 */
829 41728 : move16();
830 :
831 41728 : sg_m = EPSILON_FIX;
832 41728 : move32();
833 440430 : FOR( b = 0; b < hStereoDft->nbands; b++ )
834 : {
835 398702 : sg_m = L_add_sat( sg_m, L_shr( pSideGain[b], 5 ) ); /* Q26 */
836 : }
837 :
838 41728 : sg_m = L_deposit_l( BASOP_Util_Divide3216_Scale( sg_m, hStereoDft->nbands, &exp_sg_m ) );
839 41728 : exp_sg_m = add( exp_sg_m, -10 ); // exp_sg_m - (31 - Q26) - (15 - Q0)
840 :
841 41728 : sg_m = L_shl_sat( sg_m, sub( 31, sub( 15, exp_sg_m ) ) ); /* Q31 */
842 :
843 41728 : test();
844 41728 : IF( LT_32( sg_m, (Word32) 0x4CCCCCCD ) && GT_32( sg_m, (Word32) ( 0xB3333333 ) ) )
845 : {
846 40098 : hStereoDft->sg_mean_fx = 0; /* Q13 */
847 40098 : move32();
848 : }
849 : ELSE
850 : {
851 1630 : hStereoDft->sg_mean_fx = Madd_32_16( Mpy_32_16_1( sg_m, beta ), hStereoDft->sg_mean_fx, sub( MAX_16, beta ) ); /* LP filter delta_sg to obtain side gain stability measure */ /* Q31 */
852 1630 : move32();
853 : }
854 : }
855 654 : ELSE IF( GT_32( hStereoDft->sg_mean_fx, (Word32) 0x4CCCCCCD ) || LT_32( hStereoDft->sg_mean_fx, (Word32) ( 0xB3333333 ) ) )
856 : {
857 8 : return 1;
858 : }
859 :
860 42374 : return 0;
861 : }
|
