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