Line data Source code
1 : /*====================================================================================
2 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
3 : ====================================================================================*/
4 :
5 :
6 : #include <stdint.h>
7 : #include "options.h"
8 : #include "basop_util.h"
9 : #include "vad_basop.h"
10 : //#include "prot_fx.h"
11 : #include "rom_com.h" /* Common constants */
12 : #include "prot_fx.h" /* Function prototypes */
13 : #include "prot_fx_enc.h" /* Function prototypes */
14 : #include "basop_util.h" /* Function prototypes */
15 : /*-------------------------------------------------------------------*
16 : * ltd_stable_fx()
17 : *
18 : *
19 : *-------------------------------------------------------------------*/
20 :
21 3100 : void ltd_stable_fx(
22 : VAD_CLDFB_HANDLE hVAD_CLDFB, /* i/o: CLDFB VAD state */
23 : Word16 *ltd_stable_rate, /* o : time-domain stable rate*/
24 : const Word32 frame_energy, /* i : current frame energy Q_frames_power*/
25 : const Word16 frameloop, /* i : amount of frames*/
26 : const Word16 Q_frames_power /* i : the Scaling of frames_power*/
27 : )
28 : {
29 :
30 : Word32 i;
31 : Word32 zerop001, maxVal;
32 : Word32 mid_frame_amp_mul, tmp_mul;
33 : Word32 *frames_power_32;
34 : Word32 frame_energy_Sqr32;
35 : Word32 seg_amp32;
36 : Word32 mid_frame_ampadd32[20];
37 : Word16 tmp16[20];
38 : Word32 mid_frame_amp32[20];
39 : Word32 dif32, apow32;
40 : Word32 tmp32[20];
41 :
42 : Word16 Q_frames_power_last_32;
43 : Word16 Q_dif, Q_apow;
44 : Word16 frame_energy_Sqr;
45 : Word16 Q_frames_power32;
46 : Word16 leadingzero_tmp32;
47 : Word16 leadingzero_midamp;
48 : Word16 Qsum_dif32, Qsum_apow32;
49 : Word16 ltd_stable_rate_Qtmp;
50 : Word16 scale1;
51 : Word32 seg_amp32tmp;
52 : Word16 tmp;
53 :
54 3100 : zerop001 = 0;
55 3100 : move32();
56 3100 : Q_dif = 0;
57 3100 : move16();
58 3100 : Q_apow = 0;
59 3100 : move16();
60 3100 : frames_power_32 = hVAD_CLDFB->frames_power_32; // Q_frames_power_last_32
61 3100 : Q_frames_power_last_32 = hVAD_CLDFB->Q_frames_power_32;
62 3100 : move16();
63 3100 : leadingzero_midamp = 31;
64 3100 : move16();
65 3100 : leadingzero_tmp32 = 31;
66 3100 : move16();
67 3100 : Q_frames_power32 = Q_frames_power;
68 3100 : move16();
69 :
70 3100 : frame_energy_Sqr = FixSqrt( frame_energy, &Q_frames_power32 );
71 3100 : frame_energy_Sqr32 = L_deposit_l( frame_energy_Sqr );
72 3100 : frame_energy_Sqr32 = L_shl( frame_energy_Sqr32, 16 );
73 3100 : Q_frames_power32 = add( Q_frames_power32, 16 );
74 :
75 : /* +0.1 */
76 3100 : IF( GE_16( Q_frames_power32, 40 ) )
77 : {
78 0 : zerop001 = L_shr( CNT0P001, 1 );
79 0 : frame_energy_Sqr32 = L_shr( frame_energy_Sqr32, sub( Q_frames_power32, 39 ) ); // Q_frames_power32
80 0 : Q_frames_power32 = 39;
81 0 : move16();
82 : }
83 : ELSE
84 : {
85 3100 : Q_frames_power32 = sub( Q_frames_power32, 1 );
86 3100 : frame_energy_Sqr32 = L_shr( frame_energy_Sqr32, 1 );
87 3100 : zerop001 = L_shr( CNT0P001, sub( 40, Q_frames_power32 ) );
88 : }
89 3100 : frames_power_32[0] = L_add( frame_energy_Sqr32, zerop001 ); // Q_frames_power32
90 3100 : move32();
91 :
92 3100 : IF( LT_16( frameloop, 3 ) )
93 : {
94 360 : FOR( i = 1; i < 40; i++ )
95 : {
96 351 : frames_power_32[i] = frames_power_32[0]; // Q_frames_power32
97 351 : move32();
98 : }
99 : }
100 : ELSE
101 : {
102 : Word16 leadingzero;
103 3091 : maxVal = 0;
104 3091 : move32();
105 123640 : FOR( i = 1; i < 40; i++ )
106 : {
107 120549 : maxVal = L_max( maxVal, frames_power_32[i] ); // Q_frames_power32
108 : }
109 3091 : leadingzero = 31;
110 3091 : move16();
111 3091 : if ( maxVal )
112 3091 : leadingzero = norm_l( maxVal );
113 3091 : Q_frames_power_last_32 = add( Q_frames_power_last_32, leadingzero );
114 :
115 :
116 3091 : IF( GT_16( Q_frames_power_last_32, Q_frames_power32 ) )
117 : {
118 471 : scale1 = sub( Q_frames_power_last_32, Q_frames_power32 );
119 471 : scale1 = sub( scale1, leadingzero );
120 18840 : FOR( i = 1; i < 40; i++ )
121 : {
122 18369 : frames_power_32[i] = L_shr( frames_power_32[i], scale1 ); // Q_frames_power32
123 18369 : move32();
124 : }
125 : }
126 : ELSE
127 : {
128 2620 : scale1 = sub( Q_frames_power32, Q_frames_power_last_32 );
129 2620 : frames_power_32[0] = L_shr( frames_power_32[0], scale1 );
130 2620 : move32();
131 2620 : Q_frames_power32 = Q_frames_power_last_32;
132 2620 : move16();
133 104800 : FOR( i = 1; i < 40; i++ )
134 : {
135 102180 : frames_power_32[i] = L_shl( frames_power_32[i], leadingzero );
136 102180 : move32();
137 : }
138 : }
139 : }
140 :
141 65100 : FOR( i = 0; i < 20; i++ )
142 : {
143 62000 : mid_frame_ampadd32[i] = L_add( L_shr( frames_power_32[2 * i], 1 ), L_shr( frames_power_32[2 * i + 1], 1 ) ); // Q_frames_power32
144 62000 : move32();
145 : }
146 :
147 3100 : maxVal = 0;
148 3100 : move32();
149 65100 : FOR( i = 0; i < 20; i++ )
150 : {
151 62000 : maxVal = L_max( maxVal, mid_frame_ampadd32[i] ); // Q_frames_power32
152 : }
153 3100 : leadingzero_midamp = 31;
154 3100 : move16();
155 3100 : if ( maxVal )
156 : {
157 3100 : leadingzero_midamp = norm_l( maxVal );
158 : }
159 :
160 65100 : FOR( i = 0; i < 20; i++ )
161 : {
162 62000 : mid_frame_amp32[i] = L_shl( mid_frame_ampadd32[i], leadingzero_midamp ); // Q_frames_power32 + leadingzero_midamp
163 62000 : move32();
164 : }
165 :
166 3100 : seg_amp32 = 0;
167 3100 : move32();
168 65100 : FOR( i = 0; i < 20; i++ )
169 : {
170 62000 : seg_amp32 = L_add( seg_amp32, L_shr( mid_frame_amp32[i], 5 ) ); // Q_frames_power32 + leadingzero_midamp - 5
171 : }
172 3100 : seg_amp32 = MUL_F( seg_amp32, 0x0666 /*(1/20).Q15*/ );
173 :
174 3100 : dif32 = 0;
175 3100 : move32();
176 3100 : apow32 = 0;
177 3100 : move32();
178 :
179 3100 : seg_amp32tmp = L_shl( seg_amp32, 5 ); // Q_frames_power32 + leadingzero_midamp
180 65100 : FOR( i = 0; i < 20; i++ )
181 : {
182 62000 : tmp32[i] = L_sub( mid_frame_amp32[i], seg_amp32tmp ); // Q_frames_power32 + leadingzero_midamp
183 62000 : move32();
184 : }
185 :
186 3100 : maxVal = 0;
187 3100 : move32();
188 65100 : FOR( i = 0; i < 20; i++ )
189 : {
190 62000 : maxVal = L_max( maxVal, L_abs( tmp32[i] ) ); // Q_frames_power32 + leadingzero_midamp - 5
191 : }
192 3100 : leadingzero_tmp32 = 31;
193 3100 : move16();
194 3100 : if ( maxVal )
195 : {
196 3100 : leadingzero_tmp32 = norm_l( maxVal );
197 : }
198 :
199 65100 : FOR( i = 0; i < 20; i++ )
200 : {
201 62000 : tmp16[i] = extract_h( L_shl( tmp32[i], leadingzero_tmp32 ) ); //// Q_frames_power32 + leadingzero_midamp + leadingzero_tmp32 - 16
202 : }
203 :
204 65100 : FOR( i = 0; i < 20; i++ )
205 : {
206 62000 : tmp_mul = L_mult_sat( tmp16[i], tmp16[i] ); // 2 * (Q_frames_power32 + leadingzero_midamp + leadingzero_tmp32 - 16) + 1
207 62000 : tmp_mul = L_shr( tmp_mul, 5 ); // 2 * (Q_frames_power32 + leadingzero_midamp + leadingzero_tmp32 - 16) - 4
208 62000 : dif32 = L_add( dif32, tmp_mul );
209 :
210 62000 : tmp = extract_h( mid_frame_amp32[i] );
211 62000 : mid_frame_amp_mul = L_mult( tmp, tmp );
212 62000 : mid_frame_amp_mul = L_shr( mid_frame_amp_mul, 5 );
213 62000 : apow32 = L_add( apow32, mid_frame_amp_mul );
214 : }
215 :
216 :
217 3100 : IF( dif32 == 0 )
218 : {
219 0 : ltd_stable_rate[0] = 0;
220 0 : move16();
221 : }
222 : ELSE
223 : {
224 3100 : Q_dif = sub( norm_l( dif32 ), 1 );
225 3100 : Q_apow = norm_l( apow32 );
226 3100 : dif32 = L_shl( dif32, Q_dif );
227 3100 : apow32 = L_shl( apow32, Q_apow );
228 3100 : ltd_stable_rate[0] = div_l( dif32, extract_h( apow32 ) );
229 3100 : move16();
230 : }
231 :
232 3100 : ltd_stable_rate_Qtmp = sub( Q_dif, Q_apow );
233 3100 : ltd_stable_rate_Qtmp = add( ltd_stable_rate_Qtmp, leadingzero_tmp32 );
234 3100 : ltd_stable_rate_Qtmp = add( ltd_stable_rate_Qtmp, leadingzero_tmp32 );
235 3100 : ltd_stable_rate_Qtmp = add( ltd_stable_rate_Qtmp, 15 );
236 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, ITD_STABLE_RATE_Q );
237 :
238 3100 : ltd_stable_rate_Qtmp = limitScale16( ltd_stable_rate_Qtmp );
239 3100 : ltd_stable_rate[0] = shr( ltd_stable_rate[0], ltd_stable_rate_Qtmp );
240 3100 : move16();
241 :
242 3100 : maxVal = 0;
243 3100 : move32();
244 46500 : FOR( i = 0; i < 14; i++ )
245 : {
246 43400 : maxVal = L_max( maxVal, L_abs( mid_frame_ampadd32[i] ) );
247 : }
248 3100 : leadingzero_midamp = 31;
249 3100 : move16();
250 3100 : if ( maxVal )
251 : {
252 3100 : leadingzero_midamp = norm_l( maxVal );
253 : }
254 :
255 46500 : FOR( i = 0; i < 14; i++ )
256 : {
257 43400 : mid_frame_amp32[i] = L_shl( mid_frame_ampadd32[i], leadingzero_midamp );
258 43400 : move32();
259 : }
260 :
261 3100 : seg_amp32 = 0;
262 3100 : move32();
263 46500 : FOR( i = 0; i < 14; i++ )
264 : {
265 43400 : seg_amp32 = L_add( seg_amp32, L_shr( mid_frame_amp32[i], 4 ) ); // Q_frames_power32 - 4
266 : }
267 3100 : seg_amp32 = MUL_F( seg_amp32, 0x0924 /*(1/14).Q15*/ );
268 :
269 3100 : dif32 = 0;
270 3100 : move32();
271 3100 : apow32 = 0;
272 3100 : move32();
273 3100 : seg_amp32tmp = L_shl( seg_amp32, 4 ); // Q_frames_power32
274 46500 : FOR( i = 0; i < 14; i++ )
275 : {
276 43400 : tmp32[i] = L_sub( mid_frame_amp32[i], seg_amp32tmp );
277 43400 : move32();
278 : }
279 :
280 3100 : maxVal = 0;
281 3100 : move32();
282 46500 : FOR( i = 0; i < 14; i++ )
283 : {
284 43400 : maxVal = L_max( maxVal, L_abs( tmp32[i] ) );
285 : }
286 3100 : leadingzero_tmp32 = 31;
287 3100 : move16();
288 3100 : if ( maxVal )
289 : {
290 3100 : leadingzero_tmp32 = norm_l( maxVal );
291 : }
292 :
293 46500 : FOR( i = 0; i < 14; i++ )
294 : {
295 43400 : tmp32[i] = L_shl( tmp32[i], leadingzero_tmp32 );
296 43400 : move32();
297 : }
298 :
299 46500 : FOR( i = 0; i < 14; i++ )
300 : {
301 43400 : tmp = extract_h( tmp32[i] );
302 43400 : tmp_mul = L_mult_sat( tmp, tmp );
303 43400 : tmp_mul = L_shr( tmp_mul, 4 );
304 43400 : dif32 = L_add( dif32, tmp_mul );
305 :
306 43400 : tmp = extract_h( mid_frame_amp32[i] );
307 43400 : mid_frame_amp_mul = L_mult( tmp, tmp );
308 43400 : mid_frame_amp_mul = L_shr( mid_frame_amp_mul, 4 );
309 43400 : apow32 = L_add( apow32, mid_frame_amp_mul );
310 : }
311 :
312 3100 : Qsum_apow32 = add( Q_frames_power32, Q_frames_power32 );
313 3100 : Qsum_apow32 = add( Qsum_apow32, leadingzero_midamp );
314 3100 : Qsum_apow32 = add( Qsum_apow32, leadingzero_midamp );
315 3100 : Qsum_apow32 = sub( Qsum_apow32, 37 );
316 :
317 3100 : Qsum_dif32 = add( Qsum_apow32, leadingzero_tmp32 );
318 3100 : Qsum_dif32 = add( Qsum_dif32, leadingzero_tmp32 );
319 :
320 : /* +0.1 */
321 3100 : IF( GE_16( Qsum_apow32, 44 ) )
322 : {
323 0 : zerop001 = L_shr( CNT0P0001, 1 );
324 0 : apow32 = L_shr( apow32, limitScale32( sub( Qsum_apow32, 43 ) ) );
325 0 : Qsum_apow32 = 43;
326 0 : move16();
327 : }
328 : ELSE
329 : {
330 3100 : Qsum_apow32 = sub( Qsum_apow32, 1 );
331 3100 : apow32 = L_shr( apow32, 1 );
332 3100 : zerop001 = L_shr( CNT0P0001, limitScale32( sub( 44, Qsum_apow32 ) ) );
333 : }
334 3100 : apow32 = L_add( apow32, zerop001 );
335 3100 : IF( apow32 == 0 )
336 : {
337 0 : apow32 = CNT0P0001;
338 0 : move32();
339 0 : Qsum_apow32 = 44;
340 0 : move16();
341 : }
342 :
343 3100 : IF( dif32 == 0 )
344 : {
345 0 : ltd_stable_rate[1] = 0;
346 0 : move16();
347 : }
348 : ELSE
349 : {
350 3100 : Q_dif = sub( norm_l( dif32 ), 1 );
351 3100 : Q_apow = norm_l( apow32 );
352 3100 : dif32 = L_shl( dif32, Q_dif );
353 3100 : apow32 = L_shl( apow32, Q_apow );
354 :
355 3100 : ltd_stable_rate[1] = div_l( dif32, extract_h( apow32 ) );
356 3100 : move16();
357 : }
358 :
359 3100 : ltd_stable_rate_Qtmp = add( Qsum_dif32, Q_dif );
360 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, Qsum_apow32 );
361 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, Q_apow );
362 3100 : ltd_stable_rate_Qtmp = add( ltd_stable_rate_Qtmp, 15 );
363 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, ITD_STABLE_RATE_Q );
364 :
365 3100 : ltd_stable_rate_Qtmp = limitScale16( ltd_stable_rate_Qtmp );
366 :
367 3100 : ltd_stable_rate[1] = shr( ltd_stable_rate[1], ltd_stable_rate_Qtmp );
368 3100 : move16();
369 :
370 3100 : maxVal = 0;
371 3100 : move32();
372 27900 : FOR( i = 0; i < 8; i++ )
373 : {
374 24800 : maxVal = L_max( maxVal, L_abs( mid_frame_ampadd32[i] ) );
375 : }
376 3100 : leadingzero_midamp = 31;
377 3100 : move16();
378 3100 : IF( maxVal )
379 : {
380 3100 : leadingzero_midamp = norm_l( maxVal );
381 : }
382 :
383 27900 : FOR( i = 0; i < 8; i++ )
384 : {
385 24800 : mid_frame_amp32[i] = L_shl( mid_frame_ampadd32[i], leadingzero_midamp );
386 24800 : move32();
387 : }
388 :
389 3100 : seg_amp32 = 0;
390 3100 : move32();
391 27900 : FOR( i = 0; i < 8; i++ )
392 : {
393 24800 : seg_amp32 = L_add( seg_amp32, L_shr( mid_frame_amp32[i], 3 ) );
394 : }
395 3100 : seg_amp32 = MUL_F( seg_amp32, 0x1000 /*(1/8).Q15*/ );
396 :
397 3100 : dif32 = 0;
398 3100 : move32();
399 3100 : apow32 = 0;
400 3100 : move32();
401 3100 : seg_amp32tmp = L_shl( seg_amp32, 3 );
402 27900 : FOR( i = 0; i < 8; i++ )
403 : {
404 24800 : tmp32[i] = L_sub( mid_frame_amp32[i], seg_amp32tmp );
405 24800 : move32();
406 : }
407 :
408 3100 : maxVal = 0;
409 3100 : move32();
410 27900 : FOR( i = 0; i < 8; i++ )
411 : {
412 24800 : maxVal = L_max( maxVal, L_abs( tmp32[i] ) );
413 : }
414 3100 : leadingzero_tmp32 = 31;
415 3100 : move16();
416 3100 : IF( maxVal )
417 : {
418 3099 : leadingzero_tmp32 = norm_l( maxVal );
419 : }
420 27900 : FOR( i = 0; i < 8; i++ )
421 : {
422 24800 : tmp32[i] = L_shl( tmp32[i], leadingzero_tmp32 );
423 24800 : move32();
424 : }
425 :
426 27900 : FOR( i = 0; i < 8; i++ )
427 : {
428 24800 : tmp = extract_h( tmp32[i] );
429 24800 : tmp_mul = L_mult_sat( tmp, tmp );
430 24800 : tmp_mul = L_shr( tmp_mul, 3 );
431 24800 : dif32 = L_add( dif32, tmp_mul );
432 :
433 24800 : tmp = extract_h( mid_frame_amp32[i] );
434 24800 : mid_frame_amp_mul = L_mult_sat( tmp, tmp );
435 24800 : mid_frame_amp_mul = L_shr( mid_frame_amp_mul, 3 );
436 24800 : apow32 = L_add( apow32, mid_frame_amp_mul );
437 : }
438 :
439 3100 : Qsum_apow32 = add( Q_frames_power32, Q_frames_power32 );
440 3100 : Qsum_apow32 = add( Qsum_apow32, leadingzero_midamp );
441 3100 : Qsum_apow32 = add( Qsum_apow32, leadingzero_midamp );
442 3100 : Qsum_apow32 = sub( Qsum_apow32, 36 );
443 :
444 3100 : Qsum_dif32 = add( Qsum_apow32, leadingzero_tmp32 );
445 3100 : Qsum_dif32 = add( Qsum_dif32, leadingzero_tmp32 );
446 :
447 : /* +0.1 */
448 3100 : IF( GE_16( Qsum_apow32, 44 ) )
449 : {
450 0 : zerop001 = L_shr( CNT0P0001, 1 );
451 0 : apow32 = L_shr( apow32, limitScale32( sub( Qsum_apow32, 43 ) ) );
452 0 : Qsum_apow32 = 43;
453 0 : move16();
454 : }
455 : ELSE
456 : {
457 3100 : Qsum_apow32 = sub( Qsum_apow32, 1 );
458 3100 : apow32 = L_shr( apow32, 1 );
459 3100 : zerop001 = L_shr( CNT0P0001, limitScale32( sub( 44, Qsum_apow32 ) ) );
460 : }
461 3100 : apow32 = L_add( apow32, zerop001 );
462 3100 : IF( apow32 == 0 )
463 : {
464 0 : apow32 = CNT0P0001;
465 0 : move32();
466 0 : Qsum_apow32 = 44;
467 0 : move16();
468 : }
469 :
470 3100 : IF( dif32 == 0 )
471 : {
472 1 : ltd_stable_rate[2] = 0;
473 1 : move16();
474 : }
475 : ELSE
476 : {
477 3099 : Q_dif = sub( norm_l( dif32 ), 1 );
478 3099 : Q_apow = norm_l( apow32 );
479 3099 : dif32 = L_shl( dif32, Q_dif );
480 3099 : apow32 = L_shl( apow32, Q_apow );
481 :
482 3099 : ltd_stable_rate[2] = div_l( dif32, extract_h( apow32 ) );
483 3099 : move16();
484 : }
485 :
486 3100 : ltd_stable_rate_Qtmp = add( Qsum_dif32, Q_dif );
487 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, Qsum_apow32 );
488 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, Q_apow );
489 3100 : ltd_stable_rate_Qtmp = add( ltd_stable_rate_Qtmp, 15 );
490 3100 : ltd_stable_rate_Qtmp = sub( ltd_stable_rate_Qtmp, ITD_STABLE_RATE_Q );
491 :
492 3100 : ltd_stable_rate_Qtmp = limitScale16( ltd_stable_rate_Qtmp );
493 :
494 3100 : ltd_stable_rate[2] = shr( ltd_stable_rate[2], ltd_stable_rate_Qtmp );
495 3100 : move16();
496 3100 : ltd_stable_rate[3] = add( mult( ltd_stable_rate[3], 0x7333 /*0.9.Q15*/ ), mult( ltd_stable_rate[2], 0x0ccc /*0.1.Q15*/ ) );
497 3100 : move16();
498 :
499 173600 : FOR( i = 55; i > 0; i-- )
500 : {
501 170500 : frames_power_32[i] = frames_power_32[i - 1];
502 170500 : move32();
503 : }
504 3100 : hVAD_CLDFB->Q_frames_power_32 = Q_frames_power32;
505 3100 : move16();
506 3100 : }
|
