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