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 : /*====================================================================================
34 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
35 : ====================================================================================*/
36 :
37 : #include <stdint.h>
38 : #include <math.h>
39 : #include "options.h"
40 : #include "prot_fx.h"
41 : #include "wmc_auto.h"
42 :
43 : #define HP20_COEFF_SCALE ( 2 )
44 : #define HP20_FX_COEFF_SCALE ( 1 )
45 : /*
46 : * hp20
47 : *
48 : * Function:
49 : * 2nd order high pass filter with nominal cut off frequency at 20 Hz.
50 : *
51 : * Returns:
52 : * void
53 : */
54 :
55 15821400 : static Word32 HP50_Mode2_Mpy_32_16_fix( Word32 a, Word16 b )
56 : {
57 15821400 : Word32 result = Mpy_32_16_1( a, b );
58 : /* perform rounding towards lower value for negative results */
59 15821400 : if ( result < 0 )
60 7830017 : result = L_add( result, 1 );
61 15821400 : return result;
62 : }
63 :
64 10578600 : static Word32 HP50_Mpy_32_32_fix( Word32 a, Word32 b )
65 : {
66 10578600 : Word32 result = Mpy_32_32( a, b );
67 : /* perform rounding towards lower value for negative results */
68 10578600 : if ( result < 0 )
69 5288099 : result = L_add( result, 1 );
70 10578600 : return result;
71 : }
72 :
73 :
74 6200 : static void filter_2nd_order(
75 : Word16 signal[],
76 : const Word16 stride,
77 : const Word16 prescale,
78 : const Word16 lg,
79 : Word32 mem[4],
80 : Word32 a1,
81 : Word32 a2,
82 : Word32 b1,
83 : Word32 b2 )
84 : {
85 :
86 : Word16 i;
87 : Word16 x2, x1;
88 : Word32 L_sum, L_y1, L_y2;
89 :
90 : /*
91 : * Saturation: The states of the filter, namely L_y1 and L_y2 shall
92 : * never saturate, because that causes error in the filter feedback.
93 : * The final output written into signal[] might saturate because of
94 : * unavoidable filter overshoot.
95 : */
96 :
97 : /* Execute first 2 iterations with 32-bit x anx y memory values */
98 : BASOP_SATURATE_ERROR_ON_EVS
99 6200 : L_sum = HP50_Mpy_32_32_fix( b2, mem[2] ); /* b2*x2 */
100 6200 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( b1, mem[3] ) ); /* b1*x1 */
101 6200 : x2 = shr( signal[0], prescale );
102 6200 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b2, x2 ) ); /* b2*x0 */
103 6200 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( mem[0], a2 ) ); /* y2*a2 */
104 6200 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( mem[1], a1 ) ); /* y1*a1 */
105 :
106 6200 : L_y2 = L_shl_sat( L_sum, HP20_COEFF_SCALE );
107 : BASOP_SATURATE_ERROR_OFF_EVS
108 : BASOP_SATURATE_WARNING_OFF_EVS
109 6200 : signal[0] = round_fx_sat( L_shl_sat( L_y2, prescale ) );
110 :
111 : BASOP_SATURATE_WARNING_ON_EVS
112 :
113 : BASOP_SATURATE_ERROR_ON_EVS
114 6200 : L_sum = HP50_Mpy_32_32_fix( b2, mem[3] ); /* b2*x2 */
115 6200 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b1, x2 ) ); /* b1*x1 */
116 6200 : x1 = shr( signal[stride], prescale );
117 6200 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b2, x1 ) ); /* b2*x0 */
118 6200 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( mem[1], a2 ) ); /* y2*a2 */
119 6200 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( L_y2, a1 ) ); /* y1*a1 */
120 :
121 6200 : L_y1 = L_shl_sat( L_sum, HP20_COEFF_SCALE );
122 : BASOP_SATURATE_ERROR_OFF_EVS
123 : BASOP_SATURATE_WARNING_OFF_EVS
124 6200 : signal[stride] = round_fx_sat( L_shl_sat( L_y1, prescale ) );
125 : BASOP_SATURATE_WARNING_ON_EVS
126 6200 : move16();
127 :
128 : /* New we use a trick and toggle x1/x2 and L_y1/L_y2 to save a few cycles unrolling the loop by 2 */
129 2640000 : FOR( i = 2; i < lg; i += 2 )
130 : {
131 : /* y[i+0] = b2*x[i-2] + b1*x[i-1] + b2*x[i-0] + a2*y[i-2] + a1*y[i-1]; */
132 : BASOP_SATURATE_ERROR_ON_EVS
133 2633800 : L_sum = HP50_Mode2_Mpy_32_16_fix( b2, x2 );
134 2633800 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b1, x1 ) );
135 2633800 : x2 = shr( signal[i * stride], prescale );
136 2633800 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b2, x2 ) );
137 2633800 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( L_y2, a2 ) );
138 2633800 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( L_y1, a1 ) );
139 :
140 2633800 : L_y2 = L_shl_sat( L_sum, HP20_COEFF_SCALE );
141 : BASOP_SATURATE_ERROR_OFF_EVS
142 : BASOP_SATURATE_WARNING_OFF_EVS
143 2633800 : signal[i_mult( i, stride )] = round_fx_sat( L_shl_sat( L_y2, prescale ) );
144 : BASOP_SATURATE_WARNING_ON_EVS
145 : /* y[i+1] = b2*x[i-1] + b1*x[i-0] + b2*x[i+1] + a2*y[i-1] + a1*y[i+0]; */
146 : BASOP_SATURATE_ERROR_ON_EVS
147 2633800 : L_sum = HP50_Mode2_Mpy_32_16_fix( b2, x1 );
148 2633800 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b1, x2 ) );
149 2633800 : x1 = shr( signal[( i + 1 ) * stride], prescale );
150 2633800 : L_sum = L_add( L_sum, HP50_Mode2_Mpy_32_16_fix( b2, x1 ) );
151 2633800 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( L_y1, a2 ) );
152 2633800 : L_sum = L_add( L_sum, HP50_Mpy_32_32_fix( L_y2, a1 ) );
153 :
154 2633800 : L_y1 = L_shl_sat( L_sum, HP20_COEFF_SCALE );
155 : BASOP_SATURATE_ERROR_OFF_EVS
156 : BASOP_SATURATE_WARNING_OFF_EVS
157 2633800 : signal[i_mult( add( i, 1 ), stride )] = round_fx_sat( L_shl_sat( L_y1, prescale ) );
158 : BASOP_SATURATE_WARNING_ON_EVS
159 2633800 : move16();
160 : }
161 : /* update static filter memory from variables */
162 6200 : mem[0] = L_y2;
163 6200 : move32();
164 6200 : mem[1] = L_y1;
165 6200 : move32();
166 6200 : mem[2] = L_deposit_h( x2 );
167 6200 : move32();
168 6200 : mem[3] = L_deposit_h( x1 );
169 6200 : move32();
170 :
171 :
172 6200 : return;
173 : }
174 :
175 :
176 6200 : void hp20( Word16 signal[], /* i/o: signal to filter any */
177 : const Word16 stride, /* i : stride to be applied accessing signal */
178 : const Word16 lg, /* i : length of signal (integer) Q0 */
179 : Word32 mem[5], /* i/o: static filter memory with this layout: */
180 : /* mem[0]: y[-2] (32-bit) */
181 : /* mem[1]; y[-1] (32-bit) */
182 : /* mem[2]: x[-2] << 16 */
183 : /* mem[3]: x[-1] << 16 */
184 : /* Note: mem[0..3] need to be scaled per frame */
185 : /* mem[4]: states scale */
186 : const Word32 sFreq ) /* i : input sampling rate Q0 */
187 : {
188 : Word32 a1, b1, a2, b2;
189 : Word16 prescale, prescaleOld, diff;
190 :
191 6200 : prescale = getScaleFactor16( signal, lg );
192 6200 : prescaleOld = extract_l( mem[4] );
193 6200 : diff = norm_l( L_shl_sat( mem[2], prescaleOld ) );
194 6200 : if ( mem[2] != 0 )
195 : {
196 6163 : prescale = s_min( prescale, diff );
197 : }
198 6200 : diff = norm_l( L_shl_sat( mem[3], prescaleOld ) );
199 6200 : if ( mem[3] != 0 )
200 : {
201 6164 : prescale = s_min( prescale, diff );
202 : }
203 : /* Take into account the left shift performed into the loop + 1 bit headroom*/
204 6200 : prescale = s_max( -12, sub( 1 + HP20_COEFF_SCALE, prescale ) );
205 6200 : IF( prescale != prescaleOld )
206 : {
207 2598 : diff = sub( prescale, prescaleOld );
208 2598 : mem[0] = L_shr_sat( mem[0], diff );
209 2598 : move32();
210 2598 : mem[1] = L_shr_sat( mem[1], diff );
211 2598 : move32();
212 2598 : mem[2] = L_shr_sat( mem[2], diff );
213 2598 : move32();
214 2598 : mem[3] = L_shr_sat( mem[3], diff ); //?sat
215 2598 : move32();
216 2598 : mem[4] = L_deposit_l( prescale );
217 : }
218 :
219 6200 : IF( EQ_32( sFreq, 8000 ) )
220 : {
221 : /* hp filter 20Hz at 3dB for 8000 Hz input sampling rate
222 : [b,a] = butter(2, 20.0/4000.0, 'high');
223 : b = [0.988954248067140 -1.977908496134280 0.988954248067140]
224 : a = [1.000000000000000 -1.977786483776764 0.978030508491796]*/
225 0 : a1 = 1061816033l /* 1.977786483776764 Q29*/;
226 0 : move32();
227 0 : a2 = -525076131l /*-0.978030508491796 Q29*/;
228 0 : move32();
229 0 : b1 = -1061881538l /*-1.977908496134280 Q29*/;
230 0 : move32();
231 0 : b2 = 530940769l /* 0.988954248067140 Q29*/;
232 0 : move32();
233 : }
234 6200 : ELSE IF( EQ_32( sFreq, 16000 ) )
235 : {
236 : /* hp filter 20Hz at 3dB for 16000KHz sampling rate
237 : [b,a] = butter(2, 20.0/8000.0, 'high');
238 : b = [0.994461788958195 -1.988923577916390 0.994461788958195]
239 : a = [1.000000000000000 -1.988892905899653 0.988954249933127] */
240 0 : a1 = 1067778748l /* 1.988892905899653 Q29*/;
241 0 : move32();
242 0 : a2 = -530940770l /*-0.988954249933127 Q29*/;
243 0 : move32();
244 0 : b1 = -1067795215l /*-1.988923577916390 Q29*/;
245 0 : move32();
246 0 : b2 = 533897608l /* 0.994461788958195 Q29*/;
247 0 : move32();
248 : }
249 6200 : ELSE IF( EQ_32( sFreq, 32000 ) )
250 : {
251 : /* hp filter 20Hz at 3dB for 32000KHz sampling rate
252 : [b,a] = butter(2, 20.0/16000.0, 'high');
253 : b = [0.997227049904470 -1.994454099808940 0.997227049904470]
254 : a = [1.000000000000000 -1.994446410541927 0.994461789075954]*/
255 2100 : a1 = 1070760263l /* 1.994446410541927 Q29*/;
256 2100 : move32();
257 2100 : a2 = -533897608l /*-0.994461789075954 Q29*/;
258 2100 : move32();
259 2100 : b1 = -1070764392l /*-1.994454099808940 Q29*/;
260 2100 : move32();
261 2100 : b2 = 535382196l /* 0.997227049904470 Q29*/;
262 2100 : move32();
263 : }
264 : ELSE
265 : {
266 4100 : assert( sFreq == 48000 );
267 : /* hp filter 20Hz at 3dB for 48000KHz sampling rate
268 : [b,a] = butter(2, 20.0/24000.0, 'high');
269 : b =[0.998150511190452 -1.996301022380904 0.998150511190452]
270 : a =[1.000000000000000 -1.996297601769122 0.996304442992686]*/
271 4100 : a1 = 1071754114l /* 1.996297601769122 Q29*/;
272 4100 : move32();
273 4100 : a2 = -534886875l /*-0.996304442992686 Q29*/;
274 4100 : move32();
275 4100 : b1 = -1071755951l /*-1.996301022380904 Q29*/;
276 4100 : move32();
277 4100 : b2 = 535877975l /* 0.998150511190452 Q29*/;
278 4100 : move32();
279 : }
280 :
281 :
282 6200 : filter_2nd_order( signal, stride, prescale, lg,
283 : mem, a1, a2, b1, b2 );
284 :
285 6200 : return;
286 : }
287 :
288 :
289 1096060 : void hp20_fx_32_opt(
290 : Word32 signal_fx[],
291 : const Word16 lg,
292 : Word32 mem_fx[],
293 : const Word32 Fs )
294 : {
295 : Word32 i;
296 : Word32 a1_fx, a2_fx, b1_fx, b2_fx;
297 : Word16 prescale, prescaleOld, prescale_current_frame, diff;
298 : Word32 tmp_mem[4];
299 :
300 1096060 : prescale = L_norm_arr( signal_fx, lg );
301 1096060 : prescale_current_frame = sub( 1 + HP20_FX_COEFF_SCALE, prescale );
302 :
303 1096060 : prescaleOld = extract_l( mem_fx[4] );
304 :
305 1096060 : tmp_mem[0] = L_shl_sat( mem_fx[0], prescaleOld );
306 1096060 : tmp_mem[1] = L_shl_sat( mem_fx[1], prescaleOld );
307 1096060 : tmp_mem[2] = L_shl_sat( mem_fx[2], prescaleOld );
308 1096060 : tmp_mem[3] = L_shl_sat( mem_fx[3], prescaleOld );
309 1096060 : move32();
310 1096060 : move32();
311 1096060 : move32();
312 1096060 : move32();
313 :
314 1096060 : diff = L_norm_arr( tmp_mem, 4 );
315 :
316 1096060 : prescale = s_min( prescale, diff );
317 :
318 1096060 : prescale = sub( 1 + HP20_FX_COEFF_SCALE, prescale );
319 :
320 1096060 : if ( EQ_16( prescale_current_frame, 1 + HP20_FX_COEFF_SCALE - 31 ) ) // signal_fx buffer contains only zeros, so use the mem_fx scale_factor instead
321 : {
322 9456 : prescale_current_frame = prescale;
323 : }
324 :
325 1096060 : diff = sub( prescale, prescaleOld );
326 1096060 : mem_fx[0] = L_shr_sat( mem_fx[0], diff );
327 1096060 : mem_fx[1] = L_shr_sat( mem_fx[1], diff );
328 1096060 : mem_fx[2] = L_shr_sat( mem_fx[2], diff );
329 1096060 : mem_fx[3] = L_shr_sat( mem_fx[3], diff );
330 1096060 : move32();
331 1096060 : move32();
332 1096060 : move32();
333 1096060 : move32();
334 1096060 : mem_fx[4] = L_deposit_l( prescale_current_frame );
335 1096060 : move32();
336 :
337 1096060 : IF( EQ_32( Fs, 8000 ) )
338 : {
339 : /* hp filter 20Hz at 3dB for 8000KHz input sampling rate
340 : [b,a] = butter(2, 20.0/4000.0, 'high');
341 : b = [0.988954248067140 -1.977908496134280 0.988954248067140]
342 : a =[1.000000000000000 -1.977786483776764 0.978030508491796]*/
343 0 : a1_fx = 2123632067 /* 1.977786483776764 Q30*/;
344 0 : a2_fx = -1050152262 /*-0.978030508491796 Q30*/;
345 0 : b1_fx = -2123763076 /*-1.977908496134280 Q30*/;
346 0 : b2_fx = 1061881538 /* 0.988954248067140 Q30*/;
347 : }
348 1096060 : ELSE IF( EQ_32( Fs, 16000 ) )
349 : {
350 : /* hp filter 20Hz at 3dB for 16000KHz sampling rate
351 : [b,a] = butter(2, 20.0/8000.0, 'high');
352 : b =[ 0.994461788958195 -1.988923577916390 0.994461788958195]
353 : a =[1.000000000000000 -1.988892905899653 0.988954249933127] */
354 101093 : a1_fx = 2135557497 /* 1.988892905899653 Q30*/;
355 101093 : a2_fx = -1061881540 /*-0.988954249933127 Q30*/;
356 101093 : b1_fx = -2135590430 /*-1.988923577916390 Q30*/;
357 101093 : b2_fx = 1067795215 /* 0.994461788958195 Q30*/;
358 : }
359 994967 : ELSE IF( EQ_32( Fs, 32000 ) )
360 : {
361 : /* hp filter 20Hz at 3dB for 32000KHz sampling rate
362 : [b,a] = butter(2, 20.0/16000.0, 'high');
363 : b =[0.997227049904470 -1.994454099808940 0.997227049904470]
364 : a =[1.000000000000000 -1.994446410541927 0.994461789075954]*/
365 238576 : a1_fx = 2141520527 /* 1.994446410541927 Q30*/;
366 238576 : a2_fx = -1067795215 /*-0.994461789075954 Q30*/;
367 238576 : b1_fx = -2141528783 /*-1.994454099808940 Q30*/;
368 238576 : b2_fx = 1070764392 /* 0.997227049904470 Q30*/;
369 : }
370 : ELSE
371 : {
372 : /* hp filter 20Hz at 3dB for 48000KHz sampling rate
373 : [b,a] = butter(2, 20.0/24000.0, 'high');
374 : b =[ 0.998150511190452 -1.996301022380904 0.998150511190452]
375 : a =[1.000000000000000 -1.996297601769122 0.996304442992686]*/
376 756391 : a1_fx = 2143508228 /* 1.996297601769122 Q30*/;
377 756391 : a2_fx = -1069773750 /*-0.996304442992686 Q30*/;
378 756391 : b1_fx = -2143511901 /*-1.996301022380904 Q30*/;
379 756391 : b2_fx = 1071755951 /* 0.998150511190452 Q30*/;
380 : }
381 1096060 : move32();
382 1096060 : move32();
383 1096060 : move32();
384 1096060 : move32();
385 : Word64 W_sum, W_y0, W_y1, W_y2;
386 : Word32 x0, x1, x2;
387 :
388 1096060 : W_sum = W_mult0_32_32( b2_fx, mem_fx[2] ); /* b2*x2 */
389 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( b1_fx, mem_fx[3] ) ); /* b1*x1 */
390 1096060 : x2 = L_shr( signal_fx[0], prescale );
391 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( b2_fx, x2 ) ); /* b2*x0 */
392 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( mem_fx[0], a2_fx ) ); /* y2*a2 */
393 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( mem_fx[1], a1_fx ) ); /* y1*a1 */
394 1096060 : W_y2 = W_shl( W_sum, 1 + HP20_FX_COEFF_SCALE );
395 1096060 : signal_fx[0] = W_round64_L( W_shl( W_y2, prescale ) );
396 1096060 : move32();
397 :
398 1096060 : W_sum = W_mult0_32_32( b2_fx, mem_fx[3] ); /* b2*x2 */
399 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( b1_fx, x2 ) ); /* b1*x1 */
400 1096060 : x1 = L_shr( signal_fx[1], prescale );
401 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( b2_fx, x1 ) ); /* b2*x0 */
402 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( mem_fx[1], a2_fx ) ); /* y2*a2 */
403 1096060 : W_sum = W_add( W_sum, W_mult0_32_32( W_round64_L( W_y2 ), a1_fx ) ); /* y1*a1 */
404 1096060 : W_y1 = W_shl( W_sum, 1 + HP20_FX_COEFF_SCALE );
405 1096060 : signal_fx[1] = W_round64_L( W_shl( W_y1, prescale ) );
406 1096060 : move32();
407 :
408 1096060 : diff = sub( prescale_current_frame, prescale );
409 1096060 : W_y1 = W_shr( W_y1, diff );
410 1096060 : W_y2 = W_shr( W_y2, diff );
411 1096060 : x2 = L_shr( x2, diff );
412 1096060 : x1 = L_shr( x1, diff );
413 :
414 910077700 : FOR( i = 2; i < lg; i++ )
415 : {
416 908981640 : W_sum = W_mult0_32_32( b2_fx, x2 ); /* b2*x2 */
417 908981640 : W_sum = W_add( W_sum, W_mult0_32_32( b1_fx, x1 ) ); /* b1*x1 */
418 908981640 : x0 = L_shr( signal_fx[i], prescale_current_frame );
419 908981640 : W_sum = W_add( W_sum, W_mult0_32_32( b2_fx, x0 ) ); /* b2*x0 */
420 908981640 : W_sum = W_add( W_sum, W_mult0_32_32( W_round64_L( W_y2 ), a2_fx ) ); /* y2*a2 */
421 908981640 : W_sum = W_add( W_sum, W_mult0_32_32( W_round64_L( W_y1 ), a1_fx ) ); /* y1*a1 */
422 908981640 : W_y0 = W_shl( W_sum, 1 + HP20_FX_COEFF_SCALE );
423 :
424 908981640 : signal_fx[i] = W_round64_L( W_shl( W_y0, prescale_current_frame ) );
425 908981640 : move32();
426 :
427 908981640 : x2 = x1;
428 908981640 : x1 = x0;
429 908981640 : W_y2 = W_y1;
430 908981640 : W_y1 = W_y0;
431 :
432 908981640 : move32();
433 908981640 : move32();
434 908981640 : move64();
435 908981640 : move64();
436 : }
437 :
438 1096060 : mem_fx[0] = W_round64_L( W_y2 );
439 1096060 : mem_fx[1] = W_round64_L( W_y1 );
440 1096060 : mem_fx[2] = x2;
441 1096060 : mem_fx[3] = x1;
442 :
443 1096060 : move32();
444 1096060 : move32();
445 1096060 : move32();
446 1096060 : move32();
447 :
448 1096060 : return;
449 : }
450 :
451 :
452 1771760 : void hp20_fx_32(
453 : Word32 signal_fx[],
454 : const Word16 lg,
455 : Word32 mem_fx[],
456 : const Word32 Fs )
457 : {
458 : Word16 i;
459 : Word32 a1_fx, a2_fx, b1_fx, b2_fx;
460 : Word16 Qx0, Qx1, Qx2, Qy1, Qprev_y1, Qy2, Qprev_y2, Qmin;
461 : Word64 x0_fx64, x1_fx64, x2_fx64, y0_fx64, y1_fx64, y2_fx64, R1, R2, R3, R4, R5;
462 :
463 1771760 : IF( EQ_32( Fs, 8000 ) )
464 : {
465 : /* hp filter 20Hz at 3dB for 8000KHz input sampling rate
466 : [b,a] = butter(2, 20.0/4000.0, 'high');
467 : b = [0.988954248067140 -1.977908496134280 0.988954248067140]
468 : a =[1.000000000000000 -1.977786483776764 0.978030508491796]*/
469 0 : a1_fx = 1061816033l /* 1.977786483776764 Q29*/;
470 0 : a2_fx = -525076131l /*-0.978030508491796 Q29*/;
471 0 : b1_fx = -1061881538l /*-1.977908496134280 Q29*/;
472 0 : b2_fx = 530940769l /* 0.988954248067140 Q29*/;
473 : }
474 1771760 : ELSE IF( EQ_32( Fs, 16000 ) )
475 : {
476 : /* hp filter 20Hz at 3dB for 16000KHz sampling rate
477 : [b,a] = butter(2, 20.0/8000.0, 'high');
478 : b =[ 0.994461788958195 -1.988923577916390 0.994461788958195]
479 : a =[1.000000000000000 -1.988892905899653 0.988954249933127] */
480 101250 : a1_fx = 1067778748l /* 1.988892905899653 Q29*/;
481 101250 : a2_fx = -530940770l /*-0.988954249933127 Q29*/;
482 101250 : b1_fx = -1067795215l /*-1.988923577916390 Q29*/;
483 101250 : b2_fx = 533897608l /* 0.994461788958195 Q29*/;
484 : }
485 1670510 : ELSE IF( EQ_32( Fs, 32000 ) )
486 : {
487 : /* hp filter 20Hz at 3dB for 32000KHz sampling rate
488 : [b,a] = butter(2, 20.0/16000.0, 'high');
489 : b =[0.997227049904470 -1.994454099808940 0.997227049904470]
490 : a =[1.000000000000000 -1.994446410541927 0.994461789075954]*/
491 407748 : a1_fx = 1070760263l /* 1.994446410541927 Q29*/;
492 407748 : a2_fx = -533897608l /*-0.994461789075954 Q29*/;
493 407748 : b1_fx = -1070764392l /*-1.994454099808940 Q29*/;
494 407748 : b2_fx = 535382196l /* 0.997227049904470 Q29*/;
495 : }
496 : ELSE
497 : {
498 : /* hp filter 20Hz at 3dB for 48000KHz sampling rate
499 : [b,a] = butter(2, 20.0/24000.0, 'high');
500 : b =[ 0.998150511190452 -1.996301022380904 0.998150511190452]
501 : a =[1.000000000000000 -1.996297601769122 0.996304442992686]*/
502 1262762 : a1_fx = 1071754114l /* 1.996297601769122 Q29*/;
503 1262762 : a2_fx = -534886875l /*-0.996304442992686 Q29*/;
504 1262762 : b1_fx = -1071755951l /*-1.996301022380904 Q29*/;
505 1262762 : b2_fx = 535877975l /* 0.998150511190452 Q29*/;
506 : }
507 1771760 : move32();
508 1771760 : move32();
509 1771760 : move32();
510 1771760 : move32();
511 :
512 1771760 : Qprev_y1 = extract_l( mem_fx[4] );
513 1771760 : Qprev_y2 = extract_l( mem_fx[5] );
514 1771760 : y1_fx64 = W_deposit32_l( mem_fx[0] );
515 1771760 : y2_fx64 = W_deposit32_l( mem_fx[1] );
516 1771760 : x0_fx64 = W_deposit32_l( mem_fx[2] );
517 1771760 : x1_fx64 = W_deposit32_l( mem_fx[3] );
518 :
519 1507382000 : FOR( i = 0; i < lg; i++ )
520 : {
521 1505610240 : x2_fx64 = x1_fx64;
522 1505610240 : move64();
523 1505610240 : x1_fx64 = x0_fx64;
524 1505610240 : move64();
525 1505610240 : x0_fx64 = W_deposit32_l( signal_fx[i] );
526 :
527 1505610240 : Qy1 = W_norm( y1_fx64 );
528 1505610240 : if ( y1_fx64 == 0 )
529 : {
530 288559 : Qy1 = 62;
531 288559 : move16();
532 : }
533 1505610240 : Qy1 = sub( Qy1, 34 );
534 1505610240 : R1 = W_mult0_32_32( W_shl_sat_l( y1_fx64, Qy1 ), a1_fx );
535 1505610240 : Qy1 = add( Qy1, Qprev_y1 );
536 :
537 1505610240 : Qy2 = W_norm( y2_fx64 );
538 1505610240 : if ( y2_fx64 == 0 )
539 : {
540 293426 : Qy2 = 62;
541 293426 : move16();
542 : }
543 1505610240 : Qy2 = sub( Qy2, 34 );
544 1505610240 : R2 = W_mult0_32_32( W_shl_sat_l( y2_fx64, Qy2 ), a2_fx );
545 1505610240 : Qy2 = add( Qy2, Qprev_y2 );
546 :
547 1505610240 : Qx0 = W_norm( x0_fx64 );
548 1505610240 : if ( x0_fx64 == 0 )
549 : {
550 58191885 : Qx0 = 62;
551 58191885 : move16();
552 : }
553 1505610240 : Qx0 = sub( Qx0, 34 );
554 1505610240 : R3 = W_mult0_32_32( W_shl_sat_l( x0_fx64, Qx0 ), b2_fx );
555 :
556 1505610240 : Qx1 = W_norm( x1_fx64 );
557 1505610240 : if ( x1_fx64 == 0 )
558 : {
559 58194182 : Qx1 = 62;
560 58194182 : move16();
561 : }
562 1505610240 : Qx1 = sub( Qx1, 34 );
563 1505610240 : R4 = W_mult0_32_32( W_shl_sat_l( x1_fx64, Qx1 ), b1_fx );
564 :
565 1505610240 : Qx2 = W_norm( x2_fx64 );
566 1505610240 : if ( x2_fx64 == 0 )
567 : {
568 58196507 : Qx2 = 62;
569 58196507 : move16();
570 : }
571 1505610240 : Qx2 = sub( Qx2, 34 );
572 1505610240 : R5 = W_mult0_32_32( W_shl_sat_l( x2_fx64, Qx2 ), b2_fx );
573 :
574 1505610240 : Qmin = s_min( Qy1, Qy2 );
575 :
576 1505610240 : y0_fx64 = W_add( W_shr( R1, sub( Qy1, Qmin ) ), W_shr( R2, sub( Qy2, Qmin ) ) );
577 :
578 1505610240 : Qmin = s_min( Qmin, Qx0 );
579 1505610240 : Qmin = s_min( Qmin, Qx1 );
580 1505610240 : Qmin = s_min( Qmin, Qx2 );
581 :
582 1505610240 : y0_fx64 = W_add( W_shr( y0_fx64, sub( s_min( Qy1, Qy2 ), Qmin ) ), W_add( W_shr( R3, sub( Qx0, Qmin ) ), W_add( W_shr( R4, sub( Qx1, Qmin ) ), W_shr( R5, sub( Qx2, Qmin ) ) ) ) );
583 :
584 1505610240 : y0_fx64 = W_shr( y0_fx64, 29 );
585 :
586 1505610240 : signal_fx[i] = W_extract_l( W_shr( y0_fx64, Qmin ) );
587 1505610240 : move32();
588 1505610240 : IF( signal_fx[i] < 0 )
589 : {
590 753936679 : signal_fx[i] = L_add( signal_fx[i], 1 );
591 753936679 : move32();
592 : }
593 :
594 1505610240 : y2_fx64 = y1_fx64;
595 1505610240 : y1_fx64 = y0_fx64;
596 1505610240 : Qprev_y2 = Qprev_y1;
597 1505610240 : Qprev_y1 = Qmin;
598 1505610240 : move64();
599 1505610240 : move64();
600 1505610240 : move16();
601 1505610240 : move16();
602 : }
603 :
604 1771760 : Qy1 = W_norm( y1_fx64 );
605 1771760 : test();
606 1771760 : IF( y1_fx64 != 0 && LT_16( Qy1, 32 ) )
607 : {
608 0 : y1_fx64 = W_shr( y1_fx64, sub( 32, Qy1 ) );
609 0 : Qprev_y1 = sub( Qprev_y1, sub( 32, Qy1 ) );
610 : }
611 :
612 1771760 : Qy2 = W_norm( y2_fx64 );
613 1771760 : test();
614 1771760 : IF( y2_fx64 != 0 && LT_16( Qy2, 32 ) )
615 : {
616 0 : y2_fx64 = W_shr( y2_fx64, sub( 32, Qy2 ) );
617 0 : Qprev_y2 = sub( Qprev_y2, sub( 32, Qy2 ) );
618 : }
619 :
620 1771760 : mem_fx[0] = W_extract_l( y1_fx64 );
621 1771760 : mem_fx[1] = W_extract_l( y2_fx64 );
622 1771760 : mem_fx[2] = W_extract_l( x0_fx64 );
623 1771760 : mem_fx[3] = W_extract_l( x1_fx64 );
624 1771760 : mem_fx[4] = Qprev_y1;
625 1771760 : mem_fx[5] = Qprev_y2;
626 :
627 1771760 : move32();
628 1771760 : move32();
629 1771760 : move32();
630 1771760 : move32();
631 1771760 : move32();
632 1771760 : move32();
633 :
634 1771760 : return;
635 : }
|
