Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include <assert.h>
34 : #include <stdint.h>
35 : #include "options.h"
36 : #include <math.h>
37 : #include "cnst.h"
38 : #include "rom_enc.h"
39 : #include "rom_com.h"
40 : #include "prot_fx.h"
41 : #include "ivas_prot_fx.h"
42 : #include "ivas_cnst.h"
43 : #include "ivas_rom_com.h"
44 : #include "ivas_rom_enc.h"
45 : #include "wmc_auto.h"
46 :
47 :
48 : /*-------------------------------------------------------------------------
49 : * Local constants
50 : *------------------------------------------------------------------------*/
51 :
52 : #define LP_GCC_PHAT_UP 0.9f /* LP filter coefficient, going up */
53 : #define LP_GCC_PHAT_UP_Q31 1932735283 /* LP filter coefficient, going up */
54 : #define LP_GCC_PHAT_DOWN 0.1f /* LP filter coefficient, going down */
55 : #define LP_GCC_PHAT_DOWN_Q31 214748365 /* LP filter coefficient, going down */
56 : #define ITD_CNT_MAX 2 /* Minimum number of consecutive ITD estimates for ITD hangover */
57 : #define ITD_HO_GCC_PHAT_MAX 0.6f /* LP GCC PHAT value which gives zero hangover */
58 : #define ITD_HO_GCC_PHAT_MIN 0.3f /* LP GCC PHAT value which gives ITD_HO_MAX ITD hangover frames */
59 : #define ITD_HO_MAX 6 /* Maximum number of ITD hangover frames */
60 : #define ITD_HO_GCC_PHAT_INCL ( -( ITD_HO_MAX - 1 ) / ( ITD_HO_GCC_PHAT_MAX - ITD_HO_GCC_PHAT_MIN ) )
61 : #define ITD_HO_GCC_PHAT_INCL_Q26 -1118481021
62 : #define ITD_HO_GCC_PHAT_OFFS ( -ITD_HO_GCC_PHAT_INCL * ITD_HO_GCC_PHAT_MAX + 1 )
63 : #define ITD_HO_GCC_PHAT_OFFS_Q26 738197504
64 : #define SFM_PROD_GRP 4 /*maximum grouping of products for calculating SFM in ITD estimation*/
65 : #define B_DENOM 0.083333333333333f
66 : #define B_DENOM_Q31 178956971
67 : #define L_SAMPLES 20
68 : #define SUBDIV ( 2 * STEREO_DFT_ITD_MAX_ANA / L_SAMPLES )
69 : #define DENOM 0.05f
70 : #define DENOM_Q31 107374182
71 :
72 : #define XSPEC_ALPHA ( 1.f / 32 )
73 : #define XSPEC_ALPHA_Q31 ONE_IN_Q26
74 : #define CORR_FILT 0.8f
75 : #define CORR_FILT_Q31 1717986918
76 : #define CORR_RESET_FRAMES_MAX 20
77 :
78 : #define ITD_VAD_NOISE_INIT_FRAMES 30
79 : #define ITD_VAD_THRSHOLD 0.001f
80 : #define ITD_VAD_THRSHOLD_Q31 2147484
81 : #define ITD_VAD_MS_SNR_UPDATE_THRESH 15.0f
82 : #define ITD_VAD_MS_SNR_UPDATE_THRESH_FX 15
83 : #define HIGHT_SNR_VOICE_TH 10000.0f
84 : #define HIGHT_SNR_VOICE_TH_FX 10000
85 : #define MAX_ITD_VAD_HANGOVER 10
86 :
87 : #define XCORR_LB_NUM_BANDS 3
88 : #define ONE_BY_XCORR_LB_NUM_BANDS_Q31 715827883
89 : #define XCORR_LB_BAND_WIDTH 8
90 :
91 : #define ITD_MAX_MDCT 80
92 :
93 : #define LOG_E_2 1488522236
94 :
95 : /*-------------------------------------------------------------------------
96 : * set_band_limits()
97 : *
98 : * configure bands as used in DFT Stereo
99 : *------------------------------------------------------------------------*/
100 :
101 14690 : static void set_band_limits_fx(
102 : Word16 *nbands,
103 : Word16 band_limits[STEREO_DFT_BAND_MAX + 1],
104 : Word16 NFFT )
105 : {
106 14690 : band_limits[0] = 1;
107 14690 : move16();
108 14690 : *nbands = 0;
109 14690 : move16();
110 183900 : WHILE( band_limits[( *nbands )++] < ( NFFT / 2 ) )
111 : {
112 169210 : band_limits[*nbands] = extract_l( Mpy_32_32_r( shl( dft_band_limits_erb4[*nbands], 1 ), 1717986918 /*( (float) ( STEREO_DFT_N_NS_ENC ) / STEREO_DFT_N_NS ) in Q 30*/ ) );
113 169210 : move16();
114 : }
115 14690 : ( *nbands ) = sub( *nbands, 1 );
116 14690 : move16();
117 14690 : band_limits[*nbands] = shr( NFFT, 1 ); /*Nyquist Freq*/
118 14690 : move16();
119 :
120 14690 : return;
121 : }
122 :
123 : /*-------------------------------------------------------------------------
124 : * stereo_dft_hybrid_ITD_flag()
125 : *
126 : * Get the hybrid ITD flag
127 : *------------------------------------------------------------------------*/
128 :
129 60538 : void stereo_dft_hybrid_ITD_flag_fx(
130 : STEREO_DFT_CONFIG_DATA_HANDLE hConfig, /* o : DFT stereo configuration */
131 : const Word32 input_Fs, /* i : CPE element sampling rate */
132 : const Word16 hybrid_itd_max /* i : flag for hybrid ITD for very large ITDs */
133 : )
134 : {
135 60538 : IF( hConfig != NULL )
136 : {
137 60538 : test();
138 60538 : test();
139 60538 : test();
140 60538 : IF( hConfig->res_cod_mode || ( hConfig->ada_wb_res_cod_mode && EQ_32( input_Fs, 16000 ) ) || EQ_16( hybrid_itd_max, 1 ) )
141 : {
142 20739 : hConfig->hybrid_itd_flag = 1;
143 20739 : move16();
144 : }
145 : ELSE
146 : {
147 39799 : hConfig->hybrid_itd_flag = 0;
148 39799 : move16();
149 : }
150 : }
151 : ELSE
152 : {
153 0 : assert( 0 && "Stereo Dft Config Data Handle is uninitialized" );
154 : }
155 :
156 60538 : return;
157 : }
158 :
159 : /*-------------------------------------------------------------------------
160 : * stereo_dft_quantize_itd()
161 : *
162 : * Quantize the ITD
163 : *------------------------------------------------------------------------*/
164 :
165 74349 : static void stereo_dft_quantize_itd_fx(
166 : const Word16 in,
167 : Word32 *out, // Q16
168 : const Word32 input_Fs,
169 : Word16 *ind )
170 : {
171 : Word16 itd;
172 74349 : Word16 input_fs_by_stereo_dft_itd_fs = 0;
173 74349 : move16();
174 : // itd = (int16_t) ( sign( in ) * 0.5f + in );
175 74349 : itd = in;
176 74349 : move16();
177 :
178 : /*Limit ITD*/
179 : // if ( ( ABSVAL( itd ) > STEREO_DFT_ITD_MAX ) || ( ABSVAL( itd ) < STEREO_DFT_ITD_MIN ) )
180 74349 : test();
181 74349 : IF( GT_16( abs_s( itd ), STEREO_DFT_ITD_MAX ) || LT_16( abs_s( itd ), STEREO_DFT_ITD_MIN ) )
182 : {
183 40338 : itd = 0;
184 40338 : move16();
185 : }
186 : ELSE
187 : {
188 34011 : *ind = sub( add( shl( ( itd < 0 ), ( STEREO_DFT_ITD_NBITS - 1 ) ), abs_s( itd ) ), STEREO_DFT_ITD_MIN );
189 34011 : move16();
190 : }
191 :
192 74349 : SWITCH( input_Fs )
193 : {
194 0 : case 8000:
195 0 : input_fs_by_stereo_dft_itd_fs = 1; // Q2
196 0 : move16();
197 0 : BREAK;
198 16943 : case 16000:
199 16943 : input_fs_by_stereo_dft_itd_fs = 2; // Q2
200 16943 : move16();
201 16943 : BREAK;
202 32788 : case 32000:
203 32788 : input_fs_by_stereo_dft_itd_fs = 4; // Q2
204 32788 : move16();
205 32788 : BREAK;
206 24618 : case 48000:
207 24618 : input_fs_by_stereo_dft_itd_fs = 6; // Q2
208 24618 : move16();
209 24618 : BREAK;
210 0 : default:
211 0 : assert( 0 );
212 : }
213 : /*Convert back @ fs*/
214 : //*out = (float) ( itd * input_Fs ) / ( (float) ( STEREO_DFT_ITD_FS ) );
215 74349 : *out = L_shl( L_mult( itd, input_fs_by_stereo_dft_itd_fs ), Q13 ); // Q16
216 74349 : move32();
217 :
218 74349 : return;
219 : }
220 :
221 : /*-------------------------------------------------------------------------
222 : * itd_vad_ms_snr_calc()
223 : *
224 : *
225 : *-------------------------------------------------------------------------*/
226 :
227 74349 : static Word32 itd_vad_ms_snr_calc_fx(
228 : Word32 E_band_n[STEREO_DFT_ITD_VAD_BAND_NUM], // E_band_n_exp
229 : Word16 E_band_n_exp[STEREO_DFT_ITD_VAD_BAND_NUM],
230 : Word32 *Spd, // Q(31-Spd_e)
231 : Word16 *Spd_e,
232 : Word32 *E_band, // Q(31-E_band_e)
233 : Word16 *E_band_e,
234 : Word16 *ms_snr_e )
235 : {
236 : Word32 snr[STEREO_DFT_ITD_VAD_BAND_NUM];
237 : Word16 snr_e[STEREO_DFT_ITD_VAD_BAND_NUM];
238 : Word32 msnr[STEREO_DFT_ITD_VAD_BAND_NUM];
239 : Word16 msnr_e[STEREO_DFT_ITD_VAD_BAND_NUM];
240 : Word32 ms_snr;
241 : Word16 i, j;
242 : Word16 exp;
243 :
244 1561329 : FOR( i = 0; i < STEREO_DFT_ITD_VAD_BAND_NUM; i++ )
245 : {
246 1486980 : E_band[i] = 0;
247 1486980 : move32();
248 1486980 : E_band_e[i] = 0;
249 1486980 : move16();
250 24906915 : for ( j = itd_vad_band_tbl[i]; j < itd_vad_band_tbl[i + 1]; j++ )
251 : {
252 : // E_band[i] += Spd[j];
253 23419935 : E_band[i] = BASOP_Util_Add_Mant32Exp( E_band[i], E_band_e[i], Spd[j], Spd_e[j], &E_band_e[i] );
254 23419935 : move32();
255 : }
256 : // E_band[i] = E_band[i] / ( itd_vad_band_tbl[i + 1] - itd_vad_band_tbl[i] );
257 1486980 : E_band[i] = BASOP_Util_Divide3232_Scale_newton( E_band[i], L_sub( itd_vad_band_tbl[i + 1], itd_vad_band_tbl[i] ), &exp );
258 1486980 : move32();
259 1486980 : E_band_e[i] = add( exp, sub( E_band_e[i], 31 ) );
260 1486980 : move16();
261 : }
262 :
263 74349 : ms_snr = 0;
264 74349 : move32();
265 74349 : *ms_snr_e = 0;
266 74349 : move16();
267 1561329 : FOR( i = 0; i < STEREO_DFT_ITD_VAD_BAND_NUM; i++ )
268 : {
269 : // snr[i] = E_band[i] / E_band_n[i];
270 1486980 : snr[i] = BASOP_Util_Divide3232_Scale_newton( E_band[i], E_band_n[i], &snr_e[i] );
271 1486980 : move32();
272 1486980 : snr_e[i] = add( snr_e[i], sub( E_band_e[i], E_band_n_exp[i] ) );
273 1486980 : move16();
274 : // if ( snr[i] < 1 )
275 1486980 : IF( BASOP_Util_Cmp_Mant32Exp( snr[i], snr_e[i], 1, 31 ) < 0 )
276 : {
277 : // snr[i] = 1;
278 348467 : snr[i] = MAX_32;
279 348467 : move32();
280 348467 : snr_e[i] = 0;
281 348467 : move16();
282 : }
283 : // msnr[i] = snr[i] - 1.0f;
284 1486980 : msnr[i] = BASOP_Util_Add_Mant32Exp( snr[i], snr_e[i], MIN_32, 0, &msnr_e[i] );
285 1486980 : move32();
286 : // if ( msnr[i] < 6 )
287 1486980 : IF( BASOP_Util_Cmp_Mant32Exp( msnr[i], msnr_e[i], 6, 31 ) < 0 )
288 : {
289 : // msnr[i] = powf( msnr[i], 2 ) / 6.0f;
290 607438 : msnr[i] = Mpy_32_32( Mpy_32_32( msnr[i], msnr[i] ), 357913941 );
291 607438 : move32();
292 607438 : msnr_e[i] = shl( msnr_e[i], 1 );
293 607438 : move16();
294 : }
295 : // ms_snr += msnr[i];
296 1486980 : ms_snr = BASOP_Util_Add_Mant32Exp( ms_snr, *ms_snr_e, msnr[i], msnr_e[i], ms_snr_e );
297 : }
298 :
299 74349 : return ( ms_snr );
300 : }
301 :
302 : /*-------------------------------------------------------------------------
303 : * itd_vad_background_update()
304 : *
305 : *
306 : *-------------------------------------------------------------------------*/
307 74349 : static void itd_vad_background_update_fx(
308 : Word32 E_band_n[STEREO_DFT_ITD_VAD_BAND_NUM], // E_band_n_exp
309 : Word16 E_band_n_exp[STEREO_DFT_ITD_VAD_BAND_NUM],
310 : Word16 *vad_frm_cnt,
311 : const Word32 ms_snr, // Q(31-ms_snr_e)
312 : const Word16 ms_snr_e,
313 : Word32 *E_band, // Q(31-E_band_e)
314 : Word16 *E_band_e )
315 : {
316 : Word16 i;
317 : Word32 energy;
318 : Word16 energy_e;
319 :
320 : Word32 L_temp;
321 : Word16 L_temp_e;
322 :
323 : Word16 E_band_n_e_tmp;
324 :
325 74349 : energy = 0;
326 74349 : move32();
327 74349 : energy_e = 0;
328 74349 : move16();
329 1561329 : FOR( i = 0; i < STEREO_DFT_ITD_VAD_BAND_NUM; i++ )
330 : {
331 : // energy += E_band[i] / (float) STEREO_DFT_ITD_VAD_BAND_NUM;
332 1486980 : energy = BASOP_Util_Add_Mant32Exp( energy, energy_e, Mpy_32_32( E_band[i], 107374182 ), E_band_e[i], &energy_e ) /*/ (float) STEREO_DFT_ITD_VAD_BAND_NUM*/;
333 : }
334 :
335 74349 : IF( LT_16( *vad_frm_cnt, ITD_VAD_NOISE_INIT_FRAMES ) )
336 : {
337 10934 : ( *vad_frm_cnt ) = add( *vad_frm_cnt, 1 );
338 10934 : move16();
339 : }
340 :
341 74349 : test();
342 74349 : IF( LT_16( *vad_frm_cnt, ITD_VAD_NOISE_INIT_FRAMES ) && ( BASOP_Util_Cmp_Mant32Exp( energy, energy_e, 40000000, 31 ) < 0 ) )
343 : {
344 46473 : FOR( i = 0; i < STEREO_DFT_ITD_VAD_BAND_NUM; i++ )
345 : {
346 : /* using the init values as frame (-1) values */
347 : // E_band_n[i] = ( E_band_n[i] * ( (float) ( *vad_frm_cnt ) ) + E_band[i] ) / ( (float) ( *vad_frm_cnt + 1 ) );
348 44260 : Word16 q_temp = norm_l( *vad_frm_cnt );
349 44260 : L_temp = L_shl( *vad_frm_cnt, q_temp );
350 44260 : L_temp_e = sub( 31, q_temp );
351 44260 : L_temp = BASOP_Util_Add_Mant32Exp( Mpy_32_32( E_band_n[i], L_temp ), add( E_band_n_exp[i], L_temp_e ), E_band[i], E_band_e[i], &L_temp_e );
352 44260 : E_band_n[i] = BASOP_Util_Divide3232_Scale_newton( L_temp, L_add( *vad_frm_cnt, 1 ), &E_band_n_e_tmp );
353 44260 : move32();
354 44260 : E_band_n_exp[i] = add( E_band_n_e_tmp, sub( L_temp_e, 31 ) );
355 44260 : move16();
356 : }
357 : }
358 : ELSE
359 : {
360 : // if ( ms_snr < ITD_VAD_MS_SNR_UPDATE_THRESH )
361 72136 : IF( BASOP_Util_Cmp_Mant32Exp( ms_snr, ms_snr_e, ITD_VAD_MS_SNR_UPDATE_THRESH_FX, 31 ) < 0 )
362 : {
363 234171 : FOR( i = 0; i < STEREO_DFT_ITD_VAD_BAND_NUM; i++ )
364 : {
365 : // E_band_n[i] = 0.96f * E_band_n[i] + 0.04f * E_band[i];
366 223020 : E_band_n[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( 2061584302 /*0.96 in Q31*/, E_band_n[i] ), E_band_n_exp[i], Mpy_32_32( 85899346 /*0.04 in Q31*/, E_band[i] ), E_band_e[i], &E_band_n_e_tmp );
367 223020 : move32();
368 223020 : E_band_n_exp[i] = E_band_n_e_tmp;
369 223020 : move16();
370 : // if ( E_band_n[i] < 1.0f )
371 223020 : IF( BASOP_Util_Cmp_Mant32Exp( E_band_n[i], E_band_n_exp[i], MAX_32, 0 ) < 0 )
372 : {
373 0 : E_band_n[i] = MAX_32;
374 0 : move32();
375 0 : E_band_n_exp[i] = 0;
376 0 : move16();
377 : }
378 : }
379 : }
380 : }
381 :
382 74349 : return;
383 : }
384 : /*-------------------------------------------------------------------------
385 : * stereo_dft_enc_itd_vad()
386 : *
387 : *
388 : *-------------------------------------------------------------------------*/
389 :
390 74349 : static Word16 stereo_dft_enc_itd_vad_fx(
391 : Word32 E_band_n[STEREO_DFT_ITD_VAD_BAND_NUM], // E_band_n_exp
392 : Word16 E_band_n_exp[STEREO_DFT_ITD_VAD_BAND_NUM],
393 : Word16 *vad_frm_cnt,
394 : Word32 *Spd_L, // Q(31-Spd_L_e)
395 : Word16 *Spd_L_e,
396 : Word32 *Spd_R, // Q(31-Spd_R_e)
397 : Word16 *Spd_R_e,
398 : Word32 *mssnr, // Q(31-mssnr_e)
399 : Word16 *mssnr_e )
400 : {
401 : Word16 i;
402 : Word32 E_band[STEREO_DFT_ITD_VAD_BAND_NUM];
403 : Word16 E_band_e[STEREO_DFT_ITD_VAD_BAND_NUM];
404 : Word16 vad_flag_itd;
405 :
406 : Word32 Spd[STEREO_DFT_N_16k_ENC / 2 + 1];
407 : Word16 Spd_e[STEREO_DFT_N_16k_ENC / 2 + 1];
408 :
409 : /* Spd is later only used starting at itd_vad_band_tbl[0], so only compute values starting from there */
410 : /* -> this avoids uninitialized values in Spd_L and Spd_R at index 0 to be used */
411 23494284 : FOR( i = itd_vad_band_tbl[0]; i < STEREO_DFT_N_16k_ENC / 2; i++ )
412 : {
413 : // Spd[i] = 0.5f * ( Spd_L[i] + Spd_R[i] );
414 23419935 : Spd[i] = BASOP_Util_Add_Mant32Exp( Spd_L[i], Spd_L_e[i], Spd_R[i], Spd_R_e[i], &Spd_e[i] );
415 23419935 : move32();
416 23419935 : Spd_e[i] = sub( Spd_e[i], 1 );
417 23419935 : move16();
418 : }
419 74349 : *mssnr = itd_vad_ms_snr_calc_fx( E_band_n, E_band_n_exp, Spd, Spd_e, E_band, E_band_e, mssnr_e );
420 74349 : move32();
421 :
422 74349 : itd_vad_background_update_fx( E_band_n, E_band_n_exp, vad_frm_cnt, *mssnr, *mssnr_e, E_band, E_band_e );
423 : // if ( *mssnr < ITD_VAD_THRSHOLD )
424 74349 : IF( BASOP_Util_Cmp_Mant32Exp( *mssnr, *mssnr_e, ITD_VAD_THRSHOLD_Q31, 0 ) < 0 )
425 : {
426 5603 : vad_flag_itd = 0;
427 5603 : move16();
428 : }
429 : ELSE
430 : {
431 68746 : vad_flag_itd = 1;
432 68746 : move16();
433 : }
434 :
435 74349 : return ( vad_flag_itd );
436 : }
437 :
438 : /*-------------------------------------------------------------------------
439 : * calc_mean_E_ratio()
440 : *
441 : * calculates mean energy of main-to-background signal ratio
442 : *-------------------------------------------------------------------------*/
443 :
444 74349 : static Word32 calc_mean_E_ratio_fx(
445 : ITD_DATA_HANDLE hItd,
446 : Word16 nbands,
447 : Word16 band_limits[],
448 : const Word32 sfm, // Q31
449 : const Word32 nrg_L[STEREO_DFT_N_32k_ENC / 2], // Q(31-nrg_L_e)
450 : const Word16 nrg_L_e[STEREO_DFT_N_32k_ENC / 2],
451 : const Word32 nrg_R[STEREO_DFT_N_32k_ENC / 2], // Q(31-nrg_R_e)
452 : const Word16 nrg_R_e[STEREO_DFT_N_32k_ENC / 2],
453 : Word32 *total_mEr, // Q(31-total_mEr_e)
454 : Word16 *total_mEr_e,
455 : Word16 *sum_Er_e )
456 : {
457 : Word32 sum_xcorr[2];
458 : Word16 sum_xcorr_e[2];
459 : Word32 Sxcorr;
460 : Word16 Sxcorr_e;
461 : Word32 Er[STEREO_DFT_BAND_MAX], fi[STEREO_DFT_BAND_MAX], a, acorr;
462 : Word16 Er_e[STEREO_DFT_BAND_MAX], fi_e[STEREO_DFT_BAND_MAX], a_e, acorr_e;
463 : Word32 sum_nrg_L, sum_nrg_R;
464 : Word16 sum_nrg_L_e, sum_nrg_R_e;
465 : Word16 b, i;
466 : Word32 sum_Er;
467 : Word32 total_fi;
468 : Word16 total_fi_e;
469 : Word32 grand_nrg_L, grand_nrg_R, grand_sum_xcorr_real, grand_sum_xcorr_img;
470 : Word16 grand_nrg_L_e, grand_nrg_R_e, grand_sum_xcorr_real_e, grand_sum_xcorr_img_e;
471 :
472 : Word32 L_temp1, L_temp2;
473 : Word16 L_temp1_e, L_temp2_e;
474 :
475 : Word16 acorr_L_fx_tmp_e[STEREO_DFT_BAND_MAX];
476 74349 : set16_fx( acorr_L_fx_tmp_e, hItd->acorr_L_fx_e, nbands );
477 : Word16 acorr_R_fx_tmp_e[STEREO_DFT_BAND_MAX];
478 74349 : set16_fx( acorr_R_fx_tmp_e, hItd->acorr_R_fx_e, nbands );
479 :
480 74349 : grand_nrg_L = 0;
481 74349 : move32();
482 74349 : grand_nrg_L_e = 0;
483 74349 : move16();
484 74349 : grand_nrg_R = 0;
485 74349 : move32();
486 74349 : grand_nrg_R_e = 0;
487 74349 : move16();
488 74349 : grand_sum_xcorr_real = 0;
489 74349 : move32();
490 74349 : grand_sum_xcorr_real_e = 0;
491 74349 : move16();
492 74349 : grand_sum_xcorr_img = 0;
493 74349 : move32();
494 74349 : grand_sum_xcorr_img_e = 0;
495 74349 : move16();
496 :
497 : /*take bands up to 32kHz bandwidth as ITD is always calculated at 32kHz sampling rate*/
498 74349 : nbands = sub( nbands, ( band_limits[nbands] > STEREO_DFT_N_32k_ENC / 2 ) );
499 :
500 74349 : sum_Er = 0;
501 74349 : move32();
502 74349 : *sum_Er_e = 0;
503 74349 : move16();
504 776615 : FOR( b = 0; b < nbands; b++ )
505 : {
506 : /*reset buffers*/
507 702266 : sum_xcorr[0] = 0;
508 702266 : move32();
509 702266 : sum_xcorr_e[0] = 0;
510 702266 : move16();
511 702266 : sum_xcorr[1] = 0;
512 702266 : move32();
513 702266 : sum_xcorr_e[1] = 0;
514 702266 : move16();
515 702266 : sum_nrg_L = 0;
516 702266 : move32();
517 702266 : sum_nrg_L_e = 0;
518 702266 : move16();
519 702266 : sum_nrg_R = 0;
520 702266 : move32();
521 702266 : sum_nrg_R_e = 0;
522 702266 : move16();
523 :
524 702266 : Word16 len = s_min( band_limits[b + 1], STEREO_DFT_N_32k_ENC >> 1 );
525 42335437 : FOR( i = band_limits[b]; i < len; i++ )
526 : {
527 : // sum_xcorr[0] += hItd->xcorr_smooth[2 * i];
528 41633171 : sum_xcorr[0] = BASOP_Util_Add_Mant32Exp( sum_xcorr[0], sum_xcorr_e[0], hItd->xcorr_smooth_fx[2 * i], hItd->xcorr_smooth_fx_e[2 * i], &sum_xcorr_e[0] );
529 41633171 : move32();
530 : // sum_xcorr[1] += hItd->xcorr_smooth[2 * i + 1];
531 41633171 : sum_xcorr[1] = BASOP_Util_Add_Mant32Exp( sum_xcorr[1], sum_xcorr_e[1], hItd->xcorr_smooth_fx[2 * i + 1], hItd->xcorr_smooth_fx_e[2 * i + 1], &sum_xcorr_e[1] );
532 41633171 : move32();
533 : // sum_nrg_L += nrg_L[i];
534 41633171 : sum_nrg_L = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, nrg_L[i], nrg_L_e[i], &sum_nrg_L_e );
535 : // sum_nrg_R += nrg_R[i];
536 41633171 : sum_nrg_R = BASOP_Util_Add_Mant32Exp( sum_nrg_R, sum_nrg_R_e, nrg_R[i], nrg_R_e[i], &sum_nrg_R_e );
537 : }
538 :
539 : // Sxcorr = sum_xcorr[0] * sum_xcorr[0] + sum_xcorr[1] * sum_xcorr[1];
540 702266 : Sxcorr = BASOP_Util_Add_Mant32Exp( Mpy_32_32( sum_xcorr[0], sum_xcorr[0] ), shl( sum_xcorr_e[0], 1 ), Mpy_32_32( sum_xcorr[1], sum_xcorr[1] ), shl( sum_xcorr_e[1], 1 ), &Sxcorr_e );
541 : // hItd->acorr_L[b] = ( 1.f - sfm ) * hItd->acorr_L[b] + sfm * sum_nrg_L;
542 702266 : hItd->acorr_L_fx[b] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm ), hItd->acorr_L_fx[b] ), hItd->acorr_L_fx_e, Mpy_32_32( sfm, sum_nrg_L ), sum_nrg_L_e, &acorr_L_fx_tmp_e[b] );
543 702266 : move32();
544 : // hItd->acorr_R[b] = ( 1.f - sfm ) * hItd->acorr_R[b] + sfm * sum_nrg_R;
545 702266 : hItd->acorr_R_fx[b] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm ), hItd->acorr_R_fx[b] ), hItd->acorr_R_fx_e, Mpy_32_32( sfm, sum_nrg_R ), sum_nrg_R_e, &acorr_R_fx_tmp_e[b] );
546 702266 : move32();
547 :
548 : // a = hItd->acorr_L[b] - hItd->acorr_R[b];
549 702266 : a = BASOP_Util_Add_Mant32Exp( hItd->acorr_L_fx[b], acorr_L_fx_tmp_e[b], L_negate( hItd->acorr_R_fx[b] ), acorr_R_fx_tmp_e[b], &a_e );
550 : // acorr = hItd->acorr_L[b] + hItd->acorr_R[b];
551 702266 : acorr = BASOP_Util_Add_Mant32Exp( hItd->acorr_L_fx[b], acorr_L_fx_tmp_e[b], hItd->acorr_R_fx[b], acorr_R_fx_tmp_e[b], &acorr_e );
552 : // fi[b] = sqrtf( a * a + 4 * Sxcorr );
553 702266 : fi[b] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( a, a ), shl( a_e, 1 ), Sxcorr, add( Sxcorr_e, 2 ), &fi_e[b] );
554 702266 : move32();
555 702266 : fi[b] = Sqrt32( fi[b], &fi_e[b] );
556 702266 : move32();
557 : // Er[b] = ( acorr + fi[b] ) / ( acorr - fi[b] + EPSILON );
558 702266 : L_temp1 = BASOP_Util_Add_Mant32Exp( acorr, acorr_e, fi[b], fi_e[b], &L_temp1_e );
559 702266 : L_temp2 = BASOP_Util_Add_Mant32Exp( acorr, acorr_e, L_negate( fi[b] ), fi_e[b], &L_temp2_e );
560 702266 : L_temp2 = BASOP_Util_Add_Mant32Exp( L_temp2, L_temp2_e, EPSILON_FX_M, EPSILON_FX_E, &L_temp2_e );
561 702266 : Er[b] = BASOP_Util_Divide3232_Scale_newton( L_temp1, L_temp2, &Er_e[b] );
562 702266 : move32();
563 702266 : Er_e[b] = add( Er_e[b], sub( L_temp1_e, L_temp2_e ) );
564 702266 : move16();
565 : // sum_Er += Er[b];
566 702266 : sum_Er = BASOP_Util_Add_Mant32Exp( sum_Er, *sum_Er_e, Er[b], Er_e[b], sum_Er_e );
567 :
568 : // grand_nrg_L += sum_nrg_L;
569 702266 : grand_nrg_L = BASOP_Util_Add_Mant32Exp( grand_nrg_L, grand_nrg_L_e, sum_nrg_L, sum_nrg_L_e, &grand_nrg_L_e );
570 : // grand_nrg_R += sum_nrg_R;
571 702266 : grand_nrg_R = BASOP_Util_Add_Mant32Exp( grand_nrg_R, grand_nrg_R_e, sum_nrg_R, sum_nrg_R_e, &grand_nrg_R_e );
572 : // grand_sum_xcorr_real += sum_xcorr[0];
573 702266 : grand_sum_xcorr_real = BASOP_Util_Add_Mant32Exp( grand_sum_xcorr_real, grand_sum_xcorr_real_e, sum_xcorr[0], sum_xcorr_e[0], &grand_sum_xcorr_real_e );
574 : // grand_sum_xcorr_img += sum_xcorr[1];
575 702266 : grand_sum_xcorr_img = BASOP_Util_Add_Mant32Exp( grand_sum_xcorr_img, grand_sum_xcorr_img_e, sum_xcorr[1], sum_xcorr_e[1], &grand_sum_xcorr_img_e );
576 : }
577 :
578 : /*making common exp*/
579 74349 : Word16 max_exp1 = MIN_16, max_exp2 = MIN_16;
580 776615 : FOR( b = 0; b < nbands; b++ )
581 : {
582 702266 : max_exp1 = s_max( max_exp1, acorr_L_fx_tmp_e[b] );
583 702266 : max_exp2 = s_max( max_exp2, acorr_R_fx_tmp_e[b] );
584 : }
585 74349 : hItd->acorr_L_fx_e = max_exp1;
586 74349 : hItd->acorr_R_fx_e = max_exp2;
587 74349 : move16();
588 74349 : move16();
589 776615 : FOR( b = 0; b < nbands; b++ )
590 : {
591 702266 : hItd->acorr_L_fx[b] = L_shr_r( hItd->acorr_L_fx[b], sub( hItd->acorr_L_fx_e, acorr_L_fx_tmp_e[b] ) );
592 702266 : move32();
593 702266 : hItd->acorr_R_fx[b] = L_shr_r( hItd->acorr_R_fx[b], sub( hItd->acorr_R_fx_e, acorr_R_fx_tmp_e[b] ) );
594 702266 : move32();
595 : }
596 :
597 : // Sxcorr = grand_sum_xcorr_real * grand_sum_xcorr_real + grand_sum_xcorr_img * grand_sum_xcorr_img;
598 74349 : Sxcorr = BASOP_Util_Add_Mant32Exp( Mpy_32_32( grand_sum_xcorr_real, grand_sum_xcorr_real ), shl( grand_sum_xcorr_real_e, 1 ), Mpy_32_32( grand_sum_xcorr_img, grand_sum_xcorr_img ), shl( grand_sum_xcorr_img_e, 1 ), &Sxcorr_e );
599 : // a = grand_nrg_L - grand_nrg_R;
600 74349 : a = BASOP_Util_Add_Mant32Exp( grand_nrg_L, grand_nrg_L_e, L_negate( grand_nrg_R ), grand_nrg_R_e, &a_e );
601 : // acorr = grand_nrg_L + grand_nrg_R;
602 74349 : acorr = BASOP_Util_Add_Mant32Exp( grand_nrg_L, grand_nrg_L_e, grand_nrg_R, grand_nrg_R_e, &acorr_e );
603 : // total_fi = sqrtf( a * a + 4 * Sxcorr );
604 74349 : total_fi = BASOP_Util_Add_Mant32Exp( Mpy_32_32( a, a ), shl( a_e, 1 ), Sxcorr, add( Sxcorr_e, 2 ), &total_fi_e );
605 74349 : total_fi = Sqrt32( total_fi, &total_fi_e );
606 : //*total_mEr = ( acorr + total_fi ) / ( acorr - total_fi + EPSILON );
607 74349 : L_temp1 = BASOP_Util_Add_Mant32Exp( acorr, acorr_e, total_fi, total_fi_e, &L_temp1_e );
608 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( acorr, acorr_e, L_negate( total_fi ), total_fi_e, &L_temp2_e );
609 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( L_temp2, L_temp2_e, EPSILON_FX_M, EPSILON_FX_E, &L_temp2_e );
610 74349 : *total_mEr = BASOP_Util_Divide3232_Scale_newton( L_temp1, L_temp2, total_mEr_e );
611 74349 : move32();
612 74349 : *total_mEr_e = add( *total_mEr_e, sub( L_temp1_e, L_temp2_e ) );
613 74349 : move16();
614 :
615 : // return ( sum_Er * B_DENOM );
616 74349 : return Mpy_32_32( sum_Er, B_DENOM_Q31 );
617 : }
618 :
619 : /*-------------------------------------------------------------------------
620 : * resetEstimates()
621 : *
622 : * resets long term estimates to initial values.
623 : *-------------------------------------------------------------------------*/
624 0 : static void resetEstimates_fx(
625 : ITD_DATA_HANDLE hItd )
626 : {
627 0 : set32_fx( hItd->xcorr_smooth_fx, 0, STEREO_DFT_N_32k_ENC );
628 0 : set16_fx( hItd->xcorr_smooth_fx_e, 0, STEREO_DFT_N_32k_ENC );
629 0 : set32_fx( hItd->acorr_L_fx, 0, STEREO_DFT_BAND_MAX );
630 0 : hItd->acorr_L_fx_e = 0;
631 0 : move16();
632 0 : set32_fx( hItd->acorr_R_fx, 0, STEREO_DFT_BAND_MAX );
633 0 : hItd->acorr_R_fx_e = 0;
634 0 : move16();
635 0 : hItd->cohSNR_fx = 983040; // 15.0f in Q16
636 0 : move32();
637 :
638 0 : return;
639 : }
640 : /*-------------------------------------------------------------------------
641 : * td_sm_filter()
642 : *
643 : * time-domain smoothing filter for smoothing the cross-correlation vector
644 : *-------------------------------------------------------------------------*/
645 13348 : static void td_sm_filter_fx(
646 : Word32 *x, // Q0
647 : Word32 *y, // Q0
648 : const Word16 L )
649 : {
650 : Word16 i;
651 : Word32 tmp_x[STEREO_DFT_N_32k_ENC + 1];
652 : // Word16 a0, a1;
653 :
654 13348 : set32_fx( tmp_x, 0, STEREO_DFT_N_32k_ENC + 1 );
655 13348 : Copy32( x, tmp_x, L );
656 :
657 : // a0 = 0.5f;
658 : // a1 = 0.25f;
659 :
660 : // y[0] = a0 * tmp_x[0] + a1 * x[1];
661 13348 : y[0] = L_add( L_shr_r( tmp_x[0], 1 ), L_shr_r( x[1], 2 ) );
662 13348 : move32();
663 5352548 : FOR( i = 1; i < L; i++ )
664 : {
665 : // y[i] = a1 * tmp_x[i + 1] + a0 * tmp_x[i] + a1 * tmp_x[i - 1];
666 5339200 : y[i] = L_add( L_add( L_shr_r( tmp_x[i + 1], 2 ), L_shr_r( tmp_x[i], 1 ) ), L_shr_r( tmp_x[i - 1], 2 ) );
667 5339200 : move32();
668 : }
669 :
670 13348 : return;
671 : }
672 : /*-------------------------------------------------------------------------
673 : * peak_detect()
674 : *
675 : * function for calculating the threshold for peak detection of the
676 : * cross-correlation vector
677 : *-------------------------------------------------------------------------*/
678 74349 : static Word32 peak_detect_fx(
679 : Word32 *xcorr_itd,
680 : Word32 *max_max, // xcorr_itd_e
681 : Word16 *index,
682 : Word16 *zero_itd_flag,
683 : const Word32 snr, // Q16
684 : const Word16 vad,
685 : Word32 *second_max,
686 : Word16 *second_max_lag,
687 : const Word16 prev_itd, // Q0
688 : const Word16 flag_noisy_speech_snr,
689 : const Word16 detected_itd_flag,
690 : Word32 *prev_max, // Q31
691 : Word16 *prev_index,
692 : Word32 *prev_avg_max, // Q(31-prev_avg_max)
693 : Word16 *prev_avg_max_e,
694 : Word32 *total_max, // xcorr_itd_e
695 : Word16 *out_e )
696 : {
697 : Word16 i;
698 : Word32 tmp_max[SUBDIV], tmp_xcorr_itd[2 * STEREO_DFT_ITD_MAX_ANA + 1], tmp_xcorr_itd_sm[2 * STEREO_DFT_ITD_MAX_ANA + 1];
699 : Word16 index_subd[SUBDIV], ind;
700 : Word32 avg_max, max_low, max_high, sum_max, tmp_max_max;
701 : Word16 sum_max_e;
702 : Word32 thres_diff;
703 : Word16 thres_diff_e;
704 : Word32 wfac; // Q29
705 : Word16 d, i1, i2;
706 :
707 74349 : wfac = 1342177280; // 2.5f in Q29
708 74349 : move32();
709 : // if ( snr > 50.f )
710 74349 : IF( BASOP_Util_Cmp_Mant32Exp( snr, 15, 50, 31 ) > 0 )
711 : {
712 7138 : wfac = 1610612736; // 3.f in Q29
713 7138 : move32();
714 : }
715 :
716 : /*detect maxima outside the [-5, 5] ms boundaries */
717 74349 : maximum_l( xcorr_itd, STEREO_DFT_N_32k_ENC / 2 - STEREO_DFT_ITD_MAX_ANA, &max_low );
718 74349 : maximum_l( xcorr_itd + STEREO_DFT_N_32k_ENC / 2 + STEREO_DFT_ITD_MAX_ANA + 1, STEREO_DFT_N_32k_ENC / 2 - STEREO_DFT_ITD_MAX_ANA - 1, &max_high );
719 :
720 : /* create temp buffer that includes xcorr within [-5, 5] ms */
721 74349 : Copy32( xcorr_itd + STEREO_DFT_N_32k_ENC / 2 - STEREO_DFT_ITD_MAX_ANA, tmp_xcorr_itd, 2 * STEREO_DFT_ITD_MAX_ANA + 1 ); // Q31
722 :
723 74349 : *index = maximumAbs_l( tmp_xcorr_itd, 2 * STEREO_DFT_ITD_MAX_ANA + 1, max_max );
724 74349 : move16();
725 74349 : *total_max = *max_max; // Q31
726 74349 : move32();
727 :
728 : // d = max( 2, (int16_t) round_f( fabsf( prev_itd ) / 16.f ) );
729 74349 : d = s_max( 2, shr_r( abs_s( prev_itd ), 4 ) );
730 : // i1 = max( 0, (int16_t) prev_itd + STEREO_DFT_ITD_MAX_ANA - d );
731 74349 : i1 = s_max( 0, add( prev_itd, sub( STEREO_DFT_ITD_MAX_ANA, d ) ) );
732 : // i2 = min( 2 * STEREO_DFT_ITD_MAX_ANA, (int16_t) prev_itd + STEREO_DFT_ITD_MAX_ANA + d );
733 74349 : i2 = s_min( 2 * STEREO_DFT_ITD_MAX_ANA, add( prev_itd, add( STEREO_DFT_ITD_MAX_ANA, d ) ) );
734 : //*second_max_lag = maximumAbs( tmp_xcorr_itd + i1, i2 - i1 + 1, second_max );
735 74349 : *second_max_lag = maximumAbs_l( tmp_xcorr_itd + i1, i2 - i1 + 1, second_max ); // Q31
736 74349 : move16();
737 74349 : *second_max_lag = add( *second_max_lag, i1 );
738 74349 : move16();
739 :
740 :
741 : /*if maximum is out of boundaries signal zero itd OR maximum value is negative*/
742 74349 : test();
743 74349 : *zero_itd_flag = ( L_sub( *max_max, 4294968 /*.002 Q31*/ ) < max_low || *max_max < max_high );
744 74349 : move16();
745 : // adjustment is done for matching processing path with float code (differing due to precision loss)
746 :
747 74349 : IF( *zero_itd_flag )
748 : {
749 2421 : *out_e = 0;
750 2421 : move16();
751 2421 : return 0;
752 : }
753 : ELSE
754 : {
755 71928 : sum_max = 0;
756 71928 : move32();
757 71928 : sum_max_e = 0;
758 71928 : move16();
759 :
760 : // if ( snr >= 25.f )
761 71928 : IF( BASOP_Util_Cmp_Mant32Exp( snr, 15, 25, 31 ) >= 0 )
762 : {
763 : /*apply smoothing filter*/
764 13348 : td_sm_filter_fx( tmp_xcorr_itd, tmp_xcorr_itd_sm, 2 * STEREO_DFT_ITD_MAX_ANA + 1 ); // Q31
765 :
766 : /*subdivide the area of interest and look for local maxima*/
767 266960 : FOR( i = 0; i < SUBDIV - 1; i++ )
768 : {
769 253612 : index_subd[i] = maximumAbs_l( &tmp_xcorr_itd_sm[i * L_SAMPLES], L_SAMPLES, &tmp_max[i] );
770 253612 : move16();
771 : // sum_max += tmp_max[i];
772 253612 : sum_max = BASOP_Util_Add_Mant32Exp( sum_max, sum_max_e, tmp_max[i], 0, &sum_max_e );
773 : }
774 :
775 13348 : index_subd[i] = maximumAbs_l( &tmp_xcorr_itd_sm[i * L_SAMPLES], L_SAMPLES + 1, &tmp_max[i] );
776 13348 : move16();
777 : // sum_max += tmp_max[i];
778 13348 : sum_max = BASOP_Util_Add_Mant32Exp( sum_max, sum_max_e, tmp_max[i], 0, &sum_max_e );
779 :
780 13348 : ind = maximumAbs_l( tmp_max, SUBDIV, &tmp_max_max );
781 :
782 : /*final position of maxmimum*/
783 13348 : *index = add( index_subd[ind], imult1616( ind, L_SAMPLES ) );
784 13348 : move16();
785 13348 : *max_max = tmp_max_max;
786 13348 : move16();
787 : /*calculate average of all maxima to determine the threshold*/
788 : // avg_max = sum_max * DENOM;
789 13348 : avg_max = Mpy_32_32( sum_max, DENOM_Q31 ); // sum_max_e
790 :
791 : // d = max( 2, (int16_t) round_f( fabsf( prev_itd ) / 16.f ) );
792 13348 : d = s_max( 2, shr_r( abs_s( prev_itd ), 4 ) );
793 : // i1 = max( 0, (int16_t) prev_itd + STEREO_DFT_ITD_MAX_ANA - d );
794 13348 : i1 = s_max( 0, add( prev_itd, sub( STEREO_DFT_ITD_MAX_ANA, d ) ) );
795 : // i2 = min( 2 * STEREO_DFT_ITD_MAX_ANA, (int16_t) prev_itd + STEREO_DFT_ITD_MAX_ANA + d );
796 13348 : i2 = s_min( 2 * STEREO_DFT_ITD_MAX_ANA, add( prev_itd, add( STEREO_DFT_ITD_MAX_ANA, d ) ) );
797 13348 : *second_max_lag = maximumAbs_l( tmp_xcorr_itd_sm + i1, add( sub( i2, i1 ), 1 ), second_max );
798 13348 : move16();
799 13348 : *second_max_lag = add( *second_max_lag, i1 );
800 13348 : move16();
801 : }
802 : ELSE
803 : {
804 : /*determine weight for threshold depending on snr value*/
805 : // if ( snr <= 20.f && snr > 15.f )
806 58580 : test();
807 58580 : IF( ( BASOP_Util_Cmp_Mant32Exp( snr, 15, 20, 31 ) <= 0 ) && ( BASOP_Util_Cmp_Mant32Exp( snr, 15, 15, 31 ) > 0 ) )
808 : {
809 : // wfac = snr * 0.1f + 0.5f;
810 19641 : wfac = L_add( Mpy_32_32( L_shl( snr, 10 ) /*Q16+10=Q26*/, 1717986918 /*0.1 in Q34*/ ) /*Q26*Q34=Q29*/, ONE_IN_Q28 );
811 : }
812 : ELSE
813 : {
814 38939 : wfac = 1342177280; // 2.5f in Q29
815 38939 : move16();
816 : }
817 :
818 1171600 : FOR( i = 0; i < SUBDIV - 1; i++ )
819 : {
820 1113020 : index_subd[i] = maximumAbs_l( &tmp_xcorr_itd[i * L_SAMPLES], L_SAMPLES, &tmp_max[i] );
821 1113020 : move16();
822 : // sum_max += tmp_max[i];
823 1113020 : sum_max = BASOP_Util_Add_Mant32Exp( sum_max, sum_max_e, tmp_max[i], 0, &sum_max_e );
824 : }
825 :
826 58580 : index_subd[i] = maximumAbs_l( &tmp_xcorr_itd[i * L_SAMPLES], L_SAMPLES + 1, &tmp_max[i] );
827 58580 : move16();
828 : // sum_max += tmp_max[i];
829 58580 : sum_max = BASOP_Util_Add_Mant32Exp( sum_max, sum_max_e, tmp_max[i], 0, &sum_max_e );
830 :
831 : /*calculate average of all maxima to determine the threshold*/
832 : // avg_max = sum_max * DENOM;
833 58580 : avg_max = Mpy_32_32( sum_max, DENOM_Q31 ); // sum_max_e
834 : }
835 :
836 : /*relax threshold if threshold is very close to max: when 7<snr<=15 and (thres-max)<0.05 or 15<snr<30 and (thres-max)<0.01*/
837 :
838 : // thres_diff = wfac * avg_max - *max_max;
839 71928 : thres_diff = BASOP_Util_Add_Mant32Exp( Mpy_32_32( wfac, avg_max ), add( sum_max_e, 2 ), L_negate( *max_max ), 0, &thres_diff_e );
840 :
841 : // if ( vad && thres_diff > 0.f && ( ( thres_diff < 0.05f && ( snr <= 15 && snr > 7.f ) ) || ( thres_diff < 0.01f && ( snr > 15.f && snr < 30.f ) ) ) )
842 71928 : test();
843 71928 : test();
844 71928 : test();
845 71928 : test();
846 71928 : test();
847 71928 : test();
848 71928 : test();
849 71928 : if ( vad && thres_diff > 0 && ( ( BASOP_Util_Cmp_Mant32Exp( thres_diff, thres_diff_e, 107374182 /*.05 in Q31*/, 0 ) < 0 && ( BASOP_Util_Cmp_Mant32Exp( snr, 15, 15, 31 ) <= 0 && BASOP_Util_Cmp_Mant32Exp( snr, 15, 7, 31 ) > 0 ) ) || ( BASOP_Util_Cmp_Mant32Exp( thres_diff, thres_diff_e, 21474836 /* 0.01f in Q31*/, 0 ) < 0 && ( BASOP_Util_Cmp_Mant32Exp( snr, 15, 15, 31 ) > 0 && BASOP_Util_Cmp_Mant32Exp( snr, 15, 30, 31 ) < 0 ) ) ) )
850 : {
851 2298 : wfac = ONE_IN_Q30; // 2.0f
852 2298 : move32();
853 : }
854 :
855 71928 : IF( EQ_16( flag_noisy_speech_snr, 1 ) )
856 : {
857 37496 : test();
858 37496 : test();
859 37496 : test();
860 37496 : IF( vad == 0 )
861 : {
862 14060 : wfac = 1342177280; // 2.5f in Q29
863 14060 : move32();
864 : }
865 : // else if ( detected_itd_flag == 0 && *max_max > 1.5f * avg_max && *prev_max > 1.5f * *prev_avg_max && abs( *index - *prev_index ) <= 2 )
866 23436 : ELSE IF( detected_itd_flag == 0 && BASOP_Util_Cmp_Mant32Exp( *max_max, 0, Mpy_32_32( 1610612736 /*1.5f in Q29*/, avg_max ), add( sum_max_e, 1 ) ) > 0 && BASOP_Util_Cmp_Mant32Exp( *prev_max, 0, Mpy_32_32( 1610612736 /*1.5f in Q29*/, *prev_avg_max ), add( *prev_avg_max_e, 1 ) ) > 0 && LE_16( abs_s( sub( *index, *prev_index ) ), 2 ) )
867 : {
868 137 : wfac = 805306368; // 1.5f in Q29
869 137 : move32();
870 : }
871 : ELSE
872 : {
873 23299 : wfac = ONE_IN_Q30; // 2.0fin Q29
874 23299 : move32();
875 : }
876 : }
877 71928 : *prev_max = *max_max;
878 71928 : move32();
879 71928 : *prev_avg_max = avg_max;
880 71928 : move32();
881 71928 : *prev_avg_max_e = sum_max_e;
882 71928 : move16();
883 71928 : *prev_index = *index;
884 71928 : move16();
885 :
886 : // return ( wfac * avg_max );
887 71928 : *out_e = add( 2, sum_max_e );
888 71928 : move16();
889 71928 : return Mpy_32_32( wfac, avg_max ); // 2+sum_max_e
890 : }
891 : }
892 :
893 : /*-------------------------------------------------------------------------
894 : * Compute stereo parameter: ITD
895 : * ITD: Interchannel Time Difference
896 : *------------------------------------------------------------------------*/
897 74349 : void stereo_dft_enc_compute_itd_fx(
898 : CPE_ENC_HANDLE hCPE, /* i/o: CPE encoder structure */
899 : Word32 *DFT_L, /* i : exp : DFT_L_e */
900 : Word16 *DFT_L_e,
901 : Word32 *DFT_R, /* i : exp : DFT_R_e */
902 : Word16 *DFT_R_e,
903 : const Word16 k_offset,
904 : const Word16 input_frame,
905 : const Word16 vad_flag_dtx[],
906 : const Word16 vad_hover_flag[],
907 : Word32 *bin_nrgL, /* i : exp : bin_nrgL_e */
908 : Word16 *bin_nrgL_e,
909 : Word32 *bin_nrgR, /* i : exp : bin_nrgR_e */
910 : Word16 *bin_nrgR_e )
911 : {
912 : Word16 i, j;
913 : STEREO_DFT_ENC_DATA_HANDLE hStereoDft;
914 : ITD_DATA_HANDLE hItd;
915 : Word32 *pDFT_L, *pDFT_R;
916 : // Word32 DFT_L_tmp[STEREO_DFT_N_MAX_ENC], DFT_R_tmp[STEREO_DFT_N_MAX_ENC];
917 : // Word16 DFT_L_e_tmp[STEREO_DFT_N_MAX_ENC], DFT_R_e_tmp[STEREO_DFT_N_MAX_ENC];
918 : Word32 abs_L, abs_R, prod_L, prod_R, sum_abs_L, sum_abs_R;
919 : Word16 abs_L_e, abs_R_e, prod_L_e, prod_R_e, sum_abs_L_e, sum_abs_R_e;
920 : Word32 log_prod_L, log_prod_R;
921 : Word16 log_prod_L_e, log_prod_R_e;
922 : Word32 sum_nrg_L, sum_nrg_R;
923 : Word16 sum_nrg_L_e, sum_nrg_R_e;
924 : Word32 sfm_L, sfm_R;
925 : Word16 sfm_L_e, sfm_R_e;
926 : Word32 xcorr[STEREO_DFT_N_32k_ENC];
927 : Word16 xcorr_e[STEREO_DFT_N_32k_ENC];
928 : Word16 xcorr_e_final;
929 : Word16 itd, itd_td;
930 : Word32 xcorr_itd[STEREO_DFT_N_32k_ENC];
931 : // Word16 xcorr_itd_e[STEREO_DFT_N_32k_ENC];
932 : Word32 tmpf1, tmpf2, tmpf3;
933 : Word16 tmpf1_e, tmpf2_e, tmpf3_e;
934 : Word32 thres, alpha;
935 : Word16 thres_e;
936 : Word16 index;
937 : Word32 xcorr_max, sum_nrg_L_lb, par_L[XCORR_LB_NUM_BANDS], par_L_avrg, sum_nrg_L_tmp;
938 : Word16 xcorr_max_e, sum_nrg_L_lb_e, par_L_e[XCORR_LB_NUM_BANDS], /* par_L_avrg_e,*/ sum_nrg_L_tmp_e;
939 : Word32 xcorr_lb[STEREO_DFT_XCORR_LB_MAX];
940 : Word16 xcorr_lb_e[STEREO_DFT_XCORR_LB_MAX];
941 : Word32 num_cor, den_cor_cur, den_cor_prev, cor_lb_avrg;
942 : Word16 num_cor_e, den_cor_cur_e, den_cor_prev_e /*, cor_lb_avrg_e*/;
943 : Word32 cor_lb[XCORR_LB_NUM_BANDS];
944 : Word16 cor_lb_e[XCORR_LB_NUM_BANDS];
945 : Word32 Spd_L[STEREO_DFT_N_32k_ENC / 2 + 1];
946 : Word16 Spd_L_e[STEREO_DFT_N_32k_ENC / 2 + 1];
947 : Word32 Spd_R[STEREO_DFT_N_32k_ENC / 2 + 1];
948 : Word16 Spd_R_e[STEREO_DFT_N_32k_ENC / 2 + 1];
949 : Word16 vad_flag_itd;
950 : Word32 mssnr;
951 : Word16 mssnr_e;
952 : Word16 itd_cal_flag;
953 : Word16 NFFT, NFFT_mid;
954 : Word16 zero_itd;
955 : Word32 mEr;
956 : Word16 mEr_e;
957 : Word32 cohSNR; // Q16
958 : Word32 *pNrgL, *pNrgR;
959 : Word16 *pNrgL_e, *pNrgR_e;
960 : Word32 second_max;
961 : Word16 second_max_lag;
962 : Word16 fc_condition_1, fc_condition_2, fc_condition_3, fc_condition_4, fc_condition_5, fc_condition_6_a, fc_condition_6_b, fc_condition_6_c;
963 : Word16 fc_condition_1234;
964 : Word16 split, shift, flag_noisy_speech_snr;
965 : Word32 gcc_phat[2 * XTALK_PHAT_LEN + 1];
966 : Word32 grand_dot_prod_real, grand_dot_prod_img;
967 : Word16 grand_dot_prod_real_e, grand_dot_prod_img_e;
968 : Word32 xcorr_abs[STEREO_DFT_N_32k_ENC], sum_xcorr, prod_LL, prod_RR, total_mEr, total_max;
969 : Word16 xcorr_abs_e[STEREO_DFT_N_32k_ENC], sum_xcorr_e, prod_LL_e, prod_RR_e, total_mEr_e;
970 : STEREO_CLASSIF_HANDLE hStereoClassif;
971 : const Word16 *dft_trigo32k;
972 : Word16 trigo_enc[STEREO_DFT_N_32k_ENC / 2 + 1];
973 :
974 : Word32 cng_xcorr_filt;
975 : Word16 cng_xcorr_filt_e;
976 :
977 : Word16 prev_itd_max;
978 : Word16 itd_max_flip;
979 :
980 : Word32 L_temp;
981 : Word16 L_temp_e;
982 : Word32 L_temp2;
983 : Word16 L_temp2_e;
984 : Word16 max_exp;
985 : Word16 q_temp;
986 : Word64 W_temp;
987 : Word16 W_temp_q;
988 : Word64 W_temp1;
989 : Word16 W_temp_q1;
990 : Word32 tmp;
991 : Word16 exp;
992 74349 : IF( EQ_16( hCPE->element_mode, IVAS_CPE_DFT ) )
993 : {
994 59659 : hStereoDft = hCPE->hStereoDft;
995 59659 : hItd = hCPE->hStereoDft->hItd;
996 59659 : NFFT = s_min( STEREO_DFT_N_32k_ENC, hStereoDft->NFFT );
997 59659 : dft_trigo32k = hStereoDft->dft_trigo_32k_fx;
998 : }
999 : ELSE
1000 : {
1001 14690 : hStereoDft = NULL;
1002 14690 : hItd = hCPE->hStereoMdct->hItd;
1003 14690 : NFFT = s_min( STEREO_DFT_N_32k_ENC, hCPE->hStereoMdct->hDft_ana->NFFT );
1004 14690 : dft_trigo32k = hCPE->hStereoMdct->hDft_ana->dft_trigo_32k_fx;
1005 : }
1006 74349 : hStereoClassif = hCPE->hStereoClassif;
1007 :
1008 23866029 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC / 4; i++ )
1009 : {
1010 23791680 : trigo_enc[i] = dft_trigo32k[i];
1011 23791680 : move16();
1012 23791680 : trigo_enc[STEREO_DFT_N_32k_ENC / 2 - i] = dft_trigo32k[i];
1013 23791680 : move16();
1014 : }
1015 74349 : trigo_enc[STEREO_DFT_N_32k_ENC / 4] = dft_trigo32k[STEREO_DFT_N_32k_ENC / 4];
1016 74349 : move16();
1017 :
1018 74349 : flag_noisy_speech_snr = hCPE->hCoreCoder[0]->flag_noisy_speech_snr; /* flag from the previous frame */
1019 74349 : move16();
1020 :
1021 : /* initializations to avoid compilation warnings */
1022 74349 : sum_nrg_L = 0;
1023 74349 : move32();
1024 74349 : sum_nrg_L_e = 0;
1025 74349 : move16();
1026 74349 : sum_nrg_R = 0;
1027 74349 : move32();
1028 74349 : sum_nrg_R_e = 0;
1029 74349 : move16();
1030 74349 : sum_nrg_L_lb = 0;
1031 74349 : move32();
1032 74349 : sum_nrg_L_lb_e = 0;
1033 74349 : move16();
1034 74349 : mssnr = 0;
1035 74349 : move32();
1036 74349 : mssnr_e = 0;
1037 74349 : move16();
1038 74349 : sfm_L = 0;
1039 74349 : move32();
1040 74349 : sfm_L_e = 0;
1041 74349 : move16();
1042 :
1043 :
1044 74349 : NFFT_mid = shr( s_min( STEREO_DFT_N_16k_ENC, NFFT ), 1 );
1045 :
1046 74349 : pDFT_L = DFT_L;
1047 74349 : pDFT_R = DFT_R;
1048 :
1049 74349 : pNrgL = bin_nrgL;
1050 74349 : pNrgL_e = bin_nrgL_e;
1051 74349 : pNrgR = bin_nrgR;
1052 74349 : pNrgR_e = bin_nrgR_e;
1053 74349 : xcorr[0] = 0;
1054 74349 : move32();
1055 74349 : xcorr_e[0] = 0;
1056 74349 : move16();
1057 74349 : xcorr[1] = 0;
1058 74349 : move32();
1059 74349 : xcorr_e[1] = 0;
1060 74349 : move16();
1061 : // log_prod_L = logf( max( FLT_MIN, ABSVAL( pDFT_L[0] ) ) );
1062 74349 : IF( L_abs( pDFT_L[0] ) <= 0 )
1063 : {
1064 0 : log_prod_L = -1465264128; /* logf(FLT_MIN) in Q24 */
1065 0 : move32();
1066 0 : log_prod_L_e = 7;
1067 0 : move16();
1068 : }
1069 : ELSE
1070 : {
1071 : // log_prod_L = Mpy_32_32( L_add( BASOP_Util_Log2( L_abs( pDFT_L[0] ) ), L_shl( DFT_L_e_tmp[0], 25 ) ), LOG_E_2 );
1072 74349 : log_prod_L = BASOP_Util_Loge( L_abs( pDFT_L[0] ), DFT_L_e[0] );
1073 74349 : log_prod_L_e = 6;
1074 74349 : move16();
1075 : }
1076 :
1077 : // log_prod_R = logf( max( FLT_MIN, ABSVAL( pDFT_R[0] ) ) );
1078 74349 : IF( L_abs( pDFT_R[0] ) <= 0 )
1079 : {
1080 0 : log_prod_R = -1465264128; /* logf(FLT_MIN) in Q24 */
1081 0 : move32();
1082 0 : log_prod_R_e = 7;
1083 0 : move16();
1084 : }
1085 : ELSE
1086 : {
1087 : // log_prod_R = Mpy_32_32( L_add( BASOP_Util_Log2( L_abs( pDFT_R[0] ) ), L_shl( DFT_R_e_tmp[0], 25 ) ), LOG_E_2 );
1088 74349 : log_prod_R = BASOP_Util_Loge( L_abs( pDFT_R[0] ), DFT_R_e[0] );
1089 74349 : log_prod_R_e = 6;
1090 74349 : move16();
1091 : }
1092 :
1093 74349 : prod_L = MAX_32; // Q31
1094 74349 : move32();
1095 74349 : prod_L_e = 0;
1096 74349 : move16();
1097 74349 : prod_R = MAX_32; // Q31
1098 74349 : move32();
1099 74349 : prod_R_e = 0;
1100 74349 : move16();
1101 74349 : sum_nrg_L = Mpy_32_32( pDFT_L[0], pDFT_L[0] ) /*+ FLT_MIN Q(31-(2*DFT_L_e))*/;
1102 74349 : sum_nrg_L_e = shl( DFT_L_e[0], 1 );
1103 74349 : sum_nrg_R = Mpy_32_32( pDFT_R[0], pDFT_R[0] ) /*+ FLT_MIN //Q(31-(2*DFT_L_e))*/;
1104 74349 : sum_nrg_R_e = shl( DFT_R_e[0], 1 );
1105 74349 : sum_abs_L = BASOP_Util_Add_Mant32Exp( L_abs( pDFT_L[0] ), DFT_L_e[0], EPSILON_FX_M, EPSILON_FX_E, &sum_abs_L_e ) /*+ EPSILON*/;
1106 74349 : sum_abs_R = BASOP_Util_Add_Mant32Exp( L_abs( pDFT_R[0] ), DFT_R_e[0], EPSILON_FX_M, EPSILON_FX_E, &sum_abs_R_e ) /*+ EPSILON*/;
1107 74349 : xcorr_lb[0] = Mpy_32_32( pDFT_L[0], pDFT_L[0] ); // Q(31-(2*DFT_L_e))
1108 74349 : move32();
1109 74349 : xcorr_lb_e[0] = shl( DFT_L_e[0], 1 );
1110 74349 : move16();
1111 74349 : xcorr_lb[0] = BASOP_Util_Add_Mant32Exp( xcorr_lb[0], xcorr_lb_e[0], EPSILON_FX_M, EPSILON_FX_E, &xcorr_lb_e[0] ) /*+ EPSILON*/;
1112 74349 : move32();
1113 74349 : sum_nrg_L_lb = xcorr_lb[0];
1114 74349 : sum_nrg_L_lb_e = xcorr_lb_e[0];
1115 74349 : prod_LL = MAX_32; // Q31
1116 74349 : move32();
1117 74349 : prod_LL_e = 0;
1118 74349 : move16();
1119 74349 : prod_RR = MAX_32; // Q31
1120 74349 : move32();
1121 74349 : prod_RR_e = 0;
1122 74349 : move16();
1123 74349 : grand_dot_prod_real = EPSILON_FX_M;
1124 74349 : move32();
1125 74349 : grand_dot_prod_real_e = EPSILON_FX_E;
1126 74349 : move16();
1127 74349 : grand_dot_prod_img = EPSILON_FX_M;
1128 74349 : move32();
1129 74349 : grand_dot_prod_img_e = EPSILON_FX_E;
1130 74349 : move16();
1131 :
1132 : /* This initialization is added in fixed pt code, float code needs to be checked for this! */
1133 74349 : set32_fx( Spd_L, 0, STEREO_DFT_N_32k_ENC / 2 + 1 );
1134 74349 : set16_fx( Spd_L_e, 0, STEREO_DFT_N_32k_ENC / 2 + 1 );
1135 74349 : set32_fx( Spd_R, 0, STEREO_DFT_N_32k_ENC / 2 + 1 );
1136 74349 : set16_fx( Spd_R_e, 0, STEREO_DFT_N_32k_ENC / 2 + 1 );
1137 :
1138 74349 : j = 0; // for loop statement
1139 74349 : move16();
1140 23791680 : FOR( i = 1; i < NFFT_mid; i++ )
1141 : {
1142 : /*if ( i == 121 )
1143 : {
1144 : i = i;
1145 : }*/
1146 : // xcorr[2 * i] = pDFT_L[2 * i] * pDFT_R[2 * i] + pDFT_L[2 * i + 1] * pDFT_R[2 * i + 1];
1147 23717331 : W_temp = W_mult0_32_32( pDFT_L[2 * i], pDFT_R[2 * i] ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i])
1148 23717331 : W_temp_q = W_norm( W_temp );
1149 23717331 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] + DFT_R_e[2 * i])
1150 23717331 : W_temp_q = add( Q30, sub( W_temp_q, add( DFT_L_e[2 * i], DFT_R_e[2 * i] ) ) );
1151 23717331 : W_temp1 = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_R[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1])
1152 23717331 : W_temp_q1 = W_norm( W_temp1 );
1153 23717331 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1])
1154 23717331 : W_temp_q1 = add( Q30, sub( W_temp_q1, add( DFT_L_e[2 * i + 1], DFT_R_e[2 * i + 1] ) ) );
1155 23717331 : xcorr[2 * i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &xcorr_e[2 * i] );
1156 23717331 : move32();
1157 : // xcorr[2 * i + 1] = pDFT_L[2 * i + 1] * pDFT_R[2 * i] - pDFT_L[2 * i] * pDFT_R[2 * i + 1];
1158 23717331 : W_temp = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_R[2 * i] ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i])
1159 23717331 : W_temp_q = W_norm( W_temp );
1160 23717331 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i])
1161 23717331 : W_temp_q = add( Q30, sub( W_temp_q, add( DFT_L_e[2 * i + 1], DFT_R_e[2 * i] ) ) );
1162 23717331 : W_temp1 = W_mult0_32_32( L_negate( pDFT_L[2 * i] ), pDFT_R[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1])
1163 23717331 : W_temp_q1 = W_norm( W_temp1 );
1164 23717331 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1])
1165 23717331 : W_temp_q1 = add( Q30, sub( W_temp_q1, add( DFT_L_e[2 * i], DFT_R_e[2 * i + 1] ) ) );
1166 23717331 : xcorr[2 * i + 1] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &xcorr_e[2 * i + 1] );
1167 23717331 : move32();
1168 :
1169 : // pNrgL[i] = pDFT_L[2 * i] * pDFT_L[2 * i] + pDFT_L[2 * i + 1] * pDFT_L[2 * i + 1];
1170 23717331 : W_temp = W_mult0_32_32( pDFT_L[2 * i], pDFT_L[2 * i] ); // Q62 - (DFT_L_e[2 * i] * 2)
1171 23717331 : W_temp_q = W_norm( W_temp );
1172 23717331 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] * 2)
1173 23717331 : W_temp_q = add( Q30, sub( W_temp_q, shl( DFT_L_e[2 * i], 1 ) ) );
1174 23717331 : W_temp1 = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_L[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i + 1] * 2)
1175 23717331 : W_temp_q1 = W_norm( W_temp1 );
1176 23717331 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i + 1] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] * 2)
1177 23717331 : W_temp_q1 = add( Q30, sub( W_temp_q1, shl( DFT_L_e[2 * i + 1], 1 ) ) );
1178 23717331 : pNrgL[i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &pNrgL_e[i] );
1179 23717331 : move32();
1180 : // pNrgR[i] = pDFT_R[2 * i] * pDFT_R[2 * i] + pDFT_R[2 * i + 1] * pDFT_R[2 * i + 1];
1181 23717331 : W_temp = W_mult0_32_32( pDFT_R[2 * i], pDFT_R[2 * i] ); // Q62 - (DFT_R_e[2 * i] * 2)
1182 23717331 : W_temp_q = W_norm( W_temp );
1183 23717331 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_R_e[2 * i] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_R_e[2 * i] * 2)
1184 23717331 : W_temp_q = add( Q30, sub( W_temp_q, shl( DFT_R_e[2 * i], 1 ) ) );
1185 23717331 : W_temp1 = W_mult0_32_32( pDFT_R[2 * i + 1], pDFT_R[2 * i + 1] ); // Q62 - (DFT_R_e[2 * i + 1] * 2)
1186 23717331 : W_temp_q1 = W_norm( W_temp1 );
1187 23717331 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_R_e[2 * i + 1] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_R_e[2 * i + 1] * 2)
1188 23717331 : W_temp_q1 = add( Q30, sub( W_temp_q1, shl( DFT_R_e[2 * i + 1], 1 ) ) );
1189 23717331 : pNrgR[i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &pNrgR_e[i] );
1190 23717331 : move32();
1191 :
1192 23717331 : Spd_L[i] = pNrgL[i];
1193 23717331 : move32();
1194 23717331 : Spd_L_e[i] = pNrgL_e[i];
1195 23717331 : move16();
1196 23717331 : Spd_R[i] = pNrgR[i];
1197 23717331 : move32();
1198 23717331 : Spd_R_e[i] = pNrgR_e[i];
1199 23717331 : move16();
1200 :
1201 23717331 : abs_L_e = pNrgL_e[i];
1202 23717331 : move16();
1203 23717331 : abs_L = Sqrt32( pNrgL[i], &abs_L_e );
1204 23717331 : abs_R_e = pNrgR_e[i];
1205 23717331 : move16();
1206 23717331 : abs_R = Sqrt32( pNrgR[i], &abs_R_e );
1207 :
1208 : // sum_nrg_L += pNrgL[i];
1209 23717331 : sum_nrg_L = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, pNrgL[i], pNrgL_e[i], &sum_nrg_L_e );
1210 : // sum_nrg_R += pNrgR[i];
1211 23717331 : sum_nrg_R = BASOP_Util_Add_Mant32Exp( sum_nrg_R, sum_nrg_R_e, pNrgR[i], pNrgR_e[i], &sum_nrg_R_e );
1212 :
1213 : // sum_abs_L += abs_L;
1214 23717331 : sum_abs_L = BASOP_Util_Add_Mant32Exp( sum_abs_L, sum_abs_L_e, abs_L, abs_L_e, &sum_abs_L_e );
1215 : // sum_abs_R += abs_R;
1216 23717331 : sum_abs_R = BASOP_Util_Add_Mant32Exp( sum_abs_R, sum_abs_R_e, abs_R, abs_R_e, &sum_abs_R_e );
1217 : // prod_L *= abs_L;
1218 23717331 : W_temp = W_mult0_32_32( prod_L, abs_L ); // Q62 - (prod_L_e + abs_L_e)
1219 23717331 : W_temp_q = W_norm( W_temp );
1220 23717331 : prod_L = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (prod_L_e + abs_L_e) + (W_temp_q - Q32) = Q30 + W_temp_q - (prod_L_e + abs_L_e)
1221 23717331 : W_temp_q = add( Q30, sub( W_temp_q, add( prod_L_e, abs_L_e ) ) );
1222 23717331 : prod_L_e = sub( Q31, W_temp_q );
1223 : // prod_R *= abs_R;
1224 23717331 : W_temp = W_mult0_32_32( prod_R, abs_R ); // Q62 - (prod_R_e + abs_R_e)
1225 23717331 : W_temp_q = W_norm( W_temp );
1226 23717331 : prod_R = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (prod_R_e + abs_R_e) + (W_temp_q - Q32) = Q30 + W_temp_q - (prod_R_e + abs_R_e)
1227 23717331 : W_temp_q = add( Q30, sub( W_temp_q, add( prod_R_e, abs_R_e ) ) );
1228 23717331 : prod_R_e = sub( Q31, W_temp_q );
1229 :
1230 : // grand_dot_prod_real += xcorr[2 * i];
1231 23717331 : grand_dot_prod_real = BASOP_Util_Add_Mant32Exp( grand_dot_prod_real, grand_dot_prod_real_e, xcorr[2 * i], xcorr_e[2 * i], &grand_dot_prod_real_e );
1232 : // grand_dot_prod_img += xcorr[2 * i + 1];
1233 23717331 : grand_dot_prod_img = BASOP_Util_Add_Mant32Exp( grand_dot_prod_img, grand_dot_prod_img_e, xcorr[2 * i + 1], xcorr_e[2 * i + 1], &grand_dot_prod_img_e );
1234 :
1235 : // xcorr_abs[i] = sqrtf( xcorr[2 * i] * xcorr[2 * i] + xcorr[2 * i + 1] * xcorr[2 * i + 1] );
1236 23717331 : xcorr_abs[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( xcorr[2 * i], xcorr[2 * i] ), add( xcorr_e[2 * i], xcorr_e[2 * i] ), Mpy_32_32( xcorr[2 * i + 1], xcorr[2 * i + 1] ), add( xcorr_e[2 * i + 1], xcorr_e[2 * i + 1] ), &xcorr_abs_e[i] );
1237 23717331 : move32();
1238 23717331 : xcorr_abs[i] = Sqrt32( xcorr_abs[i], &xcorr_abs_e[i] );
1239 23717331 : move32();
1240 : /* VM: prod_LL tends to overflow, better to replace with sum(log(prod_L)) and retrain the classifier */
1241 23717331 : prod_LL = prod_L;
1242 23717331 : move32();
1243 23717331 : prod_LL_e = prod_L_e;
1244 23717331 : move16();
1245 23717331 : prod_RR = prod_R;
1246 23717331 : move32();
1247 23717331 : prod_RR_e = prod_R_e;
1248 23717331 : move16();
1249 :
1250 23717331 : test();
1251 23717331 : IF( EQ_16( j, SFM_PROD_GRP ) || EQ_16( i, sub( NFFT_mid, 1 ) ) )
1252 : {
1253 5947920 : prod_L = L_max( 0, prod_L );
1254 5947920 : prod_R = L_max( 0, prod_R );
1255 5947920 : IF( prod_L <= 0 )
1256 : {
1257 0 : L_temp = -1465264128; /* logf(FLT_MIN) in Q24 */
1258 0 : move32();
1259 0 : L_temp_e = 7;
1260 0 : move16();
1261 : }
1262 : ELSE
1263 : {
1264 : // L_temp = Mpy_32_32( L_add( BASOP_Util_Log2( prod_L ), L_shl( prod_L_e, 25 ) ), LOG_E_2 );
1265 5947920 : q_temp = norm_l( prod_L_e );
1266 5947920 : L_temp = Mpy_32_32( BASOP_Util_Add_Mant32Exp( BASOP_Util_Log2( prod_L ), 6, L_shl( prod_L_e, q_temp ), sub( 31, q_temp ), &L_temp_e ), LOG_E_2 );
1267 : }
1268 : // log_prod_L += logf( prod_L );
1269 5947920 : log_prod_L = BASOP_Util_Add_Mant32Exp( log_prod_L, log_prod_L_e, L_temp, L_temp_e, &log_prod_L_e );
1270 :
1271 5947920 : IF( L_abs( prod_R ) <= 0 )
1272 : {
1273 0 : L_temp = -1465264128; /* logf(FLT_MIN) in Q24 */
1274 0 : move32();
1275 0 : L_temp_e = 7;
1276 0 : move16();
1277 : }
1278 : ELSE
1279 : {
1280 : // L_temp = Mpy_32_32( L_add( BASOP_Util_Log2( prod_R ), L_shl( prod_R_e, 25 ) ), LOG_E_2 );
1281 5947920 : q_temp = norm_l( prod_R_e );
1282 5947920 : L_temp = Mpy_32_32( BASOP_Util_Add_Mant32Exp( BASOP_Util_Log2( prod_R ), 6, L_shl( prod_R_e, q_temp ), sub( 31, q_temp ), &L_temp_e ), LOG_E_2 );
1283 : }
1284 : // log_prod_R += logf( prod_R );
1285 5947920 : log_prod_R = BASOP_Util_Add_Mant32Exp( log_prod_R, log_prod_R_e, L_temp, L_temp_e, &log_prod_R_e );
1286 :
1287 5947920 : prod_L = MAX_32;
1288 5947920 : move32();
1289 5947920 : prod_L_e = 0;
1290 5947920 : move16();
1291 5947920 : prod_R = MAX_32;
1292 5947920 : move32();
1293 5947920 : prod_R_e = 0;
1294 5947920 : move16();
1295 :
1296 5947920 : j = 0;
1297 5947920 : move16();
1298 : }
1299 23717331 : j = add( j, 1 ); // for loop statement
1300 : }
1301 :
1302 : /* collect UNCLR classifier parameters */
1303 : {
1304 : Word32 IPD, d_IPD, g_IPD, g_ILD, angle_rot, g_side, g_pred, abs_L_R, grand_nrg_DMX;
1305 : Word16 /*IPD_e, */ d_IPD_e, g_IPD_e, g_ILD_e, /* angle_rot_e,*/ g_side_e, g_pred_e, abs_L_R_e, grand_nrg_DMX_e;
1306 :
1307 74349 : IF( NE_16( hCPE->last_element_mode, IVAS_CPE_DFT ) )
1308 : {
1309 : // hStereoClassif->prev_g_IPD = 0.5f;
1310 15449 : hStereoClassif->prev_g_IPD_fx = ONE_IN_Q28;
1311 15449 : move32();
1312 : // hStereoClassif->prev_IPD = 0.0f;
1313 15449 : hStereoClassif->prev_IPD_fx = 0;
1314 15449 : move16();
1315 : }
1316 :
1317 : // abs_L_R = sqrtf( grand_dot_prod_real * grand_dot_prod_real + grand_dot_prod_img * grand_dot_prod_img );
1318 74349 : abs_L_R = BASOP_Util_Add_Mant32Exp( Mpy_32_32( grand_dot_prod_real, grand_dot_prod_real ), shl( grand_dot_prod_real_e, 1 ), Mpy_32_32( grand_dot_prod_img, grand_dot_prod_img ), shl( grand_dot_prod_img_e, 1 ), &abs_L_R_e );
1319 74349 : abs_L_R = Sqrt32( abs_L_R, &abs_L_R_e );
1320 :
1321 : // grand_nrg_DMX = sum_nrg_L + sum_nrg_R + 2 * abs_L_R;
1322 74349 : L_temp = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, sum_nrg_R, sum_nrg_R_e, &L_temp_e );
1323 74349 : grand_nrg_DMX = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, abs_L_R, add( abs_L_R_e, 1 ), &grand_nrg_DMX_e );
1324 :
1325 : // g_ILD = sqrtf( sum_nrg_L / ( sum_nrg_R + 1.0f ) );
1326 74349 : L_temp = BASOP_Util_Add_Mant32Exp( sum_nrg_R, sum_nrg_R_e, MAX_32, 0, &L_temp_e );
1327 74349 : g_ILD = BASOP_Util_Divide3232_Scale_newton( sum_nrg_L, L_temp, &g_ILD_e );
1328 74349 : g_ILD_e = add( g_ILD_e, sub( sum_nrg_L_e, L_temp_e ) );
1329 74349 : g_ILD = Sqrt32( g_ILD, &g_ILD_e );
1330 :
1331 : // g_ILD = fabsf( ( g_ILD - 1 ) / ( g_ILD + 1 ) );
1332 74349 : L_temp = BASOP_Util_Add_Mant32Exp( g_ILD, g_ILD_e, MIN_32, 0, &L_temp_e );
1333 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( g_ILD, g_ILD_e, MAX_32, 0, &L_temp2_e );
1334 74349 : g_ILD = L_abs( BASOP_Util_Divide3232_Scale_newton( L_temp, L_temp2, &g_ILD_e ) );
1335 74349 : g_ILD_e = add( g_ILD_e, sub( L_temp_e, L_temp2_e ) );
1336 74349 : g_ILD = L_shl_sat( g_ILD, g_ILD_e ); // Q31
1337 :
1338 74349 : hStereoClassif->unclr_fv_fx[E_gainILD] = L_shr_r( g_ILD, 16 ); // Q15
1339 74349 : move32();
1340 74349 : hStereoClassif->xtalk_fv_fx[E_gainILD] = L_shr_r( g_ILD, 16 ); // Q15
1341 74349 : move32();
1342 :
1343 : // IPD = atan2f( grand_dot_prod_img, grand_dot_prod_real );
1344 74349 : IPD = BASOP_util_atan2( grand_dot_prod_img, grand_dot_prod_real, sub( grand_dot_prod_img_e, grand_dot_prod_real_e ) ); // Q13
1345 :
1346 74349 : hStereoClassif->unclr_fv_fx[E_IPD] = L_shl( IPD, 2 ); // Q15
1347 74349 : move32();
1348 74349 : hStereoClassif->xtalk_fv_fx[E_IPD] = L_shl( IPD, 2 ); // Q15
1349 74349 : move32();
1350 :
1351 : // d_IPD = fabsf( IPD - hStereoClassif->prev_IPD );
1352 74349 : d_IPD = BASOP_Util_Add_Mant32Exp( IPD, 18, L_negate( hStereoClassif->prev_IPD_fx ), 2, &d_IPD_e );
1353 74349 : d_IPD = L_abs( d_IPD );
1354 74349 : hStereoClassif->unclr_fv_fx[E_d_IPD] = L_shl( IPD, 2 ); // Q15 /* VM: need to replace IPD by d_IPD and re-train the UNCLR classifier for DFT stereo */
1355 74349 : move32();
1356 74349 : hStereoClassif->xtalk_fv_fx[E_d_IPD] = L_shr_r( d_IPD, sub( 16, d_IPD_e ) ); /// Q15
1357 74349 : move32();
1358 74349 : hStereoClassif->prev_IPD_fx = L_shl( IPD, 16 ); // Q29
1359 74349 : move32();
1360 :
1361 : // g_IPD = ( sum_nrg_L + sum_nrg_R + 2 * grand_dot_prod_real ) / grand_nrg_DMX;
1362 74349 : L_temp = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, sum_nrg_R, sum_nrg_R_e, &L_temp_e );
1363 74349 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, grand_dot_prod_real, add( grand_dot_prod_real_e, 1 ), &L_temp_e );
1364 74349 : g_IPD = BASOP_Util_Divide3232_Scale_newton( L_temp, grand_nrg_DMX, &g_IPD_e );
1365 74349 : g_IPD_e = add( g_IPD_e, sub( L_temp_e, grand_nrg_DMX_e ) );
1366 : // if ( g_IPD >= 1.0f )
1367 74349 : IF( BASOP_Util_Cmp_Mant32Exp( g_IPD, g_IPD_e, ONE_IN_Q29 - ONE_IN_Q14 /*Adjusting threshold for precision loss*/, 2 ) >= 0 )
1368 : {
1369 3438 : g_IPD = hStereoClassif->prev_g_IPD_fx;
1370 3438 : move32();
1371 3438 : g_IPD_e = 2;
1372 3438 : move16();
1373 : }
1374 : ELSE
1375 : {
1376 70911 : hStereoClassif->prev_g_IPD_fx = L_shr_r( g_IPD, sub( 2, g_IPD_e ) ); // Q29
1377 70911 : move32();
1378 : }
1379 : // g_IPD = logf( 1.0f - g_IPD );
1380 74349 : L_temp = BASOP_Util_Add_Mant32Exp( MAX_32, 0, L_negate( g_IPD ), g_IPD_e, &L_temp_e );
1381 : // g_IPD = Mpy_32_32( L_add( BASOP_Util_Log2( L_temp ), L_shl( L_temp_e, 25 ) ), LOG_E_2 );
1382 74349 : g_IPD = BASOP_Util_Loge( L_temp, L_temp_e );
1383 74349 : hStereoClassif->unclr_fv_fx[E_gainIPD] = L_shr_r( g_IPD, 10 ); // Q15
1384 74349 : move32();
1385 74349 : hStereoClassif->xtalk_fv_fx[E_gainIPD] = L_shr_r( g_IPD, 10 ); // Q15
1386 74349 : move32();
1387 :
1388 : // angle_rot = fabsf( atanf( 2.0f * ( grand_dot_prod_real ) / ( sum_nrg_L - sum_nrg_R + 1.0f ) ) );
1389 74349 : L_temp = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, L_negate( sum_nrg_R ), sum_nrg_R_e, &L_temp_e );
1390 74349 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, MAX_32, 0, &L_temp_e );
1391 74349 : L_temp2 = BASOP_Util_Divide3232_Scale_newton( grand_dot_prod_real, L_temp, &L_temp2_e );
1392 74349 : L_temp2_e = add( L_temp2_e, sub( add( grand_dot_prod_real_e, 1 ), L_temp_e ) );
1393 74349 : angle_rot = L_abs( BASOP_util_atan( L_shr_r_sat( L_temp2, ( sub( 6, L_temp2_e ) ) ) ) ); // Q14
1394 : // angle_rot = L_abs( BASOP_util_atan2( grand_dot_prod_real, L_temp, sub( add( grand_dot_prod_real_e, 1 ), L_temp_e ) ) ); // Q13
1395 :
1396 74349 : hStereoClassif->unclr_fv_fx[E_angle_rot] = L_shl( angle_rot, 1 ); // Q15
1397 74349 : move32();
1398 74349 : hStereoClassif->xtalk_fv_fx[E_angle_rot] = L_shl( angle_rot, 1 ); // Q15
1399 74349 : move32();
1400 :
1401 : // g_side = fabsf( sum_nrg_L - sum_nrg_R ) / ( grand_nrg_DMX );
1402 74349 : L_temp = L_abs( BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, L_negate( sum_nrg_R ), sum_nrg_R_e, &L_temp_e ) );
1403 74349 : g_side = BASOP_Util_Divide3232_Scale_newton( L_temp, grand_nrg_DMX, &g_side_e );
1404 74349 : g_side_e = add( g_side_e, sub( L_temp_e, grand_nrg_DMX_e ) );
1405 74349 : g_side = L_shl_sat( g_side, g_side_e ); // Q31
1406 74349 : g_side_e = 0;
1407 :
1408 : // g_side = max( 0.01f, min( g_side, 0.99f ) );
1409 74349 : g_side = L_max( 21474836, L_min( g_side, 2126008811 ) );
1410 74349 : hStereoClassif->unclr_fv_fx[E_g_side] = L_shr_r( g_side, 16 ); // Q15
1411 74349 : move32();
1412 74349 : hStereoClassif->xtalk_fv_fx[E_g_side] = L_shr_r( g_side, 16 ); // Q15
1413 74349 : move32();
1414 :
1415 : // g_pred = logf( max( 0, ( ( 1 - g_side ) * sum_nrg_L + ( 1 + g_side ) * sum_nrg_R - 2 * abs_L_R ) ) + 1.0f );
1416 74349 : L_temp = BASOP_Util_Add_Mant32Exp( MAX_32, 0, L_negate( g_side ), g_side_e, &L_temp_e );
1417 74349 : L_temp = Mpy_32_32( L_temp, sum_nrg_L );
1418 74349 : L_temp_e = add( L_temp_e, sum_nrg_L_e );
1419 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( MAX_32, 0, g_side, g_side_e, &L_temp2_e );
1420 74349 : L_temp2 = Mpy_32_32( L_temp2, sum_nrg_R );
1421 74349 : L_temp2_e = add( L_temp2_e, sum_nrg_R_e );
1422 74349 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, L_temp2, L_temp2_e, &L_temp_e );
1423 74349 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, L_negate( abs_L_R ), add( abs_L_R_e, 1 ), &L_temp_e );
1424 74349 : L_temp = L_max( 0, L_temp );
1425 74349 : g_pred = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, MAX_32, 0, &g_pred_e );
1426 : // g_pred = Mpy_32_32( L_add( BASOP_Util_Log2( g_pred ), L_shl( g_pred_e, 25 ) ), LOG_E_2 ); // Q25
1427 74349 : g_pred = BASOP_Util_Loge( g_pred, g_pred_e ); // Q25
1428 74349 : g_pred_e = 6;
1429 74349 : move16();
1430 :
1431 : // g_pred = max( 14.0f, g_pred );
1432 74349 : g_pred = L_max( 14 << 25, g_pred );
1433 74349 : hStereoClassif->unclr_fv_fx[E_g_pred] = L_shr_r( g_pred, 10 ); // Q15
1434 74349 : move32();
1435 74349 : hStereoClassif->xtalk_fv_fx[E_g_pred] = L_shr_r( g_pred, 10 ); // Q15
1436 74349 : move32();
1437 : }
1438 :
1439 : // mvr2r( &Spd_L[1], &xcorr_lb[1], STEREO_DFT_XCORR_LB_MAX - 1 );
1440 74349 : Copy32( &Spd_L[1], &xcorr_lb[1], STEREO_DFT_XCORR_LB_MAX - 1 );
1441 74349 : Copy( &Spd_L_e[1], &xcorr_lb_e[1], STEREO_DFT_XCORR_LB_MAX - 1 );
1442 :
1443 : // sum_nrg_L_lb = sum_nrg_L_lb + sum_f( &Spd_L[1], 11 );
1444 74349 : L_temp = 0;
1445 74349 : move32();
1446 74349 : L_temp_e = 0;
1447 74349 : move16();
1448 892188 : FOR( Word16 ii = 0; ii < 11; ii++ )
1449 : {
1450 817839 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, Spd_L[ii + 1], Spd_L_e[ii + 1], &L_temp_e );
1451 : }
1452 : // L_temp = sum2_32_fx( &Spd_L[1], 11, &L_temp_e );
1453 74349 : sum_nrg_L_lb = BASOP_Util_Add_Mant32Exp( sum_nrg_L_lb, sum_nrg_L_lb_e, L_temp, L_temp_e, &sum_nrg_L_lb_e );
1454 74349 : vad_flag_itd = stereo_dft_enc_itd_vad_fx( hItd->E_band_n_fx, hItd->E_band_n_exp, &( hItd->vad_frm_cnt ), Spd_L, Spd_L_e, Spd_R, Spd_R_e, &mssnr, &mssnr_e );
1455 74349 : vad_flag_itd = vad_flag_itd && vad_flag_dtx[0];
1456 :
1457 : // if ( sum_nrg_L < EPSILON )
1458 74349 : IF( BASOP_Util_Cmp_Mant32Exp( sum_nrg_L, sum_nrg_L_e, EPSILON_FX_M, EPSILON_FX_E ) < 0 )
1459 : {
1460 0 : sfm_L = 0;
1461 0 : move32();
1462 0 : sfm_L_e = 0;
1463 0 : move16();
1464 : }
1465 : ELSE
1466 : {
1467 : // sfm_L = expf( log_prod_L / ( NFFT_mid ) ) / ( sum_abs_L / ( NFFT_mid ) );
1468 74349 : L_temp = BASOP_Util_Divide3232_Scale_newton( log_prod_L, NFFT_mid, &L_temp_e );
1469 74349 : L_temp_e = add( L_temp_e, sub( log_prod_L_e, 31 ) );
1470 74349 : L_temp = BASOP_Util_fPow( 1459366444 /*2.718 in Q29*/, 2, L_temp, L_temp_e, &L_temp_e );
1471 74349 : q_temp = norm_l( NFFT_mid );
1472 74349 : L_temp2 = L_shl( NFFT_mid, q_temp );
1473 74349 : L_temp2_e = sub( 31, q_temp );
1474 74349 : L_temp = Mpy_32_32( L_temp, L_temp2 );
1475 74349 : L_temp_e = add( L_temp_e, L_temp2_e );
1476 74349 : sfm_L = BASOP_Util_Divide3232_Scale_newton( L_temp, sum_abs_L, &sfm_L_e );
1477 74349 : sfm_L_e = add( sfm_L_e, sub( L_temp_e, sum_abs_L_e ) );
1478 74349 : sfm_L = L_shl_sat( sfm_L, sfm_L_e ); // Q31 - should be rechecked for -10dB tests
1479 : }
1480 :
1481 : // if ( sum_nrg_R < EPSILON )
1482 74349 : IF( BASOP_Util_Cmp_Mant32Exp( sum_nrg_R, sum_nrg_R_e, EPSILON_FX_M, EPSILON_FX_E ) < 0 )
1483 : {
1484 0 : sfm_R = 0;
1485 0 : move32();
1486 0 : sfm_R_e = 0;
1487 0 : move16();
1488 : }
1489 : ELSE
1490 : {
1491 : // sfm_R = expf( log_prod_R / ( NFFT_mid ) ) / ( sum_abs_R / ( NFFT_mid ) );
1492 74349 : L_temp = BASOP_Util_Divide3232_Scale_newton( log_prod_R, NFFT_mid, &L_temp_e );
1493 74349 : L_temp_e = add( L_temp_e, sub( log_prod_R_e, 31 ) );
1494 74349 : L_temp = BASOP_Util_fPow( 1459366444 /*2.718 in Q29*/, 2, L_temp, L_temp_e, &L_temp_e );
1495 74349 : q_temp = norm_l( NFFT_mid );
1496 74349 : L_temp2 = L_shl( NFFT_mid, q_temp );
1497 74349 : L_temp2_e = sub( 31, q_temp );
1498 74349 : L_temp = Mpy_32_32( L_temp, L_temp2 );
1499 74349 : L_temp_e = add( L_temp_e, L_temp2_e );
1500 74349 : sfm_R = BASOP_Util_Divide3232_Scale_newton( L_temp, sum_abs_R, &sfm_L_e );
1501 74349 : sfm_R_e = add( sfm_L_e, sub( L_temp_e, sum_abs_R_e ) );
1502 : // sfm_R = L_shl_r( sfm_R, sfm_R_e ); // Q31
1503 74349 : sfm_R = L_shl_sat( sfm_R, sfm_R_e ); // Q31
1504 : }
1505 :
1506 74349 : if ( sfm_R > sfm_L )
1507 : {
1508 28665 : sfm_L = sfm_R;
1509 28665 : move32();
1510 : }
1511 74349 : IF( EQ_16( hCPE->element_mode, IVAS_CPE_DFT ) )
1512 : {
1513 59659 : hStereoDft->sfm_fx = sfm_L;
1514 : }
1515 :
1516 18444269 : FOR( ; i < NFFT / 2; i++ )
1517 : {
1518 : // xcorr[2 * i] = pDFT_L[2 * i] * pDFT_R[2 * i] + pDFT_L[2 * i + 1] * pDFT_R[2 * i + 1];
1519 18369920 : W_temp = W_mult0_32_32( pDFT_L[2 * i], pDFT_R[2 * i] ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i])
1520 18369920 : W_temp_q = W_norm( W_temp );
1521 18369920 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] + DFT_R_e[2 * i])
1522 18369920 : W_temp_q = add( Q30, sub( W_temp_q, add( DFT_L_e[2 * i], DFT_R_e[2 * i] ) ) );
1523 18369920 : W_temp1 = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_R[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1])
1524 18369920 : W_temp_q1 = W_norm( W_temp1 );
1525 18369920 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i + 1])
1526 18369920 : W_temp_q1 = add( Q30, sub( W_temp_q1, add( DFT_L_e[2 * i + 1], DFT_R_e[2 * i + 1] ) ) );
1527 18369920 : xcorr[2 * i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &xcorr_e[2 * i] );
1528 18369920 : move32();
1529 : // xcorr[2 * i + 1] = pDFT_L[2 * i + 1] * pDFT_R[2 * i] - pDFT_L[2 * i] * pDFT_R[2 * i + 1];
1530 18369920 : W_temp = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_R[2 * i] ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i])
1531 18369920 : W_temp_q = W_norm( W_temp );
1532 18369920 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] + DFT_R_e[2 * i])
1533 18369920 : W_temp_q = add( Q30, sub( W_temp_q, add( DFT_L_e[2 * i + 1], DFT_R_e[2 * i] ) ) );
1534 18369920 : W_temp1 = W_mult0_32_32( L_negate( pDFT_L[2 * i] ), pDFT_R[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1])
1535 18369920 : W_temp_q1 = W_norm( W_temp1 );
1536 18369920 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1]) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] + DFT_R_e[2 * i + 1])
1537 18369920 : W_temp_q1 = add( Q30, sub( W_temp_q1, add( DFT_L_e[2 * i], DFT_R_e[2 * i + 1] ) ) );
1538 18369920 : xcorr[2 * i + 1] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &xcorr_e[2 * i + 1] );
1539 18369920 : move32();
1540 :
1541 : // pNrgL[i] = pDFT_L[2 * i] * pDFT_L[2 * i] + pDFT_L[2 * i + 1] * pDFT_L[2 * i + 1];
1542 18369920 : W_temp = W_mult0_32_32( pDFT_L[2 * i], pDFT_L[2 * i] ); // Q62 - (DFT_L_e[2 * i] * 2)
1543 18369920 : W_temp_q = W_norm( W_temp );
1544 18369920 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_L_e[2 * i] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i] * 2)
1545 18369920 : W_temp_q = add( Q30, sub( W_temp_q, shl( DFT_L_e[2 * i], 1 ) ) );
1546 18369920 : W_temp1 = W_mult0_32_32( pDFT_L[2 * i + 1], pDFT_L[2 * i + 1] ); // Q62 - (DFT_L_e[2 * i + 1] * 2)
1547 18369920 : W_temp_q1 = W_norm( W_temp1 );
1548 18369920 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_L_e[2 * i + 1] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_L_e[2 * i + 1] * 2)
1549 18369920 : W_temp_q1 = add( Q30, sub( W_temp_q1, shl( DFT_L_e[2 * i + 1], 1 ) ) );
1550 18369920 : pNrgL[i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &pNrgL_e[i] );
1551 18369920 : move32();
1552 : // pNrgR[i] = pDFT_R[2 * i] * pDFT_R[2 * i] + pDFT_R[2 * i + 1] * pDFT_R[2 * i + 1];
1553 18369920 : W_temp = W_mult0_32_32( pDFT_R[2 * i], pDFT_R[2 * i] ); // Q62 - (DFT_R_e[2 * i] * 2)
1554 18369920 : W_temp_q = W_norm( W_temp );
1555 18369920 : L_temp = W_extract_h( W_shl( W_temp, W_temp_q ) ); // Q62 - (DFT_R_e[2 * i] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_R_e[2 * i] * 2)
1556 18369920 : W_temp_q = add( Q30, sub( W_temp_q, shl( DFT_R_e[2 * i], 1 ) ) );
1557 18369920 : W_temp1 = W_mult0_32_32( pDFT_R[2 * i + 1], pDFT_R[2 * i + 1] ); // Q62 - (DFT_R_e[2 * i + 1] * 2)
1558 18369920 : W_temp_q1 = W_norm( W_temp1 );
1559 18369920 : L_temp2 = W_extract_h( W_shl( W_temp1, W_temp_q1 ) ); // Q62 - (DFT_R_e[2 * i + 1] * 2) + (W_temp_q - Q32) = Q30 + W_temp_q - (DFT_R_e[2 * i + 1] * 2)
1560 18369920 : W_temp_q1 = add( Q30, sub( W_temp_q1, shl( DFT_R_e[2 * i + 1], 1 ) ) );
1561 18369920 : pNrgR[i] = BASOP_Util_Add_Mant32Exp( L_temp, sub( Q31, W_temp_q ), L_temp2, sub( Q31, W_temp_q1 ), &pNrgR_e[i] );
1562 18369920 : move32();
1563 : /* Calculate L and R energy power spectrum */
1564 18369920 : Spd_L[i] = pNrgL[i];
1565 18369920 : move32();
1566 18369920 : Spd_L_e[i] = pNrgL_e[i];
1567 18369920 : move16();
1568 18369920 : Spd_R[i] = pNrgR[i];
1569 18369920 : move32();
1570 18369920 : Spd_R_e[i] = pNrgR_e[i];
1571 18369920 : move16();
1572 : }
1573 :
1574 5496109 : FOR( ; i < STEREO_DFT_N_32k_ENC / 2; i++ )
1575 : {
1576 5421760 : xcorr[2 * i] = 0;
1577 5421760 : move32();
1578 5421760 : xcorr_e[2 * i] = 0;
1579 5421760 : move16();
1580 5421760 : xcorr[2 * i + 1] = 0;
1581 5421760 : move32();
1582 5421760 : xcorr_e[2 * i + 1] = 0;
1583 5421760 : move16();
1584 : }
1585 :
1586 : Word16 xcorr_smooth_fx_tmp_e[STEREO_DFT_N_32k_ENC];
1587 74349 : Copy( hItd->xcorr_smooth_fx_e, xcorr_smooth_fx_tmp_e, STEREO_DFT_N_32k_ENC );
1588 74349 : hItd->xcorr_smooth_fx[0] = 0;
1589 74349 : move32();
1590 74349 : xcorr_smooth_fx_tmp_e[0] = 0;
1591 74349 : move16();
1592 74349 : hItd->xcorr_smooth_fx[1] = 0;
1593 74349 : move32();
1594 74349 : xcorr_smooth_fx_tmp_e[1] = 0;
1595 74349 : move16();
1596 74349 : xcorr[0] = L_shl_sat( sign_fx( hItd->xcorr_smooth_fx[0] ), 31 );
1597 74349 : move32();
1598 74349 : xcorr_e[0] = 0;
1599 74349 : move16();
1600 74349 : xcorr[1] = L_shl_sat( sign_fx( hItd->xcorr_smooth_fx[1] ), 31 );
1601 74349 : move32();
1602 74349 : xcorr_e[1] = 0;
1603 74349 : move16();
1604 :
1605 74349 : test();
1606 74349 : IF( EQ_16( hCPE->element_mode, IVAS_CPE_DFT ) && sub( hItd->td_itd[k_offset], hItd->td_itd[k_offset - 1] ) )
1607 : {
1608 : Word16 alphaD, c, s, c1, s1, ctmp;
1609 : Word32 vtmp;
1610 : Word16 vtmp_e;
1611 : // alphaD = -2.f * EVS_PI * ( (float) hItd->td_itd[k_offset] - hItd->td_itd[k_offset - 1] ) / hStereoDft->NFFT;
1612 960 : alphaD = BASOP_Util_Divide1616_Scale( sub( hItd->td_itd[k_offset], hItd->td_itd[sub( k_offset, 1 )] ), hStereoDft->NFFT, &L_temp_e );
1613 960 : alphaD = mult_r( -EVS_PI_FX, alphaD );
1614 960 : L_temp_e = add( L_temp_e, 3 );
1615 960 : alphaD = shl_r( alphaD, L_temp_e - 2 ); // 2Q13
1616 :
1617 : // c1 = cosf( alphaD );
1618 960 : c1 = shl_sat( getCosWord16( alphaD ), 1 ); // Q15
1619 : // s1 = sinf( alphaD );
1620 960 : s1 = getSinWord16( alphaD ); // Q15
1621 960 : c = MAX_16; /* cos(0) */
1622 960 : move16();
1623 960 : s = 0; /* sin(0) */
1624 960 : move16();
1625 :
1626 577920 : FOR( i = 1; i < NFFT / 2; i++ )
1627 : {
1628 576960 : ctmp = c;
1629 576960 : c = sub_sat( mult_r( c, c1 ), mult_r( s, s1 ) );
1630 576960 : s = add_sat( mult_r( ctmp, s1 ), mult_r( s, c1 ) );
1631 576960 : vtmp = BASOP_Util_Add_Mant32Exp( Mult_32_16( hItd->xcorr_smooth_fx[2 * i], c ), xcorr_smooth_fx_tmp_e[2 * i], L_negate( Mult_32_16( hItd->xcorr_smooth_fx[2 * i + 1], s ) ), xcorr_smooth_fx_tmp_e[2 * i + 1], &vtmp_e );
1632 576960 : hItd->xcorr_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mult_32_16( hItd->xcorr_smooth_fx[2 * i], s ), xcorr_smooth_fx_tmp_e[2 * i], Mult_32_16( hItd->xcorr_smooth_fx[2 * i + 1], c ), xcorr_smooth_fx_tmp_e[2 * i + 1], &xcorr_smooth_fx_tmp_e[2 * i + 1] );
1633 576960 : move32();
1634 576960 : hItd->xcorr_smooth_fx[2 * i] = vtmp;
1635 576960 : move32();
1636 576960 : xcorr_smooth_fx_tmp_e[2 * i] = vtmp_e;
1637 576960 : move16();
1638 : }
1639 : }
1640 :
1641 74349 : tmpf3 = 2;
1642 74349 : move32();
1643 74349 : tmpf3_e = 31;
1644 74349 : move16();
1645 74349 : IF( flag_noisy_speech_snr )
1646 : {
1647 37975 : alpha = -1717986918 /*-0.8f*/;
1648 37975 : move32();
1649 : }
1650 : ELSE
1651 : {
1652 36374 : alpha = MIN_32;
1653 36374 : move32();
1654 : }
1655 :
1656 74349 : test();
1657 74349 : IF( hCPE->hCoreCoder[0]->Opt_DTX_ON && EQ_16( hCPE->element_mode, IVAS_CPE_DFT ) )
1658 30300 : {
1659 : Word16 Spd_L_smooth_fx_tmp_e[STEREO_DFT_N_32k_ENC / 2], Spd_R_smooth_fx_tmp_e[STEREO_DFT_N_32k_ENC / 2];
1660 30300 : set16_fx( Spd_L_smooth_fx_tmp_e, hStereoDft->Spd_L_smooth_fx_e, STEREO_DFT_N_32k_ENC / 2 );
1661 30300 : set16_fx( Spd_R_smooth_fx_tmp_e, hStereoDft->Spd_R_smooth_fx_e, STEREO_DFT_N_32k_ENC / 2 );
1662 30300 : IF( hCPE->hFrontVad[0] != NULL )
1663 : {
1664 : /* Determine if we are in hangover */
1665 30300 : test();
1666 30300 : IF( vad_hover_flag[0] && vad_hover_flag[1] )
1667 : {
1668 : /* Determine if we are in the first DTX hangover frame (also triggers for VAD hangover frame) */
1669 894 : IF( GT_16( hStereoDft->resetFrames, CORR_RESET_FRAMES_MAX ) )
1670 : {
1671 : /* Reset cross spectrum when there is hangover */
1672 166 : set32_fx( hStereoDft->xspec_smooth_fx, 0, STEREO_DFT_N_32k_ENC );
1673 166 : set16_fx( hStereoDft->xspec_smooth_fx_e, 0, STEREO_DFT_N_32k_ENC );
1674 166 : hStereoDft->resetFrames = 0;
1675 166 : move16();
1676 166 : hStereoDft->currentNumUpdates = 0;
1677 166 : move16();
1678 : /* Expected minimum number of updates including first SID */
1679 166 : hStereoDft->expectedNumUpdates = add( 1, s_min( hCPE->hFrontVad[0]->rem_dtx_ho, hCPE->hFrontVad[1]->rem_dtx_ho ) );
1680 : }
1681 728 : ELSE IF( GE_16( hStereoDft->currentNumUpdates, hStereoDft->expectedNumUpdates ) )
1682 : {
1683 135 : hStereoDft->expectedNumUpdates = add( hStereoDft->expectedNumUpdates, add( 1, s_min( hCPE->hFrontVad[0]->rem_dtx_ho, hCPE->hFrontVad[1]->rem_dtx_ho ) ) );
1684 135 : move16();
1685 : }
1686 : // cng_xcorr_filt = max( min( CORR_FILT, 10.0f * CORR_FILT / ( hStereoDft->expectedNumUpdates + hStereoDft->currentNumUpdates ) ), sfm_L );
1687 :
1688 894 : cng_xcorr_filt = BASOP_Util_Divide3232_Scale_newton( 8 /*10.0f * CORR_FILT*/, add( hStereoDft->expectedNumUpdates, hStereoDft->currentNumUpdates ), &cng_xcorr_filt_e );
1689 894 : cng_xcorr_filt = L_shl_sat( cng_xcorr_filt, cng_xcorr_filt_e ); // Q31
1690 894 : cng_xcorr_filt = L_max( L_min( CORR_FILT_Q31, cng_xcorr_filt ), sfm_L ); // Q31
1691 :
1692 894 : hStereoDft->currentNumUpdates = add( hStereoDft->currentNumUpdates, 1 );
1693 894 : move16();
1694 258400 : FOR( i = 1; i < NFFT / 4; i++ )
1695 : {
1696 : /* Low pass filter cross L/R power spectrum */
1697 : // hStereoDft->xspec_smooth[2 * i] = ( 1.f - cng_xcorr_filt ) * hStereoDft->xspec_smooth[2 * i] + cng_xcorr_filt * xcorr[2 * i];
1698 257506 : hStereoDft->xspec_smooth_fx[2 * i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hStereoDft->xspec_smooth_fx[2 * i] ), hStereoDft->xspec_smooth_fx_e[2 * i],
1699 257506 : Mpy_32_32( cng_xcorr_filt, xcorr[2 * i] ), xcorr_e[2 * i], &hStereoDft->xspec_smooth_fx_e[2 * i] );
1700 257506 : move32();
1701 : // hStereoDft->xspec_smooth[2 * i + 1] = ( 1.f - cng_xcorr_filt ) * hStereoDft->xspec_smooth[2 * i + 1] + cng_xcorr_filt * xcorr[2 * i + 1];
1702 257506 : hStereoDft->xspec_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hStereoDft->xspec_smooth_fx[2 * i + 1] ), hStereoDft->xspec_smooth_fx_e[2 * i + 1],
1703 257506 : Mpy_32_32( cng_xcorr_filt, xcorr[2 * i + 1] ), xcorr_e[2 * i + 1], &hStereoDft->xspec_smooth_fx_e[2 * i + 1] );
1704 257506 : move32();
1705 :
1706 : /* Low pass filter L/R power spectrum */
1707 : /* Calculate coherence as cross spectral density divided by L*R power spectrum */
1708 : // hStereoDft->Spd_L_smooth[i] = ( 1.f - cng_xcorr_filt ) * hStereoDft->Spd_L_smooth[i] + cng_xcorr_filt * Spd_L[i];
1709 257506 : hStereoDft->Spd_L_smooth_fx[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hStereoDft->Spd_L_smooth_fx[i] ), Spd_L_smooth_fx_tmp_e[i],
1710 257506 : Mpy_32_32( cng_xcorr_filt, Spd_L[i] ), Spd_L_e[i], &Spd_L_smooth_fx_tmp_e[i] );
1711 257506 : move32();
1712 257506 : hStereoDft->Spd_R_smooth_fx[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hStereoDft->Spd_R_smooth_fx[i] ), Spd_R_smooth_fx_tmp_e[i],
1713 257506 : Mpy_32_32( cng_xcorr_filt, Spd_R[i] ), Spd_R_e[i], &Spd_R_smooth_fx_tmp_e[i] );
1714 257506 : move32();
1715 : }
1716 : }
1717 29406 : ELSE IF( vad_flag_dtx[0] == 0 )
1718 : {
1719 10482 : hStereoDft->resetFrames = 0;
1720 10482 : move16();
1721 : }
1722 : ELSE
1723 : {
1724 18924 : if ( LT_16( hStereoDft->resetFrames, CORR_RESET_FRAMES_MAX + 1 ) )
1725 : {
1726 6758 : hStereoDft->resetFrames = add( hStereoDft->resetFrames, 1 );
1727 6758 : move16();
1728 : }
1729 18924 : test();
1730 18924 : if ( !vad_hover_flag[0] && !vad_hover_flag[1] )
1731 : {
1732 17252 : hStereoDft->expectedNumUpdates = hStereoDft->currentNumUpdates;
1733 17252 : move16();
1734 : }
1735 : }
1736 : }
1737 30300 : test();
1738 30300 : test();
1739 30300 : test();
1740 30300 : test();
1741 30300 : IF( ( vad_flag_dtx[0] == 0 ) || ( hCPE->hFrontVad[0] == NULL && ( EQ_32( hCPE->hCoreCoder[0]->last_core_brate, SID_2k40 ) || EQ_32( hCPE->hCoreCoder[0]->last_core_brate, FRAME_NO_DATA ) ) ) || hCPE->hStereoCng->first_SID_after_TD )
1742 : {
1743 14365 : IF( vad_flag_dtx[0] == 0 )
1744 : {
1745 : /* expectedNumUpdates updated after call to dtx() in SID frames */
1746 : // cng_xcorr_filt = max( min( CORR_FILT, 10.0f * CORR_FILT / ( hStereoDft->expectedNumUpdates + hStereoDft->currentNumUpdates ) ), sfm_L );
1747 10482 : IF( add( hStereoDft->expectedNumUpdates, hStereoDft->currentNumUpdates ) != 0 )
1748 : {
1749 10480 : cng_xcorr_filt = BASOP_Util_Divide3232_Scale_newton( 8 /*10.0f * CORR_FILT*/, add( hStereoDft->expectedNumUpdates, hStereoDft->currentNumUpdates ), &cng_xcorr_filt_e );
1750 10480 : cng_xcorr_filt = L_shl_sat( cng_xcorr_filt, cng_xcorr_filt_e ); // Q31
1751 10480 : cng_xcorr_filt = L_max( L_min( CORR_FILT_Q31, cng_xcorr_filt ), sfm_L ); // Q31
1752 : }
1753 : ELSE
1754 : {
1755 2 : cng_xcorr_filt = CORR_FILT_Q31;
1756 2 : move32();
1757 2 : cng_xcorr_filt = L_max( cng_xcorr_filt, sfm_L ); // Q31
1758 : }
1759 10482 : hStereoDft->currentNumUpdates = add( hStereoDft->currentNumUpdates, 1 );
1760 10482 : move16();
1761 10482 : hStereoDft->sfm_fx = cng_xcorr_filt;
1762 10482 : move32();
1763 : }
1764 : ELSE /* use sfm for active frames */
1765 : {
1766 3883 : cng_xcorr_filt = sfm_L; // Q31
1767 3883 : move32();
1768 : }
1769 :
1770 : /* Copy state of xspec_smooth to xcorr_smooth in first CNG frame */
1771 14365 : test();
1772 14365 : IF( hCPE->hStereoCng->cng_counter == 0 && vad_flag_dtx[0] == 0 )
1773 : {
1774 : // mvr2r( hStereoDft->xspec_smooth, hItd->xcorr_smooth, NFFT );
1775 188 : Copy32( hStereoDft->xspec_smooth_fx, hItd->xcorr_smooth_fx, NFFT );
1776 188 : Copy( hStereoDft->xspec_smooth_fx_e, xcorr_smooth_fx_tmp_e, NFFT );
1777 : }
1778 8274240 : FOR( i = 1; i < NFFT / 2; i++ )
1779 : {
1780 : /* Low pass filter cross L/R power spectrum */
1781 : // hStereoDft->xspec_smooth[2 * i] = ( 1.f - XSPEC_ALPHA ) * hStereoDft->xspec_smooth[2 * i] + XSPEC_ALPHA * xcorr[2 * i];
1782 8259875 : hStereoDft->xspec_smooth_fx[2 * i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( ( MAX_32 - XSPEC_ALPHA_Q31 ), hStereoDft->xspec_smooth_fx[2 * i] ), hStereoDft->xspec_smooth_fx_e[2 * i],
1783 8259875 : Mpy_32_32( XSPEC_ALPHA_Q31, xcorr[2 * i] ), xcorr_e[2 * i], &hStereoDft->xspec_smooth_fx_e[2 * i] );
1784 8259875 : move32();
1785 : // hStereoDft->xspec_smooth[2 * i + 1] = ( 1.f - XSPEC_ALPHA ) * hStereoDft->xspec_smooth[2 * i + 1] + XSPEC_ALPHA * xcorr[2 * i + 1];
1786 8259875 : hStereoDft->xspec_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( ( MAX_32 - XSPEC_ALPHA_Q31 ), hStereoDft->xspec_smooth_fx[2 * i + 1] ), hStereoDft->xspec_smooth_fx_e[2 * i + 1],
1787 8259875 : Mpy_32_32( XSPEC_ALPHA_Q31, xcorr[2 * i + 1] ), xcorr_e[2 * i + 1], &hStereoDft->xspec_smooth_fx_e[2 * i + 1] );
1788 8259875 : move32();
1789 : // hItd->xcorr_smooth[2 * i] = ( 1.f - cng_xcorr_filt ) * hItd->xcorr_smooth[2 * i] + cng_xcorr_filt * xcorr[2 * i];
1790 8259875 : hItd->xcorr_smooth_fx[2 * i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hItd->xcorr_smooth_fx[2 * i] ), xcorr_smooth_fx_tmp_e[2 * i],
1791 8259875 : Mpy_32_32( cng_xcorr_filt, xcorr[2 * i] ), xcorr_e[2 * i], &xcorr_smooth_fx_tmp_e[2 * i] );
1792 8259875 : move32();
1793 : // hItd->xcorr_smooth[2 * i + 1] = ( 1.f - cng_xcorr_filt ) * hItd->xcorr_smooth[2 * i + 1] + cng_xcorr_filt * xcorr[2 * i + 1];
1794 8259875 : hItd->xcorr_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, cng_xcorr_filt ), hItd->xcorr_smooth_fx[2 * i + 1] ), xcorr_smooth_fx_tmp_e[2 * i + 1],
1795 8259875 : Mpy_32_32( cng_xcorr_filt, xcorr[2 * i + 1] ), xcorr_e[2 * i + 1], &xcorr_smooth_fx_tmp_e[2 * i + 1] );
1796 8259875 : move32();
1797 : // tmpf1 = sqrtf( hItd->xcorr_smooth[i * 2] * hItd->xcorr_smooth[i * 2] + hItd->xcorr_smooth[i * 2 + 1] * hItd->xcorr_smooth[i * 2 + 1] );
1798 8259875 : L_temp = BASOP_Util_Add_Mant32Exp( Mpy_32_32( hItd->xcorr_smooth_fx[i * 2], hItd->xcorr_smooth_fx[i * 2] ), shl( xcorr_smooth_fx_tmp_e[2 * i], 1 ),
1799 8259875 : Mpy_32_32( hItd->xcorr_smooth_fx[i * 2 + 1], hItd->xcorr_smooth_fx[i * 2 + 1] ), shl( xcorr_smooth_fx_tmp_e[i * 2 + 1], 1 ), &L_temp_e );
1800 8259875 : L_temp = Sqrt32( L_temp, &L_temp_e );
1801 : // tmpf1 += EPSILON;
1802 8259875 : tmpf1 = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, EPSILON_FX_M, EPSILON_FX_E, &tmpf1_e );
1803 8259875 : tmpf2 = tmpf1;
1804 8259875 : move32();
1805 8259875 : tmpf2_e = tmpf1_e;
1806 8259875 : move16();
1807 : // tmpf1 = powf( tmpf1, alpha );
1808 8259875 : tmpf1 = BASOP_Util_fPow( tmpf1, tmpf1_e, alpha, 0, &tmpf1_e );
1809 : // tmpf3 += tmpf2 * tmpf1;
1810 8259875 : tmpf3 = BASOP_Util_Add_Mant32Exp( tmpf3, tmpf3_e, Mpy_32_32( tmpf2, tmpf1 ), add( tmpf2_e, tmpf1_e ), &tmpf3_e );
1811 : // xcorr[2 * i] = hItd->xcorr_smooth[2 * i] * tmpf1;
1812 8259875 : xcorr[2 * i] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i], tmpf1 );
1813 8259875 : move32();
1814 8259875 : xcorr_e[2 * i] = add( xcorr_smooth_fx_tmp_e[2 * i], tmpf1_e );
1815 8259875 : move16();
1816 : // xcorr[2 * i + 1] = hItd->xcorr_smooth[2 * i + 1] * tmpf1;
1817 8259875 : xcorr[2 * i + 1] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i + 1], tmpf1 );
1818 8259875 : move32();
1819 8259875 : xcorr_e[2 * i + 1] = add( xcorr_smooth_fx_tmp_e[2 * i + 1], tmpf1_e );
1820 8259875 : move16();
1821 :
1822 : /* Low pass filter L/R power spectrum */
1823 : /* Calculate coherence as cross spectral density divided by L*R power spectrum */
1824 : // hStereoDft->Spd_L_smooth[i] = ( 1.f - XSPEC_ALPHA ) * hStereoDft->Spd_L_smooth[i] + XSPEC_ALPHA * Spd_L[i];
1825 8259875 : hStereoDft->Spd_L_smooth_fx[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( ( MAX_32 - XSPEC_ALPHA_Q31 ), hStereoDft->Spd_L_smooth_fx[i] ), Spd_L_smooth_fx_tmp_e[i],
1826 8259875 : Mpy_32_32( XSPEC_ALPHA_Q31, Spd_L[i] ), Spd_L_e[i], &Spd_L_smooth_fx_tmp_e[i] );
1827 8259875 : move32();
1828 : // hStereoDft->Spd_R_smooth[i] = ( 1.f - XSPEC_ALPHA ) * hStereoDft->Spd_R_smooth[i] + XSPEC_ALPHA * Spd_R[i];
1829 8259875 : hStereoDft->Spd_R_smooth_fx[i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( ( MAX_32 - XSPEC_ALPHA_Q31 ), hStereoDft->Spd_R_smooth_fx[i] ), Spd_R_smooth_fx_tmp_e[i],
1830 8259875 : Mpy_32_32( XSPEC_ALPHA_Q31, Spd_R[i] ), Spd_R_e[i], &Spd_R_smooth_fx_tmp_e[i] );
1831 8259875 : move32();
1832 : }
1833 : }
1834 : ELSE
1835 : {
1836 9107520 : FOR( i = 1; i < NFFT / 2; i++ )
1837 : {
1838 : // hItd->xcorr_smooth[2 * i] = ( 1.f - sfm_L ) * hItd->xcorr_smooth[2 * i] + sfm_L * xcorr[2 * i];
1839 9091585 : hItd->xcorr_smooth_fx[2 * i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm_L ), hItd->xcorr_smooth_fx[2 * i] ), xcorr_smooth_fx_tmp_e[2 * i],
1840 9091585 : Mpy_32_32( sfm_L, xcorr[2 * i] ), xcorr_e[2 * i], &xcorr_smooth_fx_tmp_e[2 * i] );
1841 9091585 : move32();
1842 : // hItd->xcorr_smooth[2 * i + 1] = ( 1.f - sfm_L ) * hItd->xcorr_smooth[2 * i + 1] + sfm_L * xcorr[2 * i + 1];
1843 9091585 : hItd->xcorr_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm_L ), hItd->xcorr_smooth_fx[2 * i + 1] ), xcorr_smooth_fx_tmp_e[2 * i + 1],
1844 9091585 : Mpy_32_32( sfm_L, xcorr[2 * i + 1] ), xcorr_e[2 * i + 1], &xcorr_smooth_fx_tmp_e[2 * i + 1] );
1845 9091585 : move32();
1846 : // tmpf1 = sqrtf( hItd->xcorr_smooth[i * 2] * hItd->xcorr_smooth[i * 2] + hItd->xcorr_smooth[i * 2 + 1] * hItd->xcorr_smooth[i * 2 + 1] );
1847 9091585 : L_temp = BASOP_Util_Add_Mant32Exp( Mpy_32_32( hItd->xcorr_smooth_fx[i * 2], hItd->xcorr_smooth_fx[i * 2] ), shl( xcorr_smooth_fx_tmp_e[2 * i], 1 ),
1848 9091585 : Mpy_32_32( hItd->xcorr_smooth_fx[i * 2 + 1], hItd->xcorr_smooth_fx[i * 2 + 1] ), shl( xcorr_smooth_fx_tmp_e[i * 2 + 1], 1 ), &L_temp_e );
1849 9091585 : L_temp = Sqrt32( L_temp, &L_temp_e );
1850 : // tmpf1 += EPSILON;
1851 9091585 : tmpf1 = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, EPSILON_FX_M, EPSILON_FX_E, &tmpf1_e );
1852 9091585 : tmpf2 = tmpf1;
1853 9091585 : move32();
1854 9091585 : tmpf2_e = tmpf1_e;
1855 9091585 : move16();
1856 : // tmpf1 = powf( tmpf1, alpha );
1857 9091585 : tmpf1 = BASOP_Util_fPow( tmpf1, tmpf1_e, alpha, 0, &tmpf1_e );
1858 : // tmpf3 += tmpf2 * tmpf1;
1859 9091585 : tmpf3 = BASOP_Util_Add_Mant32Exp( tmpf3, tmpf3_e, Mpy_32_32( tmpf2, tmpf1 ), add( tmpf2_e, tmpf1_e ), &tmpf3_e );
1860 : // xcorr[2 * i] = hItd->xcorr_smooth[2 * i] * tmpf1;
1861 9091585 : xcorr[2 * i] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i], tmpf1 );
1862 9091585 : move32();
1863 9091585 : xcorr_e[2 * i] = add( xcorr_smooth_fx_tmp_e[2 * i], tmpf1_e );
1864 9091585 : move16();
1865 : // xcorr[2 * i + 1] = hItd->xcorr_smooth[2 * i + 1] * tmpf1;
1866 9091585 : xcorr[2 * i + 1] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i + 1], tmpf1 );
1867 9091585 : move32();
1868 9091585 : xcorr_e[2 * i + 1] = add( xcorr_smooth_fx_tmp_e[2 * i + 1], tmpf1_e );
1869 9091585 : move16();
1870 : }
1871 : }
1872 :
1873 : /* RESCALING TO COMMON EXP */
1874 30300 : max_exp = MIN_16;
1875 30300 : move16();
1876 19422300 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC / 2; i++ )
1877 : {
1878 19392000 : max_exp = s_max( max_exp, Spd_L_smooth_fx_tmp_e[i] );
1879 : }
1880 30300 : hStereoDft->Spd_L_smooth_fx_e = max_exp;
1881 30300 : move16();
1882 19422300 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC / 2; i++ )
1883 : {
1884 19392000 : hStereoDft->Spd_L_smooth_fx[i] = L_shr_r( hStereoDft->Spd_L_smooth_fx[i], sub( hStereoDft->Spd_L_smooth_fx_e, Spd_L_smooth_fx_tmp_e[i] ) );
1885 19392000 : move32();
1886 : }
1887 30300 : max_exp = MIN_16;
1888 30300 : move16();
1889 19422300 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC / 2; i++ )
1890 : {
1891 19392000 : max_exp = s_max( max_exp, Spd_R_smooth_fx_tmp_e[i] );
1892 : }
1893 30300 : hStereoDft->Spd_R_smooth_fx_e = max_exp;
1894 30300 : move16();
1895 19422300 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC / 2; i++ )
1896 : {
1897 19392000 : hStereoDft->Spd_R_smooth_fx[i] = L_shr_r( hStereoDft->Spd_R_smooth_fx[i], sub( hStereoDft->Spd_R_smooth_fx_e, Spd_R_smooth_fx_tmp_e[i] ) );
1898 19392000 : move32();
1899 : }
1900 : /* END */
1901 : }
1902 : ELSE
1903 : {
1904 24779840 : FOR( i = 1; i < shr( NFFT, 1 ); i++ )
1905 : {
1906 : // hItd->xcorr_smooth[2 * i] = ( 1.f - sfm_L ) * hItd->xcorr_smooth[2 * i] + sfm_L * xcorr[2 * i];
1907 24735791 : hItd->xcorr_smooth_fx[2 * i] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm_L ), hItd->xcorr_smooth_fx[2 * i] ), xcorr_smooth_fx_tmp_e[2 * i],
1908 24735791 : Mpy_32_32( sfm_L, xcorr[2 * i] ), xcorr_e[2 * i], &xcorr_smooth_fx_tmp_e[2 * i] );
1909 24735791 : move32();
1910 : // hItd->xcorr_smooth[2 * i + 1] = ( 1.f - sfm_L ) * hItd->xcorr_smooth[2 * i + 1] + sfm_L * xcorr[2 * i + 1];
1911 24735791 : hItd->xcorr_smooth_fx[2 * i + 1] = BASOP_Util_Add_Mant32Exp( Mpy_32_32( L_sub( MAX_32, sfm_L ), hItd->xcorr_smooth_fx[2 * i + 1] ), xcorr_smooth_fx_tmp_e[2 * i + 1],
1912 24735791 : Mpy_32_32( sfm_L, xcorr[2 * i + 1] ), xcorr_e[2 * i + 1], &xcorr_smooth_fx_tmp_e[2 * i + 1] );
1913 24735791 : move32();
1914 : // tmpf1 = sqrtf( hItd->xcorr_smooth[i * 2] * hItd->xcorr_smooth[i * 2] + hItd->xcorr_smooth[i * 2 + 1] * hItd->xcorr_smooth[i * 2 + 1] );
1915 24735791 : L_temp = BASOP_Util_Add_Mant32Exp( Mpy_32_32( hItd->xcorr_smooth_fx[i * 2], hItd->xcorr_smooth_fx[i * 2] ), shl( xcorr_smooth_fx_tmp_e[2 * i], 1 ),
1916 24735791 : Mpy_32_32( hItd->xcorr_smooth_fx[i * 2 + 1], hItd->xcorr_smooth_fx[i * 2 + 1] ), shl( xcorr_smooth_fx_tmp_e[i * 2 + 1], 1 ), &L_temp_e );
1917 24735791 : L_temp = Sqrt32( L_temp, &L_temp_e );
1918 : // tmpf1 += EPSILON;
1919 24735791 : tmpf1 = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, EPSILON_FX_M, EPSILON_FX_E, &tmpf1_e );
1920 24735791 : tmpf2 = tmpf1;
1921 24735791 : move32();
1922 24735791 : tmpf2_e = tmpf1_e;
1923 24735791 : move16();
1924 : // tmpf1 = powf( tmpf1, alpha );
1925 24735791 : tmpf1 = BASOP_Util_fPow( tmpf1, tmpf1_e, alpha, 0, &tmpf1_e );
1926 : // tmpf3 += tmpf2 * tmpf1;
1927 24735791 : tmpf3 = BASOP_Util_Add_Mant32Exp( tmpf3, tmpf3_e, Mpy_32_32( tmpf2, tmpf1 ), add( tmpf2_e, tmpf1_e ), &tmpf3_e );
1928 : // xcorr[2 * i] = hItd->xcorr_smooth[2 * i] * tmpf1;
1929 24735791 : xcorr[2 * i] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i], tmpf1 );
1930 24735791 : move32();
1931 24735791 : xcorr_e[2 * i] = add( xcorr_smooth_fx_tmp_e[2 * i], tmpf1_e );
1932 24735791 : move16();
1933 : // xcorr[2 * i + 1] = hItd->xcorr_smooth[2 * i + 1] * tmpf1;
1934 24735791 : xcorr[2 * i + 1] = Mpy_32_32( hItd->xcorr_smooth_fx[2 * i + 1], tmpf1 );
1935 24735791 : move32();
1936 24735791 : xcorr_e[2 * i + 1] = add( xcorr_smooth_fx_tmp_e[2 * i + 1], tmpf1_e );
1937 24735791 : move16();
1938 : }
1939 : }
1940 :
1941 : // tmpf1 = (float) ( NFFT / 2 + 1 ) / tmpf3;
1942 74349 : tmpf1 = BASOP_Util_Divide3232_Scale_newton( add( shr( NFFT, 1 ), 1 ), tmpf3, &tmpf1_e );
1943 74349 : tmpf1_e = add( tmpf1_e, sub( 31, tmpf3_e ) );
1944 84397549 : FOR( i = 0; i < NFFT; i++ )
1945 : {
1946 : // xcorr[i] *= tmpf1;
1947 84323200 : xcorr[i] = Mpy_32_32( xcorr[i], tmpf1 );
1948 84323200 : move32();
1949 84323200 : xcorr_e[i] = add( xcorr_e[i], tmpf1_e );
1950 84323200 : move16();
1951 : }
1952 :
1953 : /* RESCALING TO COMMON EXP */
1954 74349 : max_exp = MIN_16;
1955 74349 : move16();
1956 74349 : Copy( xcorr_smooth_fx_tmp_e, hItd->xcorr_smooth_fx_e, STEREO_DFT_N_32k_ENC );
1957 74349 : max_exp = MIN_16;
1958 74349 : move16();
1959 95241069 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC; i++ )
1960 : {
1961 95166720 : max_exp = s_max( max_exp, xcorr_e[i] );
1962 : }
1963 : // xcorr_e_final = add( max_exp, 11 /*find_guarded_bits_fx( STEREO_DFT_N_32k_ENC ) */ );
1964 74349 : xcorr_e_final = max_exp;
1965 74349 : move16();
1966 95241069 : FOR( i = 0; i < STEREO_DFT_N_32k_ENC; i++ )
1967 : {
1968 95166720 : xcorr[i] = L_shr_r( xcorr[i], sub( xcorr_e_final, xcorr_e[i] ) ); // xcorr_e_final
1969 95166720 : move32();
1970 : }
1971 : /* END */
1972 :
1973 : /*calculate mean E ratio of main to background signal for cohSNR*/
1974 74349 : IF( EQ_16( hCPE->element_mode, IVAS_CPE_DFT ) )
1975 : {
1976 59659 : mEr = calc_mean_E_ratio_fx( hItd, hStereoDft->nbands, hStereoDft->band_limits, sfm_L, pNrgL, pNrgL_e, pNrgR, pNrgR_e, &total_mEr, &total_mEr_e, &mEr_e );
1977 : }
1978 : ELSE
1979 : {
1980 : Word16 nbands;
1981 : Word16 band_limits[STEREO_DFT_BAND_MAX + 1];
1982 :
1983 14690 : set16_fx( band_limits, 0, STEREO_DFT_BAND_MAX + 1 );
1984 14690 : set_band_limits_fx( &nbands, band_limits, hCPE->hStereoMdct->hDft_ana->NFFT );
1985 14690 : mEr = calc_mean_E_ratio_fx( hItd, nbands, band_limits, sfm_L, pNrgL, pNrgL_e, pNrgR, pNrgR_e, &total_mEr, &total_mEr_e, &mEr_e );
1986 : }
1987 :
1988 : /*calculate total cohSNR for frame in dB*/
1989 : // THIS PART NEEDS TO BE RECHECKED AGAINST NEW FLOAT CODE
1990 74349 : IF( BASOP_Util_Cmp_Mant32Exp( mEr, mEr_e, MAX_32, 0 ) > 0 )
1991 : {
1992 70857 : cohSNR = Mpy_32_32( 83886080, BASOP_Util_Log10( mEr, mEr_e ) ); // Q16 (cohSNR = 20 * log10f( mEr );)
1993 : }
1994 : ELSE
1995 : {
1996 3492 : cohSNR = 0;
1997 3492 : move32();
1998 : }
1999 :
2000 : /* collect UNCLR classifier parameters */
2001 : {
2002 : Word32 es_em, d_prodL_prodR;
2003 : Word16 es_em_e, d_prodL_prodR_e;
2004 :
2005 74349 : IF( BASOP_Util_Cmp_Mant32Exp( total_mEr, total_mEr_e, 1, 31 ) < 0 )
2006 : {
2007 12688 : hStereoClassif->unclr_fv_fx[E_cohSNR] = 0;
2008 12688 : move32();
2009 : }
2010 : ELSE
2011 : {
2012 61661 : hStereoClassif->unclr_fv_fx[E_cohSNR] = Mpy_32_32( 41943040 /*20 in Q21*/, BASOP_Util_Log10( total_mEr, total_mEr_e ) ); // Q15
2013 61661 : move32();
2014 : }
2015 :
2016 : // es_em = fabsf( sum_nrg_L - sum_nrg_R ) / ( sum_nrg_L + sum_nrg_R + 1e-5f );
2017 74349 : L_temp = L_abs( BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, L_negate( sum_nrg_R ), sum_nrg_R_e, &L_temp_e ) );
2018 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, sum_nrg_R, sum_nrg_R_e, &L_temp2_e );
2019 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( L_temp2, L_temp2_e, 1407374883, -16, &L_temp2_e );
2020 74349 : es_em = BASOP_Util_Divide3232_Scale_newton( L_temp, L_temp2, &es_em_e );
2021 74349 : es_em_e = add( es_em_e, sub( L_temp_e, L_temp2_e ) );
2022 :
2023 74349 : hStereoClassif->unclr_fv_fx[E_es_em] = L_shr_r( es_em, sub( 16, es_em_e ) ); // Q15
2024 74349 : move32();
2025 74349 : hStereoClassif->xtalk_fv_fx[E_es_em] = L_shr_r( es_em, sub( 16, es_em_e ) ); // Q15
2026 74349 : move32();
2027 :
2028 : // d_prodL_prodR = logf( max( prod_LL, prod_RR ) / ( min( prod_LL, prod_RR ) + 1e-5f ) + 1.0f );
2029 74349 : IF( BASOP_Util_Cmp_Mant32Exp( prod_LL, prod_LL_e, prod_RR, prod_RR_e ) > 0 )
2030 : {
2031 33430 : L_temp = prod_LL;
2032 33430 : move32();
2033 33430 : L_temp_e = prod_LL_e;
2034 33430 : move16();
2035 33430 : L_temp2 = prod_RR;
2036 33430 : move32();
2037 33430 : L_temp2_e = prod_RR_e;
2038 33430 : move16();
2039 : }
2040 : ELSE
2041 : {
2042 40919 : L_temp = prod_RR;
2043 40919 : move32();
2044 40919 : L_temp_e = prod_RR_e;
2045 40919 : move16();
2046 40919 : L_temp2 = prod_LL;
2047 40919 : move32();
2048 40919 : L_temp2_e = prod_LL_e;
2049 40919 : move16();
2050 : }
2051 74349 : L_temp2 = BASOP_Util_Add_Mant32Exp( L_temp2, L_temp2_e, 1407374883, -16, &L_temp2_e );
2052 74349 : d_prodL_prodR = BASOP_Util_Divide3232_Scale_newton( L_temp, L_temp2, &d_prodL_prodR_e );
2053 74349 : d_prodL_prodR_e = add( d_prodL_prodR_e, sub( L_temp_e, L_temp2_e ) );
2054 74349 : d_prodL_prodR = BASOP_Util_Add_Mant32Exp( d_prodL_prodR, d_prodL_prodR_e, MAX_32, 0, &d_prodL_prodR_e );
2055 74349 : d_prodL_prodR = BASOP_Util_Loge( d_prodL_prodR, d_prodL_prodR_e ); // Q25
2056 :
2057 74349 : hStereoClassif->unclr_fv_fx[E_d_prodL_prodR] = L_shr_r( d_prodL_prodR, 10 ); // Q15
2058 74349 : move32();
2059 74349 : hStereoClassif->xtalk_fv_fx[E_d_prodL_prodR] = L_shr_r( d_prodL_prodR, 10 ); // Q15
2060 74349 : move32();
2061 :
2062 74349 : sum_xcorr = 0;
2063 74349 : move32();
2064 74349 : sum_xcorr_e = 0;
2065 74349 : move16();
2066 23791680 : FOR( i = 1; i < NFFT_mid; i++ )
2067 : {
2068 : // xcorr_abs[i] = logf( xcorr_abs[i] / ( sum_nrg_L + sum_nrg_R + 1e-5f ) + 1e-5f );
2069 23717331 : L_temp = BASOP_Util_Add_Mant32Exp( sum_nrg_L, sum_nrg_L_e, sum_nrg_R, sum_nrg_R_e, &L_temp_e );
2070 23717331 : L_temp = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, 1407374883, -16, &L_temp_e );
2071 23717331 : L_temp2 = BASOP_Util_Divide3232_Scale_newton( xcorr_abs[i], L_temp, &L_temp2_e );
2072 23717331 : L_temp2_e = add( L_temp2_e, sub( xcorr_abs_e[i], L_temp_e ) );
2073 23717331 : L_temp2 = BASOP_Util_Add_Mant32Exp( L_temp2, L_temp2_e, 1407374883, -16, &L_temp2_e );
2074 23717331 : xcorr_abs[i] = BASOP_Util_Loge( L_temp2, L_temp2_e );
2075 23717331 : move32();
2076 23717331 : xcorr_abs_e[i] = 6;
2077 23717331 : move16();
2078 :
2079 : // sum_xcorr += xcorr_abs[i];
2080 23717331 : sum_xcorr = BASOP_Util_Add_Mant32Exp( sum_xcorr, sum_xcorr_e, xcorr_abs[i], xcorr_abs_e[i], &sum_xcorr_e );
2081 : }
2082 :
2083 74349 : hStereoClassif->unclr_fv_fx[E_sum_xcorr] = L_shr_r( sum_xcorr, sub( 16, sum_xcorr_e ) ); // Q15
2084 74349 : move32();
2085 74349 : hStereoClassif->xtalk_fv_fx[E_sum_xcorr] = L_shr_r( sum_xcorr, sub( 16, sum_xcorr_e ) ); // Q15
2086 74349 : move32();
2087 : }
2088 :
2089 : /* reset estimates when silence is detected*/
2090 : // if ( ( sum_nrg_L && sum_nrg_R ) < EPSILON )
2091 74349 : IF( ( BASOP_Util_Cmp_Mant32Exp( sum_nrg_L, sum_nrg_L_e, 0, 0 ) == 0 ) && ( BASOP_Util_Cmp_Mant32Exp( sum_nrg_R, sum_nrg_R_e, 0, 0 ) == 0 ) )
2092 : {
2093 0 : resetEstimates_fx( hItd );
2094 : }
2095 :
2096 : /*smooth cohSNR with time */
2097 : // if ( ( hItd->cohSNR - cohSNR ) < 10.0f )
2098 74349 : IF( LT_32( L_sub( hItd->cohSNR_fx, cohSNR ), 655360 /*10 in Q16*/ ) )
2099 : {
2100 : // tmpf1 = max( 0.05f, min( 0.25f, sfm_L * 0.5f ) );
2101 67643 : tmpf1 = L_max( 107374182, L_min( ONE_IN_Q29, L_shr( sfm_L, 1 ) ) );
2102 : // hItd->cohSNR = ( 1.f - tmpf1 ) * hItd->cohSNR + tmpf1 * cohSNR;
2103 67643 : hItd->cohSNR_fx = L_add( Mpy_32_32( L_sub( MAX_32, tmpf1 ), hItd->cohSNR_fx ), Mpy_32_32( tmpf1, cohSNR ) );
2104 67643 : move32();
2105 : }
2106 : ELSE
2107 : {
2108 : // hItd->cohSNR = hItd->cohSNR - 0.05f;
2109 6706 : hItd->cohSNR_fx = L_sub( hItd->cohSNR_fx, 3277 /*.05 in Q16*/ );
2110 6706 : move32();
2111 : }
2112 :
2113 74349 : cohSNR = hItd->cohSNR_fx;
2114 74349 : move32();
2115 :
2116 : // rfft( xcorr, trigo_enc, STEREO_DFT_N_32k_ENC, +1 );
2117 74349 : Scale_sig32( xcorr, STEREO_DFT_N_32k_ENC, negate( find_guarded_bits_fx( STEREO_DFT_N_32k_ENC ) ) );
2118 74349 : rfft_fx( xcorr, trigo_enc, STEREO_DFT_N_32k_ENC, +1 );
2119 74349 : Scale_sig32( xcorr, STEREO_DFT_N_32k_ENC, add( find_guarded_bits_fx( STEREO_DFT_N_32k_ENC ), xcorr_e_final ) ); // Q31
2120 :
2121 74349 : itd_td = hItd->td_itd_32k[k_offset]; /* This ITD always operates at 32kHz*/
2122 74349 : move32();
2123 74349 : shift = sub( STEREO_DFT_N_32k_ENC / 2, itd_td ) % STEREO_DFT_N_32k_ENC;
2124 74349 : split = sub( STEREO_DFT_N_32k_ENC, shift );
2125 :
2126 74349 : Copy32( &xcorr[0], &xcorr_itd[shift], split ); // xcorr_e_final
2127 74349 : Copy32( &xcorr[split], &xcorr_itd[0], shift ); // xcorr_e_final
2128 :
2129 74349 : Copy32( &xcorr_itd[STEREO_DFT_N_32k_ENC / 2 - XTALK_PHAT_LEN], gcc_phat, 2 * XTALK_PHAT_LEN + 1 ); // xcorr_e_final
2130 :
2131 :
2132 74349 : thres = peak_detect_fx( xcorr_itd, &tmpf1, &index, &zero_itd, cohSNR, hCPE->hCoreCoder[0]->vad_flag, &second_max, &second_max_lag, hItd->prev_itd, flag_noisy_speech_snr, hItd->detected_itd_flag, &hItd->prev_max_fx, &hItd->prev_index, &hItd->prev_avg_max_fx, &hItd->prev_avg_max_fx_e, &total_max, &thres_e );
2133 74349 : tmpf1_e = 0;
2134 74349 : move16();
2135 :
2136 : // hStereoClassif->ave_ener_L = sum_nrg_L / ( NFFT_mid * NFFT_mid );
2137 74349 : hStereoClassif->ave_ener_L_fx = BASOP_Util_Divide3232_Scale_newton( sum_nrg_L, L_mult0( NFFT_mid, NFFT_mid ), &hStereoClassif->ave_ener_L_fx_e );
2138 74349 : move32();
2139 74349 : hStereoClassif->ave_ener_L_fx_e = add( hStereoClassif->ave_ener_L_fx_e, sub( sum_nrg_L_e, 31 ) );
2140 74349 : move16();
2141 : // hStereoClassif->ave_ener_R = sum_nrg_R / ( NFFT_mid * NFFT_mid );
2142 74349 : hStereoClassif->ave_ener_R_fx = BASOP_Util_Divide3232_Scale_newton( sum_nrg_R, L_mult0( NFFT_mid, NFFT_mid ), &hStereoClassif->ave_ener_R_fx_e );
2143 74349 : move32();
2144 74349 : hStereoClassif->ave_ener_R_fx_e = add( hStereoClassif->ave_ener_R_fx_e, sub( sum_nrg_R_e, 31 ) );
2145 74349 : move16();
2146 :
2147 74349 : IF( EQ_32( hCPE->hCoreCoder[0]->input_Fs, 16000 ) )
2148 : {
2149 : // total_max *= 2.0f;
2150 16943 : total_max = L_shl_sat( total_max, 1 ); // should be rechecked for -10dB tests
2151 : }
2152 : // hStereoClassif->unclr_fv[E_xcorr_itd_value] = total_max;
2153 74349 : hStereoClassif->unclr_fv_fx[E_xcorr_itd_value] = L_shr_r( total_max, 16 - 0 ); // Q15
2154 74349 : move16();
2155 : // hStereoClassif->xtalk_fv[E_xcorr_itd_value] = total_max;
2156 74349 : hStereoClassif->xtalk_fv_fx[E_xcorr_itd_value] = hStereoClassif->unclr_fv_fx[E_xcorr_itd_value];
2157 74349 : move16();
2158 :
2159 :
2160 : /*for tonal music items increase thresholing by a factor up to 2.*/
2161 : // if ( hCPE->hCoreCoder[0]->sp_aud_decision0 && ( index - STEREO_DFT_ITD_MAX_ANA ) != hItd->prev_itd && !flag_noisy_speech_snr && hCPE->hCoreCoder[0]->vad_flag && tmpf1 < 0.3 )
2162 74349 : test();
2163 74349 : test();
2164 74349 : test();
2165 74349 : test();
2166 74349 : IF( hCPE->hCoreCoder[0]->sp_aud_decision0 && NE_16( sub( index, STEREO_DFT_ITD_MAX_ANA ), hItd->prev_itd ) && !flag_noisy_speech_snr && hCPE->hCoreCoder[0]->vad_flag && LT_32( tmpf1, 644245094 ) )
2167 : {
2168 : // thres *= 1.0f + 1.f * min( 1.f, max( 0.f, ( -1.0f * sfm_L + 0.5f ) / ( 0.5f - 0.2f ) ) );
2169 1360 : L_temp = BASOP_Util_Add_Mant32Exp( L_negate( sfm_L ), 0, ONE_IN_Q30, 0, &L_temp_e );
2170 1360 : L_temp2 = BASOP_Util_Divide3232_Scale_newton( L_temp, 644245094, &L_temp2_e );
2171 1360 : L_temp2_e = add( L_temp2_e, L_temp_e - 0 );
2172 1360 : L_temp2 = L_shl_sat( L_temp2, sub( L_temp2_e, 1 ) ); // Q30
2173 1360 : L_temp2_e = 1;
2174 1360 : move16();
2175 1360 : L_temp2 = L_min( ONE_IN_Q30, L_max( 0, L_temp2 ) );
2176 1360 : L_temp2 = L_add_sat( ONE_IN_Q30, L_temp2 ); // Q30
2177 1360 : thres = Mpy_32_32( thres, L_temp2 );
2178 1360 : thres_e = add( thres_e, L_temp2_e );
2179 : }
2180 74349 : thres = L_shl_sat( thres, thres_e ); // Q31 - should be rechecked for -10dB tests
2181 :
2182 74349 : itd_cal_flag = 0;
2183 74349 : move16();
2184 : /*smooth threshold value depending on sfm for music items*/
2185 : // if ( hCPE->hCoreCoder[0]->ini_frame == 0 || hCPE->last_element_mode != IVAS_CPE_DFT || !hCPE->hCoreCoder[0]->sp_aud_decision0 || flag_noisy_speech_snr || cohSNR < 20 )
2186 74349 : test();
2187 74349 : test();
2188 74349 : test();
2189 74349 : test();
2190 74349 : IF( hCPE->hCoreCoder[0]->ini_frame == 0 || NE_16( hCPE->last_element_mode, IVAS_CPE_DFT ) || !hCPE->hCoreCoder[0]->sp_aud_decision0 || flag_noisy_speech_snr || BASOP_Util_Cmp_Mant32Exp( cohSNR, 15, 20, 31 ) < 0 )
2191 : {
2192 70395 : hItd->itd_thres_fx = thres;
2193 70395 : move32();
2194 : }
2195 : ELSE
2196 : {
2197 : // hItd->itd_thres = ( 1.0f - sfm_L ) * hItd->itd_thres + sfm_L * thres;
2198 3954 : hItd->itd_thres_fx = L_add( Mpy_32_32( L_sub( MAX_32, sfm_L ), hItd->itd_thres_fx ), Mpy_32_32( sfm_L, thres ) );
2199 3954 : move32();
2200 : }
2201 :
2202 : // if ( flag_noisy_speech_snr == 0 && hCPE->hCoreCoder[0]->vad_flag == 1 && hItd->detected_itd_flag == 0 && ( hItd->currFlatness < 1.5f || hCPE->hCoreCoder[0]->sp_aud_decision0 == 1 ) )
2203 74349 : test();
2204 74349 : test();
2205 74349 : test();
2206 74349 : test();
2207 74349 : IF( flag_noisy_speech_snr == 0 && EQ_16( hCPE->hCoreCoder[0]->vad_flag, 1 ) && hItd->detected_itd_flag == 0 && ( LT_32( hItd->currFlatness_fx, 3145728 ) /* 1.5 in Q21*/ || EQ_16( hCPE->hCoreCoder[0]->sp_aud_decision0, 1 ) ) )
2208 : {
2209 : // hItd->itd_thres *= 1.5f;
2210 4610 : hItd->itd_thres_fx = L_shl_sat( Mpy_32_32( hItd->itd_thres_fx, 1610612736 ), 1 ); /* Saturation added to avoid assertions (this needs to be investigated) */
2211 4610 : move32();
2212 : }
2213 74349 : test();
2214 74349 : IF( hCPE->hCoreCoder[0]->vad_flag == 0 || hItd->detected_itd_flag == 0 )
2215 : {
2216 24827 : hItd->itd_tracking = 0;
2217 24827 : move16();
2218 : }
2219 49522 : ELSE IF( GT_16( abs_s( hItd->prev_itd ), 2 ) )
2220 : {
2221 34256 : hItd->itd_tracking = 1;
2222 34256 : move16();
2223 : }
2224 :
2225 : // if ( hItd->itd_tracking == 1 && ( second_max > hItd->itd_thres || tmpf1 - second_max < min( tmpf1 * 0.5f, 0.2f ) ) )
2226 74349 : test();
2227 74349 : test();
2228 74349 : if ( EQ_16( hItd->itd_tracking, 1 ) && ( GT_32( second_max, hItd->itd_thres_fx ) || LT_32( L_sub( tmpf1, second_max ), L_min( L_shr( tmpf1, 1 ), 429496730 /*.2f in Q31*/ ) ) ) )
2229 : {
2230 35363 : index = second_max_lag;
2231 35363 : move16();
2232 : }
2233 :
2234 : // if ( hItd->itd_tracking == 1 && abs( hItd->prev_itd - ( index - STEREO_DFT_ITD_MAX_ANA ) ) <= max( 2, (int16_t) round_f( abs( hItd->prev_itd ) / 16.f ) ) )
2235 74349 : test();
2236 74349 : IF( EQ_16( hItd->itd_tracking, 1 ) && LE_16( abs_s( sub( hItd->prev_itd, sub( index, STEREO_DFT_ITD_MAX_ANA ) ) ), s_max( 2, shr_r( abs_s( hItd->prev_itd ), 4 ) ) ) )
2237 : {
2238 : // hItd->itd_thres *= 0.75f;
2239 35363 : hItd->itd_thres_fx = Mpy_32_32( hItd->itd_thres_fx, 1610612736 );
2240 35363 : move32();
2241 : }
2242 :
2243 : // if ( tmpf1 > hItd->itd_thres && !zero_itd )
2244 74349 : test();
2245 74349 : IF( GT_32( tmpf1, hItd->itd_thres_fx ) && !zero_itd )
2246 : {
2247 : /* LP filter GCC PHAT peak to follow peak envelope */
2248 58120 : IF( GT_32( tmpf1, hItd->lp_phat_peak_fx ) )
2249 : {
2250 17663 : alpha = LP_GCC_PHAT_UP_Q31;
2251 17663 : move32();
2252 : }
2253 : ELSE
2254 : {
2255 40457 : alpha = LP_GCC_PHAT_DOWN_Q31;
2256 40457 : move32();
2257 : }
2258 : // hItd->lp_phat_peak = alpha * tmpf1 + ( 1 - alpha ) * hItd->lp_phat_peak;
2259 58120 : hItd->lp_phat_peak_fx = L_add( Mpy_32_32( alpha, tmpf1 ), Mpy_32_32( L_sub( MAX_32, alpha ), hItd->lp_phat_peak_fx ) );
2260 58120 : move32();
2261 58120 : hItd->itd_cnt = add( hItd->itd_cnt, 1 );
2262 58120 : move16();
2263 58120 : test();
2264 58120 : if ( GT_16( hItd->itd_cnt, ITD_CNT_MAX ) || hItd->itd_hangover > 0 )
2265 : {
2266 : /* If max count is reached, or if an ITD candidate is found during hangover,
2267 : set itd_cnt = ITD_CNT_MAX to ensure hangover is applied */
2268 55847 : hItd->itd_cnt = ITD_CNT_MAX;
2269 55847 : move16();
2270 : }
2271 58120 : hItd->itd_hangover = 0;
2272 58120 : move16();
2273 :
2274 58120 : itd = sub( index, STEREO_DFT_ITD_MAX_ANA );
2275 58120 : hItd->itd_nonzero_cnt = 0; /* (1+0+9) <= hItd->itd_nonzero_cnt <= (1+6+3) */
2276 58120 : move16();
2277 58120 : itd_cal_flag = 1; /* Indicates P>T case */
2278 58120 : move16();
2279 58120 : hItd->valid_itd_cnt = hItd->itd_cnt; /* Store last non-zero value (when P>T) before reset */
2280 58120 : move16();
2281 58120 : hItd->detected_itd_flag = 1;
2282 58120 : move16();
2283 : }
2284 : ELSE
2285 : {
2286 : /* Set prev_itd hangover period */
2287 16229 : IF( EQ_16( hItd->itd_cnt, ITD_CNT_MAX ) )
2288 : {
2289 : // hItd->itd_hangover = max( 0, min( ITD_HO_MAX, (int16_t) ( hItd->lp_phat_peak * ITD_HO_GCC_PHAT_INCL + ITD_HO_GCC_PHAT_OFFS ) ) );
2290 706 : hItd->itd_hangover = s_max( 0, s_min( ITD_HO_MAX, extract_l( L_shr( L_add( Mpy_32_32( hItd->lp_phat_peak_fx, ITD_HO_GCC_PHAT_INCL_Q26 ), ITD_HO_GCC_PHAT_OFFS_Q26 ), 26 ) ) ) );
2291 706 : move16();
2292 : }
2293 :
2294 16229 : IF( hItd->itd_hangover > 0 )
2295 : {
2296 2073 : itd = hItd->prev_itd;
2297 2073 : move16();
2298 2073 : if ( LT_16( hItd->itd_nonzero_cnt, MAX_ITD_VAD_HANGOVER ) )
2299 : {
2300 1653 : hItd->itd_nonzero_cnt = add( hItd->itd_nonzero_cnt, 1 );
2301 1653 : move16();
2302 : }
2303 2073 : hItd->itd_hangover = sub( hItd->itd_hangover, 1 );
2304 2073 : move16();
2305 2073 : hItd->detected_itd_flag = 1;
2306 2073 : move16();
2307 : }
2308 : ELSE
2309 : {
2310 14156 : itd = 0;
2311 14156 : move16();
2312 14156 : hItd->detected_itd_flag = 0;
2313 14156 : move16();
2314 : }
2315 :
2316 : /* Reset */
2317 16229 : hItd->itd_cnt = 0;
2318 16229 : move16();
2319 16229 : hItd->lp_phat_peak_fx = 0;
2320 16229 : move16();
2321 : }
2322 :
2323 : {
2324 : /* stereo Xtalk classifier */
2325 74349 : xtalk_classifier_dft_fx( hCPE, itd, gcc_phat );
2326 : }
2327 :
2328 : /*avoid enabling ITD fine control for music*/
2329 74349 : test();
2330 74349 : test();
2331 74349 : IF( !hCPE->hCoreCoder[0]->sp_aud_decision0 || flag_noisy_speech_snr || LT_32( cohSNR, 20 << 16 ) )
2332 : {
2333 : /* ITD fine control base on vad and correlation parameters */
2334 69601 : cor_lb_avrg = 0;
2335 69601 : move32();
2336 69601 : par_L_avrg = 0;
2337 69601 : move32();
2338 278404 : FOR( i = 0; i < XCORR_LB_NUM_BANDS; i++ )
2339 : {
2340 : // num_cor = xcorr_lb[i * XCORR_LB_BAND_WIDTH] * hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH];
2341 208803 : num_cor = Mpy_32_32( xcorr_lb[i * XCORR_LB_BAND_WIDTH], hItd->prev_xcorr_lb_fx[i * XCORR_LB_BAND_WIDTH] );
2342 208803 : num_cor_e = add( xcorr_lb_e[i * XCORR_LB_BAND_WIDTH], hItd->prev_xcorr_lb_fx_e );
2343 : // den_cor_cur = xcorr_lb[i * XCORR_LB_BAND_WIDTH] * xcorr_lb[i * XCORR_LB_BAND_WIDTH] + 1.0f;
2344 208803 : den_cor_cur = BASOP_Util_Add_Mant32Exp( Mpy_32_32( xcorr_lb[i * XCORR_LB_BAND_WIDTH], xcorr_lb[i * XCORR_LB_BAND_WIDTH] ), shl( xcorr_lb_e[i * XCORR_LB_BAND_WIDTH], 1 ), MAX_32, 0, &den_cor_cur_e );
2345 : // den_cor_prev = hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH] * hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH] + 1.0f;
2346 208803 : W_temp = W_mult0_32_32( hItd->prev_xcorr_lb_fx[i * XCORR_LB_BAND_WIDTH], hItd->prev_xcorr_lb_fx[i * XCORR_LB_BAND_WIDTH] );
2347 208803 : W_temp_q = add( shl( sub( 31, hItd->prev_xcorr_lb_fx_e ), 1 ), W_norm( W_temp ) );
2348 208803 : W_temp = W_shl( W_temp, W_norm( W_temp ) );
2349 208803 : L_temp = W_extract_h( W_temp );
2350 208803 : L_temp_e = sub( 63, W_temp_q );
2351 :
2352 208803 : den_cor_prev = BASOP_Util_Add_Mant32Exp( L_temp, L_temp_e, MAX_32, 0, &den_cor_prev_e );
2353 208803 : xcorr_max = xcorr_lb[i * XCORR_LB_BAND_WIDTH];
2354 208803 : xcorr_max_e = xcorr_lb_e[i * XCORR_LB_BAND_WIDTH];
2355 208803 : sum_nrg_L_tmp = xcorr_lb[i * XCORR_LB_BAND_WIDTH];
2356 208803 : sum_nrg_L_tmp_e = xcorr_lb_e[i * XCORR_LB_BAND_WIDTH];
2357 1670424 : FOR( j = 1; j < XCORR_LB_BAND_WIDTH; j++ )
2358 : {
2359 : // num_cor += xcorr_lb[i * XCORR_LB_BAND_WIDTH + j] * hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH + j];
2360 1461621 : num_cor = BASOP_Util_Add_Mant32Exp( num_cor, num_cor_e, Mpy_32_32( xcorr_lb[i * XCORR_LB_BAND_WIDTH + j], hItd->prev_xcorr_lb_fx[i * XCORR_LB_BAND_WIDTH + j] ),
2361 1461621 : add( xcorr_lb_e[i * XCORR_LB_BAND_WIDTH + j], hItd->prev_xcorr_lb_fx_e ), &num_cor_e );
2362 : // den_cor_cur += xcorr_lb[i * XCORR_LB_BAND_WIDTH + j] * xcorr_lb[i * XCORR_LB_BAND_WIDTH + j];
2363 1461621 : den_cor_cur = BASOP_Util_Add_Mant32Exp( den_cor_cur, den_cor_cur_e, Mpy_32_32( xcorr_lb[i * XCORR_LB_BAND_WIDTH + j], xcorr_lb[i * XCORR_LB_BAND_WIDTH + j] ),
2364 1461621 : shl( xcorr_lb_e[i * XCORR_LB_BAND_WIDTH + j], 1 ), &den_cor_cur_e );
2365 : // den_cor_prev += hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH + j] * hItd->prev_xcorr_lb[i * XCORR_LB_BAND_WIDTH + j];
2366 1461621 : tmp = hItd->prev_xcorr_lb_fx[i * XCORR_LB_BAND_WIDTH + j];
2367 1461621 : move32();
2368 1461621 : exp = norm_l( tmp );
2369 1461621 : tmp = L_shl( tmp, exp );
2370 1461621 : exp = sub( hItd->prev_xcorr_lb_fx_e, exp );
2371 1461621 : den_cor_prev = BASOP_Util_Add_Mant32Exp( den_cor_prev, den_cor_prev_e, Mpy_32_32( tmp, tmp ),
2372 1461621 : shl( exp, 1 ), &den_cor_prev_e );
2373 : // if ( xcorr_lb[i * XCORR_LB_BAND_WIDTH + j] > xcorr_max )
2374 1461621 : IF( BASOP_Util_Cmp_Mant32Exp( xcorr_lb[i * XCORR_LB_BAND_WIDTH + j], xcorr_lb_e[i * XCORR_LB_BAND_WIDTH + j], xcorr_max, xcorr_max_e ) > 0 )
2375 : {
2376 391814 : xcorr_max = xcorr_lb[i * XCORR_LB_BAND_WIDTH + j];
2377 391814 : move32();
2378 391814 : xcorr_max_e = xcorr_lb_e[i * XCORR_LB_BAND_WIDTH + j];
2379 391814 : move16();
2380 : }
2381 : // sum_nrg_L_tmp += xcorr_lb[i * XCORR_LB_BAND_WIDTH + j];
2382 1461621 : sum_nrg_L_tmp = BASOP_Util_Add_Mant32Exp( sum_nrg_L_tmp, sum_nrg_L_tmp_e, xcorr_lb[i * XCORR_LB_BAND_WIDTH + j], xcorr_lb_e[i * XCORR_LB_BAND_WIDTH + j], &sum_nrg_L_tmp_e );
2383 : }
2384 : // cor_lb[i] = num_cor / ( sqrtf( den_cor_cur ) * sqrtf( den_cor_prev ) );
2385 208803 : L_temp = Mpy_32_32( den_cor_cur, den_cor_prev );
2386 208803 : L_temp_e = add( den_cor_cur_e, den_cor_prev_e );
2387 208803 : L_temp = Sqrt32( L_temp, &L_temp_e );
2388 208803 : cor_lb[i] = BASOP_Util_Divide3232_Scale_newton( num_cor, L_temp, &cor_lb_e[i] );
2389 208803 : move32();
2390 208803 : cor_lb_e[i] = add( cor_lb_e[i], sub( num_cor_e, L_temp_e ) );
2391 208803 : move16();
2392 208803 : cor_lb[i] = L_shl_sat( cor_lb[i], cor_lb_e[i] ); // Q31
2393 208803 : move32();
2394 : // cor_lb_avrg += cor_lb[i];
2395 208803 : cor_lb_avrg = L_add( cor_lb_avrg, Mpy_32_32( cor_lb[i], ONE_BY_XCORR_LB_NUM_BANDS_Q31 ) );
2396 :
2397 : // par_L[i] = xcorr_max / ( sum_nrg_L_tmp + FLT_MIN );
2398 208803 : IF( xcorr_max )
2399 : {
2400 208803 : par_L[i] = BASOP_Util_Divide3232_Scale_newton( xcorr_max, sum_nrg_L_tmp, &par_L_e[i] );
2401 208803 : move32();
2402 208803 : par_L_e[i] = add( par_L_e[i], sub( xcorr_max_e, sum_nrg_L_tmp_e ) );
2403 208803 : move16();
2404 : }
2405 : ELSE
2406 : {
2407 0 : par_L[i] = 0;
2408 0 : move32();
2409 0 : par_L_e[i] = 0;
2410 0 : move16();
2411 : }
2412 208803 : par_L[i] = L_shl_sat( par_L[i], par_L_e[i] ); // Q31
2413 208803 : move32();
2414 : // par_L_avrg += par_L[i];
2415 208803 : par_L_avrg = L_add( par_L_avrg, Mpy_32_32( par_L[i], ONE_BY_XCORR_LB_NUM_BANDS_Q31 ) );
2416 : }
2417 : // cor_lb_avrg /= XCORR_LB_NUM_BANDS;
2418 : // par_L_avrg /= XCORR_LB_NUM_BANDS;
2419 :
2420 : /*Breakdown of fine-control conditions */
2421 : // fc_condition_1 = abs( hItd->prev_itd ) > 0.2f * abs( itd );
2422 69601 : fc_condition_1 = extract_l( GT_16( abs_s( hItd->prev_itd ), mult( 6554 /*0.2f*/, abs_s( itd ) ) ) );
2423 : // fc_condition_2 = cor_lb_avrg > 0.85f;
2424 69601 : fc_condition_2 = extract_l( GT_32( cor_lb_avrg, 1825361101 /*0.85f*/ ) );
2425 : // fc_condition_3 = ( cor_lb_avrg > 0.7f && ( cor_lb[0] > 0.9f || cor_lb[1] > 0.9f || cor_lb[2] > 0.9f ) && hItd->prev_sum_nrg_L_lb > 0.5f * sum_nrg_L_lb && hItd->prev_sum_nrg_L_lb < 2.0f * sum_nrg_L_lb );
2426 69601 : test();
2427 69601 : test();
2428 69601 : test();
2429 69601 : test();
2430 69601 : test();
2431 123457 : fc_condition_3 = extract_l( GT_32( cor_lb_avrg, 1503238554 /*0.7f*/ ) && ( GT_32( cor_lb[0], 1932735283 /*0.9f*/ ) || GT_32( cor_lb[1], 1932735283 /*0.9f*/ ) || GT_32( cor_lb[2], 1932735283 /*0.9f*/ ) ) &&
2432 53856 : BASOP_Util_Cmp_Mant32Exp( hItd->prev_sum_nrg_L_lb_fx, hItd->prev_sum_nrg_L_lb_fx_e, sum_nrg_L_lb, sub( sum_nrg_L_lb_e, 1 ) ) > 0 && BASOP_Util_Cmp_Mant32Exp( hItd->prev_sum_nrg_L_lb_fx, hItd->prev_sum_nrg_L_lb_fx_e, sum_nrg_L_lb, add( sum_nrg_L_lb_e, 1 ) ) < 0 );
2433 : // fc_condition_4 = par_L_avrg > 0.6f;
2434 69601 : fc_condition_4 = extract_l( GT_32( par_L_avrg, 1288490189 /*0.6f*/ ) );
2435 69601 : fc_condition_5 = hItd->prev_itd != 0;
2436 69601 : fc_condition_6_a = L_mult0( itd, hItd->prev_itd ) < 0; /* ITD sign change */
2437 69601 : fc_condition_6_b = L_mult0( itd, hItd->prev_itd ) == 0; /* ITD jump to zero */
2438 : // fc_condition_6_c = abs( itd - hItd->prev_itd ) > 0.5f * ( ( abs( itd ) > abs( hItd->prev_itd ) ) ? abs( itd ) : abs( hItd->prev_itd ) ); /* Magnitude of the ITD jump */
2439 69601 : fc_condition_6_c = extract_l( GT_16( abs_s( sub( itd, hItd->prev_itd ) ), shr( ( ( abs_s( itd ) > abs_s( hItd->prev_itd ) ) ? abs_s( itd ) : abs_s( hItd->prev_itd ) ), 1 ) ) ); /* Magnitude of the ITD jump */
2440 :
2441 69601 : IF( GT_16( abs_s( itd ), abs_s( hItd->prev_itd ) ) )
2442 : {
2443 2271 : fc_condition_6_c = extract_l( GT_16( abs_s( sub( itd, hItd->prev_itd ) ), shr( ( abs_s( itd ) ), 1 ) ) ); /* Magnitude of the ITD jump */
2444 : }
2445 : ELSE
2446 : {
2447 67330 : fc_condition_6_c = extract_l( GT_16( abs_s( sub( itd, hItd->prev_itd ) ), shr( ( abs_s( hItd->prev_itd ) ), 1 ) ) ); /* Magnitude of the ITD jump */
2448 : }
2449 :
2450 : /* Combining conditions 1,2,3,4 */
2451 69601 : test();
2452 69601 : test();
2453 69601 : test();
2454 69601 : fc_condition_1234 = fc_condition_1 && ( fc_condition_2 || fc_condition_3 || fc_condition_4 );
2455 :
2456 69601 : test();
2457 69601 : test();
2458 69601 : test();
2459 69601 : test();
2460 69601 : test();
2461 69601 : IF( ( fc_condition_1234 && ( ( fc_condition_5 && fc_condition_6_b ) || fc_condition_6_c ) ) || ( fc_condition_1234 && fc_condition_6_a ) )
2462 : {
2463 558 : itd = hItd->prev_itd;
2464 558 : move16();
2465 :
2466 558 : if ( LT_16( hItd->itd_nonzero_cnt, MAX_ITD_VAD_HANGOVER ) )
2467 : {
2468 440 : hItd->itd_nonzero_cnt = add( hItd->itd_nonzero_cnt, 1 );
2469 440 : move16();
2470 : }
2471 558 : hItd->detected_itd_flag = 1;
2472 558 : move16();
2473 : }
2474 :
2475 : /* stop the fine control when inactive or very high mssnr is detected*/
2476 : // if ( mssnr < 6e-7f * HIGHT_SNR_VOICE_TH || mssnr > 200 * HIGHT_SNR_VOICE_TH )
2477 69601 : test();
2478 69601 : if ( BASOP_Util_Cmp_Mant32Exp( mssnr, mssnr_e, 12884902 /*6e-7f * HIGHT_SNR_VOICE_TH */, 0 ) < 0 || BASOP_Util_Cmp_Mant32Exp( mssnr, mssnr_e, 200 * HIGHT_SNR_VOICE_TH_FX, 31 ) > 0 )
2479 : {
2480 13833 : hItd->itd_nonzero_cnt = MAX_ITD_VAD_HANGOVER;
2481 13833 : move16();
2482 : }
2483 :
2484 69601 : IF( vad_flag_itd )
2485 : {
2486 : /* Fine-control for hangover if set HR period = 0 or if HR period expires */
2487 : /* However fine-control shouldn't be used when HR is disabled because itd_cnt < 2 - hence the extra last condition */
2488 56356 : test();
2489 56356 : test();
2490 56356 : test();
2491 56356 : test();
2492 56356 : test();
2493 56356 : test();
2494 56356 : IF( hItd->itd_hangover == 0 && hItd->prev_itd != 0 && itd == 0 && NE_16( itd_cal_flag, 1 ) && LT_16( hItd->itd_nonzero_cnt, MAX_ITD_VAD_HANGOVER ) && EQ_16( hItd->valid_itd_cnt, ITD_CNT_MAX ) && EQ_16( hItd->pre_vad, 1 ) )
2495 : {
2496 96 : itd = hItd->prev_itd;
2497 96 : move16();
2498 96 : hItd->itd_nonzero_cnt = add( hItd->itd_nonzero_cnt, 1 );
2499 96 : move16();
2500 96 : hItd->detected_itd_flag = 1;
2501 96 : move16();
2502 : }
2503 56356 : hItd->pre_vad = 1;
2504 56356 : move16();
2505 : }
2506 : ELSE
2507 : {
2508 13245 : hItd->pre_vad = 0;
2509 13245 : move16();
2510 : }
2511 :
2512 69601 : test();
2513 69601 : if ( itd == 0 && NE_16( itd_cal_flag, 1 ) )
2514 : {
2515 13632 : hItd->itd_nonzero_cnt = 0;
2516 13632 : move16();
2517 : }
2518 :
2519 69601 : hItd->prev_sum_nrg_L_lb_fx = sum_nrg_L_lb;
2520 69601 : move32();
2521 69601 : hItd->prev_sum_nrg_L_lb_fx_e = sum_nrg_L_lb_e;
2522 69601 : move16();
2523 : // mvr2r( xcorr_lb, hItd->prev_xcorr_lb, STEREO_DFT_XCORR_LB_MAX );
2524 69601 : max_exp = MIN_16;
2525 69601 : move16();
2526 1740025 : FOR( i = 0; i < STEREO_DFT_XCORR_LB_MAX; i++ )
2527 : {
2528 1670424 : max_exp = s_max( max_exp, xcorr_lb_e[i] );
2529 : }
2530 1740025 : FOR( i = 0; i < STEREO_DFT_XCORR_LB_MAX; i++ )
2531 : {
2532 1670424 : hItd->prev_xcorr_lb_fx[i] = L_shr_r( xcorr_lb[i], sub( max_exp, xcorr_lb_e[i] ) );
2533 1670424 : move32();
2534 : }
2535 69601 : hItd->prev_xcorr_lb_fx_e = max_exp;
2536 69601 : move16();
2537 : }
2538 : /*save previous flag*/
2539 74349 : prev_itd_max = hItd->hybrid_itd_max;
2540 74349 : move16();
2541 : /* enable hybrid ITD handling for very large ITDs*/
2542 : // hItd->hybrid_itd_max = ( abs_s( itd ) > STEREO_DFT_ITD_MAX && abs_s( itd ) < STEREO_DFT_ITD_MAX_ANA && !hCPE->hCoreCoder[0]->sp_aud_decision0 && hCPE->element_brate < IVAS_32k );
2543 74349 : test();
2544 74349 : test();
2545 74349 : test();
2546 74349 : hItd->hybrid_itd_max = ( GT_16( abs_s( itd ), STEREO_DFT_ITD_MAX ) && LT_16( abs_s( itd ), STEREO_DFT_ITD_MAX_ANA ) && !hCPE->hCoreCoder[0]->sp_aud_decision0 && LT_32( hCPE->element_brate, IVAS_32k ) );
2547 74349 : move16();
2548 : /* Update memory */
2549 74349 : hItd->prev_itd = itd;
2550 74349 : move16();
2551 :
2552 74349 : itd = check_bounds_s_fx( itd, -STEREO_DFT_ITD_MAX, STEREO_DFT_ITD_MAX );
2553 :
2554 : /*Inverse the time diff*/
2555 : // hItd->itd[k_offset] = -1.f * itd;
2556 74349 : hItd->itd_fx[k_offset] = L_deposit_h( imult1616( -1, itd ) ); // Q16
2557 74349 : move32();
2558 :
2559 : /* collect UNCLR classifier parameters */
2560 74349 : hStereoClassif->unclr_fv_fx[E_ITD] = L_shr( hItd->itd_fx[k_offset], 1 ); // Q15
2561 74349 : move32();
2562 :
2563 :
2564 : /* limit ITD range for MDCT stereo even more */
2565 74349 : test();
2566 74349 : if ( EQ_16( hCPE->element_mode, IVAS_CPE_MDCT ) && GT_32( L_abs( hItd->itd_fx[k_offset] ), ( ITD_MAX_MDCT << 16 ) ) )
2567 : {
2568 3085 : itd = 0;
2569 3085 : move16();
2570 : }
2571 :
2572 74349 : stereo_dft_quantize_itd_fx( -1 * itd, hItd->itd_fx + k_offset, input_frame * FRAMES_PER_SEC, hItd->itd_index + k_offset );
2573 :
2574 : // hItd->deltaItd[k_offset] = hItd->itd[k_offset] - hItd->td_itd[k_offset];
2575 74349 : hItd->deltaItd_fx[k_offset] = L_sub( hItd->itd_fx[k_offset], L_deposit_h( hItd->td_itd[k_offset] ) );
2576 74349 : move32();
2577 :
2578 74349 : IF( hItd->hybrid_itd_max )
2579 : {
2580 : /*check if there is an ITD flip*/
2581 : // itd_max_flip = ( hItd->itd[k_offset] * hItd->itd[k_offset - 1] < 0 );
2582 486 : itd_max_flip = ( W_mult0_32_32( hItd->itd_fx[k_offset], hItd->itd_fx[k_offset - 1] ) < 0 );
2583 :
2584 486 : test();
2585 486 : IF( hItd->deltaItd_fx[k_offset - 1] != 0 && itd_max_flip == 0 )
2586 : {
2587 : // int16_t tmp_itd2 = (int16_t) floorf( ( ( hItd->prev_itd ) * ( (float) input_frame / 640 ) ) + 0.5f );
2588 465 : Word16 tmp_itd = extract_l( Mpy_32_32_r( L_mult0( hItd->prev_itd, input_frame ), 3355443 ) );
2589 : // hItd->deltaItd[k_offset] = -1.0f * tmp_itd - hItd->td_itd[k_offset];
2590 465 : hItd->deltaItd_fx[k_offset] = L_deposit_h( sub( imult1616( -1, tmp_itd ), hItd->td_itd[k_offset] ) );
2591 465 : move32();
2592 : }
2593 : }
2594 : /*signal change for next frame*/
2595 74349 : test();
2596 74349 : if ( EQ_16( prev_itd_max, 1 ) && hItd->hybrid_itd_max == 0 )
2597 : {
2598 16 : hItd->hybrid_itd_max = -1;
2599 16 : move16();
2600 : }
2601 :
2602 :
2603 74349 : return;
2604 : }
|
