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_enc.h"
43 : #include "wmc_auto.h"
44 : #include "ivas_prot_fx.h"
45 :
46 :
47 : /*-----------------------------------------------------------------------*
48 : * Local constants
49 : *-----------------------------------------------------------------------*/
50 :
51 : #define STEREO_DMX_EVS_FIND_POC_PEAK_TAU 4
52 : #define STEREO_DMX_EVS_FIND_POC_PEAK_TAU2 8
53 :
54 : #define STEREO_DMX_EVS_POC_SMOOTH_Q30 1342177280
55 : #define STEREO_DMX_EVS_POC_FORGETTING_Q31 1675037245
56 : #define STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 1696512082
57 : #define STEREO_DMX_EVS_SHIFT_LIMIT_Q12 23040 /* ms */
58 :
59 :
60 : #define STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX 536870912 // 0.25f in Q31
61 :
62 : #define STEREO_DMX_EVS_CORR_FORGETTING_FX 1610612736 /*Q31*/
63 : #define STEREO_DMX_EVS_POC_W_FORGETTING_FX 1879048192 /*Q31*/
64 :
65 : #define Q_BAND_FX 536870912 /*Q31*/
66 :
67 : #define STEREO_DMX_EVS_ISD_THRES_L_Q31 1932735283
68 : #define STEREO_DMX_EVS_ISD_DIST_THRES_IPD_Q15 ONE_IN_Q14
69 :
70 : #define STEREO_DMX_EVS_ISD_FORGETTING_Q31 2040109465
71 : #define STEREO_DMX_EVS_ISD_1MFORGETTING_Q15 1638
72 : #define STEREO_DMX_EVS_ISD_DIST_HYST_L_Q31 773094113
73 : #define STEREO_DMX_EVS_ISD_DIST_HYST_H_Q31 923417968
74 : #define STEREO_DMX_EVS_ISD_INVTHRES_H 1270700383
75 :
76 : #define STEREO_DMX_EVS_ICCR_FORGETTING_Q31 1503238554
77 : #define STEREO_DMX_EVS_ICCR_HYST_L_Q31 1610612736
78 : #define STEREO_DMX_EVS_ICCR_HYST_H_Q31 1825361101
79 :
80 : #define STEREO_DMX_EVS_SWTCH_HYS_THRES 1
81 : #define STEREO_DMX_EVS_LR_EGY_Q27 2013265920
82 : #define STEREO_DMX_EVS_ILDS_EGY_Q17 1310720000
83 : #define STEREO_DMX_EVS_ILD_PRC_Q15 3277
84 :
85 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_16 55
86 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_32 19
87 : #define STEREO_DMX_EVS_SWTCH_PRC_THRES_48 29
88 :
89 : #define STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES 1
90 : #define STEREO_DMX_EVS_FADE_LEN_PRC_Q0 20
91 :
92 : #define STEREO_DMX_EVS_NB_SBFRM 5
93 : #define STEREO_DMX_EVS_TRNS_DTC_INST_Q0 75
94 : #define STEREO_DMX_EVS_CRST_FCTR_16_Q0 80
95 : #define STEREO_DMX_EVS_CRST_FCTR_32_Q0 40
96 : #define STEREO_DMX_EVS_CRST_FCTR_48_Q0 35
97 :
98 : #define STEREO_DMX_EVS_TRNS_EGY_FORGETTING_Q15 24576
99 :
100 : #define STEREO_DMX_EVS_POC_RENORM_TH 1048576 // 65536 * 16
101 : #define STEREO_DMX_EVS_POC_RENORM_SHIFT 10
102 :
103 : #define STEREO_DMX_EVS_FAD_R 3
104 : #define STEREO_DMX_EVS_FAD_IR 10923 /* 1/3 in Q15 */
105 : #define STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 29491 // 0.9f
106 : #define STEREO_DMX_EVS_SGC_GR_S 32919 // 1.00461543f
107 : #define STEREO_DMX_EVS_SGC_GIR_S 32617 // 1/1.00461543f
108 : #define STEREO_DMX_EVS_SGC_GL 32391 // 0.9885f
109 : #define STEREO_DMX_EVS_SGC_GH 33148 // 1.0116f
110 : #define STEREO_DMX_EVS_SGC_LEGY_THRES_16 250000000
111 : #define STEREO_DMX_EVS_SGC_LEGY_THRES_32 300000000
112 : #define STEREO_DMX_EVS_SGC_LEGY_THRES_48 500000000
113 : #define STEREO_DMX_EVS_SGC_LEGY_THRES_E 22
114 : #define STEREO_DMX_EVS_SGC_GMAX 46340 // 1.4142f
115 : #define STEREO_DMX_EVS_SGC_GMIN 23170 // 0.7071f
116 : #define STEREO_DMX_EVS_IPD_ILD_THRES_Q29 1696512082 // 3.16f (5dB)
117 : #define STEREO_DMX_EVS_IPD_SF_THRES_Q31 107374182 // 0.05f
118 :
119 : const Word32 ipd_ff_Q31[STEREO_DMX_EVS_NB_SUBBAND_MAX] = {
120 : 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520,
121 : 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520,
122 : 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520, 2027355520,
123 : 2027355520, 2023718912, 2020082304, 2016445696, 2012809088, 2009172480, 2005535872, 2001899264, 1998262656, 1994625920,
124 : 1990989312, 1987352704, 1983716096, 1980079488, 1976442880, 1972806272, 1969169664, 1965533056, 1961896448, 1958259840,
125 : 1954623232, 1950986624, 1947350016, 1943713408, 1940076800, 1936440192, 1932803584, 1929166848, 1925530240, 1921893632,
126 : 1918257024, 1914620416, 1910983808, 1907347200, 1903710592, 1900073984, 1896437376, 1892800768, 1889164160, 1885527552,
127 : 1881890944, 1878254336, 1874617600, 1870981120, 1867344384, 1863707776, 1860071168, 1856434560, 1852797952, 1849161344,
128 : 1845524736, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
129 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
130 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
131 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
132 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
133 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
134 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
135 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
136 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
137 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
138 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
139 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
140 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
141 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
142 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128,
143 : 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128, 1841888128
144 : };
145 :
146 : /*-----------------------------------------------------------------------*
147 : * Local function prototypes
148 : *-----------------------------------------------------------------------*/
149 :
150 : void estimate_itd_wnd_fft_fx(
151 : const Word32 *input, /* i : input signal Q16 */
152 : Word32 *specr, /* o : real-part spectra Q(31-spec_e) */
153 : Word32 *speci, /* o : imaginary-part spectra Q(31-spec_e) */
154 : Word16 *spec_e,
155 : const Word16 *rfft_coef, /* i : rfft coef Q15 */
156 : const Word32 *wnd, /* i : window coef Q31 */
157 : const Word16 input_frame /* i : input frame length per channel */
158 : );
159 : static void calc_poc_fx(
160 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
161 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
162 : const Word32 wnd[], /* i : window coef Q31 */
163 : const Word16 rfft_coef[], /* i : RFFT coef Q15 */
164 : const Word32 specLr[], /* i : Lch real-part spectra Q(31-spec_e) */
165 : const Word32 specLi[], /* i : Lch imaginary-part input signal Q(31-spec_e) */
166 : const Word32 specRr[], /* i : Rch real-part spectra Q(31-spec_e) */
167 : const Word32 specRi[], /* i : Rch imaginary-part input signal Q(31-spec_e) */
168 : const Word16 spec_e,
169 : const Word16 input_frame /* i : input frame length per channel */
170 : );
171 : static ivas_error estimate_itd_fx(
172 : Word16 *corr, /* o : correlation */
173 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
174 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
175 : const Word32 srcL[], /* i : Lch input signal Q16 */
176 : const Word32 srcR[], /* i : Rch input signal Q16 */
177 : Word16 itd[], /* o : estimated itd Q0 */
178 : const Word16 input_frame /* i : input frame length per channel */
179 : );
180 : static void adapt_gain_fx(
181 : const Word32 src_fx[], /* i : input signal Q16 */
182 : Word32 dst_fx[], /* o : output signal */
183 : const Word32 gain_fx, /* i : adapting gain Q31*/
184 : const Word32 old_gain_fx, /* i : adapting prev gain Q31*/
185 : const Word16 input_frame, /* i : input frame length per channel */
186 : const Word32 wnd_fx[] /* i : window coef Q31 */
187 : );
188 : static void weighted_ave_fx(
189 : const Word32 src1_fx[], /* i : Lch input signal Q16 */
190 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
191 : Word32 dst_fx[], /* o : output signal */
192 : const Word32 gain_fx, /* i : adapting gain Q31 */
193 : const Word32 old_gain_fx, /* i : adapting prev gain Q31 */
194 : const Word16 input_frame, /* i : input frame length per channel */
195 : const Word32 wnd_fx[] /* i : window coef Q31 */
196 : );
197 : static void calc_energy_fx(
198 : const Word32 src1_fx[], /* i : Lch input signal */
199 : const Word32 src2_fx[], /* i : Rch input signal */
200 : Word32 energy_fx[], /* o : calculated energy */
201 : Word16 *energy_fx_e, /* o : calculated energy */
202 : const Word16 input_frame, /* i : input frame length per channel */
203 : const Word32 ratio_float_fx );
204 :
205 : static void create_M_signal_fx(
206 : const Word32 srcL_fx[], /* i : Lch input signal Q16 */
207 : const Word32 srcR_fx[], /* i : Rch input signal Q16 */
208 : Word32 dmx_fx[], /* o : output signal Q31 */
209 : const Word32 w_curr_fx, /* i : adapting weight Q31 */
210 : const Word16 input_frame, /* i : input frame length per channel */
211 : const Word32 wnd_fx[], /* i : window coef Q31 */
212 : Word32 *w_prev_fx, /* i/o: adapting prev weight Q31*/
213 : Word32 *dmx_energy_fx, /* i/o: downmix signal energy dmx_energy_fx_e */
214 : Word16 *dmx_energy_fx_e, /* i/o: downmix signal energy */
215 : Word32 *src_energy_fx, /* i/o: input signal energy src_energy_fx_e */
216 : Word16 *src_energy_fx_e /* i/o: input signal energy */
217 : );
218 : static Word32 find_poc_peak_fx(
219 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
220 : Word16 itd_fx[], /* o : estimated itd */
221 : const Word16 input_frame, /* i : input frame length per channel */
222 : const Word32 ratio_fixed /* i : adapting ratio */
223 : );
224 : static Word32 spectral_flatness_fx(
225 : const Word32 *sig_fx, /* i : input signal (mantissa) */
226 : const Word16 *sig_fx_e, /* i : input signal (exponent) - can be NULL */
227 : const Word16 sig_length /* i : input signal length */
228 : );
229 : static void renorm_poc_fx(
230 : Word32 *real, /* i/o real-part*/
231 : Word32 *imag /* i/o imaginary-part */
232 : );
233 : /*-------------------------------------------------------------------*
234 : * estimate_itd_wnd_fft()
235 : *
236 : * Transforms input signal from time domain into frequency domain.
237 : * The input signal is windowed before being transformed.
238 : *-------------------------------------------------------------------*/
239 4200 : void estimate_itd_wnd_fft_fx(
240 : const Word32 *input, /* i : input signal Q16 */
241 : Word32 *specr, /* o : real-part spectra Q(31-spec_e) */
242 : Word32 *speci, /* o : imaginary-part spectra Q(31-spec_e) */
243 : Word16 *spec_e,
244 : const Word16 *rfft_coef, /* i : rfft coef Q15 */
245 : const Word32 *wnd, /* i : window coef Q31 */
246 : const Word16 input_frame /* i : input frame length per channel */
247 : )
248 : {
249 : Word16 n0, i;
250 : Word32 rfft_buf[L_FRAME48k];
251 : Word16 step, bias;
252 : Word16 rshift;
253 :
254 4200 : n0 = shr( input_frame, 1 );
255 4200 : IF( EQ_16( input_frame, L_FRAME16k ) )
256 : {
257 0 : step = 3;
258 0 : move16();
259 0 : bias = 1;
260 0 : move16();
261 : }
262 : ELSE
263 : {
264 4200 : step = 1;
265 4200 : move16();
266 4200 : bias = 0;
267 4200 : move16();
268 : }
269 :
270 3364200 : FOR( i = 0; i < input_frame; i++ )
271 : {
272 : /* window */
273 3360000 : rfft_buf[i] = Mpy_32_32_r( input[i], wnd[i * step + bias] ); // Q16
274 3360000 : move32();
275 : }
276 :
277 4200 : rshift = sub( getScaleFactor32( rfft_buf, input_frame ), find_guarded_bits_fx( input_frame ) );
278 4200 : scale_sig32( rfft_buf, input_frame, rshift ); // Q(16+rshift)
279 4200 : *spec_e = sub( 15, rshift );
280 4200 : move16();
281 4200 : rfft_fx( rfft_buf, rfft_coef, input_frame, -1 );
282 :
283 1680000 : FOR( i = 1; i < n0; i++ )
284 : {
285 1675800 : specr[i] = rfft_buf[i * 2];
286 1675800 : move32();
287 1675800 : speci[i] = rfft_buf[i * 2 + 1];
288 1675800 : move32();
289 : }
290 :
291 4200 : specr[0] = rfft_buf[0];
292 4200 : move32();
293 4200 : specr[n0] = rfft_buf[1];
294 4200 : move32();
295 4200 : speci[0] = 0;
296 4200 : move32();
297 4200 : speci[n0] = 0;
298 4200 : move32();
299 :
300 4200 : return;
301 : }
302 :
303 : /*-------------------------------------------------------------------*
304 : * calc_poc()
305 : *
306 : * calculate phase only correlation
307 : *-------------------------------------------------------------------*/
308 2100 : static void calc_poc_fx(
309 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
310 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
311 : const Word32 wnd[], /* i : window coef Q31 */
312 : const Word16 rfft_coef[], /* i : RFFT coef Q15 */
313 : const Word32 specLr[], /* i : Lch real-part spectra Q(31-spec_e) */
314 : const Word32 specLi[], /* i : Lch imaginary-part input signal Q(31-spec_e) */
315 : const Word32 specRr[], /* i : Rch real-part spectra Q(31-spec_e) */
316 : const Word32 specRi[], /* i : Rch imaginary-part input signal Q(31-spec_e) */
317 : const Word16 spec_e,
318 : const Word16 input_frame /* i : input frame length per channel */
319 : )
320 : {
321 : Word16 i, n1, n2;
322 : Word16 n0, *itdLR;
323 : const Word16 *s;
324 : Word32 *P;
325 : Word32 tmp1, tmp2, Lr, Li, Rr, Ri, gamma, igamma /*, iN*/;
326 :
327 : Word32 specPOr[L_FRAME48k / 2 + 1], specPOi[L_FRAME48k / 2]; /*real and imaginary values for searching phase angle Q31*/
328 : Word32 tmpPOC1[L_FRAME48k], tmpPOC2[L_FRAME48k];
329 : Word32 rfft_buf[L_FRAME48k]; // Q21
330 : Word16 step, bias;
331 : Word16 mult_angle;
332 : Word16 j;
333 : Word16 end;
334 :
335 : Word16 cos_step, cos_max;
336 : Word32 eps_cos, eps_sin, EPS;
337 :
338 : Word16 isd_cnt_h, isd_cnt_l, ild_cnt, n, freq_8k, freq_ipd_max, nsbd, input_frame_pha, pha_ipd_ild_chan2rephase;
339 : Word32 Nr, Ni, Dr, Di, tPr, tPi, Pn, energy;
340 : Word16 Nr_e, Ni_e, tPr_e, tPi_e, Pn_e, energy_e;
341 : Word16 isd_rate, isd_rate_e;
342 : Word32 eneL, eneR, IPDr, IPDi, tIPDr, tIPDi, ICCr;
343 : Word16 eneL_e, eneR_e, IPDr_e, IPDi_e;
344 : Word32 *Pr, *Pi, *ipd_ff, *p_curr_taps;
345 : Word32 rfft_pha_buf[L_FRAME48k] /*Q22*/, tEr[STEREO_DMX_EVS_NB_SUBBAND_MAX], tEl[STEREO_DMX_EVS_NB_SUBBAND_MAX];
346 : Word16 tEr_e[STEREO_DMX_EVS_NB_SUBBAND_MAX], tEl_e[STEREO_DMX_EVS_NB_SUBBAND_MAX];
347 :
348 : Word32 L_tmp, L_tmp1, L_tmp2;
349 : Word16 L_tmp_e, L_tmp1_e, L_tmp2_e;
350 : Word64 W_tmp;
351 : Word16 W_tmp_q;
352 :
353 : /* Initialization */
354 : // iN = 1.0f / (float) input_frame;
355 2100 : s = hPOC->sin_fx;
356 2100 : P = hPOC->P_fx;
357 2100 : n0 = shr( input_frame, 1 );
358 2100 : itdLR = hPOC->itdLR;
359 :
360 2100 : Pr = hPHA->Pr_fx;
361 2100 : Pi = hPHA->Pi_fx;
362 : // nsbd = n0 / STEREO_DMX_EVS_SUBBAND_SIZE;
363 2100 : nsbd = shr( n0, 1 );
364 : // input_frame_pha = input_frame / STEREO_DMX_EVS_SUBBAND_SIZE;
365 2100 : input_frame_pha = shr( input_frame, 1 );
366 :
367 : // igamma = STEREO_DMX_EVS_POC_GAMMA * iN;
368 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
369 : {
370 0 : igamma = 5033165; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
371 0 : move16();
372 : }
373 2100 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
374 : {
375 1050 : igamma = 3355444; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
376 1050 : move16();
377 : }
378 1050 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
379 : {
380 1050 : igamma = 1677722; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
381 1050 : move16();
382 : }
383 : ELSE
384 : {
385 0 : igamma = 1677722; //(STEREO_DMX_EVS_POC_GAMMA/input_frame) in Q31
386 0 : move16();
387 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
388 : }
389 2100 : gamma = L_sub( MAX_32, igamma );
390 :
391 2100 : step = 1;
392 2100 : move16();
393 2100 : bias = 0;
394 2100 : move16();
395 2100 : cos_step = 2;
396 2100 : move16();
397 2100 : cos_max = n0;
398 2100 : move16();
399 2100 : mult_angle = 3;
400 2100 : move16();
401 :
402 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
403 : {
404 0 : step = 3;
405 0 : move16();
406 0 : bias = 1;
407 0 : move16();
408 0 : cos_step = 4;
409 0 : move16();
410 0 : cos_max = input_frame;
411 0 : move16();
412 0 : mult_angle = 2; /*****/
413 0 : move16();
414 : }
415 2100 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
416 : {
417 1050 : mult_angle = 2;
418 1050 : move16();
419 : }
420 :
421 2100 : end = s_min( n0, 320 );
422 : // specPOr[0] = sign( specLr[0] * specRr[0] ) * wnd[bias];
423 2100 : specPOr[0] = imult3216( wnd[bias], sign_fx( Mpy_32_32( specLr[0], specRr[0] ) ) );
424 2100 : move32();
425 2100 : specPOi[0] = 0;
426 2100 : move32();
427 2100 : EPS = hPOC->eps_fx;
428 2100 : move16();
429 2100 : IF( EQ_32( input_frame, L_FRAME48k ) )
430 : {
431 252000 : FOR( i = 1; i < n0 / 2; i++ )
432 : {
433 : // eps_cos = s[cos_max - i * cos_step /*cos_max - i_for*/] * EPS;
434 250950 : eps_cos = Mpy_32_16_1( EPS, s[cos_max - i * cos_step /*cos_max - i_for*/] );
435 : // eps_sin = s[i * cos_step /*i_for*/] * EPS;
436 250950 : eps_sin = Mpy_32_16_1( EPS, s[i * cos_step /*i_for*/] );
437 :
438 250950 : Lr = L_add( L_add( specLr[i], Mpy_32_32_r( specRr[i], eps_cos ) ), Mpy_32_32_r( specRi[i], eps_sin ) );
439 250950 : Li = L_add( L_sub( specLi[i], Mpy_32_32_r( specRr[i], eps_sin ) ), Mpy_32_32_r( specRi[i], eps_cos ) );
440 250950 : Rr = L_add( L_add( specRr[i], Mpy_32_32_r( specLr[i], eps_cos ) ), Mpy_32_32_r( specLi[i], eps_sin ) );
441 250950 : Ri = L_add( L_sub( specRi[i], Mpy_32_32_r( specLr[i], eps_sin ) ), Mpy_32_32_r( specLi[i], eps_cos ) );
442 :
443 250950 : renorm_poc_fx( &Lr, &Li );
444 250950 : renorm_poc_fx( &Rr, &Ri );
445 :
446 250950 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
447 250950 : move32();
448 250950 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
449 250950 : move32();
450 250950 : j = sub( n0, i );
451 250950 : IF( LT_16( j, 320 ) )
452 : {
453 82950 : Lr = L_add( L_sub( specLr[j], Mpy_32_32_r( specRr[j], eps_cos ) ), Mpy_32_32_r( specRi[j], eps_sin ) );
454 82950 : Li = L_sub( L_sub( specLi[j], Mpy_32_32_r( specRr[j], eps_sin ) ), Mpy_32_32_r( specRi[j], eps_cos ) );
455 82950 : Rr = L_add( L_sub( specRr[j], Mpy_32_32_r( specLr[j], eps_cos ) ), Mpy_32_32_r( specLi[j], eps_sin ) );
456 82950 : Ri = L_sub( L_sub( specRi[j], Mpy_32_32_r( specLr[j], eps_sin ) ), Mpy_32_32_r( specLi[j], eps_cos ) );
457 :
458 82950 : renorm_poc_fx( &Lr, &Li );
459 82950 : renorm_poc_fx( &Rr, &Ri );
460 :
461 82950 : specPOr[j] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
462 82950 : move32();
463 82950 : specPOi[j] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
464 82950 : move32();
465 : }
466 : }
467 : }
468 : ELSE /* 16kHz and 32 kHz*/
469 : {
470 168000 : FOR( i = 1; i < n0 / 2; i++ )
471 : {
472 : // eps_cos = s[cos_max - i * cos_step /*cos_max - i_for*/] * EPS;
473 166950 : eps_cos = Mpy_32_16_1( EPS, s[cos_max - i * cos_step /*cos_max - i_for*/] );
474 : // eps_sin = s[i * cos_step /*i_for*/] * EPS;
475 166950 : eps_sin = Mpy_32_16_1( EPS, s[i * cos_step /*i_for*/] );
476 :
477 166950 : Lr = L_add( L_add( specLr[i], Mpy_32_32_r( specRr[i], eps_cos ) ), Mpy_32_32_r( specRi[i], eps_sin ) );
478 166950 : Li = L_add( L_sub( specLi[i], Mpy_32_32_r( specRr[i], eps_sin ) ), Mpy_32_32_r( specRi[i], eps_cos ) );
479 166950 : Rr = L_add( L_add( specRr[i], Mpy_32_32_r( specLr[i], eps_cos ) ), Mpy_32_32_r( specLi[i], eps_sin ) );
480 166950 : Ri = L_add( L_sub( specRi[i], Mpy_32_32_r( specLr[i], eps_sin ) ), Mpy_32_32_r( specLi[i], eps_cos ) );
481 :
482 166950 : renorm_poc_fx( &Lr, &Li );
483 166950 : renorm_poc_fx( &Rr, &Ri );
484 :
485 166950 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
486 166950 : move32();
487 166950 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
488 166950 : move32();
489 :
490 166950 : j = sub( n0, i );
491 166950 : Lr = L_add( L_sub( specLr[j], Mpy_32_32_r( specRr[j], eps_cos ) ), Mpy_32_32_r( specRi[j], eps_sin ) );
492 166950 : Li = L_sub( L_sub( specLi[j], Mpy_32_32_r( specRr[j], eps_sin ) ), Mpy_32_32_r( specRi[j], eps_cos ) );
493 166950 : Rr = L_add( L_sub( specRr[j], Mpy_32_32_r( specLr[j], eps_cos ) ), Mpy_32_32_r( specLi[j], eps_sin ) );
494 166950 : Ri = L_sub( L_sub( specRi[j], Mpy_32_32_r( specLr[j], eps_sin ) ), Mpy_32_32_r( specLi[j], eps_cos ) );
495 :
496 166950 : renorm_poc_fx( &Lr, &Li );
497 166950 : renorm_poc_fx( &Rr, &Ri );
498 :
499 166950 : specPOr[j] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) ); // 2*spec_e
500 166950 : move32();
501 166950 : specPOi[j] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) ); // 2*spec_e
502 166950 : move32();
503 : }
504 : }
505 : {
506 : /* i=n0/2*/
507 2100 : Lr = L_add( specLr[i], Mpy_32_32_r( specRi[i], EPS ) );
508 2100 : Li = L_sub( specLi[i], Mpy_32_32_r( specRr[i], EPS ) );
509 2100 : Rr = L_add( specRr[i], Mpy_32_32_r( specLi[i], EPS ) );
510 2100 : Ri = L_sub( specRi[i], Mpy_32_32_r( specLr[i], EPS ) );
511 :
512 2100 : renorm_poc_fx( &Lr, &Li );
513 2100 : renorm_poc_fx( &Rr, &Ri );
514 :
515 2100 : specPOr[i] = L_add( Mpy_32_32_r( Lr, Rr ), Mpy_32_32_r( Li, Ri ) );
516 2100 : move32();
517 2100 : specPOi[i] = L_sub( Mpy_32_32_r( Lr, Ri ), Mpy_32_32_r( Li, Rr ) );
518 2100 : move32();
519 : }
520 : /* complex spectrum (specPOr[i], specPOi[i]) are placed on an unit circle without using srqt()*/
521 21000 : FOR( i = 1; i < 10; i++ ) /*search from 4 angles */
522 : {
523 18900 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma );
524 :
525 18900 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), 28377 /*0.866f*/ ) ); /* low angles are more frequent for low frequency */
526 18900 : move32();
527 18900 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), ONE_IN_Q14 /*0.5f*/ ) );
528 18900 : move32();
529 18900 : gamma = L_sub( gamma, igamma );
530 : }
531 33600 : FOR( ; i < ( n0 >> 4 ); i++ ) /*search from 4 angles */
532 : {
533 31500 : tmp1 = Mpy_32_16_1( Mpy_32_32_r( wnd[i * step + bias], gamma ), 23170 /*0.7071f*/ );
534 :
535 31500 : specPOr[i] = imult3216( tmp1, sign_fx( specPOr[i] ) );
536 31500 : move32();
537 31500 : specPOi[i] = imult3216( tmp1, sign_fx( specPOi[i] ) ); /* low accuracy is adequate for low frequency */
538 31500 : move32();
539 31500 : gamma = L_sub( gamma, igamma );
540 : }
541 :
542 54600 : FOR( ; i < ( n0 >> 3 ); i++ ) /* binary search from 8 angles */
543 : {
544 52500 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma ); // Q31
545 :
546 52500 : IF( GT_32( L_abs( specPOr[i] ), L_abs( specPOi[i] ) ) )
547 : {
548 27583 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), /*0.923880f*/ s[120 * mult_angle] ) ); /* cos(PI/8) Q31*/
549 27583 : move32();
550 27583 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), /*0.382683f*/ s[40 * mult_angle] ) ); // Q31
551 27583 : move32();
552 : }
553 : ELSE
554 : {
555 24917 : specPOr[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOr[i] ), /*0.382683f*/ s[40 * mult_angle] ) ); /* cos(PI*3/8) Q31*/
556 24917 : move32();
557 24917 : specPOi[i] = Mpy_32_16_1( tmp1, imult1616( sign_fx( specPOi[i] ), /*0.923880f*/ s[120 * mult_angle] ) ); // Q31
558 24917 : move32();
559 : }
560 52500 : gamma = L_sub( gamma, igamma );
561 : }
562 569100 : FOR( ; i < end; i++ ) /* binary search from 16 angles */
563 : {
564 567000 : tmp1 = Mpy_32_32_r( wnd[i * step + bias], gamma ); // Q31
565 567000 : IF( GT_32( L_abs( specPOr[i] ), L_abs( specPOi[i] ) ) )
566 : {
567 314737 : IF( GT_32( L_abs( Mpy_32_16_1( specPOr[i], 13573 /*0.414213f*/ ) ), L_abs( specPOi[i] ) ) ) /*tan(PI/8)*/
568 : {
569 182133 : specPOr[i] = Mpy_32_16_1( tmp1 /*0.980785f */, imult1616( sign_fx( specPOr[i] ), s[140 * mult_angle] ) ); /* cos(PI/16) Q31*/
570 182133 : move32();
571 182133 : specPOi[i] = Mpy_32_16_1( tmp1 /*0.195090f */, imult1616( sign_fx( specPOi[i] ), s[20 * mult_angle] ) ); // Q31
572 182133 : move32();
573 : }
574 : ELSE
575 : {
576 132604 : specPOr[i] = Mpy_32_16_1( tmp1 /* 0.831470f */, imult1616( sign_fx( specPOr[i] ), s[100 * mult_angle] ) ); /*cos(PI*3/16) Q31*/
577 132604 : move32();
578 132604 : specPOi[i] = Mpy_32_16_1( tmp1 /* 0.555570f*/, imult1616( sign_fx( specPOi[i] ), s[60 * mult_angle] ) ); // Q31
579 132604 : move32();
580 : }
581 : }
582 : ELSE
583 : {
584 252263 : IF( GT_32( L_abs( specPOr[i] ), L_abs( Mpy_32_16_1( specPOi[i], 13573 /*0.414213f*/ ) ) ) ) /*tan(PI/8)*/
585 : {
586 127649 : specPOr[i] = Mpy_32_16_1( tmp1 /** 0.555570f*/, imult1616( sign_fx( specPOr[i] ), s[60 * mult_angle] ) ); /*cos(PI*5/16) Q31*/
587 127649 : move32();
588 127649 : specPOi[i] = Mpy_32_16_1( tmp1 /** 0.831470f*/, imult1616( sign_fx( specPOi[i] ), s[100 * mult_angle] ) ); // Q31
589 127649 : move32();
590 : }
591 : ELSE
592 : {
593 124614 : specPOr[i] = Mpy_32_16_1( tmp1 /** 0.195090f*/, imult1616( sign_fx( specPOr[i] ), s[20 * mult_angle] ) ); /*cos(PI*7/16) Q31*/
594 124614 : move32();
595 124614 : specPOi[i] = Mpy_32_16_1( tmp1 /** 0.980785f*/, imult1616( sign_fx( specPOi[i] ), s[140 * mult_angle] ) ); // Q31
596 124614 : move32();
597 : }
598 : }
599 567000 : gamma = L_sub( gamma, igamma );
600 : }
601 :
602 2100 : IF( LT_16( i, n0 ) )
603 : {
604 1050 : gamma = L_sub( gamma, imult3216( igamma, sub( n0, 320 ) ) );
605 : }
606 170100 : FOR( ; i < n0; i++ ) /*neglect higher frequency bins when 48 kHz samplng*/
607 : {
608 168000 : specPOr[i] = 0;
609 168000 : move32();
610 168000 : specPOi[i] = 0;
611 168000 : move32();
612 : }
613 2100 : specPOr[n0] = imult3216( Mpy_32_32_r( wnd[i * step + bias], gamma ), sign_fx( Mpy_32_32_r( specLr[n0], specRr[n0] ) ) ); // Q31
614 2100 : move32();
615 :
616 2100 : freq_8k = L_FRAME16k / 2;
617 2100 : move16();
618 : // freq_ipd_max = (int16_t) ( freq_8k * 5000.0f / ( 8000.0f * STEREO_DMX_EVS_SUBBAND_SIZE ) );
619 2100 : freq_ipd_max = 50;
620 2100 : move16();
621 :
622 : /* Memorize the filters N-1 */
623 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
624 : {
625 4200 : IF( hPHA->p_curr_taps_fx[n] )
626 : {
627 4194 : hPHA->p_prev_taps_fx[n] = hPHA->prev_taps_fx[n];
628 4194 : Copy32( hPHA->p_curr_taps_fx[n], hPHA->p_prev_taps_fx[n], hPHA->pha_len );
629 : }
630 : ELSE
631 : {
632 6 : hPHA->p_prev_taps_fx[n] = NULL;
633 : }
634 : }
635 :
636 : /* ISD */
637 2100 : isd_cnt_l = 0;
638 2100 : move16();
639 2100 : isd_cnt_h = 0;
640 2100 : move16();
641 338100 : FOR( i = 1; i <= freq_8k; i++ )
642 : {
643 336000 : Nr = L_sub( specLr[i], specRr[i] ); // spec_e
644 336000 : Ni = L_sub( specLi[i], specRi[i] ); // spec_e
645 336000 : Dr = L_add( specLr[i], specRr[i] ); // spec_e
646 336000 : Di = L_add( specLi[i], specRi[i] ); // spec_e
647 : // if ( ( Nr * Nr + Ni * Ni ) > STEREO_DMX_EVS_ISD_THRES_H * ( Dr * Dr + Di * Di ) )
648 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 ) ) ) )
649 : {
650 102859 : isd_cnt_h = add( isd_cnt_h, 1 );
651 : }
652 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
653 336000 : IF( LT_64( W_add( W_mult0_32_32( Nr, Nr ), W_mult0_32_32( Ni, Ni ) ), W_tmp ) ) // Q62
654 : {
655 179725 : isd_cnt_l = add( isd_cnt_l, 1 );
656 : }
657 : }
658 :
659 2100 : isd_rate = BASOP_Util_Divide1616_Scale( isd_cnt_h, freq_8k, &isd_rate_e );
660 : // Saturation to handle values close to 1.0f
661 2100 : isd_rate = shl_sat( isd_rate, isd_rate_e ); // Q15
662 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 ) );
663 2100 : move32();
664 :
665 2100 : IF( GT_32( hPHA->isd_rate_s_fx, STEREO_DMX_EVS_ISD_DIST_HYST_H_Q31 ) )
666 : {
667 0 : IF( NE_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD ) )
668 : {
669 0 : IF( EQ_32( hPHA->prev_pha, STEREO_DMX_EVS_PHA_IPD ) )
670 : {
671 0 : hPHA->pha_hys_cnt = add( hPHA->pha_hys_cnt, 1 );
672 0 : move16();
673 : }
674 : ELSE
675 : {
676 0 : hPHA->pha_hys_cnt = 0;
677 0 : move16();
678 : }
679 0 : IF( GE_32( hPHA->pha_hys_cnt, STEREO_DMX_EVS_SWTCH_HYS_THRES ) )
680 : {
681 0 : hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD;
682 0 : move32();
683 : }
684 : }
685 :
686 0 : hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD;
687 0 : move32();
688 : }
689 2100 : ELSE IF( LT_32( hPHA->isd_rate_s_fx, STEREO_DMX_EVS_ISD_DIST_HYST_L_Q31 ) )
690 : {
691 1398 : IF( NE_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD2 ) )
692 : {
693 4 : IF( EQ_32( hPHA->prev_pha, STEREO_DMX_EVS_PHA_IPD2 ) )
694 : {
695 2 : hPHA->pha_hys_cnt = add( hPHA->pha_hys_cnt, 1 );
696 2 : move16();
697 : }
698 : ELSE
699 : {
700 2 : hPHA->pha_hys_cnt = 0;
701 2 : move16();
702 : }
703 4 : IF( GE_16( hPHA->pha_hys_cnt, STEREO_DMX_EVS_SWTCH_HYS_THRES ) )
704 : {
705 2 : hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD2;
706 2 : move32();
707 : }
708 : }
709 1398 : hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD2;
710 1398 : move32();
711 : }
712 :
713 2100 : ipd_ff = hPHA->ipd_ff_fx;
714 :
715 2100 : Nr = 0;
716 2100 : move32();
717 2100 : Nr_e = 0;
718 2100 : move16();
719 2100 : Ni = 0;
720 2100 : move32();
721 2100 : Ni_e = 0;
722 2100 : move16();
723 2100 : eneL = 0;
724 2100 : move32();
725 2100 : eneL_e = 0;
726 2100 : move16();
727 2100 : eneR = 0;
728 2100 : move32();
729 2100 : eneR_e = 0;
730 2100 : move16();
731 :
732 420000 : FOR( ( n = 1, i = 1 ); n < nsbd; n++ )
733 : {
734 417900 : tPr = 0;
735 417900 : move32();
736 417900 : tPr_e = 0;
737 417900 : move16();
738 417900 : tPi = 0;
739 417900 : move32();
740 417900 : tPi_e = 0;
741 417900 : move16();
742 417900 : tEr[n] = 0;
743 417900 : move32();
744 417900 : tEr_e[n] = 0;
745 417900 : move16();
746 417900 : tEl[n] = 0;
747 417900 : move32();
748 417900 : tEl_e[n] = 0;
749 417900 : move16();
750 :
751 1253700 : FOR( j = 0; j < STEREO_DMX_EVS_SUBBAND_SIZE; ( j++, i++ ) )
752 : {
753 :
754 : /* Energy */
755 :
756 :
757 : // Left
758 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))
759 835800 : L_tmp_e = W_norm( W_tmp );
760 835800 : IF( L_tmp_e != 0 )
761 : {
762 835800 : W_tmp = W_shl( W_tmp, L_tmp_e ); // Q(63 - ((2*spec_e) - (L_tmp_e - 1))
763 : }
764 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] );
765 835800 : move32();
766 :
767 : // Right
768 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))
769 835800 : L_tmp_e = W_norm( W_tmp );
770 835800 : IF( L_tmp_e != 0 )
771 : {
772 835800 : W_tmp = W_shl( W_tmp, L_tmp_e ); // Q(63 - ((2*spec_e) - (L_tmp_e - 1))
773 : }
774 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] );
775 835800 : move32();
776 :
777 : /* IPD */
778 : // IPDr = L_add(Mpy_32_32_r(specLr[i], specRr[i]), Mpy_32_32_r(specLi[i], specRi[i])); //2*spec_e
779 835800 : W_tmp = W_add( W_mult_32_32( specLr[i], specRr[i] ), W_mult_32_32( specLi[i], specRi[i] ) );
780 835800 : W_tmp_q = W_norm( W_tmp );
781 835800 : W_tmp = W_shl( W_tmp, W_tmp_q );
782 835800 : IPDr = W_extract_h( W_tmp );
783 835800 : IPDr_e = sub( shl( spec_e, 1 ), W_tmp_q );
784 : // IPDi = L_sub(Mpy_32_32_r(specLi[i] , specRr[i]), Mpy_32_32_r(specLr[i] , specRi[i])); //2*spec_e
785 835800 : W_tmp = W_sub( W_mult_32_32( specLi[i], specRr[i] ), W_mult_32_32( specLr[i], specRi[i] ) );
786 835800 : W_tmp_q = W_norm( W_tmp );
787 835800 : W_tmp = W_shl( W_tmp, W_tmp_q );
788 835800 : IPDi = W_extract_h( W_tmp );
789 835800 : IPDi_e = sub( shl( spec_e, 1 ), W_tmp_q );
790 835800 : tPr = BASOP_Util_Add_Mant32Exp( tPr, tPr_e, IPDr, IPDr_e, &tPr_e );
791 835800 : tPi = BASOP_Util_Add_Mant32Exp( tPi, tPi_e, IPDi, IPDi_e, &tPi_e );
792 :
793 : /* ICCr */
794 : // Pn = (float) inv_sqrt( ( IPDr * IPDr + IPDi * IPDi ) + EPSILON );
795 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 );
796 835800 : L_tmp = BASOP_Util_Add_Mant32Exp( L_tmp, L_tmp_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
797 835800 : Pn_e = L_tmp_e;
798 835800 : move16();
799 835800 : Pn = ISqrt32( L_tmp, &Pn_e );
800 835800 : IPDr = L_shl_sat( Mpy_32_32_r( IPDr, Pn ), add( IPDr_e, Pn_e ) ); // Q31
801 835800 : IPDi = L_shl_sat( Mpy_32_32_r( IPDi, Pn ), add( IPDi_e, Pn_e ) ); // Q31
802 :
803 835800 : tIPDr = L_sub( Mpy_32_32_r( specRr[i], IPDr ), Mpy_32_32_r( specRi[i], IPDi ) ); // spec_e
804 835800 : tIPDi = L_add( Mpy_32_32_r( specRr[i], IPDi ), Mpy_32_32_r( specRi[i], IPDr ) ); // spec_e
805 :
806 :
807 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 );
808 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 );
809 :
810 : // 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 );
811 835800 : W_tmp = W_add( W_mult0_32_32( specLr[i], specLr[i] ), W_mult0_32_32( specLi[i], specLi[i] ) );
812 835800 : L_tmp_e = W_norm( W_tmp );
813 835800 : IF( L_tmp_e != 0 )
814 : {
815 835800 : W_tmp = W_shl( W_tmp, L_tmp_e );
816 : }
817 835800 : eneL = BASOP_Util_Add_Mant32Exp( eneL, eneL_e, W_round64_L( W_tmp ), sub( 1, L_tmp_e ), &eneL_e );
818 :
819 : // 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 );
820 835800 : W_tmp = W_add( W_mult0_32_32( specRr[i], specRr[i] ), W_mult0_32_32( specRi[i], specRi[i] ) );
821 835800 : L_tmp_e = W_norm( W_tmp );
822 835800 : IF( L_tmp_e != 0 )
823 : {
824 835800 : W_tmp = W_shl( W_tmp, L_tmp_e );
825 : }
826 835800 : eneR = BASOP_Util_Add_Mant32Exp( eneR, eneR_e, W_round64_L( W_tmp ), sub( 1, L_tmp_e ), &eneR_e );
827 : }
828 :
829 : // Pn = (float) inv_sqrt( ( tPr * tPr + tPi * tPi ) + EPSILON );
830 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 );
831 417900 : L_tmp = BASOP_Util_Add_Mant32Exp( L_tmp, L_tmp_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
832 417900 : Pn_e = L_tmp_e;
833 417900 : move16();
834 417900 : Pn = ISqrt32( L_tmp, &Pn_e );
835 417900 : tPr = L_shl_sat( Mpy_32_32_r( tPr, Pn ), add( tPr_e, Pn_e ) ); // Q31
836 417900 : tPi = L_shl_sat( Mpy_32_32_r( tPi, Pn ), add( tPi_e, Pn_e ) ); // Q31
837 :
838 417900 : 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 ) );
839 417900 : move32();
840 417900 : 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 ) );
841 417900 : move32();
842 :
843 : // Pn = (float) inv_sqrt( ( Pr[n] * Pr[n] + Pi[n] * Pi[n] ) + EPSILON );
844 417900 : 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 ) );
845 417900 : Pn = BASOP_Util_Add_Mant32Exp( Pn, 1, EPSILON_FX_M, EPSILON_FX_E, &Pn_e );
846 417900 : Pn = Isqrt_lc( Pn, &Pn_e );
847 :
848 417900 : Pr[n] = L_shl_sat( Mpy_32_32_r( Pr[n], Pn ), Pn_e ); // Q31
849 417900 : move32();
850 417900 : Pi[n] = L_shl_sat( Mpy_32_32_r( Pi[n], Pn ), Pn_e ); // Q31
851 417900 : move32();
852 : // Pr[n] = ( Pr[n] > 1.0f ) ? 1.0f : Pr[n];
853 : // Pr[n] = ( Pr[n] < -1.0f ) ? -1.0f : Pr[n];
854 : }
855 :
856 : /* Computes Spectral flatness on one channel */
857 2100 : tmp1 = spectral_flatness_fx( &tEl[1], &tEl_e[1], nsbd - 1 );
858 2100 : IF( LT_32( tmp1, STEREO_DMX_EVS_IPD_SF_THRES_Q31 ) )
859 : {
860 1831 : hPHA->pha_ipd_chanswitch_allowed = 0;
861 1831 : move16();
862 : }
863 : ELSE
864 : {
865 269 : hPHA->pha_ipd_chanswitch_allowed = 1;
866 269 : move16();
867 : }
868 :
869 : // ICCr = (float) sqrt( ( Nr * Nr + Ni * Ni ) / ( eneL * eneR + EPSILON ) );
870 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 );
871 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 );
872 2100 : ICCr = BASOP_Util_Divide3232_Scale_newton( L_tmp1, L_tmp2, &L_tmp_e );
873 2100 : L_tmp_e = add( L_tmp_e, sub( L_tmp1_e, L_tmp2_e ) );
874 2100 : ICCr = Sqrt32( ICCr, &L_tmp_e );
875 : // Saturation to handle values close to 1.0f
876 2100 : ICCr = L_shl_sat( ICCr, L_tmp_e ); // Q31
877 :
878 : // hPHA->iccr_s = STEREO_DMX_EVS_ICCR_FORGETTING * hPHA->iccr_s + ( 1.0f - STEREO_DMX_EVS_ICCR_FORGETTING ) * ICCr;
879 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
880 2100 : move32();
881 :
882 :
883 2100 : IF( EQ_32( hPHA->curr_pha, STEREO_DMX_EVS_PHA_IPD ) )
884 : {
885 2 : hPHA->force_poc = FALSE;
886 2 : hPHA->proc_pha = STEREO_DMX_EVS_PHA_IPD;
887 2 : move16();
888 2 : move32();
889 : }
890 : ELSE
891 : {
892 2098 : test();
893 2098 : test();
894 2098 : test();
895 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 ) && EQ_32( hPHA->proc_pha, STEREO_DMX_EVS_PHA_IPD2 ) && EQ_16( hPHA->force_poc, FALSE ) ) )
896 : {
897 426 : hPHA->force_poc = FALSE;
898 426 : hPHA->proc_pha = STEREO_DMX_EVS_PHA_IPD2;
899 426 : move16();
900 426 : move32();
901 : }
902 : ELSE
903 : {
904 1672 : hPHA->force_poc = TRUE;
905 1672 : move16();
906 : }
907 : }
908 :
909 2100 : IF( hPHA->proc_pha == STEREO_DMX_EVS_PHA_IPD )
910 : {
911 2 : rfft_pha_buf[0] = ONE_IN_Q22;
912 2 : move32();
913 2 : rfft_pha_buf[1] = ONE_IN_Q22;
914 2 : move32();
915 :
916 2 : ild_cnt = 0;
917 2 : move16();
918 400 : FOR( i = 1; i < nsbd; i++ )
919 : {
920 398 : rfft_pha_buf[i * 2] = L_shr_r( Pr[i], 9 ); // Q31->Q22
921 398 : move32();
922 398 : rfft_pha_buf[i * 2 + 1] = L_shr_r( Pi[i], 9 ); // Q31->Q22
923 398 : move32();
924 : // if ( ( tEr[i] > STEREO_DMX_EVS_LR_EGY * tEl[i] ) || ( tEl[i] > STEREO_DMX_EVS_LR_EGY * tEr[i] ) )
925 398 : test();
926 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 )
927 : {
928 10 : ild_cnt = add( ild_cnt, 1 );
929 10 : tEr[i] = MAX_32;
930 10 : move32();
931 10 : tEr_e[i] = 0;
932 10 : move16();
933 : }
934 : ELSE
935 : {
936 388 : tEr[i] = MIN_32;
937 388 : move32();
938 388 : tEr_e[i] = 0;
939 388 : move16();
940 : }
941 : }
942 2 : IF( GT_16( ild_cnt, mult_r( nsbd, STEREO_DMX_EVS_ILD_PRC_Q15 ) ) )
943 : {
944 0 : FOR( i = 1; i < nsbd; i++ )
945 : {
946 0 : IF( tEr[i] > 0 )
947 : {
948 0 : rfft_pha_buf[i * 2] = ONE_IN_Q22;
949 0 : move32();
950 0 : rfft_pha_buf[i * 2 + 1] = 0;
951 0 : move32();
952 : }
953 : }
954 : }
955 :
956 2 : rfft_fx( rfft_pha_buf, hPHA->rfft_ipd_coef_fx, input_frame_pha, +1 );
957 :
958 : /* Choose best channel to phase align */
959 : /* Channel selection based on ILD */
960 2 : IF( BASOP_Util_Cmp_Mant32Exp( hPHA->trns_aux_energy_fx[0], hPHA->trns_aux_energy_fx_e[0], Mpy_32_32_r( STEREO_DMX_EVS_IPD_ILD_THRES_Q29, hPHA->trns_aux_energy_fx[1] ), add( 2, hPHA->trns_aux_energy_fx_e[1] ) ) > 0 )
961 : {
962 0 : pha_ipd_ild_chan2rephase = 1;
963 : }
964 2 : ELSE IF( BASOP_Util_Cmp_Mant32Exp( hPHA->trns_aux_energy_fx[1], hPHA->trns_aux_energy_fx_e[1], Mpy_32_32_r( STEREO_DMX_EVS_IPD_ILD_THRES_Q29, hPHA->trns_aux_energy_fx[0] ), add( 2, hPHA->trns_aux_energy_fx_e[0] ) ) > 0 )
965 : {
966 0 : pha_ipd_ild_chan2rephase = 0;
967 : }
968 : ELSE
969 : {
970 2 : pha_ipd_ild_chan2rephase = -1;
971 : }
972 2 : move16();
973 :
974 : /* Channel selection based on spikyness of R2L/L2R impulse responses */
975 2 : tmp1 = spectral_flatness_fx( rfft_pha_buf, NULL, hPHA->pha_len );
976 2 : rfft_pha_buf[sub( input_frame_pha, hPHA->pha_len )] = rfft_pha_buf[0];
977 2 : move32();
978 2 : tmp2 = spectral_flatness_fx( &rfft_pha_buf[sub( input_frame_pha, hPHA->pha_len )], NULL, hPHA->pha_len );
979 :
980 : // /* Combined ILD/SF channel selection with tempo */
981 2 : IF( ( GT_32( tmp1, tmp2 ) && ( EQ_16( pha_ipd_ild_chan2rephase, -1 ) ) ) || EQ_16( pha_ipd_ild_chan2rephase, 0 ) ) /* L => R */
982 : {
983 0 : IF( EQ_16( hPHA->pha_ipd_previouschan2rephase, 0 ) )
984 : {
985 0 : hPHA->pha_ipd_chan_cnt = add( hPHA->pha_ipd_chan_cnt, 1 );
986 0 : IF( GE_32( hPHA->pha_ipd_chan_cnt, hPHA->pha_ipd_chan_thresh ) )
987 : {
988 : /* Avoid channel switch in case of too harmonic signals */
989 0 : IF( EQ_16( hPHA->pha_ipd_chanswitch_allowed, 1 ) )
990 : {
991 0 : IF( NE_16( hPHA->pha_ipd_chan2rephase, 0 ) )
992 : {
993 0 : hPHA->pha_ipd_chanswitch = 1;
994 0 : move16();
995 : }
996 : ELSE
997 : {
998 0 : hPHA->pha_ipd_chanswitch = 0;
999 0 : move16();
1000 : }
1001 0 : hPHA->pha_ipd_chan2rephase = 0;
1002 0 : move16();
1003 : }
1004 : }
1005 : }
1006 : ELSE
1007 : {
1008 0 : hPHA->pha_ipd_previouschan2rephase = 0;
1009 0 : hPHA->pha_ipd_chan_cnt = 1;
1010 0 : hPHA->pha_ipd_chanswitch = 0;
1011 0 : move16();
1012 0 : move16();
1013 0 : move16();
1014 : }
1015 : }
1016 : ELSE
1017 : {
1018 2 : IF( EQ_16( hPHA->pha_ipd_previouschan2rephase, 1 ) )
1019 : {
1020 2 : hPHA->pha_ipd_chan_cnt = add( hPHA->pha_ipd_chan_cnt, 1 );
1021 :
1022 2 : IF( GE_16( hPHA->pha_ipd_chan_cnt, hPHA->pha_ipd_chan_thresh ) )
1023 : {
1024 : /* Avoid channel switch in case of too harmonic signals */
1025 0 : IF( EQ_16( hPHA->pha_ipd_chanswitch_allowed, 1 ) )
1026 : {
1027 0 : IF( NE_16( hPHA->pha_ipd_chan2rephase, 1 ) )
1028 : {
1029 0 : hPHA->pha_ipd_chanswitch = 1;
1030 0 : move16();
1031 : }
1032 : ELSE
1033 : {
1034 0 : hPHA->pha_ipd_chanswitch = 0;
1035 0 : move16();
1036 : }
1037 0 : hPHA->pha_ipd_chan2rephase = 1;
1038 0 : move16();
1039 : }
1040 0 : hPHA->pha_ipd_chan_cnt = hPHA->pha_ipd_chan_thresh;
1041 0 : move16();
1042 : }
1043 : }
1044 : ELSE
1045 : {
1046 0 : hPHA->pha_ipd_previouschan2rephase = 1;
1047 0 : hPHA->pha_ipd_chan_cnt = 1;
1048 0 : hPHA->pha_ipd_chanswitch = 0;
1049 0 : move16();
1050 0 : move16();
1051 0 : move16();
1052 : }
1053 : }
1054 :
1055 2 : IF( EQ_16( hPHA->pha_ipd_chanswitch, 0 ) )
1056 : {
1057 2 : IF( EQ_16( hPHA->pha_ipd_chan2rephase, 0 ) )
1058 : {
1059 0 : hPHA->p_curr_taps_fx[1] = NULL;
1060 0 : hPHA->p_curr_taps_fx[0] = hPHA->curr_taps_fx[0];
1061 0 : p_curr_taps = hPHA->p_curr_taps_fx[0];
1062 :
1063 0 : L_tmp = L_shl( 1, 8 );
1064 0 : p_curr_taps[0] = W_extract_l( W_mult_32_32( L_tmp, rfft_pha_buf[0] ) ); // Q22 -> Q31
1065 0 : j = sub( input_frame_pha, 1 );
1066 0 : FOR( i = 1; i < hPHA->pha_len; i++ )
1067 : {
1068 0 : p_curr_taps[i] = W_extract_l( W_mult_32_32( L_tmp, rfft_pha_buf[j] ) ); // Q22 -> Q31
1069 0 : j = sub( j, 1 );
1070 : }
1071 : }
1072 : ELSE
1073 : {
1074 2 : hPHA->p_curr_taps_fx[0] = NULL;
1075 2 : hPHA->p_curr_taps_fx[1] = hPHA->curr_taps_fx[1];
1076 2 : Copy_Scale_sig32( rfft_pha_buf, hPHA->p_curr_taps_fx[1], hPHA->pha_len, 9 ); // Q22->Q31
1077 : }
1078 : }
1079 : }
1080 :
1081 2100 : IF( EQ_16( hPHA->pha_ipd_chanswitch, 1 ) )
1082 : {
1083 0 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1084 : {
1085 0 : hPHA->p_curr_taps_fx[n] = NULL;
1086 : }
1087 : }
1088 2100 : ELSE IF( EQ_32( hPHA->proc_pha, STEREO_DMX_EVS_PHA_IPD2 ) )
1089 : {
1090 : /* IPDn */
1091 :
1092 2098 : set32_fx( &( Pr[freq_ipd_max] ), MAX_32, sub( nsbd, freq_ipd_max ) );
1093 2098 : set32_fx( &( Pi[freq_ipd_max] ), 0, sub( nsbd, freq_ipd_max ) );
1094 :
1095 6294 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1096 : {
1097 4196 : hPHA->p_curr_taps_fx[n] = hPHA->curr_taps_fx[n];
1098 : }
1099 :
1100 2098 : rfft_pha_buf[0] = ONE_IN_Q22;
1101 2098 : move32();
1102 2098 : rfft_pha_buf[1] = ONE_IN_Q22;
1103 2098 : move32();
1104 :
1105 2098 : ild_cnt = 0;
1106 2098 : move16();
1107 2098 : isd_rate = BASOP_Util_Divide1616_Scale( isd_cnt_l, freq_8k, &isd_rate_e );
1108 : // Saturation to handle values close to 1.0f
1109 2098 : isd_rate = shl_sat( isd_rate, isd_rate_e ); // Q15
1110 419600 : FOR( i = 1; i < nsbd; i++ )
1111 : {
1112 : // rfft_pha_buf[i * 2] = (float) sqrt( ( 1.0f + Pr[i] ) / 2.0f );
1113 417502 : L_tmp = L_add( ONE_IN_Q30, L_shr( Pr[i], 1 ) );
1114 417502 : L_tmp_e = 0;
1115 417502 : move16();
1116 417502 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
1117 417502 : rfft_pha_buf[i * 2] = L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ); // Q22
1118 417502 : move32();
1119 : // rfft_pha_buf[i * 2 + 1] = (float) sqrt( ( 1.0f - Pr[i] ) / 2.0f ) * sign( Pi[i] );
1120 417502 : L_tmp = L_sub_sat( ONE_IN_Q30, L_shr( Pr[i], 1 ) ); // saturating as Pr does not exceed 1.0f
1121 417502 : L_tmp_e = 0;
1122 417502 : move16();
1123 417502 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
1124 417502 : rfft_pha_buf[i * 2 + 1] = imult3216( L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ), sign_fx( Pi[i] ) ); // Q22
1125 417502 : move32();
1126 417502 : IF( GT_16( isd_rate, STEREO_DMX_EVS_ISD_DIST_THRES_IPD_Q15 ) )
1127 : {
1128 : // rfft_pha_buf[i * 2 + 1] = (float) sqrt( ( 1.0f - rfft_pha_buf[i * 2] ) / 2.0f ) * sign( rfft_pha_buf[i * 2 + 1] );
1129 188654 : L_tmp = L_sub( ONE_IN_Q21, L_shr( rfft_pha_buf[i * 2], 1 ) );
1130 188654 : L_tmp_e = 9;
1131 188654 : move16();
1132 188654 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
1133 188654 : 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
1134 188654 : move32();
1135 : // rfft_pha_buf[i * 2] = (float) sqrt( ( 1.0f + rfft_pha_buf[i * 2] ) / 2.0f );
1136 188654 : L_tmp = L_add( ONE_IN_Q21, L_shr( rfft_pha_buf[i * 2], 1 ) ); // Q22
1137 188654 : L_tmp_e = 9;
1138 188654 : move16();
1139 188654 : L_tmp = Sqrt32( L_tmp, &L_tmp_e );
1140 188654 : rfft_pha_buf[i * 2] = L_shl_r( L_tmp, sub( L_tmp_e, 9 ) ); // Q22
1141 188654 : move32();
1142 : }
1143 :
1144 : // if ( ( tEr[i] > STEREO_DMX_EVS_LR_EGY * tEl[i] ) || ( tEl[i] > STEREO_DMX_EVS_LR_EGY * tEr[i] ) )
1145 417502 : test();
1146 417502 : 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 )
1147 : {
1148 40393 : ild_cnt = add( ild_cnt, 1 );
1149 40393 : tEr[i] = MAX_32;
1150 40393 : move32();
1151 40393 : tEr_e[i] = 0;
1152 40393 : move16();
1153 : }
1154 : ELSE
1155 : {
1156 377109 : tEr[i] = MIN_32;
1157 377109 : move32();
1158 377109 : tEr_e[i] = 0;
1159 377109 : move16();
1160 : }
1161 : }
1162 2098 : IF( GT_16( ild_cnt, mult_r( nsbd, STEREO_DMX_EVS_ILD_PRC_Q15 ) ) )
1163 : {
1164 97120 : FOR( i = 1; i < nsbd; i++ )
1165 : {
1166 96624 : IF( tEr[i] > 0 )
1167 : {
1168 27776 : rfft_pha_buf[i * 2] = ONE_IN_Q22;
1169 27776 : move32();
1170 27776 : rfft_pha_buf[i * 2 + 1] = 0;
1171 27776 : move32();
1172 : }
1173 : }
1174 : }
1175 :
1176 2098 : rfft_fx( rfft_pha_buf, hPHA->rfft_ipd_coef_fx, input_frame_pha, +1 );
1177 : // mvr2r( rfft_pha_buf, hPHA->p_curr_taps[1], hPHA->pha_len );
1178 2098 : Copy_Scale_sig32( rfft_pha_buf, hPHA->p_curr_taps_fx[1], hPHA->pha_len, 9 ); // Q22->Q31
1179 :
1180 : /* PHA L2R */
1181 2098 : p_curr_taps = hPHA->p_curr_taps_fx[0];
1182 2098 : p_curr_taps[0] = L_shl( rfft_pha_buf[0], 9 ); // Q22->Q31
1183 2098 : move32();
1184 100704 : FOR( i = 1; i < hPHA->pha_len; i++ )
1185 : {
1186 98606 : p_curr_taps[i] = L_shl( rfft_pha_buf[input_frame_pha - i], 9 ); // Q22->Q31
1187 98606 : move32();
1188 : }
1189 : }
1190 :
1191 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1192 : {
1193 4200 : IF( hPHA->p_curr_taps_fx[n] )
1194 : {
1195 205702 : FOR( i = 0; i < hPHA->pha_len; i++ )
1196 : {
1197 : // hPHA->p_curr_taps[n][i] *= hPHA->win[i];
1198 201504 : hPHA->p_curr_taps_fx[n][i] = Mpy_32_16_1( hPHA->p_curr_taps_fx[n][i], hPHA->win_fx[i] ); // Q30
1199 201504 : move32();
1200 : }
1201 :
1202 4198 : energy = 0;
1203 4198 : move32();
1204 4198 : energy_e = 0;
1205 4198 : move16();
1206 205702 : FOR( i = 0; i < hPHA->pha_len; i++ )
1207 : {
1208 201504 : 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 );
1209 : }
1210 : // energy = (float) inv_sqrt( energy + EPSILON );
1211 4198 : energy = BASOP_Util_Add_Mant32Exp( energy, energy_e, EPSILON_FX_M, EPSILON_FX_E, &energy_e );
1212 4198 : energy = ISqrt32( energy, &energy_e );
1213 205702 : FOR( i = 0; i < hPHA->pha_len; i++ )
1214 : {
1215 201504 : 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
1216 201504 : move32();
1217 : }
1218 : }
1219 : }
1220 :
1221 2100 : rfft_buf[0] = L_shr_r( specPOr[0], 10 ); // Q31->Q21
1222 2100 : move32();
1223 2100 : rfft_buf[1] = L_shr_r( specPOr[n0], 10 ); // Q31->Q21
1224 2100 : move32();
1225 840000 : FOR( i = 1; i < n0; i++ )
1226 : {
1227 837900 : rfft_buf[i * 2] = L_shr_r( specPOr[i], 10 ); // Q31->Q21
1228 837900 : move32();
1229 837900 : rfft_buf[i * 2 + 1] = L_shr_r( specPOi[i], 10 ); // Q31->Q21
1230 837900 : move32();
1231 : }
1232 :
1233 2100 : rfft_fx( rfft_buf, rfft_coef, input_frame, +1 );
1234 2100 : scale_sig32( rfft_buf, input_frame, 10 ); // Q21->Q31
1235 :
1236 2100 : tmp1 = rfft_buf[0];
1237 2100 : move32();
1238 2100 : tmpPOC1[n0] = Mpy_32_32_r( tmp1, tmp1 );
1239 2100 : move32();
1240 :
1241 474600 : FOR( i = 1; i < hPOC->shift_limit + 1; i++ )
1242 : {
1243 472500 : n1 = add( n0, i );
1244 472500 : n2 = sub( n0, i );
1245 :
1246 472500 : tmp1 = rfft_buf[i];
1247 472500 : move32();
1248 472500 : tmpPOC1[n1] = Mpy_32_32_r( tmp1, tmp1 );
1249 472500 : move32();
1250 :
1251 472500 : tmp1 = rfft_buf[input_frame - i];
1252 472500 : move32();
1253 472500 : tmpPOC1[n2] = Mpy_32_32_r( tmp1, tmp1 );
1254 472500 : move32();
1255 : }
1256 :
1257 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
1258 2100 : tmpPOC2[n0] = L_max( tmp1, 0 );
1259 2100 : move32();
1260 :
1261 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ )
1262 : {
1263 470400 : n1 = add( n0, i );
1264 470400 : n2 = sub( n0, i );
1265 : // tmp1 = STEREO_DMX_EVS_POC_SMOOTH * tmpPOC1[n1] + 0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH ) * ( tmpPOC1[n1 - 1] + tmpPOC1[n1 + 1] );
1266 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
1267 : // tmp2 = STEREO_DMX_EVS_POC_SMOOTH * tmpPOC1[n2] + 0.5f * ( 1.0f - STEREO_DMX_EVS_POC_SMOOTH ) * ( tmpPOC1[n2 - 1] + tmpPOC1[n2 + 1] );
1268 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
1269 470400 : tmpPOC2[n1] = L_max( tmp1, 0 );
1270 470400 : move32();
1271 470400 : tmpPOC2[n2] = L_max( tmp2, 0 );
1272 470400 : move32();
1273 : }
1274 :
1275 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 ) );
1276 2100 : move32();
1277 :
1278 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ )
1279 : {
1280 470400 : n1 = add( n0, i );
1281 470400 : n2 = sub( n0, i );
1282 :
1283 470400 : IF( EQ_16( i, negate( itdLR[1] ) ) )
1284 : {
1285 718 : 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 ) );
1286 718 : move32();
1287 : }
1288 : ELSE
1289 : {
1290 469682 : 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 ) );
1291 469682 : move32();
1292 : }
1293 :
1294 470400 : IF( EQ_16( i, itdLR[0] ) )
1295 : {
1296 649 : P[n2] = L_add( Mpy_32_32_r( P[n2], STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n2], MAX_32 - STEREO_DMX_EVS_TARGET_POC_FORGETTING_Q31 ) );
1297 649 : move32();
1298 : }
1299 : ELSE
1300 : {
1301 469751 : P[n2] = L_add( Mpy_32_32_r( P[n2], STEREO_DMX_EVS_POC_FORGETTING_Q31 ), Mpy_32_32_r( tmpPOC2[n2], MAX_32 - STEREO_DMX_EVS_POC_FORGETTING_Q31 ) );
1302 469751 : move32();
1303 : }
1304 : }
1305 :
1306 2100 : return;
1307 : }
1308 :
1309 :
1310 : /*-------------------------------------------------------------------*
1311 : * find_poc_peak()
1312 : *
1313 : * find peak phase only correlation
1314 : *-------------------------------------------------------------------*/
1315 2100 : static Word32 find_poc_peak_fx(
1316 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
1317 : Word16 itd_fx[], /* o : estimated itd Q0 */
1318 : const Word16 input_frame, /* i : input frame length per channel */
1319 : const Word32 ratio_fixed /* i : adapting ratio Q31 */
1320 : )
1321 : {
1322 : Word16 itd_cand[CPE_CHANNELS], i, n, cnt[CPE_CHANNELS], Lh, peak_range, *on, *itdLR, prev_off[CPE_CHANNELS], eps_fx;
1323 : 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;
1324 2100 : Word16 tmpf_e = 0, eps2_e = 0, Q_e[CPE_CHANNELS], eps_e = 15, peakQ_e[CPE_CHANNELS];
1325 :
1326 2100 : move16();
1327 2100 : move16();
1328 2100 : move16();
1329 :
1330 : /* Initialization */
1331 2100 : Lh = shr( input_frame, 1 );
1332 2100 : on = hPOC->ispeak;
1333 2100 : itdLR = hPOC->itdLR;
1334 2100 : itd_cand[0] = itd_cand[1] = 0;
1335 2100 : move16();
1336 2100 : move16();
1337 :
1338 2100 : width_fx = 816043776; /*0.38f in Q31*/
1339 2100 : eps_fx = Inv16( input_frame, &eps_e );
1340 2100 : peak_width_fx = hPOC->peak_width_fx;
1341 2100 : peakQ_fx = hPOC->peakQ_fx; // Q16
1342 2100 : move16();
1343 :
1344 2100 : set16_fx( peakQ_e, 31 - Q16, CPE_CHANNELS );
1345 2100 : Q_fx[0] = hPOC->P_fx[Lh]; // Q31
1346 2100 : Q_fx[1] = 0;
1347 2100 : move16();
1348 2100 : move16();
1349 2100 : set16_fx( Q_e, 0, CPE_CHANNELS ); // Q31
1350 :
1351 2100 : P_fx = hPOC->P_fx;
1352 :
1353 472500 : FOR( i = 1; i < hPOC->shift_limit; i++ ) /*find peaks of POC P[] with positive and negative ITD */
1354 : {
1355 470400 : IF( GT_32( P_fx[Lh - i], Q_fx[0] ) )
1356 : {
1357 8021 : Q_fx[0] = P_fx[Lh - i]; // Q31
1358 8021 : itd_cand[0] = i;
1359 8021 : Q_e[0] = 0;
1360 8021 : move32();
1361 8021 : move16();
1362 8021 : move16();
1363 : }
1364 470400 : IF( GT_32( P_fx[Lh + i], Q_fx[1] ) )
1365 : {
1366 9990 : Q_fx[1] = P_fx[Lh + i]; // Q31
1367 9990 : itd_cand[1] = -i;
1368 9990 : Q_e[1] = 0;
1369 9990 : move32();
1370 9990 : move16();
1371 9990 : move16();
1372 : }
1373 : }
1374 :
1375 2100 : Word16 tmp1, tmp11, tmp_var1, tmp12 = 0, tmp13;
1376 2100 : move16();
1377 : Word32 tmp4, tmp5, tmp6, tmp7, tmp8, tmp9, tmp10, tmp14, tmp15, tmp16;
1378 : Word16 tmp8_e, tmp9_e, tmp10_e, tmp11_e, tmp12_e, tmp13_e, tmp15_e;
1379 :
1380 6300 : FOR( n = 0; n < CPE_CHANNELS; n++ )
1381 : {
1382 4200 : prev_off[n] = !on[n];
1383 4200 : move16();
1384 :
1385 4200 : cnt[n] = 0;
1386 4200 : aQ_fx[n] = Mpy_32_32( Q_fx[n], width_fx ); // Q31+Q31 - 31
1387 4200 : cQ_fx[n] = P_fx[Lh - itd_cand[n]]; // Q31
1388 4200 : move16();
1389 4200 : move16();
1390 4200 : move16();
1391 :
1392 4200 : tmp12 = 0, tmp8_e = 0, tmp9_e = 0, tmp10_e = 0, tmp11_e = 0, tmp12_e = 0;
1393 4200 : move16();
1394 4200 : move16();
1395 4200 : move16();
1396 4200 : move16();
1397 4200 : move16();
1398 4200 : move16();
1399 :
1400 : /*compute peak_range*/
1401 4200 : tmp1 = idiv1616( hPOC->shift_limit, STEREO_DMX_EVS_FIND_POC_PEAK_TAU );
1402 4200 : peak_range = idiv1616( add( extract_l( abs_s( itd_cand[n] ) ), tmp1 ), STEREO_DMX_EVS_FIND_POC_PEAK_TAU2 ); // Q0
1403 :
1404 59377 : FOR( i = 1; i <= peak_range; i++ )
1405 : {
1406 55177 : 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] ) ) ) );
1407 55177 : tmp4 = L_add( P_fx[Lh - itd_cand[n] + i], P_fx[Lh - itd_cand[n] - i] ); // Q31
1408 55177 : cQ_fx[n] = L_add( cQ_fx[n], tmp4 );
1409 55177 : move16();
1410 55177 : move32();
1411 : }
1412 :
1413 : /*compute eps2_fx*/
1414 4200 : tmp5 = Mpy_32_32( peak_width_fx[n], ratio_fixed ); // Q31 + Q16 - Q31
1415 4200 : tmp6 = Mpy_32_32( L_deposit_h( cnt[n] ), L_sub( ONE_IN_Q31, ratio_fixed ) ); // Q16 + Q31 - 31
1416 4200 : peak_width_fx[n] = L_add( tmp5, tmp6 ); // Q16
1417 4200 : move16();
1418 :
1419 4200 : tmp7 = Mpy_32_32( L_deposit_h( eps_fx ), peak_width_fx[n] ); // eps_e + 31 - Q16
1420 :
1421 4200 : eps2_fx = L_shr( tmp7, 2 );
1422 4200 : eps2_e = add( eps_e, 31 - Q16 );
1423 :
1424 : /*compute Q_fx[n]*/
1425 4200 : tmp8 = BASOP_Util_Add_Mant32Exp( Q_fx[n], 0, eps2_fx, eps2_e, &tmp8_e );
1426 4200 : tmp_var1 = BASOP_Util_Divide3232_Scale( cQ_fx[n], L_deposit_h( ( add( shl( peak_range, 1 ), 1 ) ) ) /*Q16*/, &tmp9_e );
1427 4200 : tmp9_e = sub( tmp9_e, ( 31 - Q16 ) );
1428 4200 : tmp9 = L_deposit_h( tmp_var1 );
1429 4200 : tmp10 = BASOP_Util_Add_Mant32Exp( tmp9, tmp9_e, eps2_fx, eps2_e, &tmp10_e );
1430 4200 : tmp11 = BASOP_Util_Divide3232_Scale( tmp10, tmp8, &tmp11_e );
1431 4200 : tmp11_e = add( tmp11_e, sub( tmp10_e, tmp8_e ) );
1432 4200 : tmp12_e = BASOP_Util_Add_MantExp( ONE_IN_Q14, 1, negate( tmp11 ), tmp11_e, &tmp12 );
1433 :
1434 4200 : Q_fx[n] = L_deposit_h( tmp12 );
1435 4200 : Q_e[n] = tmp12_e;
1436 4200 : Q_fx[n] = L_max( Q_fx[n], 0 );
1437 4200 : move16();
1438 4200 : move16();
1439 4200 : move16();
1440 4200 : if ( Q_fx[n] == 0 )
1441 : {
1442 178 : Q_e[n] = 0;
1443 178 : move16();
1444 : }
1445 :
1446 4200 : IF( on[n] ) /*if channel n was active (likely to be preceding) in the previous frame*/
1447 : {
1448 1810 : tmp13_e = 0, tmp15_e = 0;
1449 1810 : tmp13 = BASOP_Util_Divide1616_Scale( abs_s( itd_cand[n] ), hPOC->shift_limit, &tmp13_e );
1450 1810 : tmp14 = L_mult( 6554 /*0.2f Q15*/, tmp13 ); // tmp13_e
1451 1810 : tmp15 = BASOP_Util_Add_Mant32Exp( 644245120 /*0.75f in Q31*/, 0, L_negate( tmp14 ), tmp13_e, &tmp15_e );
1452 1810 : tmp15 = Mpy_32_32( tmp15, peakQ_fx[n] ); // tmp15_e + peakQ_e[n]
1453 1810 : move16();
1454 1810 : move16();
1455 :
1456 1810 : tmpf_fx = tmp15;
1457 1810 : move16();
1458 1810 : tmpf_e = add( tmp15_e, peakQ_e[n] );
1459 :
1460 1810 : Word16 flag1 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmpf_fx, tmpf_e );
1461 1810 : tmp16 = Mpy_32_32( 1342177280 /*1.25f in Q30*/, peakQ_fx[n] ); // peakQ_e[n]+1
1462 1810 : Word16 flag2 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmp16, peakQ_e[n] + 1 );
1463 :
1464 1810 : IF( EQ_16( flag1, negate( 1 ) ) )
1465 : {
1466 14 : itdLR[n] = 0;
1467 14 : on[n] = 0;
1468 14 : peakQ_fx[n] = 0;
1469 14 : peakQ_e[n] = 0;
1470 14 : Q_fx[n] = 0;
1471 14 : Q_e[n] = 0;
1472 14 : move16();
1473 14 : move16();
1474 14 : move16();
1475 14 : move16();
1476 14 : move16();
1477 14 : move16();
1478 : }
1479 1796 : ELSE IF( EQ_16( flag2, 1 ) )
1480 : {
1481 17 : itdLR[n] = itd_cand[n];
1482 17 : move16();
1483 : }
1484 1810 : Word16 flag3 = BASOP_Util_Cmp_Mant32Exp( peakQ_fx[n], peakQ_e[n], Q_fx[n], Q_e[n] );
1485 1810 : IF( EQ_16( flag3, negate( 1 ) ) )
1486 : {
1487 186 : peakQ_fx[n] = Q_fx[n];
1488 186 : peakQ_e[n] = Q_e[n];
1489 186 : move16();
1490 186 : move16();
1491 : }
1492 : }
1493 : ELSE /*if channel n was not active (not likely to be preceding) in the previous frame*/
1494 : {
1495 2390 : tmp13_e = 0, tmp15_e = 0;
1496 2390 : move16();
1497 2390 : move16();
1498 2390 : tmp13 = BASOP_Util_Divide1616_Scale( abs_s( itd_cand[n] ), hPOC->shift_limit, &tmp13_e );
1499 2390 : tmp14 = L_mult( 6554 /*0.2f Q15*/, tmp13 ); // tmp13_e
1500 2390 : tmp15 = BASOP_Util_Add_Mant32Exp( 1610612736 /*0.75f in Q31*/, 0, L_negate( tmp14 ), tmp13_e, &tmp15_e );
1501 :
1502 2390 : tmpf_fx = tmp15;
1503 2390 : tmpf_e = tmp15_e;
1504 2390 : move16();
1505 2390 : move16();
1506 :
1507 2390 : Word16 flag1 = BASOP_Util_Cmp_Mant32Exp( Q_fx[n], Q_e[n], tmpf_fx, tmpf_e );
1508 :
1509 2390 : IF( EQ_16( flag1, negate( 1 ) ) )
1510 : {
1511 2373 : itdLR[n] = 0;
1512 2373 : Q_fx[n] = 0;
1513 : }
1514 : ELSE
1515 : {
1516 17 : itdLR[n] = itd_cand[n];
1517 17 : on[n] = 1;
1518 : }
1519 2390 : move16();
1520 2390 : move16();
1521 : }
1522 : }
1523 :
1524 : Word32 var0, var1, var2, var3;
1525 2100 : Word16 var0_e = 0, var1_e = 0, var2_e = 0, var3_e = 0;
1526 : Word16 var0_flag, var1_flag, var2_flag, var3_flag;
1527 2100 : move16();
1528 2100 : move16();
1529 2100 : move16();
1530 2100 : move16();
1531 2100 : var0 = BASOP_Util_Add_Mant32Exp( Q_fx[0], Q_e[0], L_negate( 214748368 /*0.1 in Q31*/ ), 0, &var0_e );
1532 2100 : var1 = BASOP_Util_Add_Mant32Exp( Q_fx[1], Q_e[1], L_negate( 214748368 /*0.1 in Q31*/ ), 0, &var1_e );
1533 2100 : var2 = BASOP_Util_Add_Mant32Exp( Q_fx[1], Q_e[1], Q_BAND_FX, 0, &var2_e );
1534 2100 : var3 = BASOP_Util_Add_Mant32Exp( Q_fx[0], Q_e[0], Q_BAND_FX, 0, &var3_e );
1535 :
1536 : /*flags for condtional checks*/
1537 2100 : var0_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[0], Q_e[0], var1, var1_e );
1538 2100 : var1_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[1], Q_e[1], var0, var0_e );
1539 2100 : var2_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[0], Q_e[0], var2, var2_e );
1540 2100 : var3_flag = BASOP_Util_Cmp_Mant32Exp( Q_fx[1], Q_e[1], var3, var3_e );
1541 :
1542 2100 : test();
1543 2100 : test();
1544 2100 : test();
1545 2100 : test();
1546 2100 : test();
1547 2100 : test();
1548 2100 : test();
1549 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 */
1550 : {
1551 0 : IF( GT_32( Q_fx[0], Q_fx[1] ) )
1552 : {
1553 0 : *itd_fx = itdLR[0];
1554 : }
1555 : ELSE
1556 : {
1557 0 : *itd_fx = itdLR[1];
1558 : }
1559 : }
1560 2100 : ELSE IF( ( on[0] && prev_off[0] ) && ( EQ_16( var0_flag, 1 ) ) ) /* if channel 0 becomes active, select channel 0*/
1561 : {
1562 9 : *itd_fx = itdLR[0];
1563 : }
1564 2091 : ELSE IF( ( on[1] && prev_off[1] ) && ( EQ_16( var1_flag, 1 ) ) ) /*if channel 1 becomes active, selsect channel 1*/
1565 : {
1566 6 : *itd_fx = itdLR[1];
1567 : }
1568 2085 : ELSE IF( EQ_16( var2_flag, 1 ) ) /* if no status change, use Q[]*/
1569 : {
1570 804 : *itd_fx = itdLR[0];
1571 : }
1572 1281 : ELSE IF( EQ_16( var3_flag, 1 ) ) /* if no status change, use Q[]*/
1573 : {
1574 385 : *itd_fx = itdLR[1];
1575 : }
1576 896 : ELSE IF( *itd_fx == 0 ) /*if no channels are likely to be preceding, follow the status of the previous frame*/
1577 : {
1578 741 : *itd_fx = 0;
1579 : }
1580 : ELSE /*follow the status of the previous frame*/
1581 : {
1582 155 : IF( *itd_fx > 0 )
1583 : {
1584 92 : *itd_fx = itdLR[0];
1585 : }
1586 : ELSE
1587 : {
1588 63 : *itd_fx = itdLR[1];
1589 : }
1590 : }
1591 :
1592 2100 : move32();
1593 :
1594 6300 : FOR( i = 0; i < CPE_CHANNELS; i++ )
1595 : {
1596 4200 : Q_fx[i] = L_shr( Q_fx[i], negate( Q_e[i] ) ); // Q31
1597 4200 : move32();
1598 4200 : peakQ_fx[i] = L_shr( peakQ_fx[i], sub( 15, peakQ_e[i] ) ); // Q16
1599 4200 : move32();
1600 : }
1601 :
1602 2100 : Word16 Q_sub_sqrt_e = 0;
1603 2100 : Word32 Q_sub_sqrt = Sqrt32( L_abs( L_sub( Q_fx[0], Q_fx[1] ) ), &Q_sub_sqrt_e );
1604 2100 : cconfidence_fx = Q_sub_sqrt;
1605 2100 : move16();
1606 2100 : move32();
1607 :
1608 2100 : Word32 cconfidence_var1 = Mpy_32_32( hPOC->confidence_fx, STEREO_DMX_EVS_CORR_FORGETTING_FX ); // Q31
1609 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
1610 2100 : hPOC->confidence_fx = L_add( cconfidence_var1, L_shl( cconfidence_var2, 1 ) ); // Q31
1611 2100 : move32();
1612 :
1613 2100 : return hPOC->confidence_fx;
1614 : }
1615 :
1616 : /*-------------------------------------------------------------------*
1617 : * estimate_itd()
1618 : *
1619 : * estimate itd
1620 : *-------------------------------------------------------------------*/
1621 2100 : static ivas_error estimate_itd_fx(
1622 : Word16 *corr, /* o : correlation */
1623 : STEREO_DMX_EVS_POC_HANDLE hPOC, /* i/o: phase only correlation structure */
1624 : STEREO_DMX_EVS_PHA_HANDLE hPHA, /* i/o: correlation filter structure */
1625 : const Word32 srcL[], /* i : Lch input signal Q16 */
1626 : const Word32 srcR[], /* i : Rch input signal Q16 */
1627 : Word16 itd[], /* o : estimated itd Q0 */
1628 : const Word16 input_frame /* i : input frame length per channel */
1629 : )
1630 : {
1631 : Word32 specLr[L_FRAME48k / 2 + 1], specLi[L_FRAME48k / 2 + 1], specRr[L_FRAME48k / 2 + 1], specRi[L_FRAME48k / 2 + 1];
1632 : Word16 rfft_coef[L_FRAME48k];
1633 : const Word16 *p_w;
1634 : Word16 n, n0, n1;
1635 : Word16 rfft_coef_step;
1636 : ivas_error error;
1637 :
1638 2100 : error = IVAS_ERR_OK;
1639 2100 : move32();
1640 :
1641 2100 : n0 = shr( input_frame, 1 );
1642 2100 : n1 = shr( input_frame, 2 );
1643 :
1644 2100 : IF( EQ_16( input_frame, L_FRAME16k ) )
1645 : {
1646 0 : p_w = dft_trigo_32k_fx;
1647 0 : rfft_coef_step = 4;
1648 0 : move16();
1649 : }
1650 2100 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
1651 : {
1652 1050 : p_w = dft_trigo_32k_fx;
1653 1050 : rfft_coef_step = 2;
1654 1050 : move16();
1655 : }
1656 1050 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
1657 : {
1658 1050 : p_w = dft_trigo_48k_fx;
1659 1050 : rfft_coef_step = 2;
1660 1050 : move16();
1661 : }
1662 : ELSE
1663 : {
1664 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "failed estimate_itd()\n" );
1665 : }
1666 :
1667 422100 : FOR( n = 0; n < n1; n++ )
1668 : {
1669 420000 : rfft_coef[n] = p_w[n * rfft_coef_step];
1670 420000 : move16();
1671 420000 : rfft_coef[n0 - n] = p_w[n * rfft_coef_step];
1672 420000 : move16();
1673 : }
1674 2100 : rfft_coef[n1] = p_w[n1 * rfft_coef_step];
1675 2100 : move16();
1676 :
1677 : Word16 specL_e, specR_e, spec_e;
1678 2100 : estimate_itd_wnd_fft_fx( srcL, specLr, specLi, &specL_e, rfft_coef, hPOC->wnd_fx, input_frame );
1679 2100 : estimate_itd_wnd_fft_fx( srcR, specRr, specRi, &specR_e, rfft_coef, hPOC->wnd_fx, input_frame );
1680 :
1681 2100 : spec_e = s_max( sub( specL_e, s_min( getScaleFactor32( specLr, input_frame / 2 + 1 ), getScaleFactor32( specLi, input_frame / 2 + 1 ) ) ),
1682 2100 : sub( specR_e, s_min( getScaleFactor32( specRr, input_frame / 2 + 1 ), getScaleFactor32( specRi, input_frame / 2 + 1 ) ) ) );
1683 2100 : spec_e = add( spec_e, 2 ); // guard bits
1684 2100 : scale_sig32( specLr, add( shr( input_frame, 1 ), 1 ), sub( specL_e, spec_e ) ); // Q(31-(specL_e-spec_e))
1685 2100 : scale_sig32( specLi, add( shr( input_frame, 1 ), 1 ), sub( specL_e, spec_e ) ); // Q(31-(specL_e-spec_e))
1686 2100 : scale_sig32( specRr, add( shr( input_frame, 1 ), 1 ), sub( specR_e, spec_e ) ); // Q(31-(specR_e-spec_e))
1687 2100 : scale_sig32( specRi, add( shr( input_frame, 1 ), 1 ), sub( specR_e, spec_e ) ); // Q(31-(specR_e-spec_e))
1688 :
1689 2100 : calc_poc_fx( hPOC, hPHA, hPOC->wnd_fx, rfft_coef, specLr, specLi, specRr, specRi, spec_e, input_frame );
1690 :
1691 2100 : *corr = round_fx( find_poc_peak_fx( hPOC, itd, input_frame, STEREO_DMX_EVS_POC_W_FORGETTING_FX ) );
1692 2100 : move16();
1693 :
1694 2100 : return error;
1695 : }
1696 :
1697 : /*-------------------------------------------------------------------*
1698 : * weighted_ave()
1699 : *
1700 : * create weighted downmix signal
1701 : *-------------------------------------------------------------------*/
1702 2100 : static void weighted_ave_fx(
1703 : const Word32 src1_fx[], /* i : Lch input signal Q16 */
1704 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
1705 : Word32 dst_fx[], /* o : output signal Q16 */
1706 : const Word32 gain_fx, /* i : adapting gain Q31 */
1707 : const Word32 old_gain_fx, /* i : adapting prev gain Q31 */
1708 : const Word16 input_frame, /* i : input frame length per channel */
1709 : const Word32 wnd_fx[] /* i : window coef Q31 */
1710 : )
1711 : {
1712 :
1713 : Word16 i, j, len, len2;
1714 2100 : Word32 gain_tmp_fx = 0, gain_sub_fx;
1715 :
1716 2100 : move32();
1717 :
1718 2100 : len = shr( input_frame, 1 );
1719 2100 : len2 = shr( input_frame, 2 );
1720 2100 : gain_sub_fx = L_sub( gain_fx, old_gain_fx );
1721 :
1722 422100 : FOR( i = 0; i < len2; i++ )
1723 : {
1724 420000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, wnd_fx[i] ) ); // Q31
1725 420000 : 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
1726 420000 : move32();
1727 : }
1728 422100 : FOR( ; i < len; i++ )
1729 : {
1730 420000 : j = add( i, 1 );
1731 420000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, L_sub( MAX_32, wnd_fx[sub( len, j )] ) ) ); // Q31
1732 420000 : 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
1733 420000 : move32();
1734 : }
1735 842100 : FOR( ; i < input_frame; i++ )
1736 : {
1737 840000 : 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
1738 840000 : move32();
1739 : }
1740 :
1741 :
1742 2100 : return;
1743 : }
1744 :
1745 : /*-------------------------------------------------------------------*
1746 : * spectral_flatness_fx()
1747 : *
1748 : * computes spectral flatness SF
1749 : * SF(x) = exp(mean_i(ln(x_i))) / mean_i(x_i)
1750 : *-------------------------------------------------------------------*/
1751 2104 : static Word32 spectral_flatness_fx(
1752 : const Word32 *sig_fx, /* i : input signal (mantissa) */
1753 : const Word16 *sig_fx_e, /* i : input signal (exponent) */
1754 : const Word16 sig_length /* i : input signal length */
1755 : )
1756 : {
1757 : Word32 L_geoMean, L_ariMean, L_tmp;
1758 : Word16 i, geoMean, ariMean, geoMean_e, ariMean_e, L_tmp_e, sf;
1759 :
1760 2104 : L_geoMean = 0;
1761 2104 : geoMean_e = 0;
1762 2104 : L_ariMean = 0;
1763 2104 : ariMean_e = 0;
1764 :
1765 2104 : IF( sig_fx_e == NULL )
1766 : {
1767 196 : FOR( i = 0; i < sig_length; i++ )
1768 : {
1769 192 : L_tmp = BASOP_Util_Add_Mant32Exp( L_abs( sig_fx[i] ), 0, EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
1770 192 : L_ariMean = BASOP_Util_Add_Mant32Exp( L_ariMean, ariMean_e, L_tmp, L_tmp_e, &ariMean_e );
1771 192 : L_geoMean = BASOP_Util_Add_Mant32Exp( L_geoMean, geoMean_e, BASOP_Util_Loge( L_tmp, L_tmp_e ), 6, &geoMean_e ); // +6 : compensate result of log /64
1772 : }
1773 : }
1774 : ELSE
1775 : {
1776 420000 : FOR( i = 0; i < sig_length; i++ )
1777 : {
1778 417900 : L_tmp = BASOP_Util_Add_Mant32Exp( L_abs( sig_fx[i] ), sig_fx_e[i], EPSILON_FX_M, EPSILON_FX_E, &L_tmp_e );
1779 417900 : L_ariMean = BASOP_Util_Add_Mant32Exp( L_ariMean, ariMean_e, L_tmp, L_tmp_e, &ariMean_e );
1780 417900 : L_geoMean = BASOP_Util_Add_Mant32Exp( L_geoMean, geoMean_e, BASOP_Util_Loge( L_tmp, L_tmp_e ), 6, &geoMean_e ); // +6 : compensate result of log /64
1781 : }
1782 : }
1783 :
1784 2104 : ariMean = BASOP_Util_Divide3216_Scale( L_ariMean, sig_length, &L_tmp_e );
1785 2104 : ariMean_e = add( sub( ariMean_e, Q15 ), L_tmp_e ); // (Q16)-1 : compensate result of division
1786 :
1787 2104 : geoMean = BASOP_Util_Divide3216_Scale( L_geoMean, sig_length, &L_tmp_e );
1788 2104 : geoMean_e = add( sub( geoMean_e, Q15 ), L_tmp_e ); // (Q16)-1 : compensate result of division
1789 2104 : geoMean = extract_h( BASOP_Util_fPow( (Word32) 1459366444, (Word16) 2, L_deposit_h( geoMean ), geoMean_e, &geoMean_e ) ); /* e=2,718281828459045 */
1790 :
1791 2104 : sf = BASOP_Util_Divide1616_Scale( geoMean, ariMean, &L_tmp_e );
1792 2104 : L_tmp_e = add( sub( geoMean_e, ariMean_e ), L_tmp_e );
1793 :
1794 2104 : return L_shl_sat( L_deposit_h( sf ), L_tmp_e );
1795 : }
1796 :
1797 : /*-------------------------------------------------------------------*
1798 : * renorm_poc_fx()
1799 : *
1800 : * two-stage renormalization of a complex number
1801 : *-------------------------------------------------------------------*/
1802 1339800 : static void renorm_poc_fx(
1803 : Word32 *real, /* i/o real-part*/
1804 : Word32 *imag /* i/o imaginary-part */
1805 : )
1806 : {
1807 1339800 : IF( ( LT_32( L_abs( *real ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
1808 : ( LT_32( L_abs( *imag ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
1809 : {
1810 699480 : *real = L_shl( *real, STEREO_DMX_EVS_POC_RENORM_SHIFT );
1811 699480 : *imag = L_shl( *imag, STEREO_DMX_EVS_POC_RENORM_SHIFT );
1812 699480 : IF( ( LT_32( L_abs( *real ), STEREO_DMX_EVS_POC_RENORM_TH ) ) &&
1813 : ( LT_32( L_abs( *imag ), STEREO_DMX_EVS_POC_RENORM_TH ) ) )
1814 : {
1815 45 : *real = L_shl( *real, STEREO_DMX_EVS_POC_RENORM_SHIFT );
1816 45 : *imag = L_shl( *imag, STEREO_DMX_EVS_POC_RENORM_SHIFT );
1817 : }
1818 : }
1819 1339800 : }
1820 :
1821 : /*-------------------------------------------------------------------*
1822 : * calc_energy()
1823 : *
1824 : * calculate energy
1825 : *-------------------------------------------------------------------*/
1826 6300 : static void calc_energy_fx(
1827 : const Word32 src1_fx[], /* i : Lch input signal Q16*/
1828 : const Word32 src2_fx[], /* i : Rch input signal Q16 */
1829 : Word32 energy_fx[], /* o : calculated energy energy_fx_e*/
1830 : Word16 *energy_fx_e, /* o : calculated energy */
1831 : const Word16 input_frame, /* i : input frame length per channel */
1832 : const Word32 ratio_float_fx // Q31
1833 : )
1834 : {
1835 : Word32 E_32_fx, wnd_fx, wnd_diff_fx;
1836 : Word16 i, adaptlen;
1837 : Word64 E_fx;
1838 :
1839 : /* Initialization */
1840 6300 : E_fx = 0;
1841 6300 : move32();
1842 6300 : adaptlen = shr( input_frame, 4 );
1843 : // wnd = 0.5f / (float) adaptlen; adaptlen= 20;in Q31
1844 6300 : wnd_fx = 53687091; // initialising for L_FRAME16k =( 0.5f /20) in Q31;
1845 6300 : move32();
1846 6300 : wnd_diff_fx = 107374182; // initialising for L_FRAME16k =( 1.0f /20) in Q31;
1847 6300 : move32();
1848 6300 : SWITCH( input_frame )
1849 : {
1850 0 : case L_FRAME16k:
1851 0 : wnd_fx = 53687091; // wnd = 0.5f / (float) adaptlen; adaptlen= 20;in Q31
1852 0 : wnd_diff_fx = 107374182; // wnd = 1.0f / (float) adaptlen;adaptlen= 20;in Q31
1853 0 : BREAK;
1854 3150 : case L_FRAME32k:
1855 3150 : wnd_fx = 26843545; // wnd = 0.5f / (float) adaptlen; adaptlen= 40;in Q31
1856 3150 : wnd_diff_fx = 53687091; // wnd = 1.0f / (float) adaptlen;adaptlen= 40;in Q31
1857 3150 : BREAK;
1858 3150 : case L_FRAME48k:
1859 3150 : wnd_fx = 17895697; // wnd = 0.5f / (float) adaptlen; adaptlen= 60;in Q31
1860 3150 : wnd_diff_fx = 35791394; // wnd = 1.0f / (float) adaptlen;adaptlen= 60;in Q31
1861 3150 : BREAK;
1862 : }
1863 6300 : move32();
1864 6300 : move32();
1865 : // wnd = 0.5f / (float) adaptlen;
1866 : // wnd_diff = 1.0f / (float) adaptlen;
1867 6300 : Word16 gb = find_guarded_bits_fx( input_frame );
1868 6300 : wnd_fx = L_shr( wnd_fx, gb );
1869 6300 : wnd_diff_fx = L_shr( wnd_diff_fx, gb );
1870 321300 : FOR( i = 0; i < adaptlen; i++ )
1871 : {
1872 : // E += ( src1[i] * wnd ) * ( src2[i] * wnd );
1873 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)
1874 :
1875 : // wnd += wnd_diff;
1876 315000 : wnd_fx = L_add( wnd_fx, wnd_diff_fx );
1877 : }
1878 4416300 : FOR( ; i < input_frame - adaptlen; i++ )
1879 : {
1880 : // E += src1[i] * src2[i];
1881 4410000 : E_fx = W_add( E_fx, W_shr( W_mult0_32_32( src1_fx[i], src2_fx[i] ), shl( gb, 1 ) ) );
1882 : }
1883 321300 : FOR( ; i < input_frame; i++ )
1884 : {
1885 : // wnd -= wnd_diff;
1886 315000 : wnd_fx = L_sub( wnd_fx, wnd_diff_fx );
1887 :
1888 : // E += ( src1[i] * wnd ) * ( src2[i] * wnd );
1889 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)
1890 : }
1891 6300 : Word16 lshift = W_norm( E_fx );
1892 6300 : E_32_fx = W_extract_h( W_shl( E_fx, lshift ) ); // 2*(Q16-gb)+lshift -32
1893 6300 : Word16 q_E = sub( add( shl( sub( Q16, gb ), 1 ), lshift ), 32 );
1894 : Word16 temp_e, q_temp32;
1895 6300 : Word32 temp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( E_32_fx, input_frame, &temp_e ) ); // Q_E +(15-temp_e) + 16
1896 6300 : q_temp32 = add( add( q_E, sub( 15, temp_e ) ), 16 );
1897 : // *energy = *energy * ratio_float + ( E / (float) input_frame ) * ( 1.0f - ratio_float );
1898 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 );
1899 6300 : move32();
1900 :
1901 6300 : return;
1902 : }
1903 :
1904 : /*-------------------------------------------------------------------*
1905 : * calc_energy_sgc()
1906 : *
1907 : * calculate energy for switch gain control
1908 : *-------------------------------------------------------------------*/
1909 8400 : static void calc_energy_sgc(
1910 : const Word32 src[], /* i : input signal */
1911 : Word32 *energy, /* i/o : calculated energy (mantissa) */
1912 : Word16 *energy_e, /* i/o : calculated energy (exponent) */
1913 : const Word16 input_frame, /* i : input frame length */
1914 : const Word16 ratio /* i : adapting ratio */
1915 : )
1916 : {
1917 : Word16 i, l_tmp_e, l_energy_e;
1918 : Word32 l_energy;
1919 : Word64 w_tmp;
1920 :
1921 8400 : l_energy = 0;
1922 8400 : l_energy_e = 0;
1923 6728400 : FOR( i = 0; i < input_frame; i++ )
1924 : {
1925 6720000 : w_tmp = W_mult0_32_32( src[i], src[i] );
1926 6720000 : l_tmp_e = W_norm( w_tmp );
1927 6720000 : IF( l_tmp_e != 0 )
1928 : {
1929 6706774 : w_tmp = W_shl( w_tmp, l_tmp_e );
1930 : }
1931 6720000 : l_energy = BASOP_Util_Add_Mant32Exp( l_energy, l_energy_e, W_round64_L( w_tmp ), sub( 32, l_tmp_e ), &l_energy_e ); // Q(2x26 - l_energy_e)
1932 : }
1933 :
1934 8400 : *energy = BASOP_Util_Add_Mant32Exp( Mpy_32_16_1( *energy, ratio ), *energy_e, Mpy_32_16_1( l_energy, sub( MAX_16, ratio ) ), l_energy_e, energy_e );
1935 8400 : move32();
1936 :
1937 8400 : return;
1938 : }
1939 :
1940 : /*-------------------------------------------------------------------*
1941 : * adapt_gain()
1942 : *
1943 : * adapt gain to the signal
1944 : *-------------------------------------------------------------------*/
1945 4200 : static void adapt_gain_fx(
1946 : const Word32 src_fx[], /* i : input signal Q16 */
1947 : Word32 dst_fx[], /* o : output signal Q16 */
1948 : const Word32 gain_fx, /* i : adapting gain Q31*/
1949 : const Word32 old_gain_fx, /* i : adapting prev gain Q31*/
1950 : const Word16 input_frame, /* i : input frame length per channel */
1951 : const Word32 wnd_fx[] /* i : window coef Q31 */
1952 : )
1953 : {
1954 :
1955 : Word16 i, j, len, len2;
1956 : Word32 gain_tmp_fx, gain_sub_fx;
1957 :
1958 4200 : len = shr( input_frame, 1 );
1959 4200 : len2 = shr( input_frame, 2 );
1960 4200 : gain_sub_fx = L_sub( gain_fx, old_gain_fx );
1961 :
1962 844200 : FOR( i = 0; i < len2; i++ )
1963 : {
1964 840000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, wnd_fx[i] ) ); // Q31
1965 840000 : dst_fx[i] = Mpy_32_32( src_fx[i], gain_tmp_fx ); // Q16
1966 840000 : move32();
1967 : }
1968 844200 : FOR( ; i < len; i++ )
1969 : {
1970 840000 : j = add( i, 1 );
1971 840000 : gain_tmp_fx = L_add( old_gain_fx, Mpy_32_32( gain_sub_fx, L_sub( MAX_32, wnd_fx[sub( len, j )] ) ) ); // Q31
1972 840000 : dst_fx[i] = Mpy_32_32( src_fx[i], gain_tmp_fx ); // Q16
1973 840000 : move32();
1974 : }
1975 1684200 : FOR( ; i < input_frame; i++ )
1976 : {
1977 1680000 : dst_fx[i] = Mpy_32_32( src_fx[i], gain_fx ); // Q16
1978 1680000 : move32();
1979 : }
1980 :
1981 :
1982 4200 : return;
1983 : }
1984 :
1985 : /*-------------------------------------------------------------------*
1986 : * create_M_signal()
1987 : *
1988 : * create downmix signal
1989 : *-------------------------------------------------------------------*/
1990 2100 : static void create_M_signal_fx(
1991 : const Word32 srcL_fx[], /* i : Lch input signal Q16 */
1992 : const Word32 srcR_fx[], /* i : Rch input signal Q16 */
1993 : Word32 dmx_fx[], /* o : output signal Q31 */
1994 : const Word32 w_curr_fx, /* i : adapting weight Q31 */
1995 : const Word16 input_frame, /* i : input frame length per channel */
1996 : const Word32 wnd_fx[], /* i : window coef Q31 */
1997 : Word32 *w_prev_fx, /* i/o: adapting prev weight Q31*/
1998 : Word32 *dmx_energy_fx, /* i/o: downmix signal energy dmx_energy_fx_e */
1999 : Word16 *dmx_energy_fx_e, /* i/o: downmix signal energy */
2000 : Word32 *src_energy_fx, /* i/o: input signal energy src_energy_fx_e */
2001 : Word16 *src_energy_fx_e /* i/o: input signal energy */
2002 : )
2003 : {
2004 : Word32 amp_mod_fx[CPE_CHANNELS];
2005 : Word32 weighted_fx[L_FRAME48k], Lbias_fx;
2006 : Word32 eps_fx, temp_32;
2007 : Word32 temp32_1, temp32_2;
2008 : Word16 temp_e;
2009 : Word16 temp_e_1, temp_e_2;
2010 :
2011 : /* Initialization */
2012 2100 : eps_fx = 1024; // 1024.0f in Q0
2013 2100 : move32();
2014 2100 : IF( w_prev_fx[2] == 0 )
2015 : {
2016 798 : Lbias_fx = 1073741824; // 4.0f in Q28
2017 798 : move32();
2018 : }
2019 : ELSE
2020 : {
2021 1302 : Lbias_fx = 67108864; // 0.25f in Q28
2022 1302 : move32();
2023 : }
2024 2100 : weighted_ave_fx( srcL_fx, srcR_fx, dmx_fx, w_curr_fx, w_prev_fx[0], input_frame, wnd_fx );
2025 2100 : calc_energy_fx( srcL_fx, srcL_fx, src_energy_fx, src_energy_fx_e, input_frame, STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX );
2026 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 );
2027 2100 : calc_energy_fx( dmx_fx, dmx_fx, dmx_energy_fx, dmx_energy_fx_e, input_frame, STEREO_DMX_EVS_DMX_EGY_FORGETTING_FX );
2028 :
2029 2100 : temp32_1 = Mpy_32_32( src_energy_fx[0], Lbias_fx ); // 31 - src_energy_fx_e + Q28-31
2030 2100 : temp_e_1 = add( src_energy_fx_e[0], 3 );
2031 2100 : Word16 flag = BASOP_Util_Cmp_Mant32Exp( temp32_1, temp_e_1, src_energy_fx[1], src_energy_fx_e[1] );
2032 : // if ( src_energy[0] * Lbias > src_energy[1] )
2033 2100 : IF( EQ_16( flag, 1 ) )
2034 : {
2035 782 : temp32_1 = BASOP_Util_Add_Mant32Exp( dmx_energy_fx[0], dmx_energy_fx_e[0], eps_fx, Q31, &temp_e_1 );
2036 782 : temp32_2 = BASOP_Util_Add_Mant32Exp( src_energy_fx[0], src_energy_fx_e[0], eps_fx, Q31, &temp_e_2 );
2037 782 : temp_32 = L_deposit_h( BASOP_Util_Divide3232_Scale( temp32_1, temp32_2, &temp_e ) ); // Q31-temp_e+temp_e_2-temp_e_1
2038 782 : temp_e_1 = sub( add( temp_e, temp_e_1 ), temp_e_2 );
2039 : // temp = sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[0] + eps ) );
2040 782 : temp32_1 = Sqrt32( temp_32, &temp_e_1 );
2041 : // amp_mod[0] = 1.0f - sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[0] + eps ) );
2042 782 : amp_mod_fx[0] = L_shl( L_sub( ONE_IN_Q30, L_shl( temp32_1, sub( temp_e_1, 1 ) ) ), 1 );
2043 782 : move32();
2044 782 : amp_mod_fx[0] = L_max( amp_mod_fx[0], 0 );
2045 782 : move32();
2046 782 : amp_mod_fx[1] = 0;
2047 782 : move32();
2048 : }
2049 : ELSE
2050 : {
2051 1318 : temp32_1 = BASOP_Util_Add_Mant32Exp( dmx_energy_fx[0], dmx_energy_fx_e[0], eps_fx, Q31, &temp_e_1 );
2052 1318 : temp32_2 = BASOP_Util_Add_Mant32Exp( src_energy_fx[1], src_energy_fx_e[1], eps_fx, Q31, &temp_e_2 );
2053 1318 : temp_32 = L_deposit_h( BASOP_Util_Divide3232_Scale( temp32_1, temp32_2, &temp_e ) ); // Q31-temp_e+temp_e_2-temp_e_1
2054 1318 : temp_e_1 = sub( add( temp_e, temp_e_1 ), temp_e_2 );
2055 : // amp_mod[1] = 1.0f - sqrtf( ( dmx_energy[0] + eps ) / ( src_energy[1] + eps ) );
2056 1318 : temp32_1 = Sqrt32( temp_32, &temp_e_1 );
2057 1318 : amp_mod_fx[1] = L_shl( L_sub( ONE_IN_Q30, L_shl( temp32_1, sub( temp_e_1, 1 ) ) ), 1 );
2058 1318 : move32();
2059 : // amp_mod[1] = max( amp_mod[1], 0.0f );
2060 1318 : amp_mod_fx[1] = L_max( amp_mod_fx[1], 0 );
2061 1318 : move32();
2062 1318 : amp_mod_fx[0] = 0;
2063 1318 : move32();
2064 : }
2065 2100 : adapt_gain_fx( srcL_fx, weighted_fx, amp_mod_fx[0], w_prev_fx[1], input_frame, wnd_fx );
2066 2100 : v_add_fx( dmx_fx, weighted_fx, dmx_fx, input_frame ); // Q16
2067 2100 : adapt_gain_fx( srcR_fx, weighted_fx, amp_mod_fx[1], w_prev_fx[2], input_frame, wnd_fx );
2068 2100 : v_add_fx( dmx_fx, weighted_fx, dmx_fx, input_frame ); // Q16
2069 2100 : w_prev_fx[0] = w_curr_fx;
2070 2100 : move32();
2071 2100 : w_prev_fx[1] = amp_mod_fx[0];
2072 2100 : move32();
2073 2100 : w_prev_fx[2] = amp_mod_fx[1];
2074 2100 : move32();
2075 :
2076 2100 : return;
2077 : }
2078 :
2079 : /*-------------------------------------------------------------------*
2080 : * apply_gain_sgc()
2081 : *
2082 : * Apply gain for switching
2083 : *-------------------------------------------------------------------*/
2084 :
2085 4200 : static void apply_gain_sgc(
2086 : Word32 data[], /* i/o : input signal */
2087 : Word32 *gain, /* i : gain */
2088 : const Word16 input_frame /* i : input frame length */
2089 : )
2090 : {
2091 : Word16 n;
2092 : Word32 lr;
2093 : Word64 W_tmp;
2094 :
2095 4200 : IF( GT_32( *gain, STEREO_DMX_EVS_SGC_GH ) )
2096 : {
2097 132 : lr = STEREO_DMX_EVS_SGC_GIR_S;
2098 132 : move32();
2099 : }
2100 4068 : ELSE IF( LT_32( *gain, STEREO_DMX_EVS_SGC_GL ) )
2101 : {
2102 194 : lr = STEREO_DMX_EVS_SGC_GR_S;
2103 194 : move32();
2104 : }
2105 : ELSE
2106 : {
2107 3874 : return;
2108 : }
2109 :
2110 272006 : FOR( n = 0; n < input_frame; n++ )
2111 : {
2112 271680 : W_tmp = W_mult_32_32( data[n], *gain );
2113 271680 : W_tmp = W_shr( W_tmp, 16 );
2114 271680 : data[n] = W_extract_l( W_tmp );
2115 : }
2116 :
2117 326 : W_tmp = W_mult_32_32( *gain, lr );
2118 326 : W_tmp = W_shr( W_tmp, 16 );
2119 326 : *gain = W_extract_l( W_tmp );
2120 :
2121 326 : return;
2122 : }
2123 :
2124 : /*-------------------------------------------------------------------*
2125 : * stereo_dmx_evs_enc()
2126 : *
2127 : * Stereo downmix for EVS encoder routine
2128 : *-------------------------------------------------------------------*/
2129 2100 : void stereo_dmx_evs_enc_fx(
2130 : STEREO_DMX_EVS_ENC_HANDLE hStereoDmxEVS, /* i/o: Stereo downmix for EVS encoder handle */
2131 : const Word32 input_Fs, /* i : input sampling rate */
2132 : Word16 data[CPE_CHANNELS * L_FRAME48k], /* i/o: input signal Q0 */
2133 : const Word16 n_samples, /* i : number of input samples */
2134 : const bool is_binaural /* i : indication that input is binaural audio */
2135 : )
2136 : {
2137 : Word16 n;
2138 : Word16 dmx_weight, corr; // Q15
2139 :
2140 : Word16 k, m, pha_len, fad_len, sbfad_len;
2141 :
2142 : Word32 data_fx[CPE_CHANNELS][L_FRAME48k]; // Q16/Q11
2143 :
2144 : Word32 mem_prev[STEREO_DMX_EVS_FAD_LEN_MAX], data_mem[STEREO_DMX_EVS_DATA_LEN_MAX]; // Q11
2145 : Word32 *p_data_mem, *p_prev_taps, *p_curr_taps, *fad_g, *p_data;
2146 :
2147 : Word32 dmx_poc_data[L_FRAME48k] /*Q11*/, dmx_pha_data[L_FRAME48k] /*Q11*/, *p_dmx_data, fx_tmp, *p_dmx_data_fo;
2148 : Word16 n_fad_r, n_fad_g, m_fad_g, n_fad_cnt;
2149 :
2150 : STEREO_DMX_EVS_PRC prev_prc;
2151 : STEREO_DMX_EVS_PHA_HANDLE hPHA;
2152 :
2153 : Word16 input_subframe, is_transient, dmx_gain_sgc;
2154 : Word32 *p_sub_frame, subframe_energy[STEREO_DMX_EVS_NB_SBFRM];
2155 : Word16 subframe_energy_e[STEREO_DMX_EVS_NB_SBFRM];
2156 :
2157 : Word16 input_frame;
2158 :
2159 : Word32 L_tmp1, L_tmp2;
2160 : Word16 L_tmp1_e, L_tmp2_e;
2161 :
2162 : Word64 W_tmp;
2163 : Word16 W_tmp_q;
2164 :
2165 2100 : push_wmops( "stereo_dmx_evs_enc" );
2166 : if ( is_binaural )
2167 : {
2168 : /* use of is_binaural flag is to be considered */
2169 : }
2170 :
2171 : // input_frame = (int16_t) ( input_Fs / FRAMES_PER_SEC );
2172 2100 : SWITCH( input_Fs )
2173 : {
2174 0 : case 8000:
2175 0 : input_frame = 160;
2176 0 : BREAK;
2177 0 : case 16000:
2178 0 : input_frame = 320;
2179 0 : BREAK;
2180 1050 : case 32000:
2181 1050 : input_frame = 640;
2182 1050 : BREAK;
2183 1050 : case 48000:
2184 1050 : input_frame = 960;
2185 1050 : BREAK;
2186 0 : default:
2187 0 : input_frame = 960;
2188 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2189 0 : BREAK;
2190 : }
2191 2100 : move16();
2192 2100 : hPHA = hStereoDmxEVS->hPHA;
2193 :
2194 1682100 : FOR( n = 0; n < input_frame; n++ )
2195 : {
2196 1680000 : data_fx[0][n] = L_deposit_h( data[2 * n] );
2197 1680000 : move32();
2198 1680000 : data_fx[1][n] = L_deposit_h( data[2 * n + 1] );
2199 1680000 : move32();
2200 : }
2201 2100 : IF( LT_16( n_samples, input_frame ) )
2202 : {
2203 0 : set32_fx( data_fx[0] + n_samples, 0, sub( input_frame, n_samples ) );
2204 0 : set32_fx( data_fx[1] + n_samples, 0, sub( input_frame, n_samples ) );
2205 : }
2206 :
2207 : // input_subframe = n_samples / STEREO_DMX_EVS_NB_SBFRM;
2208 2100 : IF( EQ_16( n_samples, L_FRAME16k ) )
2209 : {
2210 0 : input_subframe = 64;
2211 0 : move16();
2212 : }
2213 2100 : ELSE IF( EQ_16( n_samples, L_FRAME32k ) )
2214 : {
2215 1050 : input_subframe = 128;
2216 1050 : move16();
2217 : }
2218 1050 : ELSE IF( EQ_16( n_samples, L_FRAME48k ) )
2219 : {
2220 1050 : input_subframe = 192;
2221 1050 : move16();
2222 : }
2223 : ELSE
2224 : {
2225 0 : input_subframe = 192;
2226 0 : move16();
2227 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2228 : }
2229 :
2230 2100 : is_transient = 0;
2231 2100 : move16();
2232 6300 : FOR( k = 0; k < CPE_CHANNELS; k++ )
2233 : {
2234 4200 : fx_tmp = 0;
2235 4200 : move32();
2236 25200 : FOR( m = 0; m < STEREO_DMX_EVS_NB_SBFRM; m++ )
2237 : {
2238 21000 : p_sub_frame = &( data_fx[k][m * input_subframe] );
2239 21000 : subframe_energy[m] = 0;
2240 21000 : move32();
2241 21000 : subframe_energy_e[m] = 0;
2242 21000 : move16();
2243 3381000 : FOR( n = 0; n < input_subframe; n++ )
2244 : {
2245 : // subframe_energy[m] += p_sub_frame[n] * p_sub_frame[n];
2246 3360000 : W_tmp = W_mult_32_32( p_sub_frame[n], p_sub_frame[n] );
2247 3360000 : W_tmp_q = W_norm( W_tmp );
2248 3360000 : W_tmp = W_shl( W_tmp, W_tmp_q );
2249 3360000 : L_tmp1 = W_extract_h( W_tmp ); // Q(31-(30-W_tmp_q))
2250 3360000 : L_tmp1_e = sub( 15 << 1, W_tmp_q );
2251 3360000 : subframe_energy[m] = BASOP_Util_Add_Mant32Exp( subframe_energy[m], subframe_energy_e[m], L_tmp1, L_tmp1_e, &subframe_energy_e[m] );
2252 3360000 : move32();
2253 : }
2254 :
2255 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 );
2256 21000 : L_tmp2 = BASOP_Util_Divide3232_Scale_newton( subframe_energy[m], L_tmp1, &L_tmp2_e );
2257 21000 : L_tmp2_e = add( L_tmp2_e, sub( subframe_energy_e[m], L_tmp1_e ) );
2258 : // if ( subframe_energy[m] / ( hPHA->trns_aux_energy[k] + EPSILON ) > hPHA->crst_fctr )
2259 21000 : if ( BASOP_Util_Cmp_Mant32Exp( L_tmp2, L_tmp2_e, hPHA->crst_fctr_fx, 31 ) > 0 )
2260 : {
2261 27 : is_transient = 1;
2262 27 : move16();
2263 : }
2264 :
2265 : // hPHA->trns_aux_energy[k] = STEREO_DMX_EVS_TRNS_EGY_FORGETTING * hPHA->trns_aux_energy[k] + ( 1.0f - STEREO_DMX_EVS_TRNS_EGY_FORGETTING ) * subframe_energy[m];
2266 21000 : hPHA->trns_aux_energy_fx[k] = BASOP_Util_Add_Mant32Exp( Mpy_32_16_1( hPHA->trns_aux_energy_fx[k], STEREO_DMX_EVS_TRNS_EGY_FORGETTING_Q15 ), 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], &hPHA->trns_aux_energy_fx_e[k] );
2267 21000 : move32();
2268 : }
2269 :
2270 21000 : FOR( m = 1; m < STEREO_DMX_EVS_NB_SBFRM; m++ )
2271 : {
2272 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 );
2273 16800 : L_tmp2 = BASOP_Util_Divide3232_Scale_newton( subframe_energy[m], L_tmp1, &L_tmp2_e );
2274 16800 : L_tmp2_e = add( L_tmp2_e, sub( subframe_energy_e[m], L_tmp1_e ) );
2275 : // if ( subframe_energy[m] / ( subframe_energy[m - 1] + EPSILON ) > STEREO_DMX_EVS_TRNS_DTC_INST )
2276 16800 : if ( BASOP_Util_Cmp_Mant32Exp( L_tmp2, L_tmp2_e, STEREO_DMX_EVS_TRNS_DTC_INST_Q0, 31 ) > 0 )
2277 : {
2278 23 : is_transient = 1;
2279 23 : move16();
2280 : }
2281 : }
2282 : }
2283 :
2284 2100 : estimate_itd_fx( &corr, hStereoDmxEVS->hPOC, hPHA, data_fx[0], data_fx[1], &hStereoDmxEVS->itd_fx, input_frame );
2285 :
2286 : /* poc */
2287 :
2288 2100 : IF( hStereoDmxEVS->itd_fx )
2289 : {
2290 : // dmx_weight = ( ( hStereoDmxEVS->itd > 0 ) ? ( -1 ) : 1 ) * 0.5f * corr + 0.5f;
2291 999 : IF( GT_16( hStereoDmxEVS->itd_fx, 0 ) )
2292 : {
2293 551 : dmx_weight = add( negate( shr( corr, 1 ) ), ONE_IN_Q14 );
2294 : }
2295 : ELSE
2296 : {
2297 448 : dmx_weight = add( shr( corr, 1 ), ONE_IN_Q14 );
2298 : }
2299 : }
2300 : ELSE
2301 : {
2302 1101 : dmx_weight = ONE_IN_Q14;
2303 1101 : move16();
2304 : }
2305 :
2306 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,
2307 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 );
2308 :
2309 : // Downscaling signals to avoid accumulation overflows
2310 2100 : scale_sig32( data_fx[0], input_frame, -5 ); // Q31->Q26
2311 2100 : scale_sig32( data_fx[1], input_frame, -5 ); // Q31->Q26
2312 2100 : scale_sig32( dmx_poc_data, input_frame, -5 ); // Q31->Q26
2313 :
2314 : /* pha */
2315 :
2316 2100 : pha_len = hPHA->pha_len;
2317 2100 : move16();
2318 2100 : fad_len = hPHA->fad_len;
2319 2100 : move16();
2320 2100 : fad_g = hPHA->fad_g_fx;
2321 :
2322 2100 : set_zero_fx( dmx_pha_data, n_samples );
2323 2100 : set_zero_fx( mem_prev, fad_len );
2324 :
2325 6300 : FOR( k = 0; k < CPE_CHANNELS; k++ )
2326 : {
2327 4200 : p_data = data_fx[k];
2328 4200 : Copy32( hPHA->data_mem_fx[k], data_mem, pha_len );
2329 4200 : Copy32( &( p_data[sub( n_samples, pha_len )] ), hPHA->data_mem_fx[k], pha_len );
2330 4200 : p_data_mem = &( data_mem[pha_len] );
2331 4200 : Copy32( p_data, p_data_mem, n_samples );
2332 :
2333 4200 : p_prev_taps = hPHA->p_prev_taps_fx[k];
2334 4200 : IF( p_prev_taps )
2335 : {
2336 1681794 : FOR( n = 0; n < fad_len; n++ )
2337 : {
2338 82202400 : FOR( ( fx_tmp = 0, m = 0 ); m < pha_len; m++ )
2339 : {
2340 : // ftmp += p_data_mem[n - m] * p_prev_taps[m];
2341 80524800 : fx_tmp = L_add( fx_tmp, Mpy_32_32( p_data_mem[n - m], p_prev_taps[m] ) ); // Q25
2342 : }
2343 1677600 : fx_tmp = L_shl( fx_tmp, 1 ); // Q26
2344 1677600 : mem_prev[n] = L_add( mem_prev[n], Mpy_32_32( fx_tmp, INV_SQRT_2_Q31 ) );
2345 1677600 : move32();
2346 : }
2347 : }
2348 : ELSE
2349 : {
2350 2406 : FOR( n = 0; n < fad_len; n++ )
2351 : {
2352 : // mem_prev[n] += p_data[n] * INV_SQRT_2;
2353 2400 : mem_prev[n] = L_add( mem_prev[n], Mpy_32_32( p_data[n], INV_SQRT_2_Q31 ) ); // Q26
2354 2400 : move32();
2355 : }
2356 : }
2357 :
2358 4200 : p_curr_taps = hPHA->p_curr_taps_fx[k];
2359 4200 : IF( p_curr_taps )
2360 : {
2361 3362598 : FOR( n = 0; n < n_samples; n++ )
2362 : {
2363 164561600 : FOR( ( fx_tmp = 0, m = 0 ); m < pha_len; m++ )
2364 : {
2365 : // ftmp += p_data_mem[n - m] * p_curr_taps[m];
2366 161203200 : fx_tmp = L_add( fx_tmp, Mpy_32_32( p_data_mem[n - m], p_curr_taps[m] ) ); // Q25
2367 : }
2368 3358400 : fx_tmp = L_shl( fx_tmp, 1 ); // Q26
2369 : // dmx_pha_data[n] += ftmp * INV_SQRT_2;
2370 3358400 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( fx_tmp, INV_SQRT_2_Q31 ) ); // Q26
2371 3358400 : move32();
2372 : }
2373 : }
2374 : ELSE
2375 : {
2376 1602 : FOR( n = 0; n < n_samples; n++ )
2377 : {
2378 : // dmx_pha_data[n] += p_data[n] * INV_SQRT_2;
2379 1600 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( p_data[n], INV_SQRT_2_Q31 ) ); // Q26
2380 1600 : move32();
2381 : }
2382 : }
2383 : }
2384 :
2385 842100 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len; ( n++, m-- ) )
2386 : {
2387 840000 : dmx_pha_data[n] = Mpy_32_32( dmx_pha_data[n], fad_g[n] );
2388 840000 : move32();
2389 840000 : dmx_pha_data[n] = L_add( dmx_pha_data[n], Mpy_32_32( mem_prev[n], fad_g[m] ) ); // Q26
2390 840000 : move32();
2391 : }
2392 :
2393 : /* prc switch */
2394 :
2395 2100 : prev_prc = hPHA->curr_prc;
2396 2100 : move32();
2397 : // if ( abs( (int16_t) hStereoDmxEVS->itd ) > hPHA->prc_thres )
2398 2100 : IF( GT_16( abs_s( hStereoDmxEVS->itd_fx ), hPHA->prc_thres ) )
2399 : {
2400 557 : IF( NE_32( hPHA->curr_prc, STEREO_DMX_EVS_PRC_POC ) )
2401 : {
2402 6 : IF( EQ_32( hPHA->prev_prc, STEREO_DMX_EVS_PRC_POC ) )
2403 : {
2404 3 : hPHA->prc_hys_cnt = add( hPHA->prc_hys_cnt, 1 );
2405 3 : move16();
2406 : }
2407 : ELSE
2408 : {
2409 3 : hPHA->prc_hys_cnt = 0;
2410 3 : move16();
2411 : }
2412 6 : IF( GE_16( hPHA->prc_hys_cnt, STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES ) )
2413 : {
2414 3 : hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
2415 3 : move32();
2416 : }
2417 : }
2418 557 : hPHA->prev_prc = STEREO_DMX_EVS_PRC_POC;
2419 557 : move32();
2420 : }
2421 : ELSE
2422 : {
2423 1543 : IF( NE_32( hPHA->curr_prc, STEREO_DMX_EVS_PRC_PHA ) )
2424 : {
2425 1303 : IF( EQ_32( hPHA->prev_prc, STEREO_DMX_EVS_PRC_PHA ) )
2426 : {
2427 1294 : hPHA->prc_hys_cnt = add( hPHA->prc_hys_cnt, 1 );
2428 1294 : move16();
2429 : }
2430 : ELSE
2431 : {
2432 9 : hPHA->prc_hys_cnt = 0;
2433 9 : move16();
2434 : }
2435 :
2436 1303 : IF( GE_16( hPHA->prc_hys_cnt, STEREO_DMX_EVS_SWTCH_PRC_HYS_THRES ) )
2437 : {
2438 1294 : hPHA->curr_prc = STEREO_DMX_EVS_PRC_PHA;
2439 1294 : move32();
2440 : }
2441 : }
2442 1543 : hPHA->prev_prc = STEREO_DMX_EVS_PRC_PHA;
2443 1543 : move32();
2444 : }
2445 :
2446 : // 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] ) || ( ( hPHA->p_curr_taps[0] == NULL ) && ( hPHA->p_curr_taps[1] == NULL ) ) )
2447 2100 : test();
2448 2100 : test();
2449 2100 : test();
2450 2100 : test();
2451 2100 : IF( EQ_16( is_transient, 1 ) ||
2452 : 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 ||
2453 : 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 ||
2454 : EQ_16( hPHA->force_poc, TRUE ) )
2455 : {
2456 1676 : hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
2457 1676 : move32();
2458 1676 : hPHA->prc_hys_cnt = 0;
2459 1676 : move16();
2460 : }
2461 :
2462 2100 : calc_energy_sgc( dmx_poc_data, &( hPHA->dmx_poc_ener_fx ), &( hPHA->dmx_poc_ener_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2463 2100 : calc_energy_sgc( dmx_pha_data, &( hPHA->dmx_pha_ener_fx ), &( hPHA->dmx_pha_ener_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2464 :
2465 2100 : IF( NE_32( prev_prc, hPHA->curr_prc ) && EQ_16( is_transient, 0 ) && !( ( BASOP_Util_Cmp_Mant32Exp( hPHA->dmx_pha_ener_fx, hPHA->dmx_pha_ener_fx_e, hPHA->low_egy_thres_sgc, hPHA->low_egy_thres_sgc_e ) < 0 ) && ( BASOP_Util_Cmp_Mant32Exp( hPHA->dmx_poc_ener_fx, hPHA->dmx_poc_ener_fx_e, hPHA->low_egy_thres_sgc, hPHA->low_egy_thres_sgc_e ) < 0 ) ) )
2466 : {
2467 11 : IF( EQ_32( hPHA->curr_prc, STEREO_DMX_EVS_PRC_POC ) )
2468 : {
2469 6 : apply_gain_sgc( dmx_pha_data, &( hPHA->dmx_pha_gain_sgc_fx ), n_samples );
2470 6 : calc_energy_sgc( dmx_pha_data, &( hPHA->dmx_pha_ener_sgc_fx ), &( hPHA->dmx_pha_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2471 :
2472 6 : L_tmp1 = BASOP_Util_Add_Mant32Exp( hPHA->dmx_poc_ener_fx, hPHA->dmx_poc_ener_fx_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp1_e );
2473 6 : BASOP_Util_Divide_MantExp( extract_h( hPHA->dmx_pha_ener_sgc_fx ), hPHA->dmx_pha_ener_sgc_fx_e, extract_h( L_tmp1 ), L_tmp1_e, &dmx_gain_sgc, &L_tmp2_e );
2474 6 : dmx_gain_sgc = Sqrt16( dmx_gain_sgc, &L_tmp2_e );
2475 6 : hPHA->dmx_poc_gain_sgc_fx = L_shl( L_deposit_l( dmx_gain_sgc ), L_tmp2_e ); // Q15
2476 6 : hPHA->dmx_poc_gain_sgc_fx = L_min( hPHA->dmx_poc_gain_sgc_fx, STEREO_DMX_EVS_SGC_GMAX );
2477 6 : hPHA->dmx_poc_gain_sgc_fx = L_max( hPHA->dmx_poc_gain_sgc_fx, STEREO_DMX_EVS_SGC_GMIN );
2478 :
2479 6 : apply_gain_sgc( dmx_poc_data, &( hPHA->dmx_poc_gain_sgc_fx ), n_samples );
2480 6 : calc_energy_sgc( dmx_poc_data, &( hPHA->dmx_poc_ener_sgc_fx ), &( hPHA->dmx_poc_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2481 : }
2482 : ELSE
2483 : {
2484 5 : apply_gain_sgc( dmx_poc_data, &( hPHA->dmx_poc_gain_sgc_fx ), n_samples );
2485 5 : calc_energy_sgc( dmx_poc_data, &( hPHA->dmx_poc_ener_sgc_fx ), &( hPHA->dmx_poc_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2486 :
2487 5 : L_tmp1 = BASOP_Util_Add_Mant32Exp( hPHA->dmx_pha_ener_fx, hPHA->dmx_pha_ener_fx_e, EPSILON_FX_M, EPSILON_FX_E, &L_tmp1_e );
2488 5 : BASOP_Util_Divide_MantExp( extract_h( hPHA->dmx_poc_ener_sgc_fx ), hPHA->dmx_poc_ener_sgc_fx_e, extract_h( L_tmp1 ), L_tmp1_e, &dmx_gain_sgc, &L_tmp2_e );
2489 5 : dmx_gain_sgc = Sqrt16( dmx_gain_sgc, &L_tmp2_e );
2490 5 : hPHA->dmx_pha_gain_sgc_fx = L_shl( L_deposit_l( dmx_gain_sgc ), L_tmp2_e ); // Q15
2491 5 : hPHA->dmx_pha_gain_sgc_fx = L_min( hPHA->dmx_pha_gain_sgc_fx, STEREO_DMX_EVS_SGC_GMAX );
2492 5 : hPHA->dmx_pha_gain_sgc_fx = L_max( hPHA->dmx_pha_gain_sgc_fx, STEREO_DMX_EVS_SGC_GMIN );
2493 :
2494 5 : apply_gain_sgc( dmx_pha_data, &( hPHA->dmx_pha_gain_sgc_fx ), n_samples );
2495 5 : calc_energy_sgc( dmx_pha_data, &( hPHA->dmx_pha_ener_sgc_fx ), &( hPHA->dmx_pha_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2496 : }
2497 : }
2498 : ELSE
2499 : {
2500 2089 : apply_gain_sgc( dmx_poc_data, &( hPHA->dmx_poc_gain_sgc_fx ), n_samples );
2501 2089 : calc_energy_sgc( dmx_poc_data, &( hPHA->dmx_poc_ener_sgc_fx ), &( hPHA->dmx_poc_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2502 :
2503 2089 : apply_gain_sgc( dmx_pha_data, &( hPHA->dmx_pha_gain_sgc_fx ), n_samples );
2504 2089 : calc_energy_sgc( dmx_pha_data, &( hPHA->dmx_pha_ener_sgc_fx ), &( hPHA->dmx_pha_ener_sgc_fx_e ), n_samples, STEREO_DMX_EVS_SGC_EGY_FORGETTING_Q15 );
2505 : }
2506 :
2507 2100 : IF( EQ_32( hPHA->curr_prc, STEREO_DMX_EVS_PRC_POC ) )
2508 : {
2509 1854 : p_dmx_data = dmx_poc_data;
2510 1854 : p_dmx_data_fo = dmx_pha_data;
2511 : }
2512 : ELSE
2513 : {
2514 246 : p_dmx_data = dmx_pha_data;
2515 246 : p_dmx_data_fo = dmx_poc_data;
2516 : }
2517 :
2518 2100 : IF( EQ_16( is_transient, 1 ) )
2519 : {
2520 23 : n_fad_r = 1;
2521 23 : move16();
2522 : }
2523 : ELSE
2524 : {
2525 2077 : n_fad_r = STEREO_DMX_EVS_FAD_R;
2526 2077 : move16();
2527 : }
2528 :
2529 2100 : IF( NE_32( prev_prc, hPHA->curr_prc ) )
2530 : {
2531 32 : IF( EQ_16( hPHA->n_fad_g, input_frame ) )
2532 : {
2533 30 : hPHA->n_fad_g = 0;
2534 30 : hPHA->n_fad_cnt = 0;
2535 30 : move16();
2536 30 : move16();
2537 : }
2538 : ELSE
2539 : {
2540 2 : hPHA->n_fad_g = sub( input_frame, add( hPHA->n_fad_g, 1 ) );
2541 2 : hPHA->n_fad_cnt = 0;
2542 2 : move16();
2543 : }
2544 : }
2545 2068 : ELSE IF( is_transient )
2546 : {
2547 21 : hPHA->n_fad_cnt = 0;
2548 21 : move16();
2549 : }
2550 :
2551 2100 : fad_len = s_min( n_samples, sub( mult0( input_frame, n_fad_r ), add( mult0( hPHA->n_fad_g, n_fad_r ), hPHA->n_fad_cnt ) ) );
2552 :
2553 2100 : IF( NE_16( fad_len, 0 ) )
2554 : {
2555 90 : fad_g = hPHA->fad_g_prc_fx;
2556 90 : move32();
2557 90 : n_fad_g = hPHA->n_fad_g;
2558 90 : n_fad_cnt = hPHA->n_fad_cnt;
2559 90 : move16();
2560 90 : move16();
2561 90 : m_fad_g = sub( input_frame, add( n_fad_g, 1 ) );
2562 :
2563 90 : IF( EQ_16( n_fad_r, 1 ) )
2564 : {
2565 2 : n_fad_cnt = 0;
2566 2 : move16();
2567 1602 : FOR( n = 0; n < fad_len; n++ )
2568 : {
2569 1600 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n_fad_g] ); // Q26;
2570 1600 : n_fad_g = add( n_fad_g, 1 );
2571 :
2572 1600 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m_fad_g], p_dmx_data_fo[n] ) ); // Q26
2573 1600 : m_fad_g = sub( m_fad_g, 1 );
2574 : }
2575 : }
2576 : ELSE
2577 : {
2578 88 : n = 0;
2579 88 : sbfad_len = 0;
2580 88 : move16();
2581 88 : move16();
2582 88 : IF( NE_16( n_fad_cnt, 0 ) )
2583 : {
2584 25 : sbfad_len = s_min( fad_len, sub( n_fad_r, n_fad_cnt ) );
2585 63 : FOR( ; n < sbfad_len; n++ )
2586 : {
2587 38 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n_fad_g] ); // Q26;
2588 38 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m_fad_g], p_dmx_data_fo[n] ) ); // Q26
2589 : }
2590 25 : n_fad_cnt = 0;
2591 25 : move16();
2592 25 : n_fad_g = add( n_fad_g, 1 );
2593 25 : m_fad_g = sub( m_fad_g, 1 );
2594 : }
2595 :
2596 88 : sbfad_len = sub( fad_len, sbfad_len );
2597 88 : IF( EQ_16( n_fad_r, STEREO_DMX_EVS_FAD_R ) )
2598 : {
2599 88 : sbfad_len = L_mult( sbfad_len, STEREO_DMX_EVS_FAD_IR ) >> 16;
2600 : }
2601 :
2602 23637 : FOR( k = 0; k < sbfad_len; k++ )
2603 : {
2604 94196 : FOR( m = 0; m < n_fad_r; m++ )
2605 : {
2606 70647 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n_fad_g] ); // Q26;
2607 70647 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m_fad_g], p_dmx_data_fo[n] ) ); // Q26
2608 70647 : n = add( n, 1 );
2609 : }
2610 23549 : n_fad_g = add( n_fad_g, 1 );
2611 23549 : m_fad_g = sub( m_fad_g, 1 );
2612 : }
2613 :
2614 127 : FOR( ; n < fad_len; n++ )
2615 : {
2616 39 : p_dmx_data[n] = Mpy_32_32( p_dmx_data[n], fad_g[n_fad_g] ); // Q26;
2617 39 : p_dmx_data[n] = L_add( p_dmx_data[n], Mpy_32_32( fad_g[m_fad_g], p_dmx_data_fo[n] ) ); // Q26
2618 39 : n_fad_cnt = add( n_fad_cnt, 1 );
2619 39 : IF( GE_16( n_fad_cnt, n_fad_r ) )
2620 : {
2621 0 : n_fad_cnt = 0;
2622 0 : move16();
2623 0 : n_fad_g = add( n_fad_g, 1 );
2624 0 : m_fad_g = sub( m_fad_g, 1 );
2625 : }
2626 : }
2627 : }
2628 :
2629 90 : hPHA->n_fad_g = n_fad_g;
2630 90 : hPHA->n_fad_cnt = n_fad_cnt;
2631 90 : move16();
2632 90 : move16();
2633 : }
2634 :
2635 2100 : Copy_Scale_sig32_16( p_dmx_data, data, n_samples, 5 ); // Q26->Q15
2636 :
2637 2100 : pop_wmops();
2638 :
2639 2100 : return;
2640 : }
2641 :
2642 :
2643 : /*-------------------------------------------------------------------*
2644 : * stereo_dmx_evs_init_encoder()
2645 : *
2646 : * open and initialize stereo downmix for EVS encoder
2647 : *-------------------------------------------------------------------*/
2648 2 : ivas_error stereo_dmx_evs_init_encoder_fx(
2649 : STEREO_DMX_EVS_ENC_HANDLE *hStereoDmxEVS_out, /* o : Stereo downmix for EVS encoder handle */
2650 : const Word32 input_Fs /* i : input sampling rate */
2651 : )
2652 : {
2653 : STEREO_DMX_EVS_ENC_HANDLE hStereoDmxEVS;
2654 : Word16 n, input_frame;
2655 :
2656 : Word16 m, len, pha_len, fad_len, fad_len2, rfft_ipd_coef_step, n0, input_frame_pha;
2657 : Word32 *fad_g, fad_r;
2658 : Word16 *win;
2659 : const Word16 *p_ipd_w;
2660 : Word16 tmp_e;
2661 :
2662 : // input_frame = (Word16) ( input_Fs / FRAMES_PER_SEC );
2663 2 : SWITCH( input_Fs )
2664 : {
2665 0 : case 8000:
2666 0 : input_frame = 160;
2667 0 : BREAK;
2668 0 : case 16000:
2669 0 : input_frame = 320;
2670 0 : BREAK;
2671 1 : case 32000:
2672 1 : input_frame = 640;
2673 1 : BREAK;
2674 1 : case 48000:
2675 1 : input_frame = 960;
2676 1 : BREAK;
2677 0 : default:
2678 0 : input_frame = 960;
2679 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2680 0 : BREAK;
2681 : }
2682 2 : move16();
2683 2 : hStereoDmxEVS = NULL;
2684 2 : IF( ( hStereoDmxEVS = (STEREO_DMX_EVS_ENC_HANDLE) malloc( sizeof( STEREO_DMX_EVS_ENC_DATA ) ) ) == NULL )
2685 : {
2686 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_ENC_DATA\n" ) );
2687 : }
2688 2 : hStereoDmxEVS->itd_fx = 0;
2689 2 : move16();
2690 2 : hStereoDmxEVS->pre_dmx_energy_fx[0] = 0;
2691 2 : move32();
2692 2 : hStereoDmxEVS->pre_dmx_energy_fx_e[0] = 0;
2693 2 : move16();
2694 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2695 : {
2696 4 : hStereoDmxEVS->aux_dmx_energy_fx[n] = 0;
2697 4 : move32();
2698 4 : hStereoDmxEVS->aux_dmx_energy_fx_e[n] = 0;
2699 4 : move16();
2700 : }
2701 :
2702 2 : hStereoDmxEVS->dmx_weight_fx[0] = ONE_IN_Q30; // 0.5f;
2703 2 : move32();
2704 2 : hStereoDmxEVS->dmx_weight_fx[1] = 0;
2705 2 : move32();
2706 2 : hStereoDmxEVS->dmx_weight_fx[2] = 0;
2707 2 : move32();
2708 :
2709 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2710 : {
2711 0 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_16k_fx;
2712 : }
2713 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2714 : {
2715 1 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_32k_fx;
2716 : }
2717 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2718 : {
2719 1 : hStereoDmxEVS->s_wnd_fx = Stereo_dmx_s_wnd_coef_48k_fx;
2720 : }
2721 : ELSE
2722 : {
2723 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2724 : }
2725 :
2726 2 : hStereoDmxEVS->hPOC = NULL;
2727 2 : IF( ( hStereoDmxEVS->hPOC = (STEREO_DMX_EVS_POC_HANDLE) malloc( sizeof( STEREO_DMX_EVS_POC_DATA ) ) ) == NULL )
2728 : {
2729 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_POC_DATA\n" ) );
2730 : }
2731 :
2732 : // hStereoDmxEVS->hPOC->shift_limit = (int16_t) ( STEREO_DMX_EVS_SHIFT_LIMIT * input_Fs / 1000 );
2733 2 : SWITCH( input_Fs )
2734 : {
2735 0 : case 8000:
2736 0 : hStereoDmxEVS->hPOC->shift_limit = 45;
2737 0 : move16();
2738 0 : BREAK;
2739 0 : case 16000:
2740 0 : hStereoDmxEVS->hPOC->shift_limit = 90;
2741 0 : move16();
2742 0 : BREAK;
2743 1 : case 32000:
2744 1 : hStereoDmxEVS->hPOC->shift_limit = 180;
2745 1 : move16();
2746 1 : BREAK;
2747 1 : case 48000:
2748 1 : hStereoDmxEVS->hPOC->shift_limit = 270;
2749 1 : move16();
2750 1 : BREAK;
2751 0 : default:
2752 0 : IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sample rate\n" );
2753 0 : BREAK;
2754 : }
2755 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2756 : {
2757 4 : hStereoDmxEVS->hPOC->peakQ_fx[n] = 0;
2758 4 : move32();
2759 : // hStereoDmxEVS->hPOC->peak_width_fx[n] = (float) hStereoDmxEVS->hPOC->shift_limit / 2;
2760 4 : hStereoDmxEVS->hPOC->peak_width_fx[n] = L_shl( hStereoDmxEVS->hPOC->shift_limit, Q15 ); // Q16
2761 4 : move32();
2762 4 : hStereoDmxEVS->hPOC->ispeak[n] = 0;
2763 4 : move16();
2764 4 : hStereoDmxEVS->hPOC->itdLR[n] = 0;
2765 4 : move16();
2766 : }
2767 2 : set32_fx( hStereoDmxEVS->hPOC->P_fx, 0, L_FRAME48k );
2768 :
2769 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2770 : {
2771 0 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_48k_fx;
2772 : }
2773 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2774 : {
2775 1 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_32k_fx;
2776 : }
2777 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2778 : {
2779 1 : hStereoDmxEVS->hPOC->wnd_fx = Stereo_dmx_wnd_coef_48k_fx;
2780 : }
2781 : ELSE
2782 : {
2783 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2784 : }
2785 : // hStereoDmxEVS->hPOC->eps = 2.0f * EVS_PI / ( (float) input_frame );
2786 2 : SWITCH( input_frame )
2787 : {
2788 0 : case 160:
2789 0 : hStereoDmxEVS->hPOC->eps_fx = 84331486; // (2.0f * EVS_PI / (160)) in Q31
2790 0 : BREAK;
2791 0 : case 320:
2792 0 : hStereoDmxEVS->hPOC->eps_fx = 42165743; // (2.0f * EVS_PI / (160)) in Q31
2793 0 : BREAK;
2794 1 : case 640:
2795 1 : hStereoDmxEVS->hPOC->eps_fx = 21082871; // (2.0f * EVS_PI / (160)) in Q31
2796 1 : BREAK;
2797 1 : case 960:
2798 1 : hStereoDmxEVS->hPOC->eps_fx = 14055248; // (2.0f * EVS_PI / (160)) in Q31
2799 1 : BREAK;
2800 : }
2801 2 : move32();
2802 :
2803 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2804 : {
2805 0 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_32k_fx;
2806 : }
2807 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2808 : {
2809 1 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_32k_fx;
2810 : }
2811 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
2812 : {
2813 1 : hStereoDmxEVS->hPOC->sin_fx = dft_trigo_48k_fx;
2814 : }
2815 : ELSE
2816 : {
2817 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid frame length\n" );
2818 : }
2819 :
2820 2 : hStereoDmxEVS->hPOC->confidence_fx = 0;
2821 2 : move32();
2822 :
2823 2 : hStereoDmxEVS->hPHA = NULL;
2824 2 : IF( ( hStereoDmxEVS->hPHA = (STEREO_DMX_EVS_PHA_HANDLE) malloc( sizeof( STEREO_DMX_EVS_PHA_DATA ) ) ) == NULL )
2825 : {
2826 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for STEREO_DMX_EVS_CORFILT_DATA\n" ) );
2827 : }
2828 :
2829 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
2830 : {
2831 4 : hStereoDmxEVS->hPHA->p_curr_taps_fx[n] = NULL;
2832 4 : hStereoDmxEVS->hPHA->p_prev_taps_fx[n] = NULL;
2833 :
2834 4 : set_zero_fx( hStereoDmxEVS->hPHA->data_mem_fx[n], STEREO_DMX_EVS_PHA_LEN_MAX );
2835 4 : set_zero_fx( hStereoDmxEVS->hPHA->curr_taps_fx[n], STEREO_DMX_EVS_PHA_LEN_MAX );
2836 : }
2837 :
2838 2 : IF( EQ_32( input_Fs, 16000 ) )
2839 : {
2840 0 : len = STEREO_DMX_EVS_PHA_LEN_16;
2841 0 : move16();
2842 0 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_16;
2843 0 : move16();
2844 0 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_16;
2845 0 : move16();
2846 0 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_16_Q0;
2847 0 : move32();
2848 0 : hStereoDmxEVS->hPHA->low_egy_thres_sgc = STEREO_DMX_EVS_SGC_LEGY_THRES_16;
2849 0 : hStereoDmxEVS->hPHA->low_egy_thres_sgc_e = STEREO_DMX_EVS_SGC_LEGY_THRES_E;
2850 0 : move32();
2851 0 : move16();
2852 : }
2853 2 : ELSE IF( EQ_32( input_Fs, 32000 ) )
2854 : {
2855 1 : len = STEREO_DMX_EVS_PHA_LEN_32;
2856 1 : move16();
2857 1 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_32;
2858 1 : move16();
2859 1 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_32;
2860 1 : move16();
2861 1 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_32_Q0;
2862 1 : move32();
2863 1 : hStereoDmxEVS->hPHA->low_egy_thres_sgc = STEREO_DMX_EVS_SGC_LEGY_THRES_32;
2864 1 : hStereoDmxEVS->hPHA->low_egy_thres_sgc_e = STEREO_DMX_EVS_SGC_LEGY_THRES_E;
2865 1 : move32();
2866 1 : move16();
2867 : }
2868 1 : ELSE IF( EQ_32( input_Fs, 48000 ) )
2869 : {
2870 1 : len = STEREO_DMX_EVS_PHA_LEN_48;
2871 1 : move16();
2872 1 : hStereoDmxEVS->hPHA->fad_len = STEREO_DMX_EVS_FAD_LEN_48;
2873 1 : move16();
2874 1 : hStereoDmxEVS->hPHA->prc_thres = STEREO_DMX_EVS_SWTCH_PRC_THRES_48;
2875 1 : move16();
2876 1 : hStereoDmxEVS->hPHA->crst_fctr_fx = STEREO_DMX_EVS_CRST_FCTR_48_Q0;
2877 1 : move32();
2878 1 : hStereoDmxEVS->hPHA->low_egy_thres_sgc = STEREO_DMX_EVS_SGC_LEGY_THRES_48;
2879 1 : hStereoDmxEVS->hPHA->low_egy_thres_sgc_e = STEREO_DMX_EVS_SGC_LEGY_THRES_E;
2880 1 : move32();
2881 1 : move16();
2882 : }
2883 : ELSE
2884 : {
2885 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sampling frequency\n" );
2886 : }
2887 :
2888 2 : hStereoDmxEVS->hPHA->pha_len = shr( len, 1 );
2889 2 : move16();
2890 2 : hStereoDmxEVS->hPHA->isd_rate_s_fx = 0;
2891 2 : move32();
2892 2 : hStereoDmxEVS->hPHA->iccr_s_fx = 0;
2893 2 : move32();
2894 :
2895 2 : pha_len = hStereoDmxEVS->hPHA->pha_len;
2896 2 : move16();
2897 2 : fad_len = hStereoDmxEVS->hPHA->fad_len;
2898 2 : move16();
2899 :
2900 2 : set16_fx( hStereoDmxEVS->hPHA->win_fx, 29491 /*1.8f in Q14*/, pha_len );
2901 2 : hStereoDmxEVS->hPHA->win_fx[0] = ONE_IN_Q14;
2902 2 : move16();
2903 2 : IF( EQ_32( input_Fs, 16000 ) )
2904 : {
2905 0 : hStereoDmxEVS->hPHA->win_fx[sub( pha_len, 1 )] = 7373; /*0.45f in Q14*/
2906 0 : move16();
2907 : }
2908 2 : ELSE IF( EQ_32( input_Fs, 32000 ) || EQ_32( input_Fs, 48000 ) )
2909 : {
2910 2 : hStereoDmxEVS->hPHA->win_fx[sub( pha_len, 2 )] = 19302; /*1.1781f in Q14*/
2911 2 : move16();
2912 2 : hStereoDmxEVS->hPHA->win_fx[sub( pha_len, 1 )] = 2816; /*0.1718f in Q14*/
2913 2 : move16();
2914 : }
2915 :
2916 2 : fad_g = hStereoDmxEVS->hPHA->fad_g_fx;
2917 : // fad_r = 1.0f / (float) ( fad_len + 1 );
2918 2 : SWITCH( fad_len )
2919 : {
2920 0 : case STEREO_DMX_EVS_FAD_LEN_16:
2921 0 : fad_r = 853658096;
2922 0 : move32();
2923 0 : tmp_e = -6;
2924 0 : move16();
2925 0 : BREAK;
2926 1 : case STEREO_DMX_EVS_FAD_LEN_32:
2927 1 : fad_r = 856317467;
2928 1 : move32();
2929 1 : tmp_e = -7;
2930 1 : move16();
2931 1 : BREAK;
2932 1 : case STEREO_DMX_EVS_FAD_LEN_48:
2933 1 : fad_r = 571471740;
2934 1 : move32();
2935 1 : tmp_e = -7;
2936 1 : move16();
2937 1 : BREAK;
2938 0 : default:
2939 0 : fad_r = BASOP_Util_Divide3232_Scale_newton( 1, add( fad_len, 1 ), &tmp_e );
2940 0 : BREAK;
2941 : }
2942 2 : fad_r = L_shl_r( fad_r, tmp_e );
2943 2 : fad_len2 = shr( fad_len, 1 );
2944 402 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len2; ( n++, m-- ) )
2945 : {
2946 400 : fad_g[n] = imult3216( fad_r, add( n, 1 ) );
2947 400 : move32();
2948 400 : fad_g[m] = L_sub( MAX_32, fad_g[n] );
2949 400 : move32();
2950 : }
2951 :
2952 2 : hStereoDmxEVS->hPHA->curr_pha = STEREO_DMX_EVS_PHA_IPD;
2953 2 : move32();
2954 2 : hStereoDmxEVS->hPHA->prev_pha = STEREO_DMX_EVS_PHA_IPD;
2955 2 : move32();
2956 2 : hStereoDmxEVS->hPHA->pha_hys_cnt = 0;
2957 2 : move16();
2958 :
2959 : /* Compute the forgetting factor */
2960 : // replaced below logic with table as it is same for all frame lengths
2961 2 : Copy32( ipd_ff_Q31, hStereoDmxEVS->hPHA->ipd_ff_fx, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2962 :
2963 2 : hStereoDmxEVS->hPHA->proc_pha = STEREO_DMX_EVS_PHA_IPD;
2964 2 : hStereoDmxEVS->hPHA->force_poc = FALSE;
2965 2 : move16();
2966 2 : move32();
2967 :
2968 2 : hStereoDmxEVS->hPHA->pha_ipd_chan_cnt = 0;
2969 2 : move16();
2970 2 : hStereoDmxEVS->hPHA->pha_ipd_chan_thresh = 10;
2971 2 : move16();
2972 2 : hStereoDmxEVS->hPHA->pha_ipd_chan2rephase = 1;
2973 2 : move16();
2974 2 : hStereoDmxEVS->hPHA->pha_ipd_previouschan2rephase = 1;
2975 2 : move16();
2976 2 : hStereoDmxEVS->hPHA->pha_ipd_chanswitch = 0;
2977 2 : move16();
2978 2 : hStereoDmxEVS->hPHA->pha_ipd_chanswitch_allowed = 0;
2979 2 : move16();
2980 :
2981 2 : set32_fx( hStereoDmxEVS->hPHA->Pr_fx, MAX_32, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2982 2 : set_zero_fx( hStereoDmxEVS->hPHA->Pi_fx, STEREO_DMX_EVS_NB_SUBBAND_MAX );
2983 :
2984 : // n0 = input_frame / ( 4 * STEREO_DMX_EVS_SUBBAND_SIZE );
2985 2 : n0 = shr( input_frame, 3 );
2986 : // input_frame_pha = input_frame / ( 2 * STEREO_DMX_EVS_SUBBAND_SIZE );
2987 2 : input_frame_pha = shr( input_frame, 2 );
2988 :
2989 2 : IF( EQ_16( input_frame, L_FRAME16k ) )
2990 : {
2991 0 : p_ipd_w = dft_trigo_32k_fx;
2992 0 : rfft_ipd_coef_step = 4;
2993 0 : move16();
2994 : }
2995 2 : ELSE IF( EQ_16( input_frame, L_FRAME32k ) )
2996 : {
2997 1 : p_ipd_w = dft_trigo_32k_fx;
2998 1 : rfft_ipd_coef_step = 2;
2999 1 : move16();
3000 : }
3001 1 : ELSE IF( EQ_16( input_frame, L_FRAME48k ) )
3002 : {
3003 1 : p_ipd_w = dft_trigo_48k_fx;
3004 1 : rfft_ipd_coef_step = 2;
3005 1 : move16();
3006 : }
3007 : ELSE
3008 : {
3009 0 : return IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "invalid sampling frequency\n" );
3010 : }
3011 :
3012 2 : win = hStereoDmxEVS->hPHA->rfft_ipd_coef_fx;
3013 2 : len = imult1616( rfft_ipd_coef_step, STEREO_DMX_EVS_SUBBAND_SIZE );
3014 202 : FOR( n = 0; n < n0; n++ )
3015 : {
3016 200 : win[n] = p_ipd_w[n * len];
3017 200 : move16();
3018 200 : win[sub( input_frame_pha, n )] = p_ipd_w[n * len];
3019 200 : move16();
3020 : }
3021 2 : win[n0] = p_ipd_w[n0 * len];
3022 2 : move16();
3023 :
3024 2 : hStereoDmxEVS->hPHA->curr_prc = STEREO_DMX_EVS_PRC_POC;
3025 2 : move32();
3026 2 : hStereoDmxEVS->hPHA->prev_prc = STEREO_DMX_EVS_PRC_POC;
3027 2 : move32();
3028 2 : hStereoDmxEVS->hPHA->prc_hys_cnt = 0;
3029 2 : move16();
3030 :
3031 2 : fad_len = input_frame;
3032 2 : move16();
3033 :
3034 2 : fad_g = hStereoDmxEVS->hPHA->fad_g_prc_fx;
3035 2 : move32();
3036 : // fad_r = 1.0f / (float) ( fad_len + 1 );
3037 2 : SWITCH( fad_len )
3038 : {
3039 0 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 16:
3040 0 : fad_r = 856317467;
3041 0 : move32();
3042 0 : tmp_e = -7;
3043 0 : move16();
3044 0 : BREAK;
3045 1 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 32:
3046 1 : fad_r = 857653375;
3047 1 : move32();
3048 1 : tmp_e = -8;
3049 1 : move16();
3050 1 : BREAK;
3051 1 : case STEREO_DMX_EVS_FADE_LEN_PRC_Q0 * 48:
3052 1 : fad_r = 572066403;
3053 1 : move32();
3054 1 : tmp_e = -8;
3055 1 : move16();
3056 1 : BREAK;
3057 0 : default:
3058 0 : fad_r = BASOP_Util_Divide3232_Scale_newton( 1, add( fad_len, 1 ), &tmp_e );
3059 0 : BREAK;
3060 : }
3061 2 : fad_r = L_shl_r( fad_r, tmp_e );
3062 2 : fad_len2 = shr( fad_len, 1 );
3063 802 : FOR( ( n = 0, m = ( fad_len - 1 ) ); n < fad_len2; ( n++, m-- ) )
3064 : {
3065 800 : fad_g[n] = imult3216( fad_r, add( n, 1 ) );
3066 800 : move32();
3067 800 : fad_g[m] = L_sub( MAX_32, fad_g[n] );
3068 800 : move32();
3069 : }
3070 :
3071 6 : FOR( n = 0; n < CPE_CHANNELS; n++ )
3072 : {
3073 4 : hStereoDmxEVS->hPHA->trns_aux_energy_fx[n] = 0;
3074 4 : move32();
3075 4 : hStereoDmxEVS->hPHA->trns_aux_energy_fx_e[n] = 0;
3076 4 : move16();
3077 : }
3078 :
3079 :
3080 2 : hStereoDmxEVS->hPHA->dmx_poc_ener_fx = 0;
3081 2 : move32();
3082 2 : hStereoDmxEVS->hPHA->dmx_poc_ener_fx_e = 0;
3083 2 : move16();
3084 2 : hStereoDmxEVS->hPHA->dmx_pha_ener_fx = 0;
3085 2 : move32();
3086 2 : hStereoDmxEVS->hPHA->dmx_pha_ener_fx_e = 0;
3087 2 : move16();
3088 :
3089 2 : hStereoDmxEVS->hPHA->dmx_poc_ener_sgc_fx = 0;
3090 2 : move32();
3091 2 : hStereoDmxEVS->hPHA->dmx_poc_ener_sgc_fx_e = 0;
3092 2 : move16();
3093 2 : hStereoDmxEVS->hPHA->dmx_pha_ener_sgc_fx = 0;
3094 2 : move32();
3095 2 : hStereoDmxEVS->hPHA->dmx_pha_ener_sgc_fx_e = 0;
3096 2 : move16();
3097 :
3098 2 : hStereoDmxEVS->hPHA->dmx_pha_gain_sgc_fx = ONE_IN_Q15;
3099 2 : hStereoDmxEVS->hPHA->dmx_poc_gain_sgc_fx = ONE_IN_Q15;
3100 2 : move32();
3101 2 : move32();
3102 :
3103 2 : hStereoDmxEVS->hPHA->n_fad_g = input_frame;
3104 2 : hStereoDmxEVS->hPHA->n_fad_cnt = 0;
3105 2 : move16();
3106 2 : move16();
3107 :
3108 :
3109 2 : *hStereoDmxEVS_out = hStereoDmxEVS;
3110 :
3111 2 : return IVAS_ERR_OK;
3112 : }
3113 :
3114 :
3115 : /*-------------------------------------------------------------------*
3116 : * stereo_dmx_evs_close_encoder()
3117 : *
3118 : * close stereo downmix for EVS encoder
3119 : *-------------------------------------------------------------------*/
3120 628 : void stereo_dmx_evs_close_encoder_fx(
3121 : STEREO_DMX_EVS_ENC_HANDLE *hStereoDmxEVS /* i/o: Stereo downmix for EVS encoder handle */
3122 : )
3123 : {
3124 628 : test();
3125 628 : IF( hStereoDmxEVS == NULL || *hStereoDmxEVS == NULL )
3126 : {
3127 626 : return;
3128 : }
3129 :
3130 2 : IF( ( *hStereoDmxEVS )->hPOC != NULL )
3131 : {
3132 2 : free( ( *hStereoDmxEVS )->hPOC );
3133 2 : ( *hStereoDmxEVS )->hPOC = NULL;
3134 : }
3135 :
3136 2 : IF( ( *hStereoDmxEVS )->hPHA != NULL )
3137 : {
3138 2 : free( ( *hStereoDmxEVS )->hPHA );
3139 2 : ( *hStereoDmxEVS )->hPHA = NULL;
3140 : }
3141 :
3142 2 : free( ( *hStereoDmxEVS ) );
3143 2 : ( *hStereoDmxEVS ) = NULL;
3144 :
3145 2 : return;
3146 : }
|
