Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include <stdint.h>
34 : #include <math.h>
35 : #include "options.h"
36 : #include "cnst.h"
37 : #include "ivas_cnst.h"
38 : #include "rom_com.h"
39 : #include "prot_fx.h"
40 : #include "ivas_prot_fx.h"
41 : #include "ivas_rom_com.h"
42 : #include "ivas_rom_com_fx.h"
43 : #include "ivas_rom_enc.h"
44 : #include "wmc_auto.h"
45 : #include "ivas_prot_fx.h"
46 :
47 :
48 : /*-----------------------------------------------------------------------*
49 : * Local constants
50 : *-----------------------------------------------------------------------*/
51 :
52 : #define STEREO_DMX_EVS_FIND_POC_PEAK_TAU 4
53 : #define STEREO_DMX_EVS_FIND_POC_PEAK_TAU2 8
54 :
55 : #define STEREO_DMX_EVS_POC_SMOOTH_Q30 1342177280
56 : #define STEREO_DMX_EVS_POC_FORGETTING_Q31 1675037245
57 : #define STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 1696512082
58 : #define STEREO_DMX_EVS_SHIFT_LIMIT_Q12 23040 /* ms */
59 :
60 :
61 : #define STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX 536870912 // 0.25f in Q31
62 :
63 : #define STEREO_DMX_EVS_CORR_FORGETTING_FX 1610612736 /*Q31*/
64 : #define STEREO_DMX_EVS_POC_W_FORGETTING_FX 1879048192 /*Q31*/
65 :
66 : #define Q_BAND_FX 536870912 /*Q31*/
67 :
68 : #define STEREO_DMX_EVS_ISD_THRES_L_Q31 1932735283
69 : #define STEREO_DMX_EVS_ISD_DIST_THRES_IPD_Q15 ONE_IN_Q14
70 :
71 : #define STEREO_DMX_EVS_ISD_FORGETTING_Q31 2040109465
72 : #define STEREO_DMX_EVS_ISD_1MFORGETTING_Q15 1638
73 : #define STEREO_DMX_EVS_ISD_DIST_HYST_L_Q31 773094113
74 : #define STEREO_DMX_EVS_ISD_DIST_HYST_H_Q31 923417968
75 : #define STEREO_DMX_EVS_ISD_INVTHRES_H 1270700383
76 :
77 : #define STEREO_DMX_EVS_ICCR_FORGETTING_Q31 1503238554
78 : #define STEREO_DMX_EVS_ICCR_HYST_L_Q31 1610612736
79 : #define STEREO_DMX_EVS_ICCR_HYST_H_Q31 1825361101
80 :
81 : #define STEREO_DMX_EVS_SWTCH_HYS_THRES 1
82 : #define STEREO_DMX_EVS_LR_EGY_Q27 2013265920
83 : #define STEREO_DMX_EVS_ILDS_EGY_Q17 1310720000
84 : #define STEREO_DMX_EVS_ILD_PRC_Q15 3277
85 :
86 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_16 55
87 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_32 19
88 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_48 29
89 :
90 : #define STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES 1
91 : #define STEREO_DMX_EVS_FADE_LEN_PRC_Q0 20
92 :
93 : #define STEREO_DMX_EVS_NB_SBFRM 5
94 : #define STEREO_DMX_EVS_TRNS_DTC_INST_Q0 75
95 : #define STEREO_DMX_EVS_CRST_FCTR_16_Q0 80
96 : #define STEREO_DMX_EVS_CRST_FCTR_32_Q0 40
97 : #define STEREO_DMX_EVS_CRST_FCTR_48_Q0 35
98 :
99 : #define STEREO_DMX_EVS_TRNS_EGY_FORGETTING_Q15 24576
100 :
101 : #define STEREO_DMX_EVS_POC_RENORM_TH 33554432 // 65536 << 9
102 : #define STEREO_DMX_EVS_POC_RENORM_SHIFT 3
103 :
104 : const Word32 ipd_ff_Q31[STEREO_DMX_EVS_NB_SUBBAND_MAX] = {
105 : 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264,
106 : 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264,
107 : 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264, 2027355264,
108 : 2027355264, 2023718656, 2020082048, 2016445440, 2012808832, 2009172224, 2005535616, 2001899008, 1998262400, 1994625664,
109 : 1990989056, 1987352448, 1983715840, 1980079232, 1976442624, 1972806016, 1969169408, 1965532800, 1961896192, 1958259584,
110 : 1954622976, 1950986368, 1947349760, 1943713152, 1940076544, 1936439936, 1932803328, 1929166592, 1925529984, 1921893376,
111 : 1918256768, 1914620160, 1910983552, 1907346944, 1903710336, 1900073728, 1896437120, 1892800512, 1889163904, 1885527296,
112 : 1881890688, 1878254080, 1874617344, 1870980864, 1867344128, 1863707520, 1860070912, 1856434304, 1852797696, 1849161088,
113 : 1845524480, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
114 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
115 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
116 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
117 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
118 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
119 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
120 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
121 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
122 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
123 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
124 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
125 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
126 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
127 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872,
128 : 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872, 1841887872
129 : };
130 : /*-----------------------------------------------------------------------*
131 : * Local function prototypes
132 : *-----------------------------------------------------------------------*/
133 :
134 : void estimate_itd_wnd_fft_fx(
135 : const Word32 *input, /* i : input signal Q16 */
136 : Word32 *specr, /* o : real-part spectra Q(31-spec_e) */
137 : Word32 *speci, /* o : imaginary-part spectra Q(31-spec_e) */
138 : Word16 *spec_e,
139 : const Word16 *rfft_coef, /* i : rfft coef Q15 */
140 : const Word32 *wnd, /* i : window coef Q31 */
141 : const Word16 input_frame /* i : input frame length per channel */
142 : );
143 : static void calc_poc_fx(
144 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
145 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
146 : const Word32 wnd[], /* i : window coef Q31 */
147 : const Word16 rfft_coef[], /* i : RFFT coef Q15 */
148 : const Word32 specLr[], /* i : Lch real-part spectra Q(31-spec_e) */
149 : const Word32 specLi[], /* i : Lch imaginary-part input signal Q(31-spec_e) */
150 : const Word32 specRr[], /* i : Rch real-part spectra Q(31-spec_e) */
151 : const Word32 specRi[], /* i : Rch imaginary-part input signal Q(31-spec_e) */
152 : const Word16 spec_e,
153 : const Word16 input_frame /* i : input frame length per channel */
154 : );
155 : static ivas_error estimate_itd_fx(
156 : Word16 *corr, /* o : correlation */
157 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
158 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
159 : const Word32 srcL[], /* i : Lch input signal Q16 */
160 : const Word32 srcR[], /* i : Rch input signal Q16 */
161 : Word16 itd[], /* o : estimated itd Q0 */
162 : const Word16 input_frame /* i : input frame length per channel */
163 : );
164 : static void adapt_gain_fx(
165 : const Word32 src_fx[], /* i : input signal Q16 */
166 : Word32 dst_fx[], /* o : output signal */
167 : const Word32 gain_fx, /* i : adapting gain Q31*/
168 : const Word32 old_gain_fx, /* i : adapting prev gain Q31*/
169 : const Word16 input_frame, /* i : input frame length per channel */
170 : const Word32 wnd_fx[] /* i : window coef Q31 */
171 : );
172 : static void weighted_ave_fx(
173 : const Word32 src1_fx[], /* i : Lch input signal Q16 */
174 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
175 : Word32 dst_fx[], /* o : output signal */
176 : const Word32 gain_fx, /* i : adapting gain Q31 */
177 : const Word32 old_gain_fx, /* i : adapting prev gain Q31 */
178 : const Word16 input_frame, /* i : input frame length per channel */
179 : const Word32 wnd_fx[] /* i : window coef Q31 */
180 : );
181 : static void calc_energy_fx(
182 : const Word32 src1_fx[], /* i : Lch input signal */
183 : const Word32 src2_fx[], /* i : Rch input signal */
184 : Word32 energy_fx[], /* o : calculated energy */
185 : Word16 *energy_fx_e, /* o : calculated energy */
186 : const Word16 input_frame, /* i : input frame length per channel */
187 : const Word32 ratio_float_fx );
188 :
189 : static void create_M_signal_fx(
190 : const Word32 srcL_fx[], /* i : Lch input signal Q16 */
191 : const Word32 srcR_fx[], /* i : Rch input signal Q16 */
192 : Word32 dmx_fx[], /* o : output signal Q31 */
193 : const Word32 w_curr_fx, /* i : adapting weight Q31 */
194 : const Word16 input_frame, /* i : input frame length per channel */
195 : const Word32 wnd_fx[], /* i : window coef Q31 */
196 : Word32 *w_prev_fx, /* i/o: adapting prev weight Q31*/
197 : Word32 *dmx_energy_fx, /* i/o: downmix signal energy dmx_energy_fx_e */
198 : Word16 *dmx_energy_fx_e, /* i/o: downmix signal energy */
199 : Word32 *src_energy_fx, /* i/o: input signal energy src_energy_fx_e */
200 : Word16 *src_energy_fx_e /* i/o: input signal energy */
201 : );
202 : static Word32 find_poc_peak_fx(
203 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
204 : Word16 itd_fx[], /* o : estimated itd */
205 : const Word16 input_frame, /* i : input frame length per channel */
206 : const Word32 ratio_fixed /* i : adapting ratio */
207 : );
208 : /*-------------------------------------------------------------------*
209 : * estimate_itd_wnd_fft()
210 : *
211 : * Transforms input signal from time domain into frequency domain.
212 : * The input signal is windowed before being transformed.
213 : *-------------------------------------------------------------------*/
214 4200 : void estimate_itd_wnd_fft_fx(
215 : const Word32 *input, /* i : input signal Q16 */
216 : Word32 *specr, /* o : real-part spectra Q(31-spec_e) */
217 : Word32 *speci, /* o : imaginary-part spectra Q(31-spec_e) */
218 : Word16 *spec_e,
219 : const Word16 *rfft_coef, /* i : rfft coef Q15 */
220 : const Word32 *wnd, /* i : window coef Q31 */
221 : const Word16 input_frame /* i : input frame length per channel */
222 : )
223 : {
224 : Word16 n0, i;
225 : Word32 rfft_buf[L_FRAME48k];
226 : Word16 step, bias;
227 : Word16 rshift;
228 :
229 4200 : n0 = shr( input_frame, 1 );
230 4200 : IF( EQ_16( input_frame, L_FRAME16k ) )
231 : {
232 0 : step = 3;
233 0 : move16();
234 0 : bias = 1;
235 0 : move16();
236 : }
237 : ELSE
238 : {
239 4200 : step = 1;
240 4200 : move16();
241 4200 : bias = 0;
242 4200 : move16();
243 : }
244 :
245 3364200 : FOR( i = 0; i < input_frame; i++ )
246 : {
247 : /* window */
248 3360000 : rfft_buf[i] = Mpy_32_32_r( input[i], wnd[i * step + bias] ); // Q16
249 3360000 : move32();
250 : }
251 :
252 4200 : rshift = sub( getScaleFactor32( rfft_buf, input_frame ), find_guarded_bits_fx( input_frame ) );
253 4200 : scale_sig32( rfft_buf, input_frame, rshift ); // Q(16+rshift)
254 4200 : *spec_e = sub( 15, rshift );
255 4200 : move16();
256 4200 : rfft_fx( rfft_buf, rfft_coef, input_frame, -1 );
257 :
258 1680000 : FOR( i = 1; i < n0; i++ )
259 : {
260 1675800 : specr[i] = rfft_buf[i * 2];
261 1675800 : move32();
262 1675800 : speci[i] = rfft_buf[i * 2 + 1];
263 1675800 : move32();
264 : }
265 :
266 4200 : specr[0] = rfft_buf[0];
267 4200 : move32();
268 4200 : specr[n0] = rfft_buf[1];
269 4200 : move32();
270 4200 : speci[0] = 0;
271 4200 : move32();
272 4200 : speci[n0] = 0;
273 4200 : move32();
274 :
275 4200 : return;
276 : }
277 :
278 : /*-------------------------------------------------------------------*
279 : * calc_poc()
280 : *
281 : * calculate phase only correlation
282 : *-------------------------------------------------------------------*/
283 2100 : static void calc_poc_fx(
284 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
285 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
286 : const Word32 wnd[], /* i : window coef Q31 */
287 : const Word16 rfft_coef[], /* i : RFFT coef Q15 */
288 : const Word32 specLr[], /* i : Lch real-part spectra Q(31-spec_e) */
289 : const Word32 specLi[], /* i : Lch imaginary-part input signal Q(31-spec_e) */
290 : const Word32 specRr[], /* i : Rch real-part spectra Q(31-spec_e) */
291 : const Word32 specRi[], /* i : Rch imaginary-part input signal Q(31-spec_e) */
292 : const Word16 spec_e,
293 : const Word16 input_frame /* i : input frame length per channel */
294 : )
295 : {
296 : Word16 i, n1, n2;
297 : Word16 n0, *itdLR;
298 : const Word16 *s;
299 : Word32 *P;
300 : Word32 tmp1, tmp2, Lr, Li, Rr, Ri, gamma, igamma /*, iN*/;
301 :
302 : Word32 specPOr[L_FRAME48k / 2 + 1], specPOi[L_FRAME48k / 2]; /*real and imaginary values for searching phase angle Q31*/
303 : Word32 tmpPOC1[L_FRAME48k], tmpPOC2[L_FRAME48k];
304 : Word32 rfft_buf[L_FRAME48k]; // Q21
305 : Word16 step, bias;
306 : Word16 mult_angle;
307 : Word16 j;
308 : Word16 end;
309 :
310 : Word16 cos_step, cos_max;
311 : Word32 eps_cos, eps_sin, EPS;
312 :
313 : Word16 isd_cnt_h, isd_cnt_l, ild_cnt, n, freq_8k, freq_ipd_max, nsbd, input_frame_pha;
314 : Word32 Nr, Ni, Dr, Di, tPr, tPi, Pn, energy;
315 : Word16 Nr_e, Ni_e, tPr_e, tPi_e, Pn_e, energy_e;
316 : Word16 isd_rate, isd_rate_e;
317 : Word32 eneL, eneR, IPDr, IPDi, tIPDr, tIPDi, ICCr;
318 : Word16 eneL_e, eneR_e, IPDr_e, IPDi_e;
319 : Word32 *Pr, *Pi, *ipd_ff, *p_curr_taps;
320 : Word32 rfft_pha_buf[L_FRAME48k] /*Q22*/, tEr[STEREO_DMX_EVS_NB_SUBBAND_MAX], tEl[STEREO_DMX_EVS_NB_SUBBAND_MAX];
321 : Word16 tEr_e[STEREO_DMX_EVS_NB_SUBBAND_MAX], tEl_e[STEREO_DMX_EVS_NB_SUBBAND_MAX];
322 :
323 : Word32 L_tmp, L_tmp1, L_tmp2;
324 : Word16 L_tmp_e, L_tmp1_e, L_tmp2_e;
325 : Word64 W_tmp;
326 : Word16 W_tmp_q;
327 :
328 : /* Initialization */
329 : // iN = 1.0f / (float) input_frame;
330 2100 : s = hPOC->sin_fx;
331 2100 : P = hPOC->P_fx;
332 2100 : n0 = shr( input_frame, 1 );
333 2100 : itdLR = hPOC->itdLR;
334 :
335 2100 : Pr = hPHA->Pr_fx;
336 2100 : Pi = hPHA->Pi_fx;
337 : // nsbd = n0 / STEREO_DMX_EVS_SUBBAND_SIZE;
338 2100 : nsbd = shr( n0, 1 );
339 : // input_frame_pha = input_frame / STEREO_DMX_EVS_SUBBAND_SIZE;
340 2100 : input_frame_pha = shr( input_frame, 1 );
341 :
342 : // igamma = STEREO_DMX_EVS_POC_GAMMA * iN;
343 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
344 : {
345 0 : igamma = 5033165; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
346 0 : move16();
347 : }
348 2100 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
349 : {
350 1050 : igamma = 3355444; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
351 1050 : move16();
352 : }
353 1050 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
354 : {
355 1050 : igamma = 1677722; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
356 1050 : move16();
357 : }
358 : ELSE
359 : {
360 0 : igamma = 1677722; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
361 0 : move16();
362 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
363 : }
364 2100 : gamma = L_sub( MAX_32, igamma );
365 :
366 2100 : step = 1;
367 2100 : move16();
368 2100 : bias = 0;
369 2100 : move16();
370 2100 : cos_step = 2;
371 2100 : move16();
372 2100 : cos_max = n0;
373 2100 : move16();
374 2100 : mult_angle = 3;
375 2100 : move16();
376 :
377 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
378 : {
379 0 : step = 3;
380 0 : move16();
381 0 : bias = 1;
382 0 : move16();
383 0 : cos_step = 4;
384 0 : move16();
385 0 : cos_max = input_frame;
386 0 : move16();
387 0 : mult_angle = 2; /*****/
388 0 : move16();
389 : }
390 2100 : if ( EQ_16( input_frame, L_FRAME32k ) )
391 : {
392 1050 : mult_angle = 2;
393 1050 : move16();
394 : }
395 :
396 2100 : end = s_min( n0, 320 );
397 : // specPOr[0] = sign( specLr[0] * specRr[0] ) * wnd[bias];
398 2100 : specPOr[0] = imult3216( wnd[bias], sign_fx( Mpy_32_32( specLr[0], specRr[0] ) ) );
399 2100 : move32();
400 2100 : specPOi[0] = 0;
401 2100 : move32();
402 2100 : EPS = hPOC->eps_fx;
403 2100 : move16();
404 2100 : IF( EQ_32( input_frame, L_FRAME48k ) )
405 : {
406 252000 : FOR( i = 1; i < n0 / 2; i++ )
407 : {
408 : // eps_cos = s[cos_max - i * cos_step /*cos_max - i_for*/] * EPS;
409 250950 : eps_cos = Mpy_32_16_1( EPS, s[cos_max - i * cos_step /*cos_max - i_for*/] );
410 : // eps_sin = s[i * cos_step /*i_for*/] * EPS;
411 250950 : eps_sin = Mpy_32_16_1( EPS, s[i * cos_step /*i_for*/] );
412 :
413 250950 : Lr = L_add( L_add( specLr[i], Mpy_32_32_r( specRr[i], eps_cos ) ), Mpy_32_32_r( specRi[i], eps_sin ) );
414 250950 : Li = L_add( L_sub( specLi[i], Mpy_32_32_r( specRr[i], eps_sin ) ), Mpy_32_32_r( specRi[i], eps_cos ) );
415 250950 : Rr = L_add( L_add( specRr[i], Mpy_32_32_r( specLr[i], eps_cos ) ), Mpy_32_32_r( specLi[i], eps_sin ) );
416 250950 : Ri = L_add( L_sub( specRi[i], Mpy_32_32_r( specLr[i], eps_sin ) ), Mpy_32_32_r( specLi[i], eps_cos ) );
417 :
418 250950 : IF( ( LT_32( L_abs( Lr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
419 : ( LT_32( L_abs( Li ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
420 : ( LT_32( L_abs( Rr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
421 : ( LT_32( L_abs( Ri ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
422 : {
423 223262 : Lr = L_shl( Lr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
424 223262 : Li = L_shl( Li, STEREO_DMX_EVS_POC_RENORM_SHIFT );
425 223262 : Rr = L_shl( Rr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
426 223262 : Ri = L_shl( Ri, STEREO_DMX_EVS_POC_RENORM_SHIFT );
427 : }
428 :
429 250950 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
430 250950 : move32();
431 250950 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
432 250950 : move32();
433 250950 : j = sub( n0, i );
434 250950 : IF( LT_16( j, 320 ) )
435 : {
436 82950 : Lr = L_add( L_sub( specLr[j], Mpy_32_32_r( specRr[j], eps_cos ) ), Mpy_32_32_r( specRi[j], eps_sin ) );
437 82950 : Li = L_sub( L_sub( specLi[j], Mpy_32_32_r( specRr[j], eps_sin ) ), Mpy_32_32_r( specRi[j], eps_cos ) );
438 82950 : Rr = L_add( L_sub( specRr[j], Mpy_32_32_r( specLr[j], eps_cos ) ), Mpy_32_32_r( specLi[j], eps_sin ) );
439 82950 : Ri = L_sub( L_sub( specRi[j], Mpy_32_32_r( specLr[j], eps_sin ) ), Mpy_32_32_r( specLi[j], eps_cos ) );
440 :
441 82950 : IF( ( LT_32( L_abs( Lr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
442 : ( LT_32( L_abs( Li ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
443 : ( LT_32( L_abs( Rr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
444 : ( LT_32( L_abs( Ri ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
445 : {
446 80611 : Lr = L_shl( Lr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
447 80611 : Li = L_shl( Li, STEREO_DMX_EVS_POC_RENORM_SHIFT );
448 80611 : Rr = L_shl( Rr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
449 80611 : Ri = L_shl( Ri, STEREO_DMX_EVS_POC_RENORM_SHIFT );
450 : }
451 :
452 82950 : specPOr[j] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
453 82950 : move32();
454 82950 : specPOi[j] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
455 82950 : move32();
456 : }
457 : }
458 : }
459 : ELSE /* 16kHz and 32 kHz*/
460 : {
461 168000 : FOR( i = 1; i < n0 / 2; i++ )
462 : {
463 : // eps_cos = s[cos_max - i * cos_step /*cos_max - i_for*/] * EPS;
464 166950 : eps_cos = Mpy_32_16_1( EPS, s[cos_max - i * cos_step /*cos_max - i_for*/] );
465 : // eps_sin = s[i * cos_step /*i_for*/] * EPS;
466 166950 : eps_sin = Mpy_32_16_1( EPS, s[i * cos_step /*i_for*/] );
467 :
468 166950 : Lr = L_add( L_add( specLr[i], Mpy_32_32_r( specRr[i], eps_cos ) ), Mpy_32_32_r( specRi[i], eps_sin ) );
469 166950 : Li = L_add( L_sub( specLi[i], Mpy_32_32_r( specRr[i], eps_sin ) ), Mpy_32_32_r( specRi[i], eps_cos ) );
470 166950 : Rr = L_add( L_add( specRr[i], Mpy_32_32_r( specLr[i], eps_cos ) ), Mpy_32_32_r( specLi[i], eps_sin ) );
471 166950 : Ri = L_add( L_sub( specRi[i], Mpy_32_32_r( specLr[i], eps_sin ) ), Mpy_32_32_r( specLi[i], eps_cos ) );
472 :
473 166950 : IF( ( LT_32( L_abs( Lr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
474 : ( LT_32( L_abs( Li ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
475 : ( LT_32( L_abs( Rr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
476 : ( LT_32( L_abs( Ri ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
477 : {
478 145526 : Lr = L_shl( Lr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
479 145526 : Li = L_shl( Li, STEREO_DMX_EVS_POC_RENORM_SHIFT );
480 145526 : Rr = L_shl( Rr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
481 145526 : Ri = L_shl( Ri, STEREO_DMX_EVS_POC_RENORM_SHIFT );
482 : }
483 :
484 166950 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
485 166950 : move32();
486 166950 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
487 166950 : move32();
488 :
489 166950 : j = sub( n0, i );
490 166950 : Lr = L_add( L_sub( specLr[j], Mpy_32_32_r( specRr[j], eps_cos ) ), Mpy_32_32_r( specRi[j], eps_sin ) );
491 166950 : Li = L_sub( L_sub( specLi[j], Mpy_32_32_r( specRr[j], eps_sin ) ), Mpy_32_32_r( specRi[j], eps_cos ) );
492 166950 : Rr = L_add( L_sub( specRr[j], Mpy_32_32_r( specLr[j], eps_cos ) ), Mpy_32_32_r( specLi[j], eps_sin ) );
493 166950 : Ri = L_sub( L_sub( specRi[j], Mpy_32_32_r( specLr[j], eps_sin ) ), Mpy_32_32_r( specLi[j], eps_cos ) );
494 :
495 166950 : IF( ( LT_32( L_abs( Lr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
496 : ( LT_32( L_abs( Li ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
497 : ( LT_32( L_abs( Rr ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
498 : ( LT_32( L_abs( Ri ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
499 : {
500 159995 : Lr = L_shl( Lr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
501 159995 : Li = L_shl( Li, STEREO_DMX_EVS_POC_RENORM_SHIFT );
502 159995 : Rr = L_shl( Rr, STEREO_DMX_EVS_POC_RENORM_SHIFT );
503 159995 : Ri = L_shl( Ri, STEREO_DMX_EVS_POC_RENORM_SHIFT );
504 : }
505 :
506 166950 : specPOr[j] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
507 166950 : move32();
508 166950 : specPOi[j] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
509 166950 : move32();
510 : }
511 : }
512 : {
513 : /* i=n0/2*/
514 2100 : Lr = L_add( specLr[i], Mpy_32_32_r( specRi[i], EPS ) );
515 2100 : Li = L_sub( specLi[i], Mpy_32_32_r( specRr[i], EPS ) );
516 2100 : Rr = L_add( specRr[i], Mpy_32_32_r( specLi[i], EPS ) );
517 2100 : Ri = L_sub( specRi[i], Mpy_32_32_r( specLr[i], EPS ) );
518 2100 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) );
519 2100 : move32();
520 2100 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) );
521 2100 : move32();
522 : }
523 : /* complex spectrum (specPOr[i], specPOi[i]) are placed on an unit circle without using srqt()*/
524 21000 : FOR( i = 1; i < 10; i++ ) /*search from 4 angles */
525 : {
526 18900 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma );
527 :
528 18900 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), 28377 /*0.866f*/ ) ); /* low angles are more frequent for low frequency */
529 18900 : move32();
530 18900 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), ONE_IN_Q14 /*0.5f*/ ) );
531 18900 : move32();
532 18900 : gamma = L_sub( gamma, igamma );
533 : }
534 33600 : FOR( ; i < n0 >> 4; i++ ) /*search from 4 angles */
535 : {
536 31500 : tmp1 = Mpy_32_16_1( Mpy_32_32_r( wnd[i * step + bias], gamma ), 23170 /*0.7071f*/ );
537 :
538 31500 : specPOr[i] = imult3216( tmp1, sign_fx( specPOr[i] ) );
539 31500 : move32();
540 31500 : specPOi[i] = imult3216( tmp1, sign_fx( specPOi[i] ) ); /* low accuracy is adequate for low frequency */
541 31500 : move32();
542 31500 : gamma = L_sub( gamma, igamma );
543 : }
544 :
545 54600 : FOR( ; i < n0 >> 3; i++ ) /* binary search from 8 angles */
546 : {
547 52500 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma ); // Q31
548 :
549 52500 : IF( W_mult0_32_32( L_sub( specPOr[i], specPOi[i] ), L_add( specPOr[i], specPOi[i] ) ) > 0 )
550 : {
551 27583 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), /*0.923880f*/ s[120 * mult_angle] ) ); /* cos(PI/8) Q31*/
552 27583 : move32();
553 27583 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), /*0.382683f*/ s[40 * mult_angle] ) ); // Q31
554 27583 : move32();
555 : }
556 : ELSE
557 : {
558 24917 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), /*0.382683f*/ s[40 * mult_angle] ) ); /* cos(PI*3/8) Q31*/
559 24917 : move32();
560 24917 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), /*0.923880f*/ s[120 * mult_angle] ) ); // Q31
561 24917 : move32();
562 : }
563 52500 : gamma = L_sub( gamma, igamma );
564 : }
565 569100 : FOR( ; i < end; i++ ) /* binary search from 16 angles */
566 : {
567 567000 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma ); // Q31
568 567000 : IF( W_mult0_32_32( L_sub( specPOr[i], specPOi[i] ), L_add( specPOr[i], specPOi[i] ) ) > 0 )
569 : {
570 314413 : IF( W_mult0_32_32( L_sub( Mpy_32_16_1( specPOr[i], 13573 /*0.414213f*/ ), specPOi[i] ), L_add( Mpy_32_16_1( specPOr[i], 13573 /*0.414213f*/ ), specPOi[i] ) ) > 0 ) /*tan(PI/8)*/
571 : {
572 181678 : specPOr[i] = Mpy_32_16_1( tmp1 /*0.980785f */, imult1616( sign_fx( specPOr[i] ), s[140 * mult_angle] ) ); /* cos(PI/16) Q31*/
573 181678 : move32();
574 181678 : specPOi[i] = Mpy_32_16_1( tmp1 /*0.195090f */, imult1616( sign_fx( specPOi[i] ), s[20 * mult_angle] ) ); // Q31
575 181678 : move32();
576 : }
577 : ELSE
578 : {
579 132735 : specPOr[i] = Mpy_32_16_1( tmp1 /* 0.831470f */, imult1616( sign_fx( specPOr[i] ), s[100 * mult_angle] ) ); /*cos(PI*3/16) Q31*/
580 132735 : move32();
581 132735 : specPOi[i] = Mpy_32_16_1( tmp1 /* 0.555570f*/, imult1616( sign_fx( specPOi[i] ), s[60 * mult_angle] ) ); // Q31
582 132735 : move32();
583 : }
584 : }
585 : ELSE
586 : {
587 252587 : IF( W_mult0_32_32( L_sub( specPOr[i], Mpy_32_16_1( specPOi[i], 13573 /*0.414213f*/ ) ), L_add( specPOr[i], Mpy_32_16_1( specPOi[i], 13573 /*0.414213f*/ ) ) ) > 0 ) /*tan(PI/8)*/
588 : {
589 127459 : specPOr[i] = Mpy_32_16_1( tmp1 /** 0.555570f*/, imult1616( sign_fx( specPOr[i] ), s[60 * mult_angle] ) ); /*cos(PI*5/16) Q31*/
590 127459 : move32();
591 127459 : specPOi[i] = Mpy_32_16_1( tmp1 /** 0.831470f*/, imult1616( sign_fx( specPOi[i] ), s[100 * mult_angle] ) ); // Q31
592 127459 : move32();
593 : }
594 : ELSE
595 : {
596 125128 : specPOr[i] = Mpy_32_16_1( tmp1 /** 0.195090f*/, imult1616( sign_fx( specPOr[i] ), s[20 * mult_angle] ) ); /*cos(PI*7/16) Q31*/
597 125128 : move32();
598 125128 : specPOi[i] = Mpy_32_16_1( tmp1 /** 0.980785f*/, imult1616( sign_fx( specPOi[i] ), s[140 * mult_angle] ) ); // Q31
599 125128 : move32();
600 : }
601 : }
602 567000 : gamma = L_sub( gamma, igamma );
603 : }
604 :
605 2100 : IF( LT_16( i, n0 ) )
606 : {
607 1050 : gamma = L_sub( gamma, imult3216( igamma, sub( n0, 320 ) ) );
608 : }
609 170100 : FOR( ; i < n0; i++ ) /*neglect higher frequency bins when 48 kHz samplng*/
610 : {
611 168000 : specPOr[i] = 0;
612 168000 : move32();
613 168000 : specPOi[i] = 0;
614 168000 : move32();
615 : }
616 2100 : specPOr[n0] = imult3216( Mpy_32_32_r( wnd[i * step + bias], gamma ), sign_fx( Mpy_32_32_r( specLr[n0], specRr[n0] ) ) ); // Q31
617 2100 : move32();
618 :
619 :
620 2100 : hPHA->init_frmCntr = sub( hPHA->init_frmCntr, 1 );
621 2100 : move16();
622 2100 : if ( hPHA->init_frmCntr < 0 )
623 : {
624 2080 : hPHA->init_frmCntr = 0;
625 2080 : move16();
626 : }
627 2100 : freq_8k = L_FRAME16k / 2;
628 2100 : move16();
629 : // freq_ipd_max = (int16_t) ( freq_8k * 5000.0f / ( 8000.0f * STEREO_DMX_EVS_SUBBAND_SIZE ) );
630 2100 : freq_ipd_max = 50;
631 2100 : move16();
632 :
633 : /* Memorize the filters N-1 */
634 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
635 : {
636 4200 : IF( hPHA->p_curr_taps_fx[n] )
637 : {
638 854 : hPHA->p_prev_taps_fx[n] = hPHA->prev_taps_fx[n];
639 854 : Copy32( hPHA->p_curr_taps_fx[n], hPHA->p_prev_taps_fx[n], hPHA->pha_len );
640 : }
641 : ELSE
642 : {
643 3346 : hPHA->p_prev_taps_fx[n] = NULL;
644 : }
645 : }
646 :
647 : /* ISD */
648 2100 : isd_cnt_l = 0;
649 2100 : move16();
650 2100 : isd_cnt_h = 0;
651 2100 : move16();
652 338100 : FOR( i = 1; i <= freq_8k; i++ )
653 : {
654 336000 : Nr = L_sub( specLr[i], specRr[i] ); // spec_e
655 336000 : Ni = L_sub( specLi[i], specRi[i] ); // spec_e
656 336000 : Dr = L_add( specLr[i], specRr[i] ); // spec_e
657 336000 : Di = L_add( specLi[i], specRi[i] ); // spec_e
658 : // if ( ( Nr * Nr + Ni * Ni ) > STEREO_DMX_EVS_ISD_THRES_H * ( Dr * Dr + Di * Di ) )
659 336000 : IF( GT_32( Mpy_32_32_r( L_add( Mpy_32_32_r( Nr, Nr ), Mpy_32_32_r( Ni, Ni ) ), STEREO_DMX_EVS_ISD_INVTHRES_H ), L_add( Mpy_32_32_r( Dr, Dr ), Mpy_32_32_r( Di, Di ) ) ) )
660 : {
661 102859 : isd_cnt_h = add( isd_cnt_h, 1 );
662 : }
663 336000 : W_tmp = W_add( W_mult0_32_32( Mpy_32_32_r( Dr, STEREO_DMX_EVS_ISD_THRES_L_Q31 ), Dr ), W_mult0_32_32( Mpy_32_32_r( Di, STEREO_DMX_EVS_ISD_THRES_L_Q31 ), Di ) ); // Q62
664 336000 : IF( LT_64( W_add( W_mult0_32_32( Nr, Nr ), W_mult0_32_32( Ni, Ni ) ), W_tmp ) ) // Q62
665 : {
666 179725 : isd_cnt_l = add( isd_cnt_l, 1 );
667 : }
668 : }
669 :
670 2100 : isd_rate = BASOP_Util_Divide1616_Scale( isd_cnt_h, freq_8k, &isd_rate_e );
671 : // Saturation to handle values close to 1.0f
672 2100 : isd_rate = shl_sat( isd_rate, isd_rate_e ); // Q15
673 2100 : hPHA->isd_rate_s_fx = L_add( Mpy_32_32_r( STEREO_DMX_EVS_ISD_FORGETTING_Q31, hPHA->isd_rate_s_fx ), L_mult( STEREO_DMX_EVS_ISD_1MFORGETTING_Q15, isd_rate ) );
674 2100 : move32();
675 :
676 2100 : IF( GT_32( hPHA->isd_rate_s_fx, STEREO_DMX_EVS_ISD_DIST_HYST_H_Q31 ) )
677 : {
678 0 : IF( NE_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD ) )
679 : {
680 0 : IF( EQ_32( hPHA->prev_pha, STEREO_DMX_EVS_PHA_IPD ) )
681 : {
682 0 : hPHA->pha_hys_cnt = add( hPHA->pha_hys_cnt, 1 );
683 0 : move16();
684 : }
685 : ELSE
686 : {
687 0 : hPHA->pha_hys_cnt = 0;
688 0 : move16();
689 : }
690 :
691 0 : if ( GE_32( hPHA->pha_hys_cnt, STEREO_DMX_EVS_SWTCH_HYS_THRES ) )
692 : {
693 0 : hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD;
694 0 : move32();
695 : }
696 : }
697 :
698 0 : hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD;
699 0 : move32();
700 : }
701 2100 : ELSE IF( LT_32( hPHA->isd_rate_s_fx, STEREO_DMX_EVS_ISD_DIST_HYST_L_Q31 ) )
702 : {
703 1398 : IF( NE_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD2 ) )
704 : {
705 4 : IF( EQ_32( hPHA->prev_pha, STEREO_DMX_EVS_PHA_IPD2 ) )
706 : {
707 2 : hPHA->pha_hys_cnt = add( hPHA->pha_hys_cnt, 1 );
708 2 : move16();
709 : }
710 : ELSE
711 : {
712 2 : hPHA->pha_hys_cnt = 0;
713 2 : move16();
714 : }
715 :
716 4 : if ( GE_16( hPHA->pha_hys_cnt, STEREO_DMX_EVS_SWTCH_HYS_THRES ) )
717 : {
718 2 : hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD2;
719 2 : move32();
720 : }
721 : }
722 1398 : hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD2;
723 1398 : move32();
724 : }
725 :
726 2100 : ipd_ff = hPHA->ipd_ff_fx;
727 :
728 2100 : Nr = 0;
729 2100 : move32();
730 2100 : Nr_e = 0;
731 2100 : move16();
732 2100 : Ni = 0;
733 2100 : move32();
734 2100 : Ni_e = 0;
735 2100 : move16();
736 2100 : eneL = 0;
737 2100 : move32();
738 2100 : eneL_e = 0;
739 2100 : move16();
740 2100 : eneR = 0;
741 2100 : move32();
742 2100 : eneR_e = 0;
743 2100 : move16();
744 :
745 420000 : FOR( ( n = 1, i = 1 ); n < nsbd; n++ )
746 : {
747 417900 : tPr = 0;
748 417900 : move32();
749 417900 : tPr_e = 0;
750 417900 : move16();
751 417900 : tPi = 0;
752 417900 : move32();
753 417900 : tPi_e = 0;
754 417900 : move16();
755 417900 : tEr[n] = 0;
756 417900 : move32();
757 417900 : tEr_e[n] = 0;
758 417900 : move16();
759 417900 : tEl[n] = 0;
760 417900 : move32();
761 417900 : tEl_e[n] = 0;
762 417900 : move16();
763 :
764 1253700 : FOR( j = 0; j < STEREO_DMX_EVS_SUBBAND_SIZE; ( j++, i++ ) )
765 : {
766 :
767 : /* Energy */
768 :
769 :
770 : // Left
771 835800 : W_tmp = W_add( W_mult0_32_32( specLr[i], specLr[i] ), W_mult0_32_32( specLi[i], specLi[i] ) ); // Q(62-(2*specL_e)) -> Q(63 - ((2*specL_e) +1))
772 835800 : L_tmp_e = W_norm( W_tmp );
773 835800 : IF( L_tmp_e != 0 )
774 : {
775 835800 : W_tmp = W_shl( W_tmp, L_tmp_e ); // Q(63 - ((2*spec_e) - (L_tmp_e - 1))
776 : }
777 835800 : tEl[n] = BASOP_Util_Add_Mant32Exp( tEl[n], tEl_e[n], W_round64_L( W_tmp ), sub( shl( spec_e, 1 ), sub( L_tmp_e, 1 ) ), &tEl_e[n] );
778 835800 : move32();
779 :
780 : // Right
781 835800 : W_tmp = W_add( W_mult0_32_32( specRr[i], specRr[i] ), W_mult0_32_32( specRi[i], specRi[i] ) ); // Q(62-(2*specR_e)) -> Q(63 - ((2*specR_e) +1))
782 835800 : L_tmp_e = W_norm( W_tmp );
783 835800 : IF( L_tmp_e != 0 )
784 : {
785 835800 : W_tmp = W_shl( W_tmp, L_tmp_e ); // Q(63 - ((2*spec_e) - (L_tmp_e - 1))
786 : }
787 835800 : tEr[n] = BASOP_Util_Add_Mant32Exp( tEr[n], tEr_e[n], W_round64_L( W_tmp ), sub( shl( spec_e, 1 ), sub( L_tmp_e, 1 ) ), &tEr_e[n] );
788 835800 : move32();
789 :
790 : /* IPD */
791 : // IPDr = L_add(Mpy_32_32_r(specLr[i], specRr[i]), Mpy_32_32_r(specLi[i], specRi[i])); //2*spec_e
792 835800 : W_tmp = W_add( W_mult_32_32( specLr[i], specRr[i] ), W_mult_32_32( specLi[i], specRi[i] ) );
793 835800 : W_tmp_q = W_norm( W_tmp );
794 835800 : W_tmp = W_shl( W_tmp, W_tmp_q );
795 835800 : IPDr = W_extract_h( W_tmp );
796 835800 : IPDr_e = sub( shl( spec_e, 1 ), W_tmp_q );
797 : // IPDi = L_sub(Mpy_32_32_r(specLi[i] , specRr[i]), Mpy_32_32_r(specLr[i] , specRi[i])); //2*spec_e
798 835800 : W_tmp = W_sub( W_mult_32_32( specLi[i], specRr[i] ), W_mult_32_32( specLr[i], specRi[i] ) );
799 835800 : W_tmp_q = W_norm( W_tmp );
800 835800 : W_tmp = W_shl( W_tmp, W_tmp_q );
801 835800 : IPDi = W_extract_h( W_tmp );
802 835800 : IPDi_e = sub( shl( spec_e, 1 ), W_tmp_q );
803 835800 : tPr = BASOP_Util_Add_Mant32Exp( tPr, tPr_e, IPDr, IPDr_e, &tPr_e );
804 835800 : tPi = BASOP_Util_Add_Mant32Exp( tPi, tPi_e, IPDi, IPDi_e, &tPi_e );
805 :
806 : /* ICCr */
807 : // Pn = (float) inv_sqrt( ( IPDr * IPDr + IPDi * IPDi ) + EPSILON );
808 835800 : L_tmp = BASOP_Util_Add_Mant32Exp( Mpy_32_32_r( IPDr, IPDr ), shl( IPDr_e, 1 ), Mpy_32_32_r( IPDi, IPDi ), shl( IPDi_e, 1 ), &L_tmp_e );
809 835800 : L_tmp = BASOP_Util_Add_Mant32Exp( L_tmp, L_tmp_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
810 835800 : Pn_e = L_tmp_e;
811 835800 : move16();
812 835800 : Pn = ISqrt32( L_tmp, &Pn_e );
813 835800 : IPDr = L_shl_sat( Mpy_32_32_r( IPDr, Pn ), add( IPDr_e, Pn_e ) ); // Q31
814 835800 : IPDi = L_shl_sat( Mpy_32_32_r( IPDi, Pn ), add( IPDi_e, Pn_e ) ); // Q31
815 :
816 835800 : tIPDr = L_sub( Mpy_32_32_r( specRr[i], IPDr ), Mpy_32_32_r( specRi[i], IPDi ) ); // spec_e
817 835800 : tIPDi = L_add( Mpy_32_32_r( specRr[i], IPDi ), Mpy_32_32_r( specRi[i], IPDr ) ); // spec_e
818 :
819 :
820 835800 : Nr = BASOP_Util_Add_Mant32Exp( Nr, Nr_e, L_add( Mpy_32_32_r( specLr[i], tIPDr ), Mpy_32_32_r( specLi[i], tIPDi ) ), 0, &Nr_e );
821 835800 : Ni = BASOP_Util_Add_Mant32Exp( Ni, Ni_e, L_sub( Mpy_32_32_r( specLi[i], tIPDr ), Mpy_32_32_r( specLr[i], tIPDi ) ), 0, &Ni_e );
822 :
823 : // eneL = BASOP_Util_Add_Mant32Exp( eneL, eneL_e, L_add( Mpy_32_32_r( specLr[i], specLr[i] ), Mpy_32_32_r( specLi[i], specLi[i] ) ), 0, &eneL_e );
824 835800 : W_tmp = W_add( W_mult0_32_32( specLr[i], specLr[i] ), W_mult0_32_32( specLi[i], specLi[i] ) );
825 835800 : L_tmp_e = W_norm( W_tmp );
826 835800 : IF( L_tmp_e != 0 )
827 : {
828 835800 : W_tmp = W_shl( W_tmp, L_tmp_e );
829 : }
830 835800 : eneL = BASOP_Util_Add_Mant32Exp( eneL, eneL_e, W_round64_L( W_tmp ), sub( 1, L_tmp_e ), &eneL_e );
831 :
832 : // eneR = BASOP_Util_Add_Mant32Exp( eneR, eneR_e, L_add( Mpy_32_32_r( specRr[i], specRr[i] ), Mpy_32_32_r( specRi[i], specRi[i] ) ), 0, &eneR_e );
833 835800 : W_tmp = W_add( W_mult0_32_32( specRr[i], specRr[i] ), W_mult0_32_32( specRi[i], specRi[i] ) );
834 835800 : L_tmp_e = W_norm( W_tmp );
835 835800 : IF( L_tmp_e != 0 )
836 : {
837 835800 : W_tmp = W_shl( W_tmp, L_tmp_e );
838 : }
839 835800 : eneR = BASOP_Util_Add_Mant32Exp( eneR, eneR_e, W_round64_L( W_tmp ), sub( 1, L_tmp_e ), &eneR_e );
840 : }
841 :
842 : // Pn = (float) inv_sqrt( ( tPr * tPr + tPi * tPi ) + EPSILON );
843 417900 : L_tmp = BASOP_Util_Add_Mant32Exp( Mpy_32_32_r( tPr, tPr ), shl( tPr_e, 1 ), Mpy_32_32_r( tPi, tPi ), shl( tPi_e, 1 ), &L_tmp_e );
844 417900 : L_tmp = BASOP_Util_Add_Mant32Exp( L_tmp, L_tmp_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
845 417900 : Pn_e = L_tmp_e;
846 417900 : move16();
847 417900 : Pn = ISqrt32( L_tmp, &Pn_e );
848 417900 : tPr = L_shl_sat( Mpy_32_32_r( tPr, Pn ), add( tPr_e, Pn_e ) ); // Q31
849 417900 : tPi = L_shl_sat( Mpy_32_32_r( tPi, Pn ), add( tPi_e, Pn_e ) ); // Q31
850 :
851 417900 : IF( hPHA->init_frmCntr == 0 )
852 : {
853 414318 : Pr[n] = L_add( Mpy_32_32_r( ipd_ff[n], Pr[n] ), Mpy_32_32_r( L_sub( MAX_32, ipd_ff[n] ), tPr ) );
854 414318 : move32();
855 414318 : Pi[n] = L_add( Mpy_32_32_r( ipd_ff[n], Pi[n] ), Mpy_32_32_r( L_sub( MAX_32, ipd_ff[n] ), tPi ) );
856 414318 : move32();
857 :
858 : // Pn = (float) inv_sqrt( ( Pr[n] * Pr[n] + Pi[n] * Pi[n] ) + EPSILON );
859 414318 : Pn = L_add( L_shr( Mpy_32_32_r( Pr[n], Pr[n] ), 1 ), L_shr( Mpy_32_32_r( Pi[n], Pi[n] ), 1 ) );
860 414318 : Pn = BASOP_Util_Add_Mant32Exp( Pn, 1, EPSILON_FX_M, EPSILON_FX_E, &Pn_e );
861 414318 : Pn = Isqrt_lc( Pn, &Pn_e );
862 :
863 414318 : Pr[n] = L_shl_sat( Mpy_32_32_r( Pr[n], Pn ), Pn_e ); // Q31
864 414318 : move32();
865 414318 : Pi[n] = L_shl_sat( Mpy_32_32_r( Pi[n], Pn ), Pn_e ); // Q31
866 414318 : move32();
867 : }
868 : ELSE
869 : {
870 3582 : Pr[n] = tPr;
871 3582 : move32();
872 3582 : Pi[n] = tPi;
873 3582 : move32();
874 : }
875 :
876 : // Pr[n] = ( Pr[n] > 1.0f ) ? 1.0f : Pr[n];
877 : // Pr[n] = ( Pr[n] < -1.0f ) ? -1.0f : Pr[n];
878 : }
879 : // ICCr = (float) sqrt( ( Nr * Nr + Ni * Ni ) / ( eneL * eneR + EPSILON ) );
880 2100 : L_tmp1 = BASOP_Util_Add_Mant32Exp( Mpy_32_32_r( Nr, Nr ), shl( Nr_e, 1 ), Mpy_32_32_r( Ni, Ni ), shl( Ni_e, 1 ), &L_tmp1_e );
881 2100 : L_tmp2 = BASOP_Util_Add_Mant32Exp( Mpy_32_32_r( eneL, eneR ), add( eneL_e, eneR_e ), EPSILON_FX_M, EPSILON_FX_E, &L_tmp2_e );
882 2100 : ICCr = BASOP_Util_Divide3232_Scale_newton( L_tmp1, L_tmp2, &L_tmp_e );
883 2100 : L_tmp_e = add( L_tmp_e, sub( L_tmp1_e, L_tmp2_e ) );
884 2100 : ICCr = Sqrt32( ICCr, &L_tmp_e );
885 : // Saturation to handle values close to 1.0f
886 2100 : ICCr = L_shl_sat( ICCr, L_tmp_e ); // Q31
887 :
888 : // hPHA->iccr_s = STEREO_DMX_EVS_ICCR_FORGETTING * hPHA->iccr_s + ( 1.0f - STEREO_DMX_EVS_ICCR_FORGETTING ) * ICCr;
889 2100 : hPHA->iccr_s_fx = L_add( Mpy_32_32_r( STEREO_DMX_EVS_ICCR_FORGETTING_Q31, hPHA->iccr_s_fx ), Mpy_32_32_r( MAX_32 - STEREO_DMX_EVS_ICCR_FORGETTING_Q31, ICCr ) ); // Q31
890 2100 : move32();
891 :
892 2100 : IF( EQ_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD ) )
893 : {
894 2 : hPHA->p_curr_taps_fx[0] = NULL;
895 2 : hPHA->p_curr_taps_fx[1] = hPHA->curr_taps_fx[1];
896 :
897 2 : rfft_pha_buf[0] = ONE_IN_Q22;
898 2 : move32();
899 2 : rfft_pha_buf[1] = ONE_IN_Q22;
900 2 : move32();
901 :
902 2 : ild_cnt = 0;
903 2 : move16();
904 400 : FOR( i = 1; i < nsbd; i++ )
905 : {
906 398 : rfft_pha_buf[i * 2] = L_shr_r( Pr[i], 9 ); // Q31->Q22
907 398 : move32();
908 398 : rfft_pha_buf[i * 2 + 1] = L_shr_r( Pi[i], 9 ); // Q31->Q22
909 398 : move32();
910 : // if ( ( tEr[i] > STEREO_DMX_EVS_LR_EGY * tEl[i] ) || ( tEl[i] > STEREO_DMX_EVS_LR_EGY * tEr[i] ) )
911 398 : test();
912 398 : IF( BASOP_Util_Cmp_Mant32Exp( tEr[i], tEr_e[i], Mpy_32_32_r( STEREO_DMX_EVS_LR_EGY_Q27, tEl[i] ), add( 4, tEl_e[i] ) ) > 0 || BASOP_Util_Cmp_Mant32Exp( tEl[i], tEl_e[i], Mpy_32_32_r( STEREO_DMX_EVS_LR_EGY_Q27, tEr[i] ), add( 4, tEr_e[i] ) ) > 0 )
913 : {
914 10 : ild_cnt = add( ild_cnt, 1 );
915 10 : tEr[i] = MAX_32;
916 10 : move32();
917 10 : tEr_e[i] = 0;
918 10 : move16();
919 : }
920 : ELSE
921 : {
922 388 : tEr[i] = MIN_32;
923 388 : move32();
924 388 : tEr_e[i] = 0;
925 388 : move16();
926 : }
927 : }
928 2 : IF( GT_16( ild_cnt, mult_r( nsbd, STEREO_DMX_EVS_ILD_PRC_Q15 ) ) )
929 : {
930 0 : FOR( i = 1; i < nsbd; i++ )
931 : {
932 0 : IF( tEr[i] > 0 )
933 : {
934 0 : rfft_pha_buf[i * 2] = ONE_IN_Q22;
935 0 : move32();
936 0 : rfft_pha_buf[i * 2 + 1] = 0;
937 0 : move32();
938 : }
939 : }
940 : }
941 :
942 2 : rfft_fx( rfft_pha_buf, hPHA->rfft_ipd_coef_fx, input_frame_pha, +1 );
943 : // mvr2r( rfft_pha_buf, hPHA->p_curr_taps[1], hPHA->pha_len );
944 2 : Copy_Scale_sig32( rfft_pha_buf, hPHA->p_curr_taps_fx[1], hPHA->pha_len, 9 ); // Q22->Q31
945 : }
946 : ELSE
947 : {
948 2098 : test();
949 2098 : test();
950 2098 : IF( LT_32( hPHA->iccr_s_fx, STEREO_DMX_EVS_ICCR_HYST_L_Q31 ) || ( LT_32( hPHA->iccr_s_fx, STEREO_DMX_EVS_ICCR_HYST_H_Q31 ) && ( hPHA->p_curr_taps_fx[0] != NULL ) ) )
951 : {
952 : /* IPDn */
953 :
954 426 : set32_fx( &( Pr[freq_ipd_max] ), MAX_32, sub( nsbd, freq_ipd_max ) );
955 426 : set32_fx( &( Pi[freq_ipd_max] ), 0, sub( nsbd, freq_ipd_max ) );
956 :
957 1278 : FOR( n = 0; n < CPE_CHANNELS; n++ )
958 : {
959 852 : hPHA->p_curr_taps_fx[n] = hPHA->curr_taps_fx[n];
960 : }
961 :
962 426 : rfft_pha_buf[0] = ONE_IN_Q22;
963 426 : move32();
964 426 : rfft_pha_buf[1] = ONE_IN_Q22;
965 426 : move32();
966 :
967 426 : ild_cnt = 0;
968 426 : move16();
969 426 : isd_rate = BASOP_Util_Divide1616_Scale( isd_cnt_l, freq_8k, &isd_rate_e );
970 : // Saturation to handle values close to 1.0f
971 426 : isd_rate = shl_sat( isd_rate, isd_rate_e ); // Q15
972 85200 : FOR( i = 1; i < nsbd; i++ )
973 : {
974 : // rfft_pha_buf[i * 2] = (float) sqrt( ( 1.0f + Pr[i] ) / 2.0f );
975 84774 : L_tmp = L_add( ONE_IN_Q30, L_shr( Pr[i], 1 ) );
976 84774 : L_tmp_e = 0;
977 84774 : move16();
978 84774 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
979 84774 : rfft_pha_buf[i * 2] = L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ); // Q22
980 84774 : move32();
981 : // rfft_pha_buf[i * 2 + 1] = (float) sqrt( ( 1.0f - Pr[i] ) / 2.0f ) * sign( Pi[i] );
982 84774 : L_tmp = L_sub_sat( ONE_IN_Q30, L_shr( Pr[i], 1 ) ); // saturating as Pr does not exceed 1.0f
983 84774 : L_tmp_e = 0;
984 84774 : move16();
985 84774 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
986 84774 : rfft_pha_buf[i * 2 + 1] = imult3216( L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ), sign_fx( Pi[i] ) ); // Q22
987 84774 : move32();
988 84774 : IF( GT_16( isd_rate, STEREO_DMX_EVS_ISD_DIST_THRES_IPD_Q15 ) )
989 : {
990 : // rfft_pha_buf[i * 2 + 1] = (float) sqrt( ( 1.0f - rfft_pha_buf[i * 2] ) / 2.0f ) * sign( rfft_pha_buf[i * 2 + 1] );
991 44019 : L_tmp = L_sub( ONE_IN_Q21, L_shr( rfft_pha_buf[i * 2], 1 ) );
992 44019 : L_tmp_e = 9;
993 44019 : move16();
994 44019 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
995 44019 : rfft_pha_buf[i * 2 + 1] = imult3216( L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ), sign_fx( rfft_pha_buf[i * 2 + 1] ) ); // Q22
996 44019 : move32();
997 : // rfft_pha_buf[i * 2] = (float) sqrt( ( 1.0f + rfft_pha_buf[i * 2] ) / 2.0f );
998 44019 : L_tmp = L_add( ONE_IN_Q21, L_shr( rfft_pha_buf[i * 2], 1 ) ); // Q22
999 44019 : L_tmp_e = 9;
1000 44019 : move16();
1001 44019 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
1002 44019 : rfft_pha_buf[i * 2] = L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ); // Q22
1003 44019 : move32();
1004 : }
1005 :
1006 : // if ( ( tEr[i] > STEREO_DMX_EVS_LR_EGY * tEl[i] ) || ( tEl[i] > STEREO_DMX_EVS_LR_EGY * tEr[i] ) )
1007 84774 : test();
1008 84774 : IF( BASOP_Util_Cmp_Mant32Exp( tEr[i], tEr_e[i], Mpy_32_32_r( STEREO_DMX_EVS_LR_EGY_Q27, tEl[i] ), add( 4, tEl_e[i] ) ) > 0 || BASOP_Util_Cmp_Mant32Exp( tEl[i], tEl_e[i], Mpy_32_32_r( STEREO_DMX_EVS_LR_EGY_Q27, tEr[i] ), add( 4, tEr_e[i] ) ) > 0 )
1009 : {
1010 16858 : ild_cnt = add( ild_cnt, 1 );
1011 16858 : tEr[i] = MAX_32;
1012 16858 : move32();
1013 16858 : tEr_e[i] = 0;
1014 16858 : move16();
1015 : }
1016 : ELSE
1017 : {
1018 67916 : tEr[i] = MIN_32;
1019 67916 : move32();
1020 67916 : tEr_e[i] = 0;
1021 67916 : move16();
1022 : }
1023 : }
1024 426 : IF( GT_16( ild_cnt, mult_r( nsbd, STEREO_DMX_EVS_ILD_PRC_Q15 ) ) )
1025 : {
1026 45440 : FOR( i = 1; i < nsbd; i++ )
1027 : {
1028 45209 : IF( tEr[i] > 0 )
1029 : {
1030 14470 : rfft_pha_buf[i * 2] = ONE_IN_Q22;
1031 14470 : move32();
1032 14470 : rfft_pha_buf[i * 2 + 1] = 0;
1033 14470 : move32();
1034 : }
1035 : }
1036 : }
1037 :
1038 426 : rfft_fx( rfft_pha_buf, hPHA->rfft_ipd_coef_fx, input_frame_pha, +1 );
1039 : // mvr2r( rfft_pha_buf, hPHA->p_curr_taps[1], hPHA->pha_len );
1040 426 : Copy_Scale_sig32( rfft_pha_buf, hPHA->p_curr_taps_fx[1], hPHA->pha_len, 9 ); // Q22->Q31
1041 :
1042 : /* PHA L2R */
1043 426 : p_curr_taps = hPHA->p_curr_taps_fx[0];
1044 426 : p_curr_taps[0] = L_shl( rfft_pha_buf[0], 9 ); // Q22->Q31
1045 426 : move32();
1046 20448 : FOR( i = 1; i < hPHA->pha_len; i++ )
1047 : {
1048 20022 : p_curr_taps[i] = L_shl( rfft_pha_buf[input_frame_pha - i], 9 ); // Q22->Q31
1049 20022 : move32();
1050 : }
1051 : }
1052 : ELSE
1053 : {
1054 5016 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1055 : {
1056 3344 : hPHA->p_curr_taps_fx[n] = NULL;
1057 : }
1058 : }
1059 : }
1060 :
1061 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1062 : {
1063 4200 : IF( hPHA->p_curr_taps_fx[n] )
1064 : {
1065 41846 : FOR( i = 0; i < hPHA->pha_len; i++ )
1066 : {
1067 : // hPHA->p_curr_taps[n][i] *= hPHA->win[i];
1068 40992 : hPHA->p_curr_taps_fx[n][i] = Mpy_32_16_1( hPHA->p_curr_taps_fx[n][i], hPHA->win_fx[i] ); // Q30
1069 40992 : move32();
1070 : }
1071 :
1072 854 : energy = 0;
1073 854 : move32();
1074 854 : energy_e = 0;
1075 854 : move16();
1076 41846 : FOR( i = 0; i < hPHA->pha_len; i++ )
1077 : {
1078 40992 : energy = BASOP_Util_Add_Mant32Exp( energy, energy_e, Mpy_32_32_r( hPHA->p_curr_taps_fx[n][i], hPHA->p_curr_taps_fx[n][i] ), 0, &energy_e );
1079 : }
1080 : // energy = (float) inv_sqrt( energy + EPSILON );
1081 854 : energy = BASOP_Util_Add_Mant32Exp( energy, energy_e, EPSILON_FX_M, EPSILON_FX_E, &energy_e );
1082 854 : energy = ISqrt32( energy, &energy_e );
1083 41846 : FOR( i = 0; i < hPHA->pha_len; i++ )
1084 : {
1085 40992 : hPHA->p_curr_taps_fx[n][i] = L_shl_r( Mpy_32_32_r( hPHA->p_curr_taps_fx[n][i], energy ), sub( energy_e, 1 ) ); // Q30
1086 40992 : move32();
1087 : }
1088 : }
1089 : }
1090 :
1091 :
1092 2100 : rfft_buf[0] = L_shr_r( specPOr[0], 10 ); // Q31->Q21
1093 2100 : move32();
1094 2100 : rfft_buf[1] = L_shr_r( specPOr[n0], 10 ); // Q31->Q21
1095 2100 : move32();
1096 840000 : FOR( i = 1; i < n0; i++ )
1097 : {
1098 837900 : rfft_buf[i * 2] = L_shr_r( specPOr[i], 10 ); // Q31->Q21
1099 837900 : move32();
1100 837900 : rfft_buf[i * 2 + 1] = L_shr_r( specPOi[i], 10 ); // Q31->Q21
1101 837900 : move32();
1102 : }
1103 :
1104 2100 : rfft_fx( rfft_buf, rfft_coef, input_frame, +1 );
1105 2100 : scale_sig32( rfft_buf, input_frame, 10 ); // Q21->Q31
1106 :
1107 2100 : tmp1 = rfft_buf[0];
1108 2100 : move32();
1109 2100 : tmpPOC1[n0] = Mpy_32_32_r( tmp1, tmp1 );
1110 2100 : move32();
1111 :
1112 474600 : FOR( i = 1; i < hPOC->shift_limit + 1; i++ )
1113 : {
1114 472500 : n1 = add( n0, i );
1115 472500 : n2 = sub( n0, i );
1116 :
1117 472500 : tmp1 = rfft_buf[i];
1118 472500 : move32();
1119 472500 : tmpPOC1[n1] = Mpy_32_32_r( tmp1, tmp1 );
1120 472500 : move32();
1121 :
1122 472500 : tmp1 = rfft_buf[input_frame - i];
1123 472500 : move32();
1124 472500 : tmpPOC1[n2] = Mpy_32_32_r( tmp1, tmp1 );
1125 472500 : move32();
1126 : }
1127 :
1128 2100 : tmp1 = L_shl( L_add( Mpy_32_32_r( STEREO_DMX_EVS_POC_SMOOTH_Q30, tmpPOC1[n0] ), Mpy_32_32_r( -ONE_IN_Q28 /*0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH )*/, L_add( L_shr( tmpPOC1[n0 - 1], 1 ), L_shr( tmpPOC1[n0 + 1], 1 ) ) ) ), 1 ); // Q31
1129 2100 : tmpPOC2[n0] = L_max( tmp1, 0 );
1130 2100 : move32();
1131 :
1132 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ )
1133 : {
1134 470400 : n1 = add( n0, i );
1135 470400 : n2 = sub( n0, i );
1136 : // tmp1 = STEREO_DMX_EVS_POC_SMOOTH * tmpPOC1[n1] + 0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH ) * ( tmpPOC1[n1 - 1] + tmpPOC1[n1 + 1] );
1137 470400 : tmp1 = L_shl( Mpy_32_32_r( STEREO_DMX_EVS_POC_SMOOTH_Q30, tmpPOC1[n1] ) + Mpy_32_32_r( -ONE_IN_Q28 /*0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH )*/, L_add( L_shr( tmpPOC1[n1 - 1], 1 ), L_shr( tmpPOC1[n1 + 1], 1 ) ) ), 1 ); // Q31
1138 : // tmp2 = STEREO_DMX_EVS_POC_SMOOTH * tmpPOC1[n2] + 0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH ) * ( tmpPOC1[n2 - 1] + tmpPOC1[n2 + 1] );
1139 470400 : tmp2 = L_shl( Mpy_32_32_r( STEREO_DMX_EVS_POC_SMOOTH_Q30, tmpPOC1[n2] ) + Mpy_32_32_r( -ONE_IN_Q28 /*0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH )*/, L_add( L_shr( tmpPOC1[n2 - 1], 1 ), L_shr( tmpPOC1[n2 + 1], 1 ) ) ), 1 ); // Q31
1140 470400 : tmpPOC2[n1] = L_max( tmp1, 0 );
1141 470400 : move32();
1142 470400 : tmpPOC2[n2] = L_max( tmp2, 0 );
1143 470400 : move32();
1144 : }
1145 :
1146 2100 : P[n0] = L_add( Mpy_32_32_r( P[n0], STEREO_DMX_EVS_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n0], MAX_32 - STEREO_DMX_EVS_POC_FORGETTING_Q31 ) );
1147 2100 : move32();
1148 :
1149 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ )
1150 : {
1151 470400 : n1 = add( n0, i );
1152 470400 : n2 = sub( n0, i );
1153 :
1154 470400 : IF( EQ_16( i, negate( itdLR[1] ) ) )
1155 : {
1156 720 : P[n1] = L_add( Mpy_32_32_r( P[n1], STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n1], MAX_32 - STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ) );
1157 720 : move32();
1158 : }
1159 : ELSE
1160 : {
1161 469680 : P[n1] = L_add( Mpy_32_32_r( P[n1], STEREO_DMX_EVS_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n1], MAX_32 - STEREO_DMX_EVS_POC_FORGETTING_Q31 ) );
1162 469680 : move32();
1163 : }
1164 :
1165 470400 : IF( EQ_16( i, itdLR[0] ) )
1166 : {
1167 649 : P[n2] = L_add( Mpy_32_32_r( P[n2], STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n2], MAX_32 - STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ) );
1168 649 : move32();
1169 : }
1170 : ELSE
1171 : {
1172 469751 : P[n2] = L_add( Mpy_32_32_r( P[n2], STEREO_DMX_EVS_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n2], MAX_32 - STEREO_DMX_EVS_POC_FORGETTING_Q31 ) );
1173 469751 : move32();
1174 : }
1175 : }
1176 :
1177 2100 : return;
1178 : }
1179 :
1180 :
1181 : /*-------------------------------------------------------------------*
1182 : * find_poc_peak()
1183 : *
1184 : * find peak phase only correlation
1185 : *-------------------------------------------------------------------*/
1186 2100 : static Word32 find_poc_peak_fx(
1187 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
1188 : Word16 itd_fx[], /* o : estimated itd Q0 */
1189 : const Word16 input_frame, /* i : input frame length per channel */
1190 : const Word32 ratio_fixed /* i : adapting ratio Q31 */
1191 : )
1192 : {
1193 : Word16 itd_cand[CPE_CHANNELS], i, n, cnt[CPE_CHANNELS], Lh, peak_range, *on, *itdLR, prev_off[CPE_CHANNELS], eps_fx;
1194 : Word32 Q_fx[CPE_CHANNELS], aQ_fx[CPE_CHANNELS], cQ_fx[CPE_CHANNELS], width_fx, *peak_width_fx, *peakQ_fx, cconfidence_fx, *P_fx, tmpf_fx, eps2_fx;
1195 2100 : Word16 tmpf_e = 0, eps2_e = 0, Q_e[CPE_CHANNELS], eps_e = 15, peakQ_e[CPE_CHANNELS];
1196 :
1197 2100 : move16();
1198 2100 : move16();
1199 2100 : move16();
1200 :
1201 : /* Initialization */
1202 2100 : Lh = shr( input_frame, 1 );
1203 2100 : on = hPOC->ispeak;
1204 2100 : itdLR = hPOC->itdLR;
1205 2100 : itd_cand[0] = itd_cand[1] = 0;
1206 2100 : move16();
1207 2100 : move16();
1208 :
1209 2100 : width_fx = 816043776; /*0.38f in Q31*/
1210 2100 : eps_fx = Inv16( input_frame, &eps_e );
1211 2100 : peak_width_fx = hPOC->peak_width_fx;
1212 2100 : peakQ_fx = hPOC->peakQ_fx; // Q16
1213 2100 : move16();
1214 :
1215 2100 : set16_fx( peakQ_e, 31 - Q16, CPE_CHANNELS );
1216 2100 : Q_fx[0] = hPOC->P_fx[Lh]; // Q31
1217 2100 : Q_fx[1] = 0;
1218 2100 : move16();
1219 2100 : move16();
1220 2100 : set16_fx( Q_e, 0, CPE_CHANNELS ); // Q31
1221 :
1222 2100 : P_fx = hPOC->P_fx;
1223 :
1224 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ ) /*find peaks of POC P[] with positive and negative ITD */
1225 : {
1226 470400 : IF( GT_32( P_fx[Lh - i], Q_fx[0] ) )
1227 : {
1228 8043 : Q_fx[0] = P_fx[Lh - i]; // Q31
1229 8043 : itd_cand[0] = i;
1230 8043 : Q_e[0] = 0;
1231 8043 : move32();
1232 8043 : move16();
1233 8043 : move16();
1234 : }
1235 470400 : IF( GT_32( P_fx[Lh + i], Q_fx[1] ) )
1236 : {
1237 9982 : Q_fx[1] = P_fx[Lh + i]; // Q31
1238 9982 : itd_cand[1] = -i;
1239 9982 : Q_e[1] = 0;
1240 9982 : move32();
1241 9982 : move16();
1242 9982 : move16();
1243 : }
1244 : }
1245 :
1246 2100 : Word16 tmp1, tmp11, tmp_var1, tmp12 = 0, tmp13;
1247 2100 : move16();
1248 : Word32 tmp4, tmp5, tmp6, tmp7, tmp8, tmp9, tmp10, tmp14, tmp15, tmp16;
1249 : Word16 tmp8_e, tmp9_e, tmp10_e, tmp11_e, tmp12_e, tmp13_e, tmp15_e;
1250 :
1251 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1252 : {
1253 4200 : prev_off[n] = !on[n];
1254 4200 : move16();
1255 :
1256 4200 : cnt[n] = 0;
1257 4200 : aQ_fx[n] = Mpy_32_32( Q_fx[n], width_fx ); // Q31+Q31 - 31
1258 4200 : cQ_fx[n] = P_fx[Lh - itd_cand[n]]; // Q31
1259 4200 : move16();
1260 4200 : move16();
1261 4200 : move16();
1262 :
1263 4200 : tmp12 = 0, tmp8_e = 0, tmp9_e = 0, tmp10_e = 0, tmp11_e = 0, tmp12_e = 0;
1264 4200 : move16();
1265 4200 : move16();
1266 4200 : move16();
1267 4200 : move16();
1268 4200 : move16();
1269 4200 : move16();
1270 :
1271 : /*compute peak_range*/
1272 4200 : tmp1 = idiv1616( hPOC->shift_limit, STEREO_DMX_EVS_FIND_POC_PEAK_TAU );
1273 4200 : peak_range = idiv1616( add( extract_l( abs_s( itd_cand[n] ) ), tmp1 ), STEREO_DMX_EVS_FIND_POC_PEAK_TAU2 ); // Q0
1274 :
1275 59352 : FOR( i = 1; i <= peak_range; i++ )
1276 : {
1277 55152 : cnt[n] = add( cnt[n], extract_l( L_add( GT_32( P_fx[Lh - itd_cand[n] + i], aQ_fx[n] ), GT_32( P_fx[Lh - itd_cand[n] - i], aQ_fx[n] ) ) ) );
1278 55152 : tmp4 = L_add( P_fx[Lh - itd_cand[n] + i], P_fx[Lh - itd_cand[n] - i] ); // Q31
1279 55152 : cQ_fx[n] = L_add( cQ_fx[n], tmp4 );
1280 55152 : move16();
1281 55152 : move32();
1282 : }
1283 :
1284 : /*compute eps2_fx*/
1285 4200 : tmp5 = Mpy_32_32( peak_width_fx[n], ratio_fixed ); // Q31 + Q16 - Q31
1286 4200 : tmp6 = Mpy_32_32( L_deposit_h( cnt[n] ), L_sub( ONE_IN_Q31, ratio_fixed ) ); // Q16 + Q31 - 31
1287 4200 : peak_width_fx[n] = L_add( tmp5, tmp6 ); // Q16
1288 4200 : move16();
1289 :
1290 4200 : tmp7 = Mpy_32_32( L_deposit_h( eps_fx ), peak_width_fx[n] ); // eps_e + 31 - Q16
1291 :
1292 4200 : eps2_fx = L_shr( tmp7, 2 );
1293 4200 : eps2_e = add( eps_e, 31 - Q16 );
1294 :
1295 : /*compute Q_fx[n]*/
1296 4200 : tmp8 = BASOP_Util_Add_Mant32Exp( Q_fx[n], 0, eps2_fx, eps2_e, &tmp8_e );
1297 4200 : tmp_var1 = BASOP_Util_Divide3232_Scale( cQ_fx[n], L_deposit_h( ( add( shl( peak_range, 1 ), 1 ) ) ) /*Q16*/, &tmp9_e );
1298 4200 : tmp9_e = sub( tmp9_e, ( 31 - Q16 ) );
1299 4200 : tmp9 = L_deposit_h( tmp_var1 );
1300 4200 : tmp10 = BASOP_Util_Add_Mant32Exp( tmp9, tmp9_e, eps2_fx, eps2_e, &tmp10_e );
1301 4200 : tmp11 = BASOP_Util_Divide3232_Scale( tmp10, tmp8, &tmp11_e );
1302 4200 : tmp11_e = add( tmp11_e, sub( tmp10_e, tmp8_e ) );
1303 4200 : tmp12_e = BASOP_Util_Add_MantExp( ONE_IN_Q14, 1, negate( tmp11 ), tmp11_e, &tmp12 );
1304 :
1305 4200 : Q_fx[n] = L_deposit_h( tmp12 );
1306 4200 : Q_e[n] = tmp12_e;
1307 4200 : Q_fx[n] = L_max( Q_fx[n], 0 );
1308 4200 : move16();
1309 4200 : move16();
1310 4200 : move16();
1311 4200 : if ( Q_fx[n] == 0 )
1312 : {
1313 178 : Q_e[n] = 0;
1314 178 : move16();
1315 : }
1316 :
1317 4200 : IF( on[n] ) /*if channel n was active (likely to be preceding) in the previous frame*/
1318 : {
1319 1812 : tmp13_e = 0, tmp15_e = 0;
1320 1812 : tmp13 = BASOP_Util_Divide1616_Scale( abs_s( itd_cand[n] ), hPOC->shift_limit, &tmp13_e );
1321 1812 : tmp14 = L_mult( 6554 /*0.2f Q15*/, tmp13 ); // tmp13_e
1322 1812 : tmp15 = BASOP_Util_Add_Mant32Exp( 644245120 /*0.75f in Q31*/, 0, L_negate( tmp14 ), tmp13_e, &tmp15_e );
1323 1812 : tmp15 = Mpy_32_32( tmp15, peakQ_fx[n] ); // tmp15_e + peakQ_e[n]
1324 1812 : move16();
1325 1812 : move16();
1326 :
1327 1812 : tmpf_fx = tmp15;
1328 1812 : move16();
1329 1812 : tmpf_e = add( tmp15_e, peakQ_e[n] );
1330 :
1331 1812 : Word16 flag1 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmpf_fx, tmpf_e );
1332 1812 : tmp16 = Mpy_32_32( 1342177280 /*1.25f in Q30*/, peakQ_fx[n] ); // peakQ_e[n]+1
1333 1812 : Word16 flag2 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmp16, peakQ_e[n] + 1 );
1334 :
1335 1812 : IF( EQ_16( flag1, negate( 1 ) ) )
1336 : {
1337 14 : itdLR[n] = 0;
1338 14 : on[n] = 0;
1339 14 : peakQ_fx[n] = 0;
1340 14 : peakQ_e[n] = 0;
1341 14 : Q_fx[n] = 0;
1342 14 : Q_e[n] = 0;
1343 14 : move16();
1344 14 : move16();
1345 14 : move16();
1346 14 : move16();
1347 14 : move16();
1348 14 : move16();
1349 : }
1350 1798 : ELSE IF( EQ_16( flag2, 1 ) )
1351 : {
1352 17 : itdLR[n] = itd_cand[n];
1353 17 : move16();
1354 : }
1355 1812 : Word16 flag3 = BASOP_Util_Cmp_Mant32Exp( peakQ_fx[n], peakQ_e[n], Q_fx[n], Q_e[n] );
1356 1812 : IF( EQ_16( flag3, negate( 1 ) ) )
1357 : {
1358 184 : peakQ_fx[n] = Q_fx[n];
1359 184 : peakQ_e[n] = Q_e[n];
1360 184 : move16();
1361 184 : move16();
1362 : }
1363 : }
1364 : ELSE /*if channel n was not active (not likely to be preceding) in the previous frame*/
1365 : {
1366 2388 : tmp13_e = 0, tmp15_e = 0;
1367 2388 : move16();
1368 2388 : move16();
1369 2388 : tmp13 = BASOP_Util_Divide1616_Scale( abs_s( itd_cand[n] ), hPOC->shift_limit, &tmp13_e );
1370 2388 : tmp14 = L_mult( 6554 /*0.2f Q15*/, tmp13 ); // tmp13_e
1371 2388 : tmp15 = BASOP_Util_Add_Mant32Exp( 1610612736 /*0.75f in Q31*/, 0, L_negate( tmp14 ), tmp13_e, &tmp15_e );
1372 :
1373 2388 : tmpf_fx = tmp15;
1374 2388 : tmpf_e = tmp15_e;
1375 2388 : move16();
1376 2388 : move16();
1377 :
1378 2388 : Word16 flag1 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmpf_fx, tmpf_e );
1379 :
1380 2388 : IF( EQ_16( flag1, negate( 1 ) ) )
1381 : {
1382 2371 : itdLR[n] = 0;
1383 2371 : Q_fx[n] = 0;
1384 : }
1385 : ELSE
1386 : {
1387 17 : itdLR[n] = itd_cand[n];
1388 17 : on[n] = 1;
1389 : }
1390 2388 : move16();
1391 2388 : move16();
1392 : }
1393 : }
1394 :
1395 : Word32 var0, var1, var2, var3;
1396 2100 : Word16 var0_e = 0, var1_e = 0, var2_e = 0, var3_e = 0;
1397 : Word16 var0_flag, var1_flag, var2_flag, var3_flag;
1398 2100 : move16();
1399 2100 : move16();
1400 2100 : move16();
1401 2100 : move16();
1402 2100 : var0 = BASOP_Util_Add_Mant32Exp( Q_fx[0], Q_e[0], L_negate( 214748368 /*0.1 in Q31*/ ), 0, &var0_e );
1403 2100 : var1 = BASOP_Util_Add_Mant32Exp( Q_fx[1], Q_e[1], L_negate( 214748368 /*0.1 in Q31*/ ), 0, &var1_e );
1404 2100 : var2 = BASOP_Util_Add_Mant32Exp( Q_fx[1], Q_e[1], Q_BAND_FX, 0, &var2_e );
1405 2100 : var3 = BASOP_Util_Add_Mant32Exp( Q_fx[0], Q_e[0], Q_BAND_FX, 0, &var3_e );
1406 :
1407 : /*flags for condtional checks*/
1408 2100 : var0_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[0], Q_e[0], var1, var1_e );
1409 2100 : var1_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[1], Q_e[1], var0, var0_e );
1410 2100 : var2_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[0], Q_e[0], var2, var2_e );
1411 2100 : var3_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[1], Q_e[1], var3, var3_e );
1412 :
1413 2100 : test();
1414 2100 : test();
1415 2100 : test();
1416 2100 : test();
1417 2100 : test();
1418 2100 : test();
1419 2100 : test();
1420 2100 : IF( ( on[0] && prev_off[0] ) && ( on[1] && prev_off[1] ) ) /*if both channels have newly detected as active (possibility of preceding), select channel by peakness Q[] of POC */
1421 : {
1422 0 : IF( GT_32( Q_fx[0], Q_fx[1] ) )
1423 : {
1424 0 : *itd_fx = itdLR[0];
1425 : }
1426 : ELSE
1427 : {
1428 0 : *itd_fx = itdLR[1];
1429 : }
1430 : }
1431 2100 : ELSE IF( ( on[0] && prev_off[0] ) && ( EQ_16( var0_flag, 1 ) ) ) /* if channel 0 becomes active, select channel 0*/
1432 : {
1433 9 : *itd_fx = itdLR[0];
1434 : }
1435 2091 : ELSE IF( ( on[1] && prev_off[1] ) && ( EQ_16( var1_flag, 1 ) ) ) /*if channel 1 becomes active, selsect channel 1*/
1436 : {
1437 6 : *itd_fx = itdLR[1];
1438 : }
1439 2085 : ELSE IF( EQ_16( var2_flag, 1 ) ) /* if no status change, use Q[]*/
1440 : {
1441 803 : *itd_fx = itdLR[0];
1442 : }
1443 1282 : ELSE IF( EQ_16( var3_flag, 1 ) ) /* if no status change, use Q[]*/
1444 : {
1445 385 : *itd_fx = itdLR[1];
1446 : }
1447 897 : ELSE IF( *itd_fx == 0 ) /*if no channels are likely to be preceding, follow the status of the previous frame*/
1448 : {
1449 739 : *itd_fx = 0;
1450 : }
1451 : ELSE /*follow the status of the previous frame*/
1452 : {
1453 158 : IF( *itd_fx > 0 )
1454 : {
1455 92 : *itd_fx = itdLR[0];
1456 : }
1457 : ELSE
1458 : {
1459 66 : *itd_fx = itdLR[1];
1460 : }
1461 : }
1462 :
1463 2100 : move32();
1464 :
1465 6300 : FOR( i = 0; i < CPE_CHANNELS; i++ )
1466 : {
1467 4200 : Q_fx[i] = L_shr( Q_fx[i], negate( Q_e[i] ) ); // Q31
1468 4200 : move32();
1469 : }
1470 6300 : FOR( i = 0; i < CPE_CHANNELS; i++ )
1471 : {
1472 4200 : peakQ_fx[i] = L_shr( peakQ_fx[i], sub( 15, peakQ_e[i] ) ); // Q16
1473 4200 : move32();
1474 : }
1475 :
1476 2100 : Word16 Q_sub_sqrt_e = 0;
1477 2100 : Word32 Q_sub_sqrt = Sqrt32( L_abs( L_sub( Q_fx[0], Q_fx[1] ) ), &Q_sub_sqrt_e );
1478 2100 : cconfidence_fx = Q_sub_sqrt;
1479 2100 : move16();
1480 2100 : move32();
1481 :
1482 2100 : Word32 cconfidence_var1 = Mpy_32_32( hPOC->confidence_fx, STEREO_DMX_EVS_CORR_FORGETTING_FX ); // Q31
1483 2100 : Word32 cconfidence_var2 = Mpy_32_32( L_shr( cconfidence_fx, negate( Q_sub_sqrt_e ) ) /*Q31*/, L_sub( ONE_IN_Q30, L_shr( STEREO_DMX_EVS_CORR_FORGETTING_FX, 1 ) ) ); // Q30
1484 2100 : hPOC->confidence_fx = L_add( cconfidence_var1, L_shl( cconfidence_var2, 1 ) ); // Q31
1485 2100 : move32();
1486 :
1487 2100 : return hPOC->confidence_fx;
1488 : }
1489 :
1490 : /*-------------------------------------------------------------------*
1491 : * estimate_itd()
1492 : *
1493 : * estimate itd
1494 : *-------------------------------------------------------------------*/
1495 2100 : static ivas_error estimate_itd_fx(
1496 : Word16 *corr, /* o : correlation */
1497 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
1498 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
1499 : const Word32 srcL[], /* i : Lch input signal Q16 */
1500 : const Word32 srcR[], /* i : Rch input signal Q16 */
1501 : Word16 itd[], /* o : estimated itd Q0 */
1502 : const Word16 input_frame /* i : input frame length per channel */
1503 : )
1504 : {
1505 : Word32 specLr[L_FRAME48k / 2 + 1], specLi[L_FRAME48k / 2 + 1], specRr[L_FRAME48k / 2 + 1], specRi[L_FRAME48k / 2 + 1];
1506 : Word16 rfft_coef[L_FRAME48k];
1507 : const Word16 *p_w;
1508 : Word16 n, n0, n1;
1509 : Word16 rfft_coef_step;
1510 : ivas_error error;
1511 :
1512 2100 : error = IVAS_ERR_OK;
1513 2100 : move32();
1514 :
1515 2100 : n0 = shr( input_frame, 1 );
1516 2100 : n1 = shr( input_frame, 2 );
1517 :
1518 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
1519 : {
1520 0 : p_w = dft_trigo_32k_fx;
1521 0 : rfft_coef_step = 4;
1522 0 : move16();
1523 : }
1524 2100 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
1525 : {
1526 1050 : p_w = dft_trigo_32k_fx;
1527 1050 : rfft_coef_step = 2;
1528 1050 : move16();
1529 : }
1530 1050 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
1531 : {
1532 1050 : p_w = dft_trigo_48k_fx;
1533 1050 : rfft_coef_step = 2;
1534 1050 : move16();
1535 : }
1536 : ELSE
1537 : {
1538 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "failed estimate_itd()\n" );
1539 : }
1540 :
1541 422100 : FOR( n = 0; n < n1; n++ )
1542 : {
1543 420000 : rfft_coef[n] = p_w[n * rfft_coef_step];
1544 420000 : move16();
1545 420000 : rfft_coef[n0 - n] = p_w[n * rfft_coef_step];
1546 420000 : move16();
1547 : }
1548 2100 : rfft_coef[n1] = p_w[n1 * rfft_coef_step];
1549 2100 : move16();
1550 :
1551 : Word16 specL_e, specR_e, spec_e;
1552 2100 : estimate_itd_wnd_fft_fx( srcL, specLr, specLi, &specL_e, rfft_coef, hPOC->wnd_fx, input_frame );
1553 2100 : estimate_itd_wnd_fft_fx( srcR, specRr, specRi, &specR_e, rfft_coef, hPOC->wnd_fx, input_frame );
1554 :
1555 2100 : spec_e = s_max( sub( specL_e, s_min( getScaleFactor32( specLr, input_frame / 2 + 1 ), getScaleFactor32( specLi, input_frame / 2 + 1 ) ) ),
1556 2100 : sub( specR_e, s_min( getScaleFactor32( specRr, input_frame / 2 + 1 ), getScaleFactor32( specRi, input_frame / 2 + 1 ) ) ) );
1557 2100 : spec_e = add( spec_e, 2 ); // guard bits
1558 2100 : scale_sig32( specLr, add( shr( input_frame, 1 ), 1 ), sub( specL_e, spec_e ) ); // Q(31-(specL_e-spec_e))
1559 2100 : scale_sig32( specLi, add( shr( input_frame, 1 ), 1 ), sub( specL_e, spec_e ) ); // Q(31-(specL_e-spec_e))
1560 2100 : scale_sig32( specRr, add( shr( input_frame, 1 ), 1 ), sub( specR_e, spec_e ) ); // Q(31-(specR_e-spec_e))
1561 2100 : scale_sig32( specRi, add( shr( input_frame, 1 ), 1 ), sub( specR_e, spec_e ) ); // Q(31-(specR_e-spec_e))
1562 :
1563 2100 : calc_poc_fx( hPOC, hPHA, hPOC->wnd_fx, rfft_coef, specLr, specLi, specRr, specRi, spec_e, input_frame );
1564 :
1565 2100 : *corr = round_fx( find_poc_peak_fx( hPOC, itd, input_frame, STEREO_DMX_EVS_POC_W_FORGETTING_FX ) );
1566 2100 : move16();
1567 :
1568 2100 : return error;
1569 : }
1570 :
1571 : /*-------------------------------------------------------------------*
1572 : * weighted_ave()
1573 : *
1574 : * create weighted downmix signal
1575 : *-------------------------------------------------------------------*/
1576 2100 : static void weighted_ave_fx(
1577 : const Word32 src1_fx[], /* i : Lch input signal Q16 */
1578 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
1579 : Word32 dst_fx[], /* o : output signal Q16 */
1580 : const Word32 gain_fx, /* i : adapting gain Q31 */
1581 : const Word32 old_gain_fx, /* i : adapting prev gain Q31 */
1582 : const Word16 input_frame, /* i : input frame length per channel */
1583 : const Word32 wnd_fx[] /* i : window coef Q31 */
1584 : )
1585 : {
1586 : Word16 i, len;
1587 2100 : Word32 gain_tmp_fx = 0, gain_sub_fx;
1588 :
1589 2100 : move32();
1590 2100 : len = shr( input_frame, 4 );
1591 2100 : gain_sub_fx = L_sub( gain_fx, old_gain_fx );
1592 107100 : FOR( i = 0; i < len; i++ )
1593 : {
1594 105000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, wnd_fx[i] ) ); // Q31
1595 105000 : dst_fx[i] = L_add_sat( Mpy_32_32( src1_fx[i], gain_tmp_fx ), Mpy_32_32( src2_fx[i], L_sub( MAX_32, gain_tmp_fx ) ) ); // Q16
1596 105000 : move32();
1597 : }
1598 1577100 : FOR( ; i < input_frame; i++ )
1599 : {
1600 1575000 : dst_fx[i] = L_add_sat( Mpy_32_32( src1_fx[i], gain_fx ), Mpy_32_32( src2_fx[i], L_sub( MAX_32, gain_tmp_fx ) ) ); // Q16
1601 1575000 : move32();
1602 : }
1603 :
1604 2100 : return;
1605 : }
1606 : /*-------------------------------------------------------------------*
1607 : * calc_energy()
1608 : *
1609 : * calculate energy
1610 : *-------------------------------------------------------------------*/
1611 6300 : static void calc_energy_fx(
1612 : const Word32 src1_fx[], /* i : Lch input signal Q16*/
1613 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
1614 : Word32 energy_fx[], /* o : calculated energy energy_fx_e*/
1615 : Word16 *energy_fx_e, /* o : calculated energy */
1616 : const Word16 input_frame, /* i : input frame length per channel */
1617 : const Word32 ratio_float_fx // Q31
1618 : )
1619 : {
1620 : Word32 E_32_fx, wnd_fx, wnd_diff_fx;
1621 : Word16 i, adaptlen;
1622 : Word64 E_fx;
1623 :
1624 : /* Initialization */
1625 6300 : E_fx = 0;
1626 6300 : move32();
1627 6300 : adaptlen = shr( input_frame, 4 );
1628 : // wnd = 0.5f / (float) adaptlen; adaptlen= 20;in Q31
1629 6300 : wnd_fx = 53687091; // initialising for L_FRAME16k =( 0.5f /20) in Q31;
1630 6300 : move32();
1631 6300 : wnd_diff_fx = 107374182; // initialising for L_FRAME16k =( 1.0f /20) in Q31;
1632 6300 : move32();
1633 6300 : SWITCH( input_frame )
1634 : {
1635 0 : case L_FRAME16k:
1636 0 : wnd_fx = 53687091; // wnd = 0.5f / (float) adaptlen; adaptlen= 20;in Q31
1637 0 : wnd_diff_fx = 107374182; // wnd = 1.0f / (float) adaptlen;adaptlen= 20;in Q31
1638 0 : BREAK;
1639 3150 : case L_FRAME32k:
1640 3150 : wnd_fx = 26843545; // wnd = 0.5f / (float) adaptlen; adaptlen= 40;in Q31
1641 3150 : wnd_diff_fx = 53687091; // wnd = 1.0f / (float) adaptlen;adaptlen= 40;in Q31
1642 3150 : BREAK;
1643 3150 : case L_FRAME48k:
1644 3150 : wnd_fx = 17895697; // wnd = 0.5f / (float) adaptlen; adaptlen= 60;in Q31
1645 3150 : wnd_diff_fx = 35791394; // wnd = 1.0f / (float) adaptlen;adaptlen= 60;in Q31
1646 3150 : BREAK;
1647 : }
1648 6300 : move32();
1649 6300 : move32();
1650 : // wnd = 0.5f / (float) adaptlen;
1651 : // wnd_diff = 1.0f / (float) adaptlen;
1652 6300 : Word16 gb = find_guarded_bits_fx( input_frame );
1653 6300 : wnd_fx = L_shr( wnd_fx, gb );
1654 6300 : wnd_diff_fx = L_shr( wnd_diff_fx, gb );
1655 321300 : FOR( i = 0; i < adaptlen; i++ )
1656 : {
1657 : // E += ( src1[i] * wnd ) * ( src2[i] * wnd );
1658 315000 : E_fx = W_add( E_fx, W_mult0_32_32( Mpy_32_32( src1_fx[i], wnd_fx ), Mpy_32_32( src2_fx[i], wnd_fx ) ) ); // 2*(Q16-gb)
1659 :
1660 : // wnd += wnd_diff;
1661 315000 : wnd_fx = L_add( wnd_fx, wnd_diff_fx );
1662 : }
1663 4416300 : FOR( ; i < input_frame - adaptlen; i++ )
1664 : {
1665 : // E += src1[i] * src2[i];
1666 4410000 : E_fx = W_add( E_fx, W_shr( W_mult0_32_32( src1_fx[i], src2_fx[i] ), shl( gb, 1 ) ) );
1667 : }
1668 321300 : FOR( ; i < input_frame; i++ )
1669 : {
1670 : // wnd -= wnd_diff;
1671 315000 : wnd_fx = L_sub( wnd_fx, wnd_diff_fx );
1672 :
1673 : // E += ( src1[i] * wnd ) * ( src2[i] * wnd );
1674 315000 : E_fx = W_add( E_fx, W_mult0_32_32( Mpy_32_32( src1_fx[i], wnd_fx ), Mpy_32_32( src2_fx[i], wnd_fx ) ) ); // 2*(Q16-gb)
1675 : }
1676 6300 : Word16 lshift = W_norm( E_fx );
1677 6300 : E_32_fx = W_extract_h( W_shl( E_fx, lshift ) ); // 2*(Q16-gb)+lshift -32
1678 6300 : Word16 q_E = sub( add( shl( sub( Q16, gb ), 1 ), lshift ), 32 );
1679 : Word16 temp_e, q_temp32;
1680 6300 : Word32 temp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( E_32_fx, input_frame, &temp_e ) ); // Q_E +(15-temp_e) + 16
1681 6300 : q_temp32 = add( add( q_E, sub( 15, temp_e ) ), 16 );
1682 : // *energy = *energy * ratio_float + ( E / (float) input_frame ) * ( 1.0f - ratio_float );
1683 6300 : *energy_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( *energy_fx, ratio_float_fx ), *energy_fx_e, Mpy_32_32( temp32, L_sub( MAX_32, ratio_float_fx ) ), sub( 31, q_temp32 ), energy_fx_e );
1684 6300 : move32();
1685 :
1686 6300 : return;
1687 : }
1688 :
1689 : /*-------------------------------------------------------------------*
1690 : * adapt_gain()
1691 : *
1692 : * adapt gain to the signal
1693 : *-------------------------------------------------------------------*/
1694 4200 : static void adapt_gain_fx(
1695 : const Word32 src_fx[], /* i : input signal Q16 */
1696 : Word32 dst_fx[], /* o : output signal Q16 */
1697 : const Word32 gain_fx, /* i : adapting gain Q31*/
1698 : const Word32 old_gain_fx, /* i : adapting prev gain Q31*/
1699 : const Word16 input_frame, /* i : input frame length per channel */
1700 : const Word32 wnd_fx[] /* i : window coef Q31 */
1701 : )
1702 : {
1703 : Word16 i, len;
1704 : Word32 gain_tmp_fx, gain_sub_fx;
1705 :
1706 4200 : len = shr( input_frame, 4 );
1707 : // gain_sub = gain - old_gain;
1708 4200 : gain_sub_fx = L_sub( gain_fx, old_gain_fx ); // Q31
1709 :
1710 214200 : FOR( i = 0; i < len; i++ )
1711 : {
1712 : // gain_tmp = old_gain + gain_sub * wnd[i];
1713 210000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, wnd_fx[i] ) ); // Q31 // dst[i] = src[i] * gain_tmp;
1714 210000 : dst_fx[i] = Mpy_32_32( src_fx[i], gain_tmp_fx ); // Q16
1715 210000 : move32();
1716 : }
1717 3154200 : FOR( ; i < input_frame; i++ )
1718 : {
1719 : // dst[i] = src[i] * gain;
1720 3150000 : dst_fx[i] = Mpy_32_32( src_fx[i], gain_fx ); // Q16
1721 3150000 : move32();
1722 : }
1723 :
1724 4200 : return;
1725 : }
1726 :
1727 : /*-------------------------------------------------------------------*
1728 : * create_M_signal()
1729 : *
1730 : * create downmix signal
1731 : *-------------------------------------------------------------------*/
1732 2100 : static void create_M_signal_fx(
1733 : const Word32 srcL_fx[], /* i : Lch input signal Q16 */
1734 : const Word32 srcR_fx[], /* i : Rch input signal Q16 */
1735 : Word32 dmx_fx[], /* o : output signal Q31 */
1736 : const Word32 w_curr_fx, /* i : adapting weight Q31 */
1737 : const Word16 input_frame, /* i : input frame length per channel */
1738 : const Word32 wnd_fx[], /* i : window coef Q31 */
1739 : Word32 *w_prev_fx, /* i/o: adapting prev weight Q31*/
1740 : Word32 *dmx_energy_fx, /* i/o: downmix signal energy dmx_energy_fx_e */
1741 : Word16 *dmx_energy_fx_e, /* i/o: downmix signal energy */
1742 : Word32 *src_energy_fx, /* i/o: input signal energy src_energy_fx_e */
1743 : Word16 *src_energy_fx_e /* i/o: input signal energy */
1744 : )
1745 : {
1746 : Word32 amp_mod_fx[CPE_CHANNELS];
1747 : Word32 weighted_fx[L_FRAME48k], Lbias_fx;
1748 : Word32 eps_fx, temp_32;
1749 : Word32 temp32_1, temp32_2;
1750 : Word16 temp_e;
1751 : Word16 temp_e_1, temp_e_2;
1752 :
1753 : /* Initialization */
1754 2100 : eps_fx = 1024; // 1024.0f in Q0
1755 2100 : move32();
1756 2100 : IF( w_prev_fx[2] == 0 )
1757 : {
1758 798 : Lbias_fx = 1073741824; // 4.0f in Q28
1759 798 : move32();
1760 : }
1761 : ELSE
1762 : {
1763 1302 : Lbias_fx = 67108864; // 0.25f in Q28
1764 1302 : move32();
1765 : }
1766 2100 : weighted_ave_fx( srcL_fx, srcR_fx, dmx_fx, w_curr_fx, w_prev_fx[0], input_frame, wnd_fx );
1767 2100 : calc_energy_fx( srcL_fx, srcL_fx, src_energy_fx, src_energy_fx_e, input_frame, STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX );
1768 2100 : calc_energy_fx( srcR_fx, srcR_fx, src_energy_fx + 1, src_energy_fx_e + 1, input_frame, STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX );
1769 2100 : calc_energy_fx( dmx_fx, dmx_fx, dmx_energy_fx, dmx_energy_fx_e, input_frame, STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX );
1770 :
1771 2100 : temp32_1 = Mpy_32_32( src_energy_fx[0], Lbias_fx ); // 31 - src_energy_fx_e + Q28-31
1772 2100 : temp_e_1 = add( src_energy_fx_e[0], 3 );
1773 2100 : Word16 flag = BASOP_Util_Cmp_Mant32Exp( temp32_1, temp_e_1, src_energy_fx[1], src_energy_fx_e[1] );
1774 : // if ( src_energy[0] * Lbias > src_energy[1] )
1775 2100 : IF( EQ_16( flag, 1 ) )
1776 : {
1777 782 : temp32_1 = BASOP_Util_Add_Mant32Exp( dmx_energy_fx[0], dmx_energy_fx_e[0], eps_fx, Q31, &temp_e_1 );
1778 782 : temp32_2 = BASOP_Util_Add_Mant32Exp( src_energy_fx[0], src_energy_fx_e[0], eps_fx, Q31, &temp_e_2 );
1779 782 : temp_32 = L_deposit_h( BASOP_Util_Divide3232_Scale( temp32_1, temp32_2, &temp_e ) ); // Q31-temp_e+temp_e_2-temp_e_1
1780 782 : temp_e_1 = sub( add( temp_e, temp_e_1 ), temp_e_2 );
1781 : // temp = sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[0] + eps ) );
1782 782 : temp32_1 = Sqrt32( temp_32, &temp_e_1 );
1783 : // amp_mod[0] = 1.0f - sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[0] + eps ) );
1784 782 : amp_mod_fx[0] = L_shl( L_sub( ONE_IN_Q30, L_shl( temp32_1, sub( temp_e_1, 1 ) ) ), 1 );
1785 782 : move32();
1786 782 : amp_mod_fx[0] = L_max( amp_mod_fx[0], 0 );
1787 782 : move32();
1788 782 : amp_mod_fx[1] = 0;
1789 782 : move32();
1790 : }
1791 : ELSE
1792 : {
1793 1318 : temp32_1 = BASOP_Util_Add_Mant32Exp( dmx_energy_fx[0], dmx_energy_fx_e[0], eps_fx, Q31, &temp_e_1 );
1794 1318 : temp32_2 = BASOP_Util_Add_Mant32Exp( src_energy_fx[1], src_energy_fx_e[1], eps_fx, Q31, &temp_e_2 );
1795 1318 : temp_32 = L_deposit_h( BASOP_Util_Divide3232_Scale( temp32_1, temp32_2, &temp_e ) ); // Q31-temp_e+temp_e_2-temp_e_1
1796 1318 : temp_e_1 = sub( add( temp_e, temp_e_1 ), temp_e_2 );
1797 : // amp_mod[1] = 1.0f - sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[1] + eps ) );
1798 1318 : temp32_1 = Sqrt32( temp_32, &temp_e_1 );
1799 1318 : amp_mod_fx[1] = L_shl( L_sub( ONE_IN_Q30, L_shl( temp32_1, sub( temp_e_1, 1 ) ) ), 1 );
1800 1318 : move32();
1801 : // amp_mod[1] = max( amp_mod[1], 0.0f );
1802 1318 : amp_mod_fx[1] = L_max( amp_mod_fx[1], 0 );
1803 1318 : move32();
1804 1318 : amp_mod_fx[0] = 0;
1805 1318 : move32();
1806 : }
1807 2100 : adapt_gain_fx( srcL_fx, weighted_fx, amp_mod_fx[0], w_prev_fx[1], input_frame, wnd_fx );
1808 2100 : v_add_fx( dmx_fx, weighted_fx, dmx_fx, input_frame ); // Q16
1809 2100 : adapt_gain_fx( srcR_fx, weighted_fx, amp_mod_fx[1], w_prev_fx[2], input_frame, wnd_fx );
1810 2100 : v_add_fx( dmx_fx, weighted_fx, dmx_fx, input_frame ); // Q16
1811 2100 : w_prev_fx[0] = w_curr_fx;
1812 2100 : move32();
1813 2100 : w_prev_fx[1] = amp_mod_fx[0];
1814 2100 : move32();
1815 2100 : w_prev_fx[2] = amp_mod_fx[1];
1816 2100 : move32();
1817 :
1818 2100 : return;
1819 : }
1820 :
1821 : /*-------------------------------------------------------------------*
1822 : * stereo_dmx_evs_enc()
1823 : *
1824 : * Stereo downmix for EVS encoder routine
1825 : *-------------------------------------------------------------------*/
1826 2100 : void stereo_dmx_evs_enc_fx(
1827 : STEREO_DMX_EVS_ENC_HANDLE hStereoDmxEVS, /* i/o: Stereo downmix for EVS encoder handle */
1828 : const Word32 input_Fs, /* i : input sampling rate */
1829 : Word16 data[CPE_CHANNELS * L_FRAME48k], /* i/o: input signal Q0 */
1830 : const Word16 n_samples, /* i : number of input samples */
1831 : const bool is_binaural /* i : indication that input is binaural audio */
1832 : )
1833 : {
1834 : Word16 n;
1835 : Word16 dmx_weight, corr; // Q15
1836 : Word32 data_fx[CPE_CHANNELS][L_FRAME48k]; // Q16/Q11
1837 :
1838 : Word16 k, m, pha_len, fad_len;
1839 : Word32 mem_prev[STEREO_DMX_EVS_FAD_LEN_MAX], data_mem[STEREO_DMX_EVS_DATA_LEN_MAX]; // Q11
1840 : Word32 *p_data_mem, *p_prev_taps, *p_curr_taps, *fad_g, *p_data;
1841 : Word32 dmx_poc_data[L_FRAME48k] /*Q11*/, dmx_pha_data[L_FRAME48k] /*Q11*/, *p_dmx_data, fx_tmp;
1842 : Word16 fx_tmp_e;
1843 : STEREO_DMX_EVS_PRC curr_prc;
1844 : Word16 input_subframe, is_transient;
1845 : Word32 *p_sub_frame, subframe_energy[STEREO_DMX_EVS_NB_SBFRM];
1846 : Word16 subframe_energy_e[STEREO_DMX_EVS_NB_SBFRM];
1847 :
1848 : Word16 input_frame;
1849 :
1850 : Word32 L_tmp1, L_tmp2;
1851 : Word16 L_tmp1_e, L_tmp2_e;
1852 :
1853 : Word64 W_tmp;
1854 : Word16 W_tmp_q;
1855 :
1856 2100 : push_wmops( "stereo_dmx_evs_enc" );
1857 : if ( is_binaural )
1858 : {
1859 : /* use of is_binaural flag is to be considered */
1860 : }
1861 :
1862 : // input_frame = (int16_t) ( input_Fs / FRAMES_PER_SEC );
1863 2100 : SWITCH( input_Fs )
1864 : {
1865 0 : case 8000:
1866 0 : input_frame = 160;
1867 0 : BREAK;
1868 0 : case 16000:
1869 0 : input_frame = 320;
1870 0 : BREAK;
1871 1050 : case 32000:
1872 1050 : input_frame = 640;
1873 1050 : BREAK;
1874 1050 : case 48000:
1875 1050 : input_frame = 960;
1876 1050 : BREAK;
1877 0 : default:
1878 0 : input_frame = 960;
1879 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
1880 0 : BREAK;
1881 : }
1882 2100 : move16();
1883 :
1884 1682100 : FOR( n = 0; n < input_frame; n++ )
1885 : {
1886 1680000 : data_fx[0][n] = L_deposit_h( data[2 * n] );
1887 1680000 : move32();
1888 1680000 : data_fx[1][n] = L_deposit_h( data[2 * n + 1] );
1889 1680000 : move32();
1890 : }
1891 2100 : IF( LT_16( n_samples, input_frame ) )
1892 : {
1893 0 : set32_fx( data_fx[0] + n_samples, 0, sub( input_frame, n_samples ) );
1894 0 : set32_fx( data_fx[1] + n_samples, 0, sub( input_frame, n_samples ) );
1895 : }
1896 :
1897 :
1898 : // input_subframe = n_samples / STEREO_DMX_EVS_NB_SBFRM;
1899 2100 : IF( EQ_16( n_samples, L_FRAME16k ) )
1900 : {
1901 0 : input_subframe = 64;
1902 0 : move16();
1903 : }
1904 2100 : ELSE IF( EQ_16( n_samples, L_FRAME32k ) )
1905 : {
1906 1050 : input_subframe = 128;
1907 1050 : move16();
1908 : }
1909 1050 : ELSE IF( EQ_16( n_samples, L_FRAME48k ) )
1910 : {
1911 1050 : input_subframe = 192;
1912 1050 : move16();
1913 : }
1914 : ELSE
1915 : {
1916 0 : input_subframe = 192;
1917 0 : move16();
1918 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
1919 : }
1920 :
1921 2100 : is_transient = 0;
1922 2100 : move16();
1923 6300 : FOR( k = 0; k < CPE_CHANNELS; k++ )
1924 : {
1925 4200 : fx_tmp = 0;
1926 4200 : move32();
1927 4200 : fx_tmp_e = 0;
1928 4200 : move16();
1929 25200 : FOR( m = 0; m < STEREO_DMX_EVS_NB_SBFRM; m++ )
1930 : {
1931 21000 : p_sub_frame = &( data_fx[k][m * input_subframe] );
1932 21000 : subframe_energy[m] = 0;
1933 21000 : move32();
1934 21000 : subframe_energy_e[m] = 0;
1935 21000 : move16();
1936 3381000 : FOR( n = 0; n < input_subframe; n++ )
1937 : {
1938 : // subframe_energy[m] += p_sub_frame[n] * p_sub_frame[n];
1939 3360000 : W_tmp = W_mult_32_32( p_sub_frame[n], p_sub_frame[n] );
1940 3360000 : W_tmp_q = W_norm( W_tmp );
1941 3360000 : W_tmp = W_shl( W_tmp, W_tmp_q );
1942 3360000 : L_tmp1 = W_extract_h( W_tmp ); // Q(31-(30-W_tmp_q))
1943 3360000 : L_tmp1_e = sub( 15 * 2, W_tmp_q );
1944 3360000 : subframe_energy[m] = BASOP_Util_Add_Mant32Exp( subframe_energy[m], subframe_energy_e[m], L_tmp1, L_tmp1_e, &subframe_energy_e[m] );
1945 3360000 : move32();
1946 : }
1947 :
1948 21000 : L_tmp1 = BASOP_Util_Add_Mant32Exp( hStereoDmxEVS->hPHA->trns_aux_energy_fx[k], hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[k], EPSILON_FX_M, EPSILON_FX_E, &L_tmp1_e );
1949 21000 : L_tmp2 = BASOP_Util_Divide3232_Scale_newton( subframe_energy[m], L_tmp1, &L_tmp2_e );
1950 21000 : L_tmp2_e = add( L_tmp2_e, sub( subframe_energy_e[m], L_tmp1_e ) );
1951 : // if ( subframe_energy[m] / ( hStereoDmxEVS->hPHA->trns_aux_energy[k] + EPSILON ) > hStereoDmxEVS->hPHA->crst_fctr )
1952 21000 : if ( BASOP_Util_Cmp_Mant32Exp( L_tmp2, L_tmp2_e, hStereoDmxEVS->hPHA->crst_fctr_fx, 31 ) > 0 )
1953 : {
1954 23 : is_transient = 1;
1955 23 : move16();
1956 : }
1957 :
1958 21000 : IF( hStereoDmxEVS->hPHA->init_frmCntr == 0 )
1959 : {
1960 : // hStereoDmxEVS->hPHA->trns_aux_energy[k] = STEREO_DMX_EVS_TRNS_EGY_FORGETTING * hStereoDmxEVS->hPHA->trns_aux_energy[k] + ( 1.0f - STEREO_DMX_EVS_TRNS_EGY_FORGETTING ) * subframe_energy[m];
1961 20800 : hStereoDmxEVS->hPHA->trns_aux_energy_fx[k] = BASOP_Util_Add_Mant32Exp( Mpy_32_16_1( hStereoDmxEVS->hPHA->trns_aux_energy_fx[k], STEREO_DMX_EVS_TRNS_EGY_FORGETTING_Q15 ), hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[k], Mpy_32_16_1( subframe_energy[m], sub( MAX_16, STEREO_DMX_EVS_TRNS_EGY_FORGETTING_Q15 ) ), subframe_energy_e[m], &hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[k] );
1962 20800 : move32();
1963 : }
1964 : ELSE
1965 : {
1966 : // hStereoDmxEVS->hPHA->trns_aux_energy[k] = 0.5f * hStereoDmxEVS->hPHA->trns_aux_energy[k] + 0.5f * subframe_energy[m];
1967 200 : hStereoDmxEVS->hPHA->trns_aux_energy_fx[k] = BASOP_Util_Add_Mant32Exp( hStereoDmxEVS->hPHA->trns_aux_energy_fx[k], add( hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[k], 1 ), subframe_energy[m], add( subframe_energy_e[m], 1 ), &hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[k] );
1968 200 : move32();
1969 : }
1970 :
1971 21000 : fx_tmp = BASOP_Util_Add_Mant32Exp( fx_tmp, fx_tmp_e, subframe_energy[m], subframe_energy_e[m], &fx_tmp_e );
1972 : }
1973 :
1974 21000 : FOR( m = 1; m < STEREO_DMX_EVS_NB_SBFRM; m++ )
1975 : {
1976 16800 : L_tmp1 = BASOP_Util_Add_Mant32Exp( subframe_energy[m - 1], subframe_energy_e[m - 1], EPSILON_FX_M, EPSILON_FX_E, &L_tmp1_e );
1977 16800 : L_tmp2 = BASOP_Util_Divide3232_Scale_newton( subframe_energy[m], L_tmp1, &L_tmp2_e );
1978 16800 : L_tmp2_e = add( L_tmp2_e, sub( subframe_energy_e[m], L_tmp1_e ) );
1979 : // if ( subframe_energy[m] / ( subframe_energy[m - 1] + EPSILON ) > STEREO_DMX_EVS_TRNS_DTC_INST )
1980 16800 : if ( BASOP_Util_Cmp_Mant32Exp( L_tmp2, L_tmp2_e, STEREO_DMX_EVS_TRNS_DTC_INST_Q0, 31 ) > 0 )
1981 : {
1982 23 : is_transient = 1;
1983 23 : move16();
1984 : }
1985 : }
1986 : }
1987 :
1988 2100 : estimate_itd_fx( &corr, hStereoDmxEVS->hPOC, hStereoDmxEVS->hPHA, data_fx[0], data_fx[1], &hStereoDmxEVS->itd_fx, input_frame );
1989 :
1990 : /* poc */
1991 :
1992 2100 : IF( hStereoDmxEVS->itd_fx )
1993 : {
1994 : // dmx_weight = ( ( hStereoDmxEVS->itd > 0 ) ? ( -1 ) : 1 ) * 0.5f * corr + 0.5f;
1995 1001 : IF( hStereoDmxEVS->itd_fx > 0 )
1996 : {
1997 550 : dmx_weight = add( negate( shr( corr, 1 ) ), ONE_IN_Q14 );
1998 : }
1999 : ELSE
2000 : {
2001 451 : dmx_weight = add( shr( corr, 1 ), ONE_IN_Q14 );
2002 : }
2003 : }
2004 : ELSE
2005 : {
2006 1099 : dmx_weight = ONE_IN_Q14;
2007 1099 : move16();
2008 : }
2009 :
2010 2100 : create_M_signal_fx( data_fx[0], data_fx[1], dmx_poc_data, L_deposit_h( dmx_weight ), input_frame, hStereoDmxEVS->s_wnd_fx,
2011 2100 : hStereoDmxEVS->dmx_weight_fx, hStereoDmxEVS->pre_dmx_energy_fx, hStereoDmxEVS->pre_dmx_energy_fx_e, hStereoDmxEVS->aux_dmx_energy_fx, hStereoDmxEVS->aux_dmx_energy_fx_e );
2012 :
2013 : // Downscaling signals to avoid accumulation overflows
2014 2100 : scale_sig32( data_fx[0], input_frame, -5 ); // Q31->Q26
2015 2100 : scale_sig32( data_fx[1], input_frame, -5 ); // Q31->Q26
2016 2100 : scale_sig32( dmx_poc_data, input_frame, -5 ); // Q31->Q26
2017 :
2018 : /* pha */
2019 :
2020 2100 : pha_len = hStereoDmxEVS->hPHA->pha_len;
2021 2100 : move16();
2022 2100 : fad_len = hStereoDmxEVS->hPHA->fad_len;
2023 2100 : move16();
2024 2100 : fad_g = hStereoDmxEVS->hPHA->fad_g_fx;
2025 :
2026 2100 : set_zero_fx( dmx_pha_data, n_samples );
2027 2100 : set_zero_fx( mem_prev, fad_len );
2028 :
2029 6300 : FOR( k = 0; k < CPE_CHANNELS; k++ )
2030 : {
2031 4200 : p_data = data_fx[k];
2032 4200 : Copy32( hStereoDmxEVS->hPHA->data_mem_fx[k], data_mem, pha_len );
2033 4200 : Copy32( &( p_data[n_samples - pha_len] ), hStereoDmxEVS->hPHA->data_mem_fx[k], pha_len );
2034 4200 : p_data_mem = &( data_mem[pha_len] );
2035 4200 : Copy32( p_data, p_data_mem, n_samples );
2036 :
2037 4200 : p_prev_taps = hStereoDmxEVS->hPHA->p_prev_taps_fx[k];
2038 4200 : IF( p_prev_taps )
2039 : {
2040 342454 : FOR( n = 0; n < fad_len; n++ )
2041 : {
2042 16738400 : FOR( ( fx_tmp = 0, m = 0 ); m < pha_len; m++ )
2043 : {
2044 : // ftmp += p_data_mem[n - m] * p_prev_taps[m];
2045 16396800 : fx_tmp = L_add( fx_tmp, Mpy_32_32( p_data_mem[n - m], p_prev_taps[m] ) ); // Q25
2046 : }
2047 341600 : fx_tmp = L_shl( fx_tmp, 1 ); // Q26
2048 341600 : mem_prev[n] = L_add( mem_prev[n], Mpy_32_32( fx_tmp, INV_SQRT_2_Q31 ) );
2049 341600 : move32();
2050 : }
2051 : }
2052 : ELSE
2053 : {
2054 1341746 : FOR( n = 0; n < fad_len; n++ )
2055 : {
2056 : // mem_prev[n] += p_data[n] * INV_SQRT_2;
2057 1338400 : mem_prev[n] = L_add( mem_prev[n], Mpy_32_32( p_data[n], INV_SQRT_2_Q31 ) ); // Q26
2058 1338400 : move32();
2059 : }
2060 : }
2061 :
2062 4200 : p_curr_taps = hStereoDmxEVS->hPHA->p_curr_taps_fx[k];
2063 4200 : IF( p_curr_taps )
2064 : {
2065 684054 : FOR( n = 0; n < n_samples; n++ )
2066 : {
2067 33476800 : FOR( ( fx_tmp = 0, m = 0 ); m < pha_len; m++ )
2068 : {
2069 : // ftmp += p_data_mem[n - m] * p_curr_taps[m];
2070 32793600 : fx_tmp = L_add( fx_tmp, Mpy_32_32( p_data_mem[n - m], p_curr_taps[m] ) ); // Q25
2071 : }
2072 683200 : fx_tmp = L_shl( fx_tmp, 1 ); // Q26
2073 : // dmx_pha_data[n] += ftmp * INV_SQRT_2;
2074 683200 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( fx_tmp, INV_SQRT_2_Q31 ) ); // Q26
2075 683200 : move32();
2076 : }
2077 : }
2078 : ELSE
2079 : {
2080 2680146 : FOR( n = 0; n < n_samples; n++ )
2081 : {
2082 : // dmx_pha_data[n] += p_data[n] * INV_SQRT_2;
2083 2676800 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( p_data[n], INV_SQRT_2_Q31 ) ); // Q26
2084 2676800 : move32();
2085 : }
2086 : }
2087 : }
2088 :
2089 842100 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len; ( n++, m-- ) )
2090 : {
2091 840000 : dmx_pha_data[n] = Mpy_32_32( dmx_pha_data[n], fad_g[n] );
2092 840000 : move32();
2093 840000 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( mem_prev[n], fad_g[m] ) ); // Q26
2094 840000 : move32();
2095 : }
2096 :
2097 : /* prc switch */
2098 :
2099 2100 : curr_prc = hStereoDmxEVS->hPHA->curr_prc;
2100 2100 : move32();
2101 : // if ( abs( (int16_t) hStereoDmxEVS->itd ) > hStereoDmxEVS->hPHA->prc_thres )
2102 2100 : IF( GT_16( abs_s( hStereoDmxEVS->itd_fx ), hStereoDmxEVS->hPHA->prc_thres ) )
2103 : {
2104 559 : IF( NE_32( hStereoDmxEVS->hPHA->curr_prc, STEREO_DMX_EVS_PRC_POC ) )
2105 : {
2106 6 : IF( EQ_32( hStereoDmxEVS->hPHA->prev_prc, STEREO_DMX_EVS_PRC_POC ) )
2107 : {
2108 3 : hStereoDmxEVS->hPHA->prc_hys_cnt = add( hStereoDmxEVS->hPHA->prc_hys_cnt, 1 );
2109 3 : move16();
2110 : }
2111 : ELSE
2112 : {
2113 3 : hStereoDmxEVS->hPHA->prc_hys_cnt = 0;
2114 3 : move16();
2115 : }
2116 :
2117 6 : if ( GE_16( hStereoDmxEVS->hPHA->prc_hys_cnt, STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES ) )
2118 : {
2119 3 : hStereoDmxEVS->hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
2120 3 : move32();
2121 : }
2122 : }
2123 559 : hStereoDmxEVS->hPHA->prev_prc = STEREO_DMX_EVS_PRC_POC;
2124 559 : move32();
2125 : }
2126 : ELSE
2127 : {
2128 1541 : IF( NE_32( hStereoDmxEVS->hPHA->curr_prc, STEREO_DMX_EVS_PRC_PHA ) )
2129 : {
2130 1301 : IF( EQ_32( hStereoDmxEVS->hPHA->prev_prc, STEREO_DMX_EVS_PRC_PHA ) )
2131 : {
2132 1292 : hStereoDmxEVS->hPHA->prc_hys_cnt = add( hStereoDmxEVS->hPHA->prc_hys_cnt, 1 );
2133 1292 : move16();
2134 : }
2135 : ELSE
2136 : {
2137 9 : hStereoDmxEVS->hPHA->prc_hys_cnt = 0;
2138 9 : move16();
2139 : }
2140 :
2141 1301 : if ( GE_16( hStereoDmxEVS->hPHA->prc_hys_cnt, STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES ) )
2142 : {
2143 1292 : hStereoDmxEVS->hPHA->curr_prc = STEREO_DMX_EVS_PRC_PHA;
2144 1292 : move32();
2145 : }
2146 : }
2147 1541 : hStereoDmxEVS->hPHA->prev_prc = STEREO_DMX_EVS_PRC_PHA;
2148 1541 : move32();
2149 : }
2150 :
2151 : // if ( ( is_transient == 1 ) || ( hStereoDmxEVS->aux_dmx_energy[0] > STEREO_DMX_EVS_ILDS_EGY * hStereoDmxEVS->aux_dmx_energy[1] ) || ( hStereoDmxEVS->aux_dmx_energy[1] > STEREO_DMX_EVS_ILDS_EGY * hStereoDmxEVS->aux_dmx_energy[0] ) || ( ( hStereoDmxEVS->hPHA->p_curr_taps[0] == NULL ) && ( hStereoDmxEVS->hPHA->p_curr_taps[1] == NULL ) ) )
2152 2100 : test();
2153 2100 : test();
2154 2100 : test();
2155 2100 : test();
2156 2100 : IF( EQ_16( is_transient, 1 ) ||
2157 : BASOP_Util_Cmp_Mant32Exp( hStereoDmxEVS->aux_dmx_energy_fx[0], hStereoDmxEVS->aux_dmx_energy_fx_e[0], Mpy_32_32( STEREO_DMX_EVS_ILDS_EGY_Q17, hStereoDmxEVS->aux_dmx_energy_fx[1] ), add( hStereoDmxEVS->aux_dmx_energy_fx_e[1], 14 ) ) > 0 ||
2158 : BASOP_Util_Cmp_Mant32Exp( hStereoDmxEVS->aux_dmx_energy_fx[1], hStereoDmxEVS->aux_dmx_energy_fx_e[1], Mpy_32_32( STEREO_DMX_EVS_ILDS_EGY_Q17, hStereoDmxEVS->aux_dmx_energy_fx[0] ), add( hStereoDmxEVS->aux_dmx_energy_fx_e[0], 14 ) ) > 0 ||
2159 : ( ( hStereoDmxEVS->hPHA->p_curr_taps_fx[0] == NULL ) && ( hStereoDmxEVS->hPHA->p_curr_taps_fx[1] == NULL ) ) )
2160 : {
2161 1676 : hStereoDmxEVS->hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
2162 1676 : move32();
2163 1676 : hStereoDmxEVS->hPHA->prc_hys_cnt = 0;
2164 1676 : move16();
2165 : }
2166 :
2167 2100 : IF( EQ_32( hStereoDmxEVS->hPHA->curr_prc, STEREO_DMX_EVS_PRC_POC ) )
2168 : {
2169 1854 : p_dmx_data = dmx_poc_data;
2170 :
2171 1854 : IF( NE_32( curr_prc, hStereoDmxEVS->hPHA->curr_prc ) )
2172 : {
2173 16 : fad_len = hStereoDmxEVS->hPHA->fad_len_prc;
2174 16 : move16();
2175 16 : fad_g = hStereoDmxEVS->hPHA->fad_g_prc_fx;
2176 :
2177 13136 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len; ( n++, m-- ) )
2178 : {
2179 13120 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n] ); // Q26
2180 13120 : move32();
2181 13120 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m], dmx_pha_data[n] ) ); // Q26
2182 13120 : move32();
2183 : }
2184 : }
2185 : }
2186 : ELSE
2187 : {
2188 246 : p_dmx_data = dmx_pha_data;
2189 :
2190 246 : IF( NE_32( curr_prc, hStereoDmxEVS->hPHA->curr_prc ) )
2191 : {
2192 16 : fad_len = hStereoDmxEVS->hPHA->fad_len_prc;
2193 16 : move16();
2194 16 : fad_g = hStereoDmxEVS->hPHA->fad_g_prc_fx;
2195 :
2196 13136 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len; ( n++, m-- ) )
2197 : {
2198 13120 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n] ); // Q26
2199 13120 : move32();
2200 13120 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m], dmx_poc_data[n] ) ); // Q26
2201 13120 : move32();
2202 : }
2203 : }
2204 : }
2205 :
2206 2100 : Copy_Scale_sig32_16( p_dmx_data, data, n_samples, 5 ); // Q26->Q15
2207 :
2208 2100 : pop_wmops();
2209 :
2210 2100 : return;
2211 : }
2212 :
2213 :
2214 : /*-------------------------------------------------------------------*
2215 : * stereo_dmx_evs_init_encoder()
2216 : *
2217 : * open and initialize stereo downmix for EVS encoder
2218 : *-------------------------------------------------------------------*/
2219 2 : ivas_error stereo_dmx_evs_init_encoder_fx(
2220 : STEREO_DMX_EVS_ENC_HANDLE *hStereoDmxEVS_out, /* o : Stereo downmix for EVS encoder handle */
2221 : const Word32 input_Fs /* i : input sampling rate */
2222 : )
2223 : {
2224 : STEREO_DMX_EVS_ENC_HANDLE hStereoDmxEVS;
2225 : Word16 n, input_frame;
2226 :
2227 : Word16 m, len, pha_len, fad_len, fad_len2, rfft_ipd_coef_step, n0, input_frame_pha;
2228 : Word32 *fad_g, fad_r, *ipd_ff;
2229 : Word16 *win;
2230 : const Word16 *p_ipd_w;
2231 : Word16 tmp_e;
2232 :
2233 : // input_frame = (Word16) ( input_Fs / FRAMES_PER_SEC );
2234 2 : SWITCH( input_Fs )
2235 : {
2236 0 : case 8000:
2237 0 : input_frame = 160;
2238 0 : BREAK;
2239 0 : case 16000:
2240 0 : input_frame = 320;
2241 0 : BREAK;
2242 1 : case 32000:
2243 1 : input_frame = 640;
2244 1 : BREAK;
2245 1 : case 48000:
2246 1 : input_frame = 960;
2247 1 : BREAK;
2248 0 : default:
2249 0 : input_frame = 960;
2250 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2251 0 : BREAK;
2252 : }
2253 2 : move16();
2254 2 : hStereoDmxEVS = NULL;
2255 2 : IF( ( hStereoDmxEVS = (STEREO_DMX_EVS_ENC_HANDLE) malloc( sizeof( STEREO_DMX_EVS_ENC_DATA ) ) ) == NULL )
2256 : {
2257 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_ENC_DATA\n" ) );
2258 : }
2259 2 : hStereoDmxEVS->itd_fx = 0;
2260 2 : move16();
2261 2 : hStereoDmxEVS->pre_dmx_energy_fx[0] = 0;
2262 2 : move32();
2263 2 : hStereoDmxEVS->pre_dmx_energy_fx_e[0] = 0;
2264 2 : move16();
2265 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2266 : {
2267 4 : hStereoDmxEVS->aux_dmx_energy_fx[n] = 0;
2268 4 : move32();
2269 4 : hStereoDmxEVS->aux_dmx_energy_fx_e[n] = 0;
2270 4 : move16();
2271 : }
2272 :
2273 2 : hStereoDmxEVS->dmx_weight_fx[0] = ONE_IN_Q30; // 0.5f;
2274 2 : move32();
2275 2 : hStereoDmxEVS->dmx_weight_fx[1] = 0;
2276 2 : move32();
2277 2 : hStereoDmxEVS->dmx_weight_fx[2] = 0;
2278 2 : move32();
2279 :
2280 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2281 : {
2282 0 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_16k_fx;
2283 : }
2284 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2285 : {
2286 1 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_32k_fx;
2287 : }
2288 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2289 : {
2290 1 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_48k_fx;
2291 : }
2292 : ELSE
2293 : {
2294 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2295 : }
2296 :
2297 2 : hStereoDmxEVS->hPOC = NULL;
2298 2 : IF( ( hStereoDmxEVS->hPOC = (STEREO_DMX_EVS_POC_HANDLE) malloc( sizeof( STEREO_DMX_EVS_POC_DATA ) ) ) == NULL )
2299 : {
2300 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_POC_DATA\n" ) );
2301 : }
2302 :
2303 : // hStereoDmxEVS->hPOC->shift_limit = (int16_t) ( STEREO_DMX_EVS_SHIFT_LIMIT * input_Fs / 1000 );
2304 2 : SWITCH( input_Fs )
2305 : {
2306 0 : case 8000:
2307 0 : hStereoDmxEVS->hPOC->shift_limit = 45;
2308 0 : move16();
2309 0 : BREAK;
2310 0 : case 16000:
2311 0 : hStereoDmxEVS->hPOC->shift_limit = 90;
2312 0 : move16();
2313 0 : BREAK;
2314 1 : case 32000:
2315 1 : hStereoDmxEVS->hPOC->shift_limit = 180;
2316 1 : move16();
2317 1 : BREAK;
2318 1 : case 48000:
2319 1 : hStereoDmxEVS->hPOC->shift_limit = 270;
2320 1 : move16();
2321 1 : BREAK;
2322 0 : default:
2323 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2324 0 : BREAK;
2325 : }
2326 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2327 : {
2328 4 : hStereoDmxEVS->hPOC->peakQ_fx[n] = 0;
2329 4 : move32();
2330 : // hStereoDmxEVS->hPOC->peak_width_fx[n] = (float) hStereoDmxEVS->hPOC->shift_limit / 2;
2331 4 : hStereoDmxEVS->hPOC->peak_width_fx[n] = L_shl( hStereoDmxEVS->hPOC->shift_limit, Q15 ); // Q16
2332 4 : move32();
2333 4 : hStereoDmxEVS->hPOC->ispeak[n] = 0;
2334 4 : move16();
2335 4 : hStereoDmxEVS->hPOC->itdLR[n] = 0;
2336 4 : move16();
2337 : }
2338 2 : set32_fx( hStereoDmxEVS->hPOC->P_fx, 0, L_FRAME48k );
2339 :
2340 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2341 : {
2342 0 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_48k_fx;
2343 : }
2344 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2345 : {
2346 1 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_32k_fx;
2347 : }
2348 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2349 : {
2350 1 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_48k_fx;
2351 : }
2352 : ELSE
2353 : {
2354 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2355 : }
2356 : // hStereoDmxEVS->hPOC->eps = 2.0f * EVS_PI / ( (float) input_frame );
2357 2 : SWITCH( input_frame )
2358 : {
2359 0 : case 160:
2360 0 : hStereoDmxEVS->hPOC->eps_fx = 84331486; // (2.0f * EVS_PI / (160)) in Q31
2361 0 : BREAK;
2362 0 : case 320:
2363 0 : hStereoDmxEVS->hPOC->eps_fx = 42165743; // (2.0f * EVS_PI / (160)) in Q31
2364 0 : BREAK;
2365 1 : case 640:
2366 1 : hStereoDmxEVS->hPOC->eps_fx = 21082871; // (2.0f * EVS_PI / (160)) in Q31
2367 1 : BREAK;
2368 1 : case 960:
2369 1 : hStereoDmxEVS->hPOC->eps_fx = 14055248; // (2.0f * EVS_PI / (160)) in Q31
2370 1 : BREAK;
2371 : }
2372 2 : move32();
2373 :
2374 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2375 : {
2376 0 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_32k_fx;
2377 : }
2378 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2379 : {
2380 1 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_32k_fx;
2381 : }
2382 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2383 : {
2384 1 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_48k_fx;
2385 : }
2386 : ELSE
2387 : {
2388 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2389 : }
2390 :
2391 2 : hStereoDmxEVS->hPOC->confidence_fx = 0;
2392 2 : move32();
2393 :
2394 2 : hStereoDmxEVS->hPHA = NULL;
2395 2 : IF( ( hStereoDmxEVS->hPHA = (STEREO_DMX_EVS_PHA_HANDLE) malloc( sizeof( STEREO_DMX_EVS_PHA_DATA ) ) ) == NULL )
2396 : {
2397 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_CORFILT_DATA\n" ) );
2398 : }
2399 :
2400 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2401 : {
2402 4 : hStereoDmxEVS->hPHA->p_curr_taps_fx[n] = NULL;
2403 4 : hStereoDmxEVS->hPHA->p_prev_taps_fx[n] = NULL;
2404 :
2405 4 : set_zero_fx( hStereoDmxEVS->hPHA->data_mem_fx[n], STEREO_DMX_EVS_PHA_LEN_MAX );
2406 4 : set_zero_fx( hStereoDmxEVS->hPHA->curr_taps_fx[n], STEREO_DMX_EVS_PHA_LEN_MAX );
2407 : }
2408 :
2409 2 : IF( EQ_32( input_Fs, 16000 ) )
2410 : {
2411 0 : len = STEREO_DMX_EVS_PHA_LEN_16;
2412 0 : move16();
2413 0 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_16;
2414 0 : move16();
2415 0 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_16;
2416 0 : move16();
2417 0 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_16_Q0;
2418 0 : move32();
2419 : }
2420 2 : ELSE IF( EQ_32( input_Fs, 32000 ) )
2421 : {
2422 1 : len = STEREO_DMX_EVS_PHA_LEN_32;
2423 1 : move16();
2424 1 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_32;
2425 1 : move16();
2426 1 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_32;
2427 1 : move16();
2428 1 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_32_Q0;
2429 1 : move32();
2430 : }
2431 1 : ELSE IF( EQ_32( input_Fs, 48000 ) )
2432 : {
2433 1 : len = STEREO_DMX_EVS_PHA_LEN_48;
2434 1 : move16();
2435 1 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_48;
2436 1 : move16();
2437 1 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_48;
2438 1 : move16();
2439 1 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_48_Q0;
2440 1 : move32();
2441 : }
2442 : ELSE
2443 : {
2444 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sampling frequency\n" );
2445 : }
2446 :
2447 2 : hStereoDmxEVS->hPHA->pha_len = shr( len, 1 );
2448 2 : move16();
2449 2 : hStereoDmxEVS->hPHA->init_frmCntr = 10; // (int16_t)(FRAMES_PER_SEC * 0.2f)
2450 2 : move16();
2451 2 : hStereoDmxEVS->hPHA->isd_rate_s_fx = 0;
2452 2 : move32();
2453 2 : hStereoDmxEVS->hPHA->iccr_s_fx = 0;
2454 2 : move32();
2455 :
2456 2 : pha_len = hStereoDmxEVS->hPHA->pha_len;
2457 2 : move16();
2458 2 : fad_len = hStereoDmxEVS->hPHA->fad_len;
2459 2 : move16();
2460 :
2461 2 : set16_fx( hStereoDmxEVS->hPHA->win_fx, 29491 /*1.8f in Q14*/, pha_len );
2462 2 : hStereoDmxEVS->hPHA->win_fx[0] = ONE_IN_Q14;
2463 2 : move16();
2464 2 : IF( EQ_32( input_Fs, 16000 ) )
2465 : {
2466 0 : hStereoDmxEVS->hPHA->win_fx[pha_len - 1] = 7373; /*0.45f in Q14*/
2467 0 : move16();
2468 : }
2469 2 : ELSE IF( EQ_32( input_Fs, 32000 ) || EQ_32( input_Fs, 48000 ) )
2470 : {
2471 2 : hStereoDmxEVS->hPHA->win_fx[sub( pha_len, 2 )] = 19302; /*1.1781f in Q14*/
2472 2 : move16();
2473 2 : hStereoDmxEVS->hPHA->win_fx[sub( pha_len, 1 )] = 2816; /*0.1718f in Q14*/
2474 2 : move16();
2475 : }
2476 :
2477 2 : fad_g = hStereoDmxEVS->hPHA->fad_g_fx;
2478 : // fad_r = 1.0f / (float) ( fad_len + 1 );
2479 2 : SWITCH( fad_len )
2480 : {
2481 0 : case STEREO_DMX_EVS_FAD_LEN_16:
2482 0 : fad_r = 853658096;
2483 0 : move32();
2484 0 : tmp_e = -6;
2485 0 : move16();
2486 0 : BREAK;
2487 1 : case STEREO_DMX_EVS_FAD_LEN_32:
2488 1 : fad_r = 856317467;
2489 1 : move32();
2490 1 : tmp_e = -7;
2491 1 : move16();
2492 1 : BREAK;
2493 1 : case STEREO_DMX_EVS_FAD_LEN_48:
2494 1 : fad_r = 571471740;
2495 1 : move32();
2496 1 : tmp_e = -7;
2497 1 : move16();
2498 1 : BREAK;
2499 0 : default:
2500 0 : fad_r = BASOP_Util_Divide3232_Scale_newton( 1, add( fad_len, 1 ), &tmp_e );
2501 0 : BREAK;
2502 : }
2503 2 : fad_r = L_shl_r( fad_r, tmp_e );
2504 2 : fad_len2 = shr( fad_len, 1 );
2505 402 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len2; ( n++, m-- ) )
2506 : {
2507 400 : fad_g[n] = imult3216( fad_r, add( n, 1 ) );
2508 400 : move32();
2509 400 : fad_g[m] = L_sub( MAX_32, fad_g[n] );
2510 400 : move32();
2511 : }
2512 :
2513 2 : hStereoDmxEVS->hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD;
2514 2 : move32();
2515 2 : hStereoDmxEVS->hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD;
2516 2 : move32();
2517 2 : hStereoDmxEVS->hPHA->pha_hys_cnt = 0;
2518 2 : move16();
2519 :
2520 : /* Compute the forgetting factor */
2521 : // replaced below logic with table as it is same for all frame lengths
2522 2 : ipd_ff = hStereoDmxEVS->hPHA->ipd_ff_fx;
2523 2 : Copy32( ipd_ff_Q31, ipd_ff, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2524 :
2525 : // a_min = 0.8576958985908941f;
2526 : // a_max = 0.9440608762859234f;
2527 : // itrh = (int16_t)((3000 * input_frame) / (input_Fs * STEREO_DMX_EVS_SUBBAND_SIZE)); /* 3kHz */
2528 : // n0 = L_FRAME16k / (2 * STEREO_DMX_EVS_SUBBAND_SIZE);
2529 : // a_step = (a_min - a_max) / (n0 + 1 - itrh);
2530 : // for (n = 0; n < itrh; n++)
2531 : //{
2532 : // ipd_ff[n] = a_max;
2533 : // }
2534 : // for (; n < (n0 + 1); n++) /* 8kHz */
2535 : //{
2536 : // ipd_ff[n] = a_max + (n - itrh) * a_step;
2537 : // }
2538 : // for (; n < STEREO_DMX_EVS_NB_SUBBAND_MAX; n++)
2539 : //{
2540 : // ipd_ff[n] = a_min;
2541 : // }
2542 :
2543 2 : set32_fx( hStereoDmxEVS->hPHA->Pr_fx, MAX_32, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2544 2 : set_zero_fx( hStereoDmxEVS->hPHA->Pi_fx, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2545 :
2546 : // n0 = input_frame / ( 4 * STEREO_DMX_EVS_SUBBAND_SIZE );
2547 2 : n0 = shr( input_frame, 3 );
2548 : // input_frame_pha = input_frame / ( 2 * STEREO_DMX_EVS_SUBBAND_SIZE );
2549 2 : input_frame_pha = shr( input_frame, 2 );
2550 :
2551 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2552 : {
2553 0 : p_ipd_w = dft_trigo_32k_fx;
2554 0 : rfft_ipd_coef_step = 4;
2555 0 : move16();
2556 : }
2557 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2558 : {
2559 1 : p_ipd_w = dft_trigo_32k_fx;
2560 1 : rfft_ipd_coef_step = 2;
2561 1 : move16();
2562 : }
2563 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2564 : {
2565 1 : p_ipd_w = dft_trigo_48k_fx;
2566 1 : rfft_ipd_coef_step = 2;
2567 1 : move16();
2568 : }
2569 : ELSE
2570 : {
2571 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sampling frequency\n" );
2572 : }
2573 :
2574 2 : win = hStereoDmxEVS->hPHA->rfft_ipd_coef_fx;
2575 2 : len = imult1616( rfft_ipd_coef_step, STEREO_DMX_EVS_SUBBAND_SIZE );
2576 202 : FOR( n = 0; n < n0; n++ )
2577 : {
2578 200 : win[n] = p_ipd_w[n * len];
2579 200 : move16();
2580 200 : win[input_frame_pha - n] = p_ipd_w[n * len];
2581 200 : move16();
2582 : }
2583 2 : win[n0] = p_ipd_w[n0 * len];
2584 2 : move16();
2585 :
2586 2 : hStereoDmxEVS->hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
2587 2 : move32();
2588 2 : hStereoDmxEVS->hPHA->prev_prc = STEREO_DMX_EVS_PRC_POC;
2589 2 : move32();
2590 2 : hStereoDmxEVS->hPHA->prc_hys_cnt = 0;
2591 2 : move16();
2592 :
2593 : // hStereoDmxEVS->hPHA->fad_len_prc = (int16_t) ( STEREO_DMX_EVS_FADE_LEN_PRC * (float) input_Fs / 1000.0f );
2594 2 : SWITCH( input_Fs )
2595 : {
2596 0 : case 8000:
2597 0 : hStereoDmxEVS->hPHA->fad_len_prc = STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 8;
2598 0 : move16();
2599 0 : BREAK;
2600 0 : case 16000:
2601 0 : hStereoDmxEVS->hPHA->fad_len_prc = STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 16;
2602 0 : move16();
2603 0 : BREAK;
2604 1 : case 32000:
2605 1 : hStereoDmxEVS->hPHA->fad_len_prc = STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 32;
2606 1 : move16();
2607 1 : BREAK;
2608 1 : case 48000:
2609 1 : hStereoDmxEVS->hPHA->fad_len_prc = STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 48;
2610 1 : move16();
2611 1 : BREAK;
2612 0 : default:
2613 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2614 0 : BREAK;
2615 : }
2616 2 : fad_len = hStereoDmxEVS->hPHA->fad_len_prc;
2617 2 : move16();
2618 2 : fad_g = hStereoDmxEVS->hPHA->fad_g_prc_fx;
2619 : // fad_r = 1.0f / (float) ( fad_len + 1 );
2620 2 : SWITCH( fad_len )
2621 : {
2622 0 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 16:
2623 0 : fad_r = 856317467;
2624 0 : move32();
2625 0 : tmp_e = -7;
2626 0 : move16();
2627 0 : BREAK;
2628 1 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 32:
2629 1 : fad_r = 857653375;
2630 1 : move32();
2631 1 : tmp_e = -8;
2632 1 : move16();
2633 1 : BREAK;
2634 1 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 48:
2635 1 : fad_r = 572066403;
2636 1 : move32();
2637 1 : tmp_e = -8;
2638 1 : move16();
2639 1 : BREAK;
2640 0 : default:
2641 0 : fad_r = BASOP_Util_Divide3232_Scale_newton( 1, add( fad_len, 1 ), &tmp_e );
2642 0 : BREAK;
2643 : }
2644 2 : fad_r = L_shl_r( fad_r, tmp_e );
2645 2 : fad_len2 = shr( fad_len, 1 );
2646 802 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len2; ( n++, m-- ) )
2647 : {
2648 800 : fad_g[n] = imult3216( fad_r, add( n, 1 ) );
2649 800 : move32();
2650 800 : fad_g[m] = L_sub( MAX_32, fad_g[n] );
2651 800 : move32();
2652 : }
2653 :
2654 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2655 : {
2656 4 : hStereoDmxEVS->hPHA->trns_aux_energy_fx[n] = 0;
2657 4 : move32();
2658 4 : hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[n] = 0;
2659 4 : move16();
2660 : }
2661 :
2662 2 : *hStereoDmxEVS_out = hStereoDmxEVS;
2663 :
2664 2 : return IVAS_ERR_OK;
2665 : }
2666 :
2667 :
2668 : /*-------------------------------------------------------------------*
2669 : * stereo_dmx_evs_close_encoder()
2670 : *
2671 : * close stereo downmix for EVS encoder
2672 : *-------------------------------------------------------------------*/
2673 624 : void stereo_dmx_evs_close_encoder_fx(
2674 : STEREO_DMX_EVS_ENC_HANDLE *hStereoDmxEVS /* i/o: Stereo downmix for EVS encoder handle */
2675 : )
2676 : {
2677 624 : test();
2678 624 : IF( hStereoDmxEVS == NULL || *hStereoDmxEVS == NULL )
2679 : {
2680 622 : return;
2681 : }
2682 :
2683 2 : IF( ( *hStereoDmxEVS )->hPOC != NULL )
2684 : {
2685 2 : free( ( *hStereoDmxEVS )->hPOC );
2686 2 : ( *hStereoDmxEVS )->hPOC = NULL;
2687 : }
2688 :
2689 2 : IF( ( *hStereoDmxEVS )->hPHA != NULL )
2690 : {
2691 2 : free( ( *hStereoDmxEVS )->hPHA );
2692 2 : ( *hStereoDmxEVS )->hPHA = NULL;
2693 : }
2694 :
2695 2 : free( ( *hStereoDmxEVS ) );
2696 2 : ( *hStereoDmxEVS ) = NULL;
2697 :
2698 2 : return;
2699 : }
|
