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