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