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