Line data Source code
1 : /*====================================================================================
2 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
3 : ====================================================================================*/
4 :
5 : #include <stdint.h>
6 : #include "options.h"
7 : #include "prot_fx.h"
8 : #include "rom_com.h"
9 : #include "log2.h"
10 :
11 : /*-------------------------------------------------------------------*
12 : * Local constants
13 : *-------------------------------------------------------------------*/
14 : #define WARP_OS_RATE 8
15 : #define LL 256
16 : #define LL_OS ( WARP_OS_RATE * LL )
17 : #define OSLENGTH 12
18 : #define CUTFREE_ABS_RANGE 6
19 : #define CUTFREE_REL_RANGE 0.25
20 : #define CUTFREE_ABS_RANGE_Q3 48
21 : #define CUTFREE_REL_RANGE_Q2 1
22 : #define WI_THRESHLD 0.8
23 : #define WI_SAMPLE_THLD 20
24 :
25 : /*#define _POLY1(x, c) ((c)[0] * (x) + (c)[1]) */
26 : /*#define _POLY2(x, c) (_POLY1((x), (c)) * (x) + (c)[2]) */
27 : /*#define _POLY3(x, c) (_POLY2((x), (c)) * (x) + (c)[3]) */
28 :
29 : /*-------------------------------------------------------------------*
30 : * Local functions
31 : *--------------------------------------------------------------------*/
32 :
33 : static Word32 DTFS_setEngy_fx( DTFS_STRUCTURE *X_DTFS_FX, Word32 en2_fx );
34 :
35 : /*-------------------------------------------------------------------*
36 : * DTFS_new_fx()
37 : *
38 : * DTFS structure initialization.
39 : *-------------------------------------------------------------------*/
40 0 : ivas_error DTFS_new_fx(
41 : DTFS_STRUCTURE **dtfs_out )
42 : {
43 :
44 : Word16 i;
45 :
46 0 : DTFS_STRUCTURE *dtfs_fx = NULL;
47 0 : dtfs_fx = (DTFS_STRUCTURE *) calloc( 1, sizeof( DTFS_STRUCTURE ) );
48 : // PMT("verif mem alloc")
49 0 : IF( dtfs_fx == NULL )
50 : {
51 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for DTFS (SC-VBR) structure\n" ) );
52 : }
53 :
54 0 : dtfs_fx->lag_fx = 0;
55 0 : move16();
56 0 : dtfs_fx->nH_fx = 0;
57 0 : move16();
58 0 : dtfs_fx->nH_4kHz_fx = 0;
59 0 : move16();
60 0 : dtfs_fx->upper_cut_off_freq_of_interest_fx = 3300; // Q0
61 0 : move16();
62 0 : dtfs_fx->upper_cut_off_freq_fx = 4000; // Q0
63 0 : move16();
64 0 : dtfs_fx->Q = 0;
65 0 : move16();
66 :
67 0 : FOR( i = 0; i < MAXLAG_WI; i++ )
68 : {
69 0 : dtfs_fx->a_fx[i] = 0; // dtfs_fx->Q
70 0 : move16();
71 0 : dtfs_fx->b_fx[i] = 0; // dtfs_fx->Q
72 0 : move16();
73 : }
74 0 : *dtfs_out = dtfs_fx;
75 0 : return IVAS_ERR_OK;
76 : }
77 : /*-------------------------------------------------------------------*
78 : * DTFS_copy_fx()
79 : *
80 : * Copy from one DTFS STRUCTURE to another.
81 : *-------------------------------------------------------------------*/
82 :
83 0 : void DTFS_copy_fx(
84 : DTFS_STRUCTURE *Xout_fx, /* o: DTFS structure */
85 : DTFS_STRUCTURE Xinp_fx /* i: DTFS structure */
86 : )
87 :
88 : {
89 :
90 : Word16 k;
91 0 : FOR( k = 0; k < MAXLAG_WI; k++ )
92 : {
93 0 : Xout_fx->a_fx[k] = Xinp_fx.a_fx[k]; /* Q(DTFS_STRUCTURE.Q) */
94 0 : move16();
95 : }
96 :
97 0 : FOR( k = 0; k < MAXLAG_WI; k++ )
98 : {
99 0 : Xout_fx->b_fx[k] = Xinp_fx.b_fx[k]; /* Q(DTFS_STRUCTURE.Q) */
100 0 : move16();
101 : }
102 :
103 0 : Xout_fx->lag_fx = Xinp_fx.lag_fx;
104 0 : move16();
105 0 : Xout_fx->nH_fx = Xinp_fx.nH_fx;
106 0 : move16();
107 0 : Xout_fx->nH_4kHz_fx = Xinp_fx.nH_4kHz_fx;
108 0 : move16();
109 0 : Xout_fx->upper_cut_off_freq_of_interest_fx = Xinp_fx.upper_cut_off_freq_of_interest_fx; // Q0
110 0 : move16();
111 0 : Xout_fx->upper_cut_off_freq_fx = Xinp_fx.upper_cut_off_freq_fx; // Q0
112 0 : move16();
113 0 : Xout_fx->Q = Xinp_fx.Q;
114 0 : move16();
115 0 : return;
116 : }
117 :
118 :
119 : /*-------------------------------------------------------------------*
120 : * DTFS_sub_fx()
121 : *
122 : * Subtract one DTFS STRUCTURE from another.
123 : *-------------------------------------------------------------------*/
124 0 : void DTFS_sub_fx(
125 : DTFS_STRUCTURE *tmp, /* o: X1 - X2 */
126 : DTFS_STRUCTURE X1, /* i: DTFS input 1 */
127 : DTFS_STRUCTURE X2 /* i: DTFS input 2 */
128 : )
129 : {
130 : Word16 i, sft, tmp_loop;
131 0 : sft = sub( X1.Q, X2.Q );
132 :
133 0 : IF( sft > 0 )
134 : {
135 0 : tmp_loop = shr( X1.lag_fx, 1 );
136 0 : FOR( i = 0; i <= tmp_loop; i++ )
137 : {
138 :
139 0 : tmp->a_fx[i] = sub( shr( X1.a_fx[i], sft ), X2.a_fx[i] );
140 0 : move16();
141 0 : tmp->b_fx[i] = sub( shr( X1.b_fx[i], sft ), X2.b_fx[i] );
142 0 : move16();
143 : }
144 0 : tmp->Q = X2.Q;
145 0 : move16();
146 : }
147 : ELSE
148 : {
149 0 : tmp_loop = shr( X1.lag_fx, 1 );
150 0 : FOR( i = 0; i <= tmp_loop; i++ )
151 : {
152 :
153 0 : tmp->a_fx[i] = sub( X1.a_fx[i], shl( X2.a_fx[i], sft ) );
154 0 : move16();
155 0 : tmp->b_fx[i] = sub( X1.b_fx[i], shl( X2.b_fx[i], sft ) );
156 0 : move16();
157 : }
158 0 : tmp->Q = X1.Q;
159 0 : move16();
160 : }
161 0 : tmp->lag_fx = s_max( X1.lag_fx, X2.lag_fx );
162 0 : move16();
163 0 : tmp->nH_fx = s_max( X1.nH_fx, X2.nH_fx );
164 0 : move16();
165 0 : tmp->nH_4kHz_fx = s_max( X1.nH_4kHz_fx, X2.nH_4kHz_fx );
166 0 : move16();
167 0 : tmp->upper_cut_off_freq_of_interest_fx = X1.upper_cut_off_freq_of_interest_fx;
168 0 : move16();
169 0 : tmp->upper_cut_off_freq_fx = X1.upper_cut_off_freq_fx;
170 0 : move16();
171 0 : return;
172 : }
173 : /*-------------------------------------------------------------------*
174 : * DTFS_fast_fs_inv()
175 : *
176 : * DTFS inverse.
177 : *-------------------------------------------------------------------*/
178 :
179 0 : static void DTFS_fast_fs_inv_fx( DTFS_STRUCTURE *X_fx, Word16 *out_fx /* Q0 */, Word16 N_fx, Word16 LOG2N )
180 : {
181 : Word16 i, M_2, N_2, s;
182 : Word16 dbuf_fx[256 + 1];
183 :
184 0 : M_2 = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
185 0 : N_2 = shr( N_fx, 1 );
186 0 : s = negate( X_fx->Q );
187 :
188 0 : dbuf_fx[0] = X_fx->a_fx[0];
189 0 : move16();
190 0 : dbuf_fx[1] = 0;
191 0 : move16();
192 :
193 0 : FOR( i = 1; i <= M_2; i++ )
194 : {
195 0 : dbuf_fx[2 * i] = shl_r( X_fx->a_fx[i], s );
196 0 : move16();
197 0 : dbuf_fx[( 2 * i + 1 )] = shl_r( X_fx->b_fx[i], s );
198 0 : move16();
199 : }
200 :
201 0 : FOR( ; i < N_2; i++ )
202 : {
203 0 : dbuf_fx[2 * i] = 0;
204 0 : move16();
205 0 : dbuf_fx[( 2 * i + 1 )] = 0;
206 0 : move16();
207 : }
208 :
209 : /* do IFFT */
210 0 : r_fft_4_fx( dbuf_fx, N_fx, sub( LOG2N, 1 ), -1 );
211 :
212 0 : FOR( i = 0; i < N_fx; i++ )
213 : {
214 0 : out_fx[i] = dbuf_fx[i]; /* Q0 */
215 0 : move16();
216 : }
217 0 : }
218 :
219 :
220 : /*===================================================================*/
221 : /* FUNCTION : DTFS_alignment_weight_fx () */
222 : /*-------------------------------------------------------------------*/
223 : /* PURPOSE : search for alignment */
224 : /*-------------------------------------------------------------------*/
225 : /* INPUT ARGUMENTS : */
226 : /* _ (struct DTFS_STRUCTURE) X1_fx : a/b in X1.Q */
227 : /* _ (struct DTFS_STRUCTURE) X2_fx : a/b in X2.Q */
228 : /* _ (Word16) Eshift: Q7 */
229 : /* _ (Word16 *) LPC1: lpc coefficients. Q12 */
230 : /* _ (Word16 *) LPC2: lpc coefficients. Q12 */
231 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
232 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
233 : /*-------------------------------------------------------------------*/
234 : /* OUTPUT ARGUMENTS : */
235 : /* _ (Word16) fshift_fx : Q7 */
236 : /*-------------------------------------------------------------------*/
237 : /* INPUT/OUTPUT ARGUMENTS : */
238 : /* _ None */
239 : /*-------------------------------------------------------------------*/
240 : /* RETURN ARGUMENTS : _ None. */
241 : /*-------------------------------------------------------------------*/
242 : /* CALLED FROM : TX/RX */
243 : /*===================================================================*/
244 :
245 0 : static Word16 DTFS_alignment_weight_fx(
246 : DTFS_STRUCTURE *X_fx, /* i : X1 the reference DTFS to keep fixed */
247 : DTFS_STRUCTURE X2, /* i : X2 the test DTFS to shift to find best match */
248 : Word16 Eshift, /* i : Expected shift - coarse value */
249 : const Word16 *LPC1, /* i : LPC to filter to find correlation in spch */
250 : const Word16 *LPC2, /* i : LPC to filter to find correlation in spch */
251 : Word16 *S_fx,
252 : Word16 *C_fx,
253 : Word16 *pf_temp1,
254 : Word16 *pf_temp2,
255 : Word16 *pf_temp,
256 : Word16 *pf_n2 )
257 : {
258 : /* Eshift is w.r.t X2 */
259 : Word16 k, HalfLag, start, end, ab1[MAXLAG_WI], ab2[MAXLAG_WI];
260 : Word32 corr_fx;
261 : Word32 maxcorr_fx, wcorr_fx, diff_corr;
262 : DTFS_STRUCTURE X1;
263 : Word16 temp, temp1, Qcorr, Qmaxcorr, inv_lag, n_fx, diff_fx, fshift_fx, Adiff_fx;
264 : Word16 tmplpc_fx[M + 1];
265 : Word16 exp, tmp;
266 : Word32 L_tmp;
267 0 : diff_fx = 0; /* to avoid compilation warnings */
268 0 : move16();
269 :
270 0 : DTFS_copy_fx( &X1, *X_fx ); /* X1 = *X_fx ; */
271 0 : DTFS_adjustLag_fx( &X1, X2.lag_fx );
272 :
273 : /* PORTING: Handling the functions with variable no. of arguments */
274 0 : poleFilter_setup_fx( LPC1, add( M, 1 ), X1, S_fx, C_fx, pf_temp1, pf_temp2, pf_temp, pf_n2 );
275 0 : DTFS_poleFilter_fx_9( &X1, pf_temp1, pf_temp2, pf_temp, pf_n2 );
276 0 : FOR( k = 0; k < M + 1; k++ )
277 : {
278 0 : tmplpc_fx[k] = mult_r( LPC1[k], pwf_fx[k] ); /* Q12 */
279 0 : move16();
280 : }
281 0 : DTFS_zeroFilter_fx( &X1, tmplpc_fx, M + 1, S_fx, C_fx );
282 :
283 : /* PORTING: Handling the functions with variable no. of arguments */
284 0 : poleFilter_setup_fx( LPC2, add( M, 1 ), X2, S_fx, C_fx, pf_temp1, pf_temp2, pf_temp, pf_n2 );
285 0 : DTFS_poleFilter_fx_9( &X2, pf_temp1, pf_temp2, pf_temp, pf_n2 );
286 0 : FOR( k = 0; k < M + 1; k++ )
287 : {
288 0 : tmplpc_fx[k] = mult_r( LPC2[k], pwf_fx[k] ); /* Q12 */
289 0 : move16();
290 : }
291 0 : DTFS_zeroFilter_fx( &X2, tmplpc_fx, M + 1, S_fx, C_fx );
292 :
293 0 : maxcorr_fx = L_add( MIN_32, 0 );
294 0 : Qmaxcorr = 0;
295 0 : move16();
296 0 : fshift_fx = Eshift;
297 0 : move16();
298 0 : Adiff_fx = s_max( 768, mult_r( 4915, shl( X2.lag_fx, 7 ) ) ); /* Q7, 768=6*128, 4915 = 0.15*32768 */
299 :
300 0 : if ( LT_16( X2.lag_fx, 60 ) )
301 : {
302 0 : diff_fx = 32;
303 0 : move16(); /* Q7 of 0.25 */
304 : }
305 0 : if ( GE_16( X2.lag_fx, 60 ) )
306 : {
307 0 : diff_fx = 64;
308 0 : move16(); /* Q7 of 0.5 */
309 : }
310 :
311 0 : HalfLag = s_min( shr( X2.lag_fx, 1 ), X2.nH_4kHz_fx );
312 0 : exp = norm_s( X2.lag_fx );
313 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X2.lag_fx );
314 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
315 0 : inv_lag = round_fx( L_tmp );
316 :
317 0 : FOR( k = 0; k <= HalfLag; k++ )
318 : {
319 0 : ab1[k] = round_fx_sat( L_mac0_sat( L_mult0( X1.a_fx[k], X2.a_fx[k] ), X1.b_fx[k], X2.b_fx[k] ) ); /* Q(-15) */
320 0 : ab2[k] = round_fx_sat( L_msu0_sat( L_mult0( X1.a_fx[k], X2.b_fx[k] ), X1.b_fx[k], X2.a_fx[k] ) ); /* Q(-15) */
321 0 : move16();
322 0 : move16();
323 : }
324 :
325 0 : start = sub( Eshift, Adiff_fx );
326 0 : end = add( Eshift, Adiff_fx );
327 :
328 0 : FOR( n_fx = start; n_fx <= end; n_fx += diff_fx )
329 : {
330 : /* Q7 */
331 0 : corr_fx = L_deposit_l( 0 );
332 0 : temp = 0;
333 0 : move16();
334 0 : temp1 = round_fx( L_shr( L_mult( inv_lag, n_fx ), 2 ) ); /* Q(19+7+1-2-16)=Q9 of n/X2.lag */
335 :
336 0 : if ( temp1 < 0 )
337 : {
338 0 : temp1 = add( temp1, 0x200 ); /* avoid negative, 0x200 is Q9 of 1 ( 2*pi ) */
339 : }
340 :
341 0 : FOR( k = 0; k <= HalfLag; k++ )
342 : {
343 0 : corr_fx = L_mac0_sat( corr_fx, ab1[k], cos_table[s_and( temp, 511 )] );
344 0 : corr_fx = L_mac0_sat( corr_fx, ab2[k], cos_table[s_and( add( temp, 128 ), 511 )] );
345 0 : move32();
346 0 : move32();
347 0 : temp = add( temp, temp1 );
348 : }
349 0 : temp = sub( 8192, mult_r( 20972, abs_s( sub( n_fx, Eshift ) ) ) ); /* Q13, 20972 = Q21 of 0.01. */
350 0 : Qcorr = norm_l( corr_fx );
351 0 : if ( corr_fx == 0 )
352 : {
353 0 : Qcorr = 31;
354 0 : move16();
355 : }
356 :
357 0 : temp1 = round_fx_sat( (Word32) L_shl_sat( corr_fx, Qcorr ) ); /* Q(Qcorr-16) */
358 0 : wcorr_fx = L_mult_sat( temp1, shl_sat( temp, 2 ) ); /* Q(Qcorr-16+13+2+1)=Q(Qcorr) */ //!!sat
359 0 : IF( GE_16( Qmaxcorr, Qcorr ) )
360 : {
361 0 : diff_corr = L_sub_sat( wcorr_fx, L_shl_sat( maxcorr_fx, sub( Qcorr, Qmaxcorr ) ) ); /* Qcorr */
362 : }
363 : ELSE
364 : {
365 0 : diff_corr = L_sub_sat( L_shl_sat( wcorr_fx, sub( Qmaxcorr, Qcorr ) ), maxcorr_fx ); /* Qmaxcorr */
366 : }
367 :
368 0 : IF( diff_corr > 0 )
369 : {
370 0 : fshift_fx = n_fx;
371 0 : move16();
372 0 : maxcorr_fx = L_add( wcorr_fx, 0 );
373 0 : Qmaxcorr = Qcorr;
374 0 : move16();
375 : }
376 : }
377 :
378 0 : return fshift_fx;
379 : }
380 :
381 :
382 : /*===========================================================================*/
383 : /* FUNCTION : DTFS_alignment_full_fx () */
384 : /*---------------------------------------------------------------------------*/
385 : /* PURPOSE : search for alignment */
386 : /*---------------------------------------------------------------------------*/
387 : /* INPUT ARGUMENTS : */
388 : /* _ (struct DTFS_STRUCTURE) X1_DTFS_fx : a_fx/b_fx in X1_DTFS_fx.Q */
389 : /* _ (struct DTFS_STRUCTURE) X2_DTFS_fx : a_fx/b_fx in X2_DTFS_fx.Q */
390 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
391 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
392 : /*---------------------------------------------------------------------------*/
393 : /* OUTPUT ARGUMENTS : */
394 : /* _ (Word16) fshift_fx : Q1 */
395 : /*---------------------------------------------------------------------------*/
396 : /* INPUT/OUTPUT ARGUMENTS : */
397 : /* _ None */
398 : /*---------------------------------------------------------------------------*/
399 : /* RETURN ARGUMENTS : _ None. */
400 : /*---------------------------------------------------------------------------*/
401 : /* CALLED FROM : TX */
402 : /*===========================================================================*/
403 :
404 0 : Word16 DTFS_alignment_full_fx(
405 : DTFS_STRUCTURE X1_DTFS_fx,
406 : DTFS_STRUCTURE X2_DTFS_fx,
407 : Word16 ph_offset_fx,
408 : Word16 *S_fx,
409 : Word16 *C_fx,
410 : Word16 FR_flag )
411 : {
412 : Word16 temp, temp1, k, start, end, HalfLag, ab1[MAXLAG_WI], ab2[MAXLAG_WI];
413 : Word16 n, fshift_fx;
414 : Word32 corr_fx, maxcorr_fx;
415 : Word16 Eshift, Adiff_fx;
416 :
417 : /* Calculating the expected alignment shift */
418 0 : Eshift = mult_r( ph_offset_fx, shl( X2_DTFS_fx.lag_fx, 7 ) ); /* confirmed I<2 by smv12.org, Q7 */
419 0 : find_rem( (Word16) L_FRAME, shr( add( X2_DTFS_fx.lag_fx, X1_DTFS_fx.lag_fx ), 1 ), &temp );
420 0 : temp = add_sat( shl( temp, 7 ), Eshift ); /* Q7 */
421 0 : IF( temp < 0 )
422 : {
423 0 : temp = add_sat( temp, shl_sat( X1_DTFS_fx.lag_fx, 7 ) ); /* Q7 */
424 : }
425 0 : find_rem( temp, shl( X1_DTFS_fx.lag_fx, 7 ), &Eshift ); /* Q7 */
426 0 : Eshift = shl( shr( Eshift, 7 ), 1 ); /* Q1 but integer */
427 :
428 0 : IF( GT_16( X2_DTFS_fx.lag_fx, 60 ) )
429 : {
430 0 : Adiff_fx = shl( X2_DTFS_fx.lag_fx, 1 - 3 ); /* lag_fx/8 in Q1 */
431 : }
432 : ELSE
433 : {
434 0 : Adiff_fx = shl( X2_DTFS_fx.lag_fx, 1 - 2 ); /* lag_fx/4 in Q1 */
435 : }
436 :
437 :
438 0 : IF( LT_16( X1_DTFS_fx.lag_fx, X2_DTFS_fx.lag_fx ) )
439 : {
440 0 : DTFS_zeroPadd_fx( X2_DTFS_fx.lag_fx, &X1_DTFS_fx );
441 : }
442 :
443 0 : maxcorr_fx = L_add( MIN_32, 0 );
444 0 : HalfLag = s_min( shr( X2_DTFS_fx.lag_fx, 1 ), X2_DTFS_fx.nH_4kHz_fx );
445 :
446 0 : FOR( k = 0; k <= HalfLag; k++ )
447 : {
448 0 : ab1[k] = round_fx_sat( L_mac_sat( L_mult_sat( X1_DTFS_fx.a_fx[k], X2_DTFS_fx.a_fx[k] ), X1_DTFS_fx.b_fx[k], X2_DTFS_fx.b_fx[k] ) ); /* Q(-15); */
449 0 : ab2[k] = round_fx_sat( L_msu_sat( L_mult_sat( X1_DTFS_fx.b_fx[k], X2_DTFS_fx.a_fx[k] ), X1_DTFS_fx.a_fx[k], X2_DTFS_fx.b_fx[k] ) ); /* Q(-15); */
450 : }
451 0 : IF( FR_flag == 0 )
452 : {
453 0 : start = sub( Eshift, Adiff_fx ); /*Q1*/
454 0 : end = add( Eshift, Adiff_fx ); /*Q1*/
455 : }
456 : ELSE
457 : {
458 : /*
459 : in FR mode, we cannot save and cary forward ph_offset as in VBR mode encoder "ph_offset_E_fx",
460 : so we set ph_offset_fx = 0, as a result we cannot accurately estimate the expected alignment shift Eshift to limit the search,
461 : so we search the full range [0 to X2_DTFS_fx.lag_fx] similar to FL
462 : */
463 0 : start = 0; /*Q1*/
464 0 : end = shl( X2_DTFS_fx.lag_fx, 1 ); /*Q1*/
465 : }
466 :
467 0 : fshift_fx = start;
468 0 : move16(); /*Q1*/
469 :
470 0 : FOR( n = start; n <= end; n++ ) /*n in Q1*/
471 : {
472 : /* Q1 */
473 0 : corr_fx = L_deposit_l( 0 );
474 0 : temp = 0;
475 0 : move16();
476 0 : temp1 = add( n, shl( X2_DTFS_fx.lag_fx, 1 ) ); /* add lag_fx in Q1to make positive */
477 0 : FOR( k = 0; k <= HalfLag; k++ )
478 : {
479 0 : corr_fx = L_mac_sat( corr_fx, ab1[k], C_fx[( 2 * temp ) % ( 4 * X2_DTFS_fx.lag_fx )] );
480 0 : corr_fx = L_mac_sat( corr_fx, ab2[k], S_fx[( 2 * temp ) % ( 4 * X2_DTFS_fx.lag_fx )] );
481 0 : temp = add_sat( temp, temp1 );
482 : }
483 :
484 0 : IF( GT_32( corr_fx, maxcorr_fx ) )
485 : {
486 0 : fshift_fx = n; /* Q1 */
487 0 : move16();
488 0 : maxcorr_fx = L_add( corr_fx, 0 );
489 : }
490 : }
491 :
492 0 : return fshift_fx; /*Q1*/
493 : }
494 : /*===================================================================*/
495 : /* FUNCTION : DTFS_phaseShift_fx () */
496 : /*-------------------------------------------------------------------*/
497 : /* PURPOSE : Phase is shifted by 2pi*ph/Lag */
498 : /*-------------------------------------------------------------------*/
499 : /* INPUT ARGUMENTS : */
500 : /* _ (Word16) ph: shift index, Q7 */
501 : /* _ (Word16) Lag: Pitch Lag value as for shift */
502 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
503 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
504 : /*-------------------------------------------------------------------*/
505 : /* OUTPUT ARGUMENTS : */
506 : /* _ None. */
507 : /*-------------------------------------------------------------------*/
508 : /* INPUT/OUTPUT ARGUMENTS : */
509 : /* _ (struct DTFS_fx) X1 : a/b in X1.Q */
510 : /*-------------------------------------------------------------------*/
511 : /* RETURN ARGUMENTS : _ None. */
512 : /*-------------------------------------------------------------------*/
513 : /* CALLED FROM : TX/RX */
514 : /*===================================================================*/
515 :
516 0 : void DTFS_phaseShift_fx( DTFS_STRUCTURE *X_fx, Word16 ph, Word16 Lag, Word16 *S_fx, Word16 *C_fx )
517 : {
518 : Word16 k;
519 : Word16 temp, HalfLag;
520 : Word32 L_temp, L_temp2;
521 :
522 0 : L_temp2 = L_deposit_l( 0 );
523 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
524 :
525 0 : IF( ph > 0 )
526 : {
527 0 : FOR( k = 0; k <= HalfLag; k++ )
528 : {
529 0 : temp = X_fx->a_fx[k];
530 0 : L_temp = L_shr( L_temp2, 5 ); /* Q2 */
531 0 : X_fx->a_fx[k] = round_fx( L_msu( L_mult( temp, C_fx[L_temp % ( 4 * Lag )] ), X_fx->b_fx[k], S_fx[L_temp % ( 4 * Lag )] ) ); /* X.Q */
532 0 : move16();
533 0 : X_fx->b_fx[k] = round_fx( L_mac( L_mult( X_fx->b_fx[k], C_fx[L_temp % ( 4 * Lag )] ), temp, S_fx[L_temp % ( 4 * Lag )] ) );
534 0 : move16();
535 0 : L_temp2 = L_add( L_temp2, ph );
536 : }
537 : }
538 :
539 0 : IF( ph < 0 )
540 : {
541 0 : FOR( k = 0; k <= HalfLag; k++ )
542 : {
543 0 : temp = X_fx->a_fx[k];
544 0 : L_temp = L_shr( L_negate( L_temp2 ), 5 ); /* Q2 */
545 0 : X_fx->a_fx[k] = round_fx( L_mac( L_mult( temp, C_fx[L_temp % ( 4 * Lag )] ), X_fx->b_fx[k], S_fx[L_temp % ( 4 * Lag )] ) ); /* X.Q */
546 0 : move16();
547 0 : X_fx->b_fx[k] = round_fx( L_msu( L_mult( X_fx->b_fx[k], C_fx[L_temp % ( 4 * Lag )] ), temp, S_fx[L_temp % ( 4 * Lag )] ) );
548 0 : move16();
549 0 : L_temp2 = L_add( L_temp2, ph );
550 : }
551 : }
552 0 : return;
553 : }
554 :
555 : /*===================================================================*/
556 : /* FUNCTION : Q2phaseShift_fx () */
557 : /*-------------------------------------------------------------------*/
558 : /* PURPOSE : Phase is shifted by 2pi*ph/Lag */
559 : /*-------------------------------------------------------------------*/
560 : /* INPUT ARGUMENTS : */
561 : /* _ (Word16) ph: shift index, Q2 */
562 : /* _ (Word16) Lag: Pitch Lag value as for shift */
563 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
564 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
565 : /*-------------------------------------------------------------------*/
566 : /* OUTPUT ARGUMENTS : */
567 : /* _ None. */
568 : /*-------------------------------------------------------------------*/
569 : /* INPUT/OUTPUT ARGUMENTS : */
570 : /* _ (struct DTFS_STRUCTURE) X_fx : a/b in X_fx->Q */
571 : /*-------------------------------------------------------------------*/
572 : /* RETURN ARGUMENTS : _ None. */
573 : /*-------------------------------------------------------------------*/
574 : /* CALLED FROM : TX/RX */
575 : /*===================================================================*/
576 :
577 0 : void Q2phaseShift_fx(
578 : DTFS_STRUCTURE *X_fx,
579 : Word16 ph,
580 : Word16 Lag,
581 : Word16 *S_fx,
582 : Word16 *C_fx )
583 : {
584 : Word16 k;
585 : Word16 temp, HalfLag;
586 : Word32 temp2;
587 0 : temp2 = L_deposit_l( 0 );
588 :
589 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
590 :
591 :
592 0 : IF( ph > 0 )
593 : {
594 0 : FOR( k = 0; k <= HalfLag; k++ )
595 : {
596 0 : temp = X_fx->a_fx[k];
597 0 : X_fx->a_fx[k] = round_fx_sat( L_msu_sat( L_mult_sat( temp, C_fx[temp2 % ( 4 * Lag )] ), X_fx->b_fx[k], S_fx[temp2 % ( 4 * Lag )] ) ); /* X.Q */
598 0 : X_fx->b_fx[k] = round_fx_sat( L_mac_sat( L_mult_sat( X_fx->b_fx[k], C_fx[temp2 % ( 4 * Lag )] ), temp, S_fx[temp2 % ( 4 * Lag )] ) );
599 0 : temp2 = L_add_sat( temp2, (Word32) ph );
600 0 : move16();
601 0 : move16();
602 : }
603 : }
604 :
605 :
606 0 : IF( ph < 0 )
607 : {
608 0 : FOR( k = 0; k <= HalfLag; k++ )
609 : {
610 0 : temp = X_fx->a_fx[k];
611 0 : X_fx->a_fx[k] = round_fx_sat( L_mac_sat( L_mult_sat( temp, C_fx[temp2 % ( 4 * Lag )] ), X_fx->b_fx[k], S_fx[temp2 % ( 4 * Lag )] ) ); /* X.Q */
612 0 : X_fx->b_fx[k] = round_fx_sat( L_msu_sat( L_mult_sat( X_fx->b_fx[k], C_fx[temp2 % ( 4 * Lag )] ), temp, S_fx[temp2 % ( 4 * Lag )] ) );
613 0 : temp2 = add_sat( (Word16) temp2, negate( ph ) );
614 0 : move16();
615 0 : move16();
616 : }
617 : }
618 0 : }
619 : /*===================================================================*/
620 : /* FUNCTION : DTFS_zeroPadd_fx () */
621 : /*-------------------------------------------------------------------*/
622 : /* PURPOSE : zeroPadding */
623 : /*-------------------------------------------------------------------*/
624 : /* INPUT ARGUMENTS : */
625 : /* _ (Word16) N_fx: length , Q0 */
626 : /*-------------------------------------------------------------------*/
627 : /* OUTPUT ARGUMENTS : */
628 : /* _ None. */
629 : /*-------------------------------------------------------------------*/
630 : /* INPUT/OUTPUT ARGUMENTS : */
631 : /* _ (struct DTFS_STRUCTURE) X_fx : a/b in X_fx.Q i.e Q6 */
632 : /* _ (Word16) lag: pitch lag of *X_fx, Q0 */
633 : /*-------------------------------------------------------------------*/
634 : /* RETURN ARGUMENTS : _ None. */
635 : /*-------------------------------------------------------------------*/
636 : /* CALLED FROM : TX/RX */
637 : /*===================================================================*/
638 :
639 0 : void DTFS_zeroPadd_fx(
640 : Word16 N_fx,
641 : DTFS_STRUCTURE *X_fx )
642 : {
643 : Word16 i, start, end, diff_fx, rem_fx;
644 :
645 0 : IF( EQ_16( N_fx, X_fx->lag_fx ) )
646 : {
647 0 : return;
648 : }
649 0 : start = add( shr( X_fx->lag_fx, 1 ), 1 );
650 0 : end = shr( N_fx, 1 );
651 :
652 0 : move16();
653 0 : FOR( i = start; i <= end; i++ )
654 : {
655 0 : X_fx->a_fx[i] = 0;
656 0 : move16();
657 0 : X_fx->b_fx[i] = 0;
658 0 : move16();
659 : }
660 :
661 0 : X_fx->lag_fx = N_fx;
662 0 : move16();
663 :
664 : /* recompute nH for new lag */
665 0 : diff_fx = find_rem( 12800, X_fx->lag_fx, &rem_fx );
666 0 : X_fx->nH_fx = find_rem( X_fx->upper_cut_off_freq_fx, diff_fx, &rem_fx );
667 0 : move16();
668 :
669 0 : if ( GE_16( sub( X_fx->upper_cut_off_freq_fx, shr( extract_l( L_mult( diff_fx, X_fx->nH_fx ) ), 1 ) ), diff_fx ) )
670 : {
671 0 : X_fx->nH_fx = add( X_fx->nH_fx, 1 );
672 0 : move16();
673 : }
674 : }
675 : /*===================================================================*/
676 : /* FUNCTION : to_fs_fx () */
677 : /*-------------------------------------------------------------------*/
678 : /* PURPOSE : */
679 : /*-------------------------------------------------------------------*/
680 : /* INPUT ARGUMENTS : */
681 : /* _ (Word16 *) x: input time domain series. Q0 */
682 : /* _ (Word16) N: Lag */
683 : /* _ (struct DTFS_STRUCTURE) PREVCW_FX: a/b in PREVCW_FX.Q */
684 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
685 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
686 : /*-------------------------------------------------------------------*/
687 : /* OUTPUT ARGUMENTS : */
688 : /* _ (struct DTFS_STRUCTURE) (Word16 a[]) : in Q */
689 : /* _ (struct DTFS_STRUCTURE) (Word16 b[]) : in Q */
690 : /* _ (struct DTFS_STRUCTURE) (Word16 lag) : Q0 */
691 : /* _ (struct DTFS_STRUCTURE) (Word16 Q ) : Q value of a/b */
692 : /*-------------------------------------------------------------------*/
693 : /* INPUT/OUTPUT ARGUMENTS : */
694 : /* _ None. */
695 : /*-------------------------------------------------------------------*/
696 : /* RETURN ARGUMENTS : _ None. */
697 : /*-------------------------------------------------------------------*/
698 : /* CALLED FROM : TX/RX */
699 : /*===================================================================*/
700 0 : void DTFS_to_fs_fx(
701 : const Word16 *x, /* i : time domain signal */
702 : Word16 N, /* i : Length of input vector */
703 : DTFS_STRUCTURE *X_fx, /* o : DTFS structure with a, b, lag */
704 : const Word16 Fs, /* i : sampling rate */
705 : const Word16 FR_flag, /* i : FR flag */
706 : Word16 *S_fx,
707 : Word16 *C_fx )
708 :
709 : {
710 : Word16 n, temp, temp_neg, inv_lag, sum, diff_fx;
711 : Word16 nH, k, nH_band, nH_4kHz;
712 : Word32 L_temp, Lx0;
713 : Word32 L_a, L_b, L_tmp;
714 : Word32 La[MAXLAG_WI], Lb[MAXLAG_WI], Labmax;
715 : Word16 exp, tmp;
716 : Word32 L_tmp1;
717 :
718 0 : IF( !FR_flag )
719 : {
720 0 : IF( EQ_16( Fs, 16000 ) )
721 : {
722 0 : X_fx->upper_cut_off_freq_of_interest_fx = 4000;
723 0 : move16();
724 0 : X_fx->upper_cut_off_freq_fx = 6400;
725 0 : move16();
726 0 : X_fx->Fs_fx = INT_FS_FX;
727 0 : move16();
728 : }
729 0 : ELSE IF( EQ_16( Fs, 8000 ) )
730 : {
731 0 : X_fx->upper_cut_off_freq_of_interest_fx = 3300;
732 0 : move16();
733 0 : X_fx->upper_cut_off_freq_fx = 4000;
734 0 : move16();
735 0 : X_fx->Fs_fx = INT_FS_FX;
736 0 : move16();
737 : }
738 : }
739 : ELSE
740 : {
741 0 : X_fx->upper_cut_off_freq_of_interest_fx = 8000;
742 0 : move16();
743 0 : X_fx->upper_cut_off_freq_fx = 8000;
744 0 : move16();
745 0 : X_fx->Fs_fx = 16000;
746 0 : move16();
747 : }
748 0 : X_fx->lag_fx = N;
749 0 : move16();
750 :
751 0 : exp = norm_s( X_fx->lag_fx );
752 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X_fx->lag_fx ); /* Q29-exp */
753 0 : L_tmp1 = L_mult( 12800, tmp ); /* Q(30-exp) */
754 0 : diff_fx = extract_h( L_shl_sat( L_tmp1, sub( exp, 14 ) ) ); /* Q0 */
755 :
756 0 : exp = norm_s( diff_fx );
757 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), diff_fx ); /* Q29-exp */
758 0 : L_tmp1 = L_mult_sat( X_fx->upper_cut_off_freq_fx, tmp ); /* Q(30-exp) */
759 0 : nH_band = extract_h( L_shl_sat( L_tmp1, sub( exp, 14 ) ) ); /* Q0 */
760 0 : nH_4kHz = mult( 10240, ( X_fx->lag_fx ) ); /* 4000/12800 in Q15 */
761 :
762 0 : if ( GE_16( sub( X_fx->upper_cut_off_freq_fx, shr( (Word16) L_mult( diff_fx, nH_band ), 1 ) ), diff_fx ) )
763 : {
764 0 : nH_band = add( nH_band, 1 );
765 : }
766 :
767 0 : if ( GE_16( sub( 4000, shr( (Word16) L_mult( diff_fx, nH_4kHz ), 1 ) ), diff_fx ) )
768 : {
769 0 : nH_4kHz = add( nH_4kHz, 1 );
770 : }
771 :
772 : /* Number of harmonics excluding the ones at 0 and at pi */
773 0 : nH = shr( sub( N, 1 ), 1 );
774 :
775 : /* The DC component */
776 0 : X_fx->a_fx[0] = 0;
777 0 : move16();
778 0 : X_fx->b_fx[0] = 0;
779 0 : move16();
780 :
781 0 : exp = norm_s( N );
782 0 : tmp = div_s( shl( 1, ( sub( 14, exp ) ) ), N );
783 0 : L_tmp = L_shl_sat( tmp, add( exp, 6 ) );
784 0 : inv_lag = round_fx_sat( L_tmp );
785 0 : Lx0 = L_deposit_h( x[0] );
786 0 : Labmax = L_deposit_l( 0 );
787 0 : FOR( k = 1; k <= nH; k++ )
788 : {
789 0 : L_a = L_shr( Lx0, 1 ); /* Q16 */
790 :
791 0 : L_b = L_deposit_l( 0 );
792 0 : sum = k;
793 0 : move16();
794 0 : temp = k;
795 0 : move16();
796 0 : FOR( n = 1; n < N; n++ )
797 : {
798 0 : L_a = L_mac0_sat( L_a, x[n], C_fx[( 4 * sum ) % ( 4 * N )] ); /* Q16 of x[n]*cos(sum) */
799 0 : L_b = L_mac0_sat( L_b, x[n], S_fx[( 4 * sum ) % ( 4 * N )] ); /* Q16 of x[n]*sin(sum) */
800 0 : sum = add( sum, temp );
801 : }
802 0 : La[k] = L_shr( L_a, 6 ); /* Q8 of a[k]*2.0 */
803 0 : move32();
804 0 : Lb[k] = L_shr( L_b, 6 ); /* Q8 of b[k]*2.0 */
805 0 : move32();
806 :
807 0 : L_temp = L_abs( La[k] );
808 :
809 0 : if ( GT_32( L_temp, Labmax ) )
810 : {
811 0 : Labmax = L_temp;
812 0 : move32();
813 : }
814 0 : L_temp = L_abs( Lb[k] );
815 :
816 0 : if ( GT_32( L_temp, Labmax ) )
817 : {
818 0 : Labmax = L_temp;
819 0 : move32();
820 : }
821 : }
822 :
823 :
824 : /* The harmonic at 'pi' */
825 :
826 : /* IF ( N%2 == 0 ) */
827 0 : IF( s_and( N, 1 ) == 0 )
828 : {
829 :
830 0 : L_a = L_deposit_l( 0 );
831 0 : temp = 1;
832 0 : move16();
833 0 : temp_neg = negate( temp );
834 0 : FOR( n = 0; n < N - 1; n += 2 )
835 : {
836 0 : L_a = L_mac_sat( L_a, x[n], temp ); /* Q1 */
837 0 : L_a = L_mac_sat( L_a, x[n + 1], temp_neg );
838 : /*temp= negate(temp); */
839 : }
840 0 : if ( s_and( N, 1 ) ) /*if N is odd we need to calculate last */
841 : {
842 0 : L_a = L_mac_sat( L_a, x[n], temp ); /* Q1 */
843 : }
844 :
845 0 : La[k] = L_shl( L_a, 7 );
846 0 : move32(); /* Q8 of a[k]*1.0 */
847 :
848 :
849 0 : L_temp = L_abs( La[k] );
850 :
851 0 : if ( GT_32( L_temp, Labmax ) )
852 : {
853 0 : Labmax = L_add( L_temp, 0 );
854 : }
855 :
856 0 : X_fx->b_fx[k] = 0;
857 0 : move16();
858 : }
859 :
860 0 : temp = norm_l( Labmax );
861 0 : if ( Labmax == 0 )
862 : {
863 0 : temp = 31;
864 0 : move16();
865 : }
866 :
867 0 : FOR( k = 1; k <= nH; k++ )
868 : {
869 0 : X_fx->a_fx[k] = round_fx_sat( L_shl_sat( La[k], temp ) ); /* Q(8+temp-16)=Q(temp-8) */
870 0 : move16();
871 0 : X_fx->a_fx[k] = mult_r_sat( X_fx->a_fx[k], inv_lag );
872 0 : move16(); /* Q(temp-8+19+1-16)=Q(temp-4) of a[k]*2.0/N */
873 0 : X_fx->b_fx[k] = round_fx_sat( L_shl_sat( Lb[k], temp ) ); /* Q(8+temp-16)=Q(temp-8) */
874 0 : move16();
875 0 : X_fx->b_fx[k] = mult_r_sat( X_fx->b_fx[k], inv_lag );
876 0 : move16(); /* Q(temp-8+19+1-16)=Q(temp-4) of b[k]*2.0/N */
877 : }
878 :
879 : /* IF ( N%2 == 0 ) */
880 0 : IF( s_and( N, 1 ) == 0 )
881 : {
882 0 : X_fx->a_fx[k] = round_fx_sat( L_shl_sat( La[k], temp ) ); /* Q(8+temp-16)=Q(temp-8) */
883 0 : X_fx->a_fx[k] = mult_r_sat( X_fx->a_fx[k], inv_lag );
884 0 : move16();
885 0 : move16(); /* Q(temp-8+19+1-16)=Q(temp-4) of a[k]*1.0/N */
886 0 : X_fx->b_fx[k] = 0;
887 0 : move16();
888 : }
889 :
890 0 : X_fx->Q = sub( temp, 4 );
891 0 : move16();
892 :
893 0 : tmp = s_min( shr( X_fx->lag_fx, 1 ), sub( MAXLAG_WI, 1 ) );
894 0 : FOR( k = ( nH_band + 1 ); k <= tmp; k++ )
895 : {
896 0 : X_fx->a_fx[k] = 0;
897 0 : move16();
898 0 : X_fx->b_fx[k] = 0;
899 0 : move16();
900 : }
901 0 : X_fx->nH_fx = nH_band;
902 0 : move16();
903 0 : X_fx->nH_4kHz_fx = nH_4kHz;
904 0 : move16();
905 0 : return;
906 : }
907 :
908 :
909 : /*===================================================================*/
910 : /* FUNCTION : DTFS_transform_fx () */
911 : /*-------------------------------------------------------------------*/
912 : /* PURPOSE : */
913 : /*-------------------------------------------------------------------*/
914 : /* INPUT ARGUMENTS : */
915 : /* _ (struct DTFS_STRUCTURE) X_fx: a/b in X1.Q, lag in Q0 */
916 : /* _ (struct DTFS_STRUCTURE) X2_fx. a/b in X2.Q, lag in Q0 */
917 : /* _ (Word16 *) phase_fx: 2pi normalized, Q12 */
918 : /* _ (Word16) N: length of series. */
919 : /*-------------------------------------------------------------------*/
920 : /* OUTPUT ARGUMENTS : */
921 : /* _ (Word16 *) out_fx: output transformed series. Q0. */
922 : /*-------------------------------------------------------------------*/
923 : /* INPUT/OUTPUT ARGUMENTS : */
924 : /* _ None. */
925 : /*-------------------------------------------------------------------*/
926 : /* RETURN ARGUMENTS : _ None. */
927 : /*-------------------------------------------------------------------*/
928 : /* CALLED FROM : TX/RX */
929 : /*===================================================================*/
930 0 : static void DTFS_transform_fx(
931 : DTFS_STRUCTURE X_fx, /* i : Starting DTFS to use in WI */
932 : DTFS_STRUCTURE X2_fx, /* i : Ending DTFS to use in WI */
933 : const Word32 *phase_fx, /* i : Phase contour */
934 : Word16 *out_fx, /* o : Output time domain waveform */
935 : Word16 N, /* i : Number of samples to generate */
936 : const Word16 FR_flag /* i : Flag to indicate called in FR context */
937 : )
938 : {
939 :
940 : Word16 i, j;
941 : Word32 L_temp_fx;
942 : Word16 w_fx;
943 : Word16 inv1_fx, inv2_fx, inv_fx, q1, q2;
944 : Word32 Lw_fx, temp32_fx;
945 : Word16 x2_256_fx[256], x1_256_fx[256];
946 : Word16 k, m, l1;
947 : Word32 x_r_fx[256];
948 : Word32 tmptmp3_40_fx;
949 : Word16 temp_w;
950 : Word16 N1;
951 0 : Word16 nrg_flag = 0;
952 0 : move16();
953 : Word32 L_tmp;
954 : Word16 tmp, tmp1, tmp2, frac, exp1, exp2;
955 : Word16 expa, expb, fraca, fracb, scale;
956 :
957 : ivas_error error;
958 :
959 : DTFS_STRUCTURE *tmp1_dtfs_fx;
960 : DTFS_STRUCTURE *tmp2_dtfs_fx;
961 : DTFS_STRUCTURE *tmp3_dtfs_fx;
962 :
963 0 : error = IVAS_ERR_OK;
964 0 : move16();
965 0 : set32_fx( x_r_fx, 0, 256 );
966 :
967 0 : IF( ( error = DTFS_new_fx( &tmp1_dtfs_fx ) ) != IVAS_ERR_OK )
968 : {
969 0 : IVAS_ERROR( error, "Error creating DTFS structure 1" );
970 : }
971 0 : IF( ( error = DTFS_new_fx( &tmp2_dtfs_fx ) ) != IVAS_ERR_OK )
972 : {
973 0 : IVAS_ERROR( error, "Error creating DTFS structure 2" );
974 : }
975 0 : IF( ( error = DTFS_new_fx( &tmp3_dtfs_fx ) ) != IVAS_ERR_OK )
976 : {
977 0 : IVAS_ERROR( error, "Error creating DTFS structure 3" );
978 : }
979 :
980 0 : DTFS_copy_fx( tmp1_dtfs_fx, X_fx );
981 0 : DTFS_copy_fx( tmp2_dtfs_fx, X2_fx );
982 :
983 0 : tmp2 = 0; /* to avoid compilation warnings */
984 0 : move16();
985 :
986 0 : DTFS_fast_fs_inv_fx( tmp1_dtfs_fx, x1_256_fx, 256, 8 );
987 0 : DTFS_fast_fs_inv_fx( tmp2_dtfs_fx, x2_256_fx, 256, 8 );
988 :
989 :
990 : /* L_temp_fx=invert_dp((Word40)N,4,&n,1); = 1/M, Q(61-n) */
991 : /* inv1_fx=round_fx(L_temp_fx); = 1/M in Q(45-n) */
992 : /* q1=sub(n,15); */
993 0 : exp1 = norm_s( N );
994 0 : inv1_fx = div_s( shl( 1, sub( 14, exp1 ) ), N ); /* 29-exp1 */
995 0 : q1 = add( exp1, 1 );
996 :
997 0 : tmp1 = sub( X_fx.lag_fx, N );
998 0 : exp2 = norm_s( tmp1 );
999 :
1000 0 : if ( tmp1 < 0 )
1001 : {
1002 0 : tmp2 = negate( tmp1 );
1003 : }
1004 0 : tmp = div_s( shl( 1, sub( 14, exp2 ) ), tmp2 ); /* 29-exp2 */
1005 0 : L_tmp = L_shl( tmp, 16 );
1006 :
1007 0 : if ( tmp1 < 0 )
1008 : {
1009 0 : L_tmp = L_negate( L_tmp );
1010 : }
1011 :
1012 0 : q2 = sub( exp2, 3 );
1013 :
1014 :
1015 : /*L_temp_fx=(Word32) Mpy_32_16(extract_h(L_tmp),extract_l(L_tmp),22904); move32(); */ /* L_temp=log(0.2)*log10(e)/(lag-M), Q(61-n) */
1016 0 : L_temp_fx = Mult_32_16( L_tmp, 22904 ); /* L_temp=log(0.2)*log10(e)/(lag-M), Q(61-n) */
1017 0 : inv2_fx = round_fx( L_temp_fx ); /* log(0.2)*log10(e)/(lag-M), Q(45-n) */
1018 : /* q2=sub(n,19); */ /* adjust Q factor to Q26 */
1019 :
1020 :
1021 0 : IF( GT_16( sub( N, WI_SAMPLE_THLD ), X_fx.lag_fx ) )
1022 : {
1023 0 : inv_fx = inv2_fx;
1024 0 : move16();
1025 : }
1026 : ELSE
1027 : {
1028 0 : inv_fx = inv1_fx;
1029 0 : move16();
1030 0 : exp2 = exp1;
1031 0 : move16();
1032 : }
1033 :
1034 0 : Lw_fx = L_deposit_l( inv_fx );
1035 :
1036 0 : FOR( i = 0; i < N; i++ )
1037 : {
1038 0 : IF( FR_flag == 0 )
1039 : {
1040 0 : IF( GT_16( sub( N, WI_SAMPLE_THLD ), X_fx.lag_fx ) )
1041 : {
1042 :
1043 0 : L_tmp = L_shl( Lw_fx, q2 ); /* Q29-exp2+q2 */
1044 0 : L_tmp = Mult_32_16( L_tmp, 27213 ); /* 3.321928 in Q13 -> 16+13+1 */ /*27-exp2 */
1045 0 : L_tmp = L_shl( L_tmp, sub( exp2, add( q2, 11 ) ) );
1046 0 : frac = L_Extract_lc( L_tmp, &exp1 ); /* Extract exponent */
1047 0 : L_temp_fx = Pow2( 14, frac );
1048 0 : exp1 = sub( exp1, 14 );
1049 0 : L_temp_fx = L_shl( L_temp_fx, add( exp1, 15 ) ); /* Q15 */
1050 :
1051 0 : w_fx = sub( 16384, extract_h( L_shl( L_temp_fx, 15 ) ) ); /* w_fx in Q14 1- exp(- (i+1) * log(.2)/(lag-M)) */
1052 : }
1053 : ELSE
1054 : {
1055 0 : w_fx = round_fx( L_shl( Lw_fx, q1 ) ); /* Q14 */
1056 : }
1057 : }
1058 : ELSE
1059 : {
1060 0 : IF( nrg_flag )
1061 : {
1062 0 : w_fx = round_fx( L_shl( Lw_fx, q1 ) ); /* Q14 */
1063 : }
1064 : ELSE
1065 : {
1066 :
1067 0 : N1 = sub( N, tmp2_dtfs_fx->lag_fx );
1068 :
1069 0 : IF( LT_16( i, N1 ) )
1070 : /* w = (i+1)/N1; */
1071 : {
1072 :
1073 : /* w = (i+1)/N1; */
1074 0 : IF( N1 )
1075 : {
1076 0 : expa = norm_s( N1 );
1077 0 : fraca = shl( N1, expa );
1078 0 : expa = sub( 14, expa );
1079 :
1080 0 : tmp = add( i, 1 );
1081 0 : expb = norm_s( tmp );
1082 0 : fracb = shl( tmp, expb );
1083 0 : expb = sub( 14, expb );
1084 :
1085 0 : scale = shr( sub( fraca, fracb ), 15 );
1086 0 : fracb = shl( fracb, scale );
1087 0 : expb = sub( expb, scale );
1088 :
1089 0 : w_fx = div_s( fracb, fraca );
1090 0 : exp1 = sub( expb, expa );
1091 0 : w_fx = shl( w_fx, sub( exp1, 1 ) ); /*Q14*/
1092 : }
1093 : ELSE
1094 : {
1095 0 : w_fx = 0;
1096 0 : move16();
1097 : }
1098 :
1099 0 : Lw_fx = L_deposit_h( w_fx );
1100 : }
1101 : ELSE
1102 : {
1103 0 : w_fx = 16384; // 1.Q14
1104 0 : move16();
1105 : }
1106 : }
1107 : }
1108 :
1109 0 : Lw_fx = L_add( Lw_fx, inv_fx ); /* (i+1)*inv */
1110 : /* mapping phase to 8x256 length signal */
1111 0 : temp32_fx = phase_fx[i]; /* Q(27-11)=Q16 due to multiplication by pow(2.0,11) */
1112 0 : move32();
1113 0 : j = rint_new_fx( temp32_fx );
1114 0 : j = s_and( j, 0x07ff );
1115 :
1116 : /* k=j%8; */
1117 0 : k = s_and( j, 7 );
1118 0 : l1 = shr( j, 3 ); /* reminder and quotient */
1119 :
1120 0 : temp_w = sub( 16384, w_fx );
1121 0 : tmptmp3_40_fx = L_deposit_l( 0 );
1122 :
1123 0 : FOR( j = 0; j < 12; j++ )
1124 : {
1125 0 : m = L_add( 1000 * LL - OSLENGTH / 2, add( l1, j ) ) % LL; /* use circular addressing */
1126 0 : x_r_fx[m] = L_mac( L_mult( x1_256_fx[m], temp_w ), x2_256_fx[m], w_fx );
1127 0 : move32();
1128 : }
1129 0 : tmptmp3_40_fx = L_deposit_l( 0 );
1130 :
1131 :
1132 0 : FOR( j = 0; j < 12; j++ )
1133 : {
1134 0 : m = L_add( 1000 * LL - OSLENGTH / 2, add( l1, j ) ) % LL; /* use circular addressing */
1135 0 : tmptmp3_40_fx = L_add( tmptmp3_40_fx, Mult_32_16( x_r_fx[m], sinc_fx[k][j] ) );
1136 : }
1137 0 : out_fx[i] = round_fx_sat( L_shl_sat( tmptmp3_40_fx, 2 ) );
1138 0 : move16();
1139 : }
1140 :
1141 :
1142 0 : free( tmp1_dtfs_fx );
1143 0 : free( tmp2_dtfs_fx );
1144 0 : free( tmp3_dtfs_fx );
1145 0 : return;
1146 : }
1147 : /*===================================================================*/
1148 : /* FUNCTION : zeroFilter_fx() */
1149 : /*-------------------------------------------------------------------*/
1150 : /* PURPOSE : zero filtering */
1151 : /*-------------------------------------------------------------------*/
1152 : /* INPUT ARGUMENTS : */
1153 : /* _ (Word16) lpc[] : lpc coefficients in Q12 */
1154 : /* _ (Word16) N : lpc order */
1155 : /* _ (Word16) this->lag: in Q0 */
1156 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
1157 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
1158 : /*-------------------------------------------------------------------*/
1159 : /* OUTPUT ARGUMENTS : */
1160 : /* _ None. */
1161 : /*-------------------------------------------------------------------*/
1162 : /* INPUT/OUTPUT ARGUMENTS : */
1163 : /* _ (Word16) this->a[] : in Q(this->Q) */
1164 : /* _ (Word16) this->b[] : in Q(this->Q) */
1165 : /* _ (Word16) this->Q: in Q0 */
1166 : /*-------------------------------------------------------------------*/
1167 : /* RETURN ARGUMENTS : _ None. */
1168 : /*-------------------------------------------------------------------*/
1169 : /* CALLED FROM : TX/RX */
1170 : /*===================================================================*/
1171 :
1172 0 : void DTFS_zeroFilter_fx(
1173 : DTFS_STRUCTURE *X_fx,
1174 : Word16 *LPC,
1175 : Word16 N,
1176 : Word16 *S_fx,
1177 : Word16 *C_fx )
1178 : {
1179 : Word32 sum1_fx, sum2_fx;
1180 : Word16 k, n, HalfLag;
1181 : Word16 temp, temp1, temp2;
1182 : Word32 L_temp1, L_temp2;
1183 : Word16 Qmin, Qab[MAXLAG_WI], na, nb;
1184 0 : Qmin = 32767;
1185 0 : move16();
1186 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
1187 0 : FOR( k = 0; k <= HalfLag; k++ )
1188 : {
1189 0 : temp = k;
1190 0 : move16();
1191 0 : temp2 = k;
1192 0 : move16();
1193 :
1194 : /* Calculate sum1 and sum2 */
1195 0 : sum1_fx = L_deposit_h( 4096 ); /* 1: Q(12+15+1) */
1196 0 : sum2_fx = L_deposit_l( 0 );
1197 :
1198 0 : FOR( n = 0; n < N; n++ )
1199 : {
1200 0 : sum1_fx = L_mac_sat( sum1_fx, LPC[n], C_fx[( 4 * temp2 ) % ( 4 * X_fx->lag_fx )] ); /* Q(12+15+1) */
1201 0 : sum2_fx = L_mac_sat( sum2_fx, LPC[n], S_fx[( 4 * temp2 ) % ( 4 * X_fx->lag_fx )] );
1202 0 : temp2 = add( temp2, temp );
1203 : }
1204 :
1205 0 : temp1 = round_fx_sat( sum1_fx ); /* Q(12+15+1-16)=Q(12) */
1206 0 : temp2 = round_fx_sat( sum2_fx ); /* Q(12) */
1207 :
1208 : /* Calculate the circular convolution */
1209 0 : L_temp1 = L_mult( temp1, X_fx->a_fx[k] );
1210 0 : L_temp1 = L_msu_sat( L_temp1, temp2, X_fx->b_fx[k] ); /* Q(12+Q+1) */
1211 0 : L_temp2 = L_mult( temp1, X_fx->b_fx[k] );
1212 :
1213 0 : L_temp2 = L_mac_sat( L_temp2, temp2, X_fx->a_fx[k] ); /* Q(12+Q+1) */
1214 : /* normalization */
1215 0 : na = norm_l( L_temp1 );
1216 0 : if ( L_temp1 == 0 )
1217 : {
1218 0 : na = 31;
1219 0 : move16();
1220 : }
1221 0 : nb = norm_l( L_temp2 );
1222 0 : if ( L_temp2 == 0 )
1223 : {
1224 0 : nb = 31;
1225 0 : move16();
1226 : }
1227 :
1228 0 : if ( LT_16( na, nb ) )
1229 : {
1230 0 : nb = na;
1231 0 : move16();
1232 : }
1233 0 : X_fx->a_fx[k] = round_fx_sat( (Word32) L_shl_sat( L_temp1, nb ) ); /* Q(13+Q+nb-16)=Q(Q+nb-3) */
1234 0 : X_fx->b_fx[k] = round_fx_sat( (Word32) L_shl_sat( L_temp2, nb ) ); /* Q(Q+nb-3) */
1235 0 : move32();
1236 0 : move32();
1237 :
1238 0 : Qab[k] = sub( nb, 3 );
1239 0 : move16();
1240 :
1241 0 : if ( LT_16( Qab[k], Qmin ) )
1242 : {
1243 0 : Qmin = Qab[k];
1244 0 : move16();
1245 : }
1246 : }
1247 : /* bring to the same Q */
1248 0 : FOR( k = 0; k <= HalfLag; k++ )
1249 : {
1250 0 : X_fx->a_fx[k] = shl_sat( X_fx->a_fx[k], sub( Qmin, Qab[k] ) );
1251 0 : move16(); /* Q(Q+Qab[k]+Qmin-Qab[k]=Q(Q+Qmin) */
1252 0 : X_fx->b_fx[k] = shl_sat( X_fx->b_fx[k], sub( Qmin, Qab[k] ) );
1253 0 : move16(); /* Q(Q+Qmin) */
1254 : }
1255 :
1256 0 : X_fx->Q = add( X_fx->Q, Qmin );
1257 0 : move16();
1258 0 : return;
1259 : }
1260 : /*===================================================================*/
1261 : /* FUNCTION : DTFS_poleFilter_fx() */
1262 : /*-------------------------------------------------------------------*/
1263 : /* PURPOSE : pole filtering */
1264 : /*-------------------------------------------------------------------*/
1265 : /* INPUT ARGUMENTS : */
1266 : /* _ (Word16) lpc[] : lpc coefficients in Q12 */
1267 : /* _ (Word16) N : lpc order */
1268 : /* _ (Word16) lag: in Q0 */
1269 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
1270 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
1271 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
1272 : /*-------------------------------------------------------------------*/
1273 : /* OUTPUT ARGUMENTS : */
1274 : /* _ None */
1275 : /*-------------------------------------------------------------------*/
1276 : /* INPUT/OUTPUT ARGUMENTS : */
1277 : /* _ (Word16) this->a[] : in Q(this->Q) */
1278 : /* _ (Word16) this->b[] : in Q(this->Q) */
1279 : /* _ (Word16) this->Q: in Q0 */
1280 : /*-------------------------------------------------------------------*/
1281 : /* RETURN ARGUMENTS : _ None. */
1282 : /*-------------------------------------------------------------------*/
1283 : /* CALLED FROM : TX/RX */
1284 : /*===================================================================*/
1285 :
1286 : /* PORTING: Handling the functions with variable no. of arguments */
1287 :
1288 0 : void DTFS_poleFilter_fx_9(
1289 : DTFS_STRUCTURE *X_fx,
1290 : Word16 *pf_temp1,
1291 : Word16 *pf_temp2,
1292 : Word16 *pf_temp,
1293 : Word16 *pf_n2_temp1 )
1294 : {
1295 : Word16 temp, temp1, temp2, HalfLag;
1296 : Word32 sum1_fx, sum2_fx;
1297 : Word32 L_temp1, L_temp2;
1298 : Word16 k, n2_temp1, na, nb;
1299 : Word16 Qmin, Qab[MAXLAG_WI];
1300 :
1301 0 : Qmin = 32767;
1302 0 : move16();
1303 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
1304 0 : FOR( k = 0; k <= HalfLag; k++ )
1305 : {
1306 0 : temp = temp2 = k;
1307 0 : move16();
1308 0 : move16();
1309 : /* Calculate sum1 and sum2 */
1310 :
1311 0 : n2_temp1 = pf_n2_temp1[k];
1312 0 : move16();
1313 :
1314 0 : temp1 = pf_temp1[k];
1315 0 : move16(); /* Q(12+15+1+n2-16)=Q(12+n2) */
1316 0 : temp2 = pf_temp2[k];
1317 0 : move16(); /* Q(12+n2) */
1318 :
1319 : /* Calculate the circular convolution */
1320 :
1321 0 : L_temp1 = L_mac( L_mult( temp1, X_fx->a_fx[k] ), temp2, X_fx->b_fx[k] ); /* Q(12+n2+Q+1)=Q(13+n2+Q) */
1322 0 : L_temp2 = L_msu( L_mult( temp1, X_fx->b_fx[k] ), temp2, X_fx->a_fx[k] ); /* Q(12+n2+Q+1)=Q(13+n2+Q) */
1323 :
1324 0 : temp = pf_temp[k];
1325 0 : move16(); /* Q(61-25-2*n2-temp1-16)=Q(20-2*n2-temp1) */
1326 :
1327 0 : sum1_fx = Mult_32_16( L_temp1, temp ); /* Q(13+n2+Q+15+exp-15) = Q(13+n2+Q+exp) */
1328 0 : sum2_fx = Mult_32_16( L_temp2, temp );
1329 : /* normalization */
1330 0 : na = norm_l( sum1_fx );
1331 0 : if ( sum1_fx == 0 )
1332 : {
1333 0 : na = 31;
1334 0 : move16();
1335 : }
1336 0 : nb = norm_l( sum2_fx );
1337 0 : if ( sum2_fx == 0 )
1338 : {
1339 0 : nb = 31;
1340 0 : move16();
1341 : }
1342 :
1343 0 : if ( LT_16( na, nb ) )
1344 : {
1345 0 : nb = na;
1346 0 : move16();
1347 : }
1348 0 : nb = sub( nb, 1 ); /* leave one more sign bit */
1349 0 : X_fx->a_fx[k] = round_fx( (Word32) L_shl( sum1_fx, nb ) ); /* Q(-3+n2+Q+exp+nb ) */
1350 0 : move16();
1351 0 : X_fx->b_fx[k] = round_fx( (Word32) L_shl( sum2_fx, nb ) );
1352 0 : move16();
1353 :
1354 0 : Qab[k] = add( sub( nb, 3 ), n2_temp1 );
1355 0 : move16();
1356 :
1357 0 : if ( LT_16( Qab[k], Qmin ) )
1358 : {
1359 0 : Qmin = Qab[k];
1360 0 : move16();
1361 : }
1362 : }
1363 : /* bring to the same Q */
1364 :
1365 0 : FOR( k = 0; k <= HalfLag; k++ )
1366 : {
1367 0 : X_fx->a_fx[k] = shl( X_fx->a_fx[k], sub( Qmin, Qab[k] ) );
1368 0 : move16(); /* Q(Q+Qab[k]+Qmin-Qab[k])=Q(Q+Qmin) */
1369 0 : X_fx->b_fx[k] = shl( X_fx->b_fx[k], sub( Qmin, Qab[k] ) );
1370 0 : move16(); /* Q(Q+Qab[k]+Qmin-Qab[k])=Q(Q+Qmin) */
1371 : }
1372 :
1373 0 : X_fx->Q = add( X_fx->Q, Qmin );
1374 0 : move16();
1375 0 : return;
1376 : }
1377 : /*===================================================================*/
1378 : /* FUNCTION : DTFS_adjustLag_fx () */
1379 : /*-------------------------------------------------------------------*/
1380 : /* PURPOSE : */
1381 : /*-------------------------------------------------------------------*/
1382 : /* INPUT ARGUMENTS : */
1383 : /* _ (Word16) N_fx: lag value, Q0 */
1384 : /*-------------------------------------------------------------------*/
1385 : /* OUTPUT ARGUMENTS : */
1386 : /* _ (struct DTFS_fx) : a/b in X1.Q */
1387 : /*-------------------------------------------------------------------*/
1388 : /* INPUT/OUTPUT ARGUMENTS : */
1389 : /* _ (struct DTFS_fx) X1 : lag in Q0 */
1390 : /*-------------------------------------------------------------------*/
1391 : /* RETURN ARGUMENTS : _ None. */
1392 : /*-------------------------------------------------------------------*/
1393 : /* CALLED FROM : TX/RX */
1394 : /*===================================================================*/
1395 :
1396 0 : void DTFS_adjustLag_fx(
1397 : DTFS_STRUCTURE *X_DTFS_FX, /* i/o : DTFS to adjust lag for */
1398 : Word16 N_fx /* i : Target lag */
1399 : )
1400 : {
1401 : Word32 en_fx;
1402 : Word32 temp32_fx, tempnH_fx, mul1_fx, mul2_fx;
1403 : Word16 k, diff_fx;
1404 : Word16 exp, tmp;
1405 : Word32 L_tmp;
1406 :
1407 0 : IF( EQ_16( N_fx, X_DTFS_FX->lag_fx ) )
1408 : {
1409 0 : return;
1410 : }
1411 :
1412 0 : IF( GT_16( N_fx, X_DTFS_FX->lag_fx ) )
1413 : {
1414 0 : DTFS_zeroPadd_fx( N_fx, X_DTFS_FX );
1415 : }
1416 : ELSE
1417 : {
1418 0 : en_fx = DTFS_getEngy_fx( X_DTFS_FX ); /* Q = 2*(X_DTFS_FX->Q) */
1419 :
1420 0 : tmp = s_min( shr( X_DTFS_FX->lag_fx, 1 ), X_DTFS_FX->nH_fx );
1421 0 : FOR( k = ( ( N_fx >> 1 ) + 1 ); k <= tmp; k++ )
1422 : {
1423 0 : X_DTFS_FX->a_fx[k] = 0;
1424 0 : move16();
1425 0 : X_DTFS_FX->b_fx[k] = 0;
1426 0 : move16();
1427 : }
1428 0 : DTFS_setEngy_fx( X_DTFS_FX, en_fx );
1429 0 : X_DTFS_FX->lag_fx = N_fx;
1430 0 : move16();
1431 :
1432 :
1433 : /* recompute nH for new lag */
1434 0 : exp = norm_s( X_DTFS_FX->lag_fx );
1435 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X_DTFS_FX->lag_fx ); /* 29 - exp */
1436 0 : L_tmp = L_mult0( 12800, tmp );
1437 0 : temp32_fx = L_shl( L_tmp, sub( exp, 23 ) );
1438 0 : diff_fx = extract_l( L_shl( L_tmp, sub( exp, 29 ) ) );
1439 :
1440 :
1441 0 : exp = norm_s( diff_fx );
1442 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), diff_fx ); /* 29 - exp */
1443 0 : L_tmp = L_mult0( X_DTFS_FX->upper_cut_off_freq_fx, tmp );
1444 0 : X_DTFS_FX->nH_fx = extract_l( L_shl( L_tmp, sub( exp, 29 ) ) );
1445 0 : move16();
1446 :
1447 0 : L_tmp = L_mult0( 4000, tmp );
1448 0 : tempnH_fx = L_shl( L_tmp, sub( exp, 23 ) );
1449 0 : X_DTFS_FX->nH_4kHz_fx = extract_l( L_shl( L_tmp, sub( exp, 29 ) ) );
1450 0 : move16();
1451 :
1452 0 : IF( GE_16( sub( X_DTFS_FX->upper_cut_off_freq_fx, shr( extract_l( L_mult( diff_fx, X_DTFS_FX->nH_fx ) ), 1 ) ), diff_fx ) )
1453 : {
1454 0 : X_DTFS_FX->nH_fx = add( X_DTFS_FX->nH_fx, 1 );
1455 0 : move16();
1456 : }
1457 :
1458 0 : mul1_fx = L_shl( temp32_fx, 13 ); /* Q19 */
1459 0 : mul2_fx = L_shl( (Word32) X_DTFS_FX->nH_4kHz_fx, 18 ); /* Q18 */
1460 0 : tempnH_fx = Mult_32_32( mul1_fx, mul2_fx ); /* Q6 */
1461 0 : tempnH_fx = L_sub( (Word32) 256000, tempnH_fx ); /* Q6 */
1462 :
1463 0 : IF( GE_32( tempnH_fx, temp32_fx ) )
1464 : {
1465 0 : X_DTFS_FX->nH_4kHz_fx = add( X_DTFS_FX->nH_4kHz_fx, 1 );
1466 0 : move16();
1467 : }
1468 : }
1469 0 : return;
1470 : }
1471 : /*===================================================================*/
1472 : /* FUNCTION : DTFS_getEngy_fx () */
1473 : /*-------------------------------------------------------------------*/
1474 : /* PURPOSE : */
1475 : /*-------------------------------------------------------------------*/
1476 : /* INPUT ARGUMENTS : */
1477 : /* _ (struct DTFS_STRUCTURE) : a/b in X_fx.Q i.e Q6, lag in Q0*/
1478 : /*-------------------------------------------------------------------*/
1479 : /* OUTPUT ARGUMENTS : _ None */
1480 : /*-------------------------------------------------------------------*/
1481 : /* INPUT/OUTPUT ARGUMENTS : _ None */
1482 : /*-------------------------------------------------------------------*/
1483 : /* RETURN ARGUMENTS : */
1484 : /* _ (Word40) en_fx: output energy, 2*X1.Q */
1485 : /*-------------------------------------------------------------------*/
1486 : /* CALLED FROM : TX/RX */
1487 : /*===================================================================*/
1488 :
1489 :
1490 0 : Word32 DTFS_getEngy_fx(
1491 : DTFS_STRUCTURE *X_fx )
1492 : {
1493 : Word16 k, HalfLag_fx;
1494 0 : Word32 en_fx = 0;
1495 0 : move32();
1496 : Word16 temp_a_fx, temp_b_fx;
1497 0 : HalfLag_fx = shr( sub( X_fx->lag_fx, 1 ), 1 );
1498 0 : HalfLag_fx = s_min( HalfLag_fx, X_fx->nH_fx );
1499 0 : FOR( k = 1; k <= HalfLag_fx; k++ )
1500 : {
1501 0 : temp_a_fx = X_fx->a_fx[k];
1502 0 : move16();
1503 0 : temp_b_fx = X_fx->b_fx[k];
1504 0 : move16();
1505 0 : en_fx = L_mac0_sat( en_fx, temp_a_fx, temp_a_fx );
1506 0 : en_fx = L_mac0_sat( en_fx, temp_b_fx, temp_b_fx );
1507 : }
1508 :
1509 0 : en_fx = L_shr( en_fx, 1 );
1510 0 : temp_a_fx = X_fx->a_fx[0];
1511 0 : move16();
1512 0 : en_fx = L_mac0_sat( en_fx, temp_a_fx, temp_a_fx );
1513 : /* IF (X_fx->lag_fx%2 == 0) */
1514 0 : IF( s_and( X_fx->lag_fx, 1 ) == 0 )
1515 : {
1516 0 : temp_a_fx = X_fx->a_fx[k];
1517 0 : move16();
1518 0 : temp_b_fx = X_fx->b_fx[k];
1519 0 : move16();
1520 0 : en_fx = L_mac0_sat( en_fx, temp_a_fx, temp_a_fx );
1521 0 : en_fx = L_mac0_sat( en_fx, temp_b_fx, temp_b_fx );
1522 : }
1523 :
1524 0 : return en_fx; /* 2*X1.Q+1=Q13 */
1525 : }
1526 :
1527 :
1528 : /*===================================================================*/
1529 : /* FUNCTION : DTFS_getEngy_P2A_fx () */
1530 : /*-------------------------------------------------------------------*/
1531 : /* PURPOSE : */
1532 : /*-------------------------------------------------------------------*/
1533 : /* INPUT ARGUMENTS : */
1534 : /* _ (struct DTFS_STRUCTURE) : a/b in X_fx.Q, lag in Q0 */
1535 : /*-------------------------------------------------------------------*/
1536 : /* OUTPUT ARGUMENTS : _ None */
1537 : /*-------------------------------------------------------------------*/
1538 : /* INPUT/OUTPUT ARGUMENTS : _ None */
1539 : /*-------------------------------------------------------------------*/
1540 : /* RETURN ARGUMENTS : */
1541 : /* _ (Word40) en_fx: output energy, 2*X1.Q */
1542 : /*-------------------------------------------------------------------*/
1543 : /* CALLED FROM : TX/RX */
1544 : /*===================================================================*/
1545 :
1546 0 : Word32 DTFS_getEngy_P2A_fx(
1547 : DTFS_STRUCTURE *X_fx )
1548 : {
1549 : Word16 k, HalfLag_fx;
1550 0 : Word32 en_fx = 0;
1551 0 : move32();
1552 0 : HalfLag_fx = shr( sub( X_fx->lag_fx, 1 ), 1 );
1553 0 : HalfLag_fx = s_min( HalfLag_fx, X_fx->nH_fx );
1554 0 : FOR( k = 1; k <= HalfLag_fx; k++ )
1555 : {
1556 0 : en_fx = L_mac0_sat( en_fx, X_fx->a_fx[k], X_fx->a_fx[k] );
1557 0 : en_fx = L_mac0_sat( en_fx, X_fx->b_fx[k], X_fx->b_fx[k] );
1558 : }
1559 0 : en_fx = L_shr( en_fx, 1 );
1560 0 : en_fx = L_mac0_sat( en_fx, X_fx->a_fx[0], X_fx->a_fx[0] );
1561 : /* IF (X_fx->lag_fx%2 == 0) */
1562 0 : IF( s_and( X_fx->lag_fx, 1 ) == 0 )
1563 : {
1564 0 : en_fx = L_mac0_sat( en_fx, X_fx->a_fx[k], X_fx->a_fx[k] );
1565 0 : en_fx = L_mac0_sat( en_fx, X_fx->b_fx[k], X_fx->b_fx[k] );
1566 : }
1567 0 : return en_fx; /* 2*X1.Q */
1568 : }
1569 :
1570 :
1571 : /*===================================================================*/
1572 : /* FUNCTION : DTFS_setEngy_fx ( ) */
1573 : /*-------------------------------------------------------------------*/
1574 : /* PURPOSE : */
1575 : /*-------------------------------------------------------------------*/
1576 : /* INPUT ARGUMENTS : */
1577 : /* _ (Word40) en2_fx: 2*X1.Q+1 i.e. Q13 */
1578 : /*-------------------------------------------------------------------*/
1579 : /* OUTPUT ARGUMENTS : */
1580 : /* _ (Word40) en1_fx : 2*X1.Q+1 i.e. Q13 */
1581 : /*-------------------------------------------------------------------*/
1582 : /* INPUT/OUTPUT ARGUMENTS : */
1583 : /* _ (struct DTFS_STRUCTURE) X_DTFS_FX : a/b in X1.Q i.e. Q6, */
1584 : /* lag in Q0 */
1585 : /*-------------------------------------------------------------------*/
1586 : /* RETURN ARGUMENTS : _ None. */
1587 : /*-------------------------------------------------------------------*/
1588 : /* CALLED FROM : TX/RX */
1589 : /*===================================================================*/
1590 :
1591 0 : static Word32 DTFS_setEngy_fx(
1592 : DTFS_STRUCTURE *X_DTFS_FX,
1593 : Word32 en2_fx )
1594 : {
1595 : Word16 k, HalfLag_fx;
1596 : Word32 en1_fx;
1597 : Word32 L_temp_fx;
1598 : Word16 expa, expb, fraca, fracb, scale, tmp, exp;
1599 : Word32 L_tmp, factor_fx;
1600 :
1601 0 : HalfLag_fx = s_min( shr( X_DTFS_FX->lag_fx, 1 ), X_DTFS_FX->nH_fx );
1602 0 : move16();
1603 0 : en1_fx = DTFS_getEngy_fx( X_DTFS_FX );
1604 0 : move16();
1605 :
1606 0 : IF( en1_fx == 0 )
1607 : {
1608 0 : return 0;
1609 : }
1610 :
1611 0 : IF( en2_fx == 0 )
1612 : {
1613 0 : factor_fx = 0;
1614 0 : move32();
1615 : }
1616 : ELSE
1617 : {
1618 0 : expa = norm_l( en2_fx );
1619 0 : fraca = extract_h( L_shl( en2_fx, expa ) );
1620 0 : expa = sub( 30, add( expa, shl( X_DTFS_FX->Q, 1 ) ) );
1621 :
1622 :
1623 0 : expb = norm_l( en1_fx );
1624 0 : fracb = round_fx_sat( L_shl_sat( en1_fx, expb ) );
1625 0 : expb = sub( 30, add( expb, shl( X_DTFS_FX->Q, 1 ) ) );
1626 :
1627 :
1628 0 : scale = shr( sub( fraca, fracb ), 15 );
1629 0 : fracb = shl( fracb, scale );
1630 0 : expb = sub( expb, scale );
1631 :
1632 0 : tmp = div_s( fracb, fraca );
1633 0 : exp = sub( expb, expa );
1634 :
1635 0 : L_tmp = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
1636 0 : factor_fx = L_shr( L_tmp, 1 );
1637 : }
1638 :
1639 0 : FOR( k = 0; k <= HalfLag_fx; k++ )
1640 : {
1641 0 : L_temp_fx = Mult_32_16( factor_fx, X_DTFS_FX->a_fx[k] ); /* Q(temp+X1.Q-15) */
1642 0 : X_DTFS_FX->a_fx[k] = round_fx( L_temp_fx ); /* Q(temp+X1.Q-15-16)=Q(temp+X1.Q-31); */
1643 0 : move16();
1644 :
1645 0 : L_temp_fx = Mult_32_16( factor_fx, X_DTFS_FX->b_fx[k] ); /* Q(temp+X1.Q-15) */
1646 0 : X_DTFS_FX->b_fx[k] = round_fx( L_temp_fx ); /* Q(temp+X1.Q-15-16)=Q(temp+X1.Q-31); */
1647 0 : move16();
1648 : }
1649 :
1650 :
1651 0 : return en1_fx; /* 2*X1.Q+1 = Q13 */
1652 : }
1653 :
1654 : /*===================================================================*/
1655 : /* FUNCTION : struct DTFS_car2pol_fx () */
1656 : /*-------------------------------------------------------------------*/
1657 : /* PURPOSE : Cartesian to polar representation */
1658 : /* returning amplitudes and 0 phases */
1659 : /*-------------------------------------------------------------------*/
1660 : /* INPUT ARGUMENTS : */
1661 : /* _ (struct DTFS_STRUCTURE) X_fx : prototype in Cartesian domain*/
1662 : /* (Word16) lag: length of prototype in time domain */
1663 : /* (Word16 []) a,b: re/im of harmonics, normalized */
1664 : /* (Word16) Q: norm factor of a/b */
1665 : /*-------------------------------------------------------------------*/
1666 : /* OUTPUT ARGUMENTS : */
1667 : /* _ (struct DTFS_STRUCTURE) X_fx : prototype in polar domain */
1668 : /* (Word16) lag: length of prototype in time domain */
1669 : /* (Word16 []) a: amplitude of harmonics, normalized */
1670 : /* (Word16 []) b: phase of harmonics,cleared to 0 */
1671 : /* (Word16) Q: norm factor of a */
1672 : /*-------------------------------------------------------------------*/
1673 : /* INPUT/OUTPUT ARGUMENTS : */
1674 : /* _ None */
1675 : /*-------------------------------------------------------------------*/
1676 : /* RETURN ARGUMENTS : _ None. */
1677 : /*-------------------------------------------------------------------*/
1678 : /* CALLED FROM : TX/RX */
1679 : /*===================================================================*/
1680 : /* NOTE: output X.b (angle) is not computed and should be ignored */
1681 : /* When phases are needed, such as in QPPP, instead uses the */
1682 : /* Cartesian representation to avoid computing phases by arctan*/
1683 : /*===================================================================*/
1684 0 : void DTFS_car2pol_fx(
1685 : DTFS_STRUCTURE *X_fx /* i/o : DTFS structure a, b, lag */
1686 : )
1687 :
1688 : {
1689 : Word16 k, HalfLag_fx;
1690 : Word32 Ltemp_fx;
1691 : Word32 Lacc_fx;
1692 : Word16 exp, tmp, frac;
1693 0 : HalfLag_fx = s_min( shr( sub( X_fx->lag_fx, 1 ), 1 ), X_fx->nH_fx );
1694 0 : FOR( k = 1; k <= HalfLag_fx; k++ )
1695 : {
1696 :
1697 0 : Lacc_fx = L_mult( X_fx->a_fx[k], X_fx->a_fx[k] ); /* a[k]^2, 2Q+1 */
1698 0 : Lacc_fx = L_mac_sat( Lacc_fx, X_fx->b_fx[k], X_fx->b_fx[k] ); /* a[k]^2+b[k]^2, 2Q+1 */
1699 0 : Lacc_fx = L_shr( Lacc_fx, 3 ); /* Lacc=(a[k]^2+b[k]^2)/4, 2Q */
1700 :
1701 0 : IF( Lacc_fx )
1702 : {
1703 0 : exp = norm_l( Lacc_fx );
1704 0 : frac = extract_h( L_shl( Lacc_fx, exp ) ); /* Q14 */
1705 0 : exp = sub( exp, sub( 30, ( 2 * X_fx->Q ) ) );
1706 :
1707 0 : tmp = div_s( 16384, frac ); /* Q15 */
1708 0 : Ltemp_fx = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
1709 :
1710 0 : X_fx->a_fx[k] = extract_h( L_shl( Ltemp_fx, sub( add( X_fx->Q, exp ), 15 ) ) ); /* Q */
1711 0 : move16();
1712 : }
1713 : ELSE
1714 : {
1715 0 : X_fx->a_fx[k] = 0;
1716 0 : move16();
1717 : }
1718 :
1719 0 : X_fx->b_fx[k] = 0;
1720 0 : move16(); /* clear b[k] */
1721 : }
1722 :
1723 :
1724 0 : IF( s_and( X_fx->lag_fx, 1 ) == 0 )
1725 : {
1726 0 : IF( X_fx->a_fx[k] )
1727 : {
1728 0 : Lacc_fx = L_mult0( X_fx->a_fx[k], X_fx->a_fx[k] ); /* a[k]^2, 2Q+1 */
1729 0 : Lacc_fx = L_mac0( Lacc_fx, X_fx->b_fx[k], X_fx->b_fx[k] ); /* a[k]^2+b[k]^2, 2Q+1 */
1730 :
1731 0 : exp = norm_l( Lacc_fx );
1732 0 : frac = extract_h( L_shl( Lacc_fx, exp ) ); /* Q14 */
1733 0 : exp = sub( exp, sub( 30, shl( X_fx->Q, 1 ) ) );
1734 :
1735 0 : tmp = div_s( 16384, frac ); /* Q15 */
1736 0 : Ltemp_fx = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
1737 :
1738 0 : X_fx->a_fx[k] = extract_h( L_shl( Ltemp_fx, sub( add( X_fx->Q, exp ), 15 ) ) ); /* Q */
1739 0 : move16();
1740 : }
1741 : ELSE
1742 : {
1743 0 : X_fx->a_fx[k] = 0;
1744 0 : move16();
1745 : }
1746 :
1747 0 : X_fx->b_fx[k] = 0;
1748 0 : move16(); /* clear b[k]; */
1749 : }
1750 0 : return;
1751 : }
1752 :
1753 :
1754 : /*==============================================================================*/
1755 : /* FUNCTION : DTFS_setEngyHarm_fx ( ) */
1756 : /*------------------------------------------------------------------------------*/
1757 : /* PURPOSE : Set a band of harmonics to specified energy */
1758 : /*----------------------------------------------------------------------- ------*/
1759 : /* INPUT ARGUMENTS : */
1760 : /* _ (struct DTFS_fx) : lag in Q0 */
1761 : /* _ (Word16) f1_fx: lower bound of input, normalized by 12800, Q15 */
1762 : /* _ (Word16) f2_fx: upper bound of input, normalized by 12800, Q15 */
1763 : /* _ (Word16) g1_fx: lower bound of output, normalized by 12800, Q15 */
1764 : /* _ (Word16) g2_fx: upper bound of output, normalized by 12800, Q15 */
1765 : /* _ (Word32) en2_fx: in Q(Qen2) */
1766 : /* _ (Word16) Qen2_fx: Q value of en2 */
1767 : /*---------------------------------------------------------------------------- --*/
1768 : /* OUTPUT ARGUMENTS : */
1769 : /* _ (Word16) Qa_fx: Q value of output a[]. */
1770 : /*----------------------------------------------------------------------------- -*/
1771 : /* INPUT/OUTPUT ARGUMENTS : */
1772 : /* _(struct DTFS_STRUCTURE) : a_fx[] in X1_fx.Q at start, then changed to */
1773 : /* *Qa later. */
1774 : /*---------------------------------------------------------------------------- --*/
1775 : /* RETURN ARGUMENTS : _ None. */
1776 : /* _ (Word32) en1_fx: Q(2*X1.Q) */
1777 : /*----------------------------------------------------------------------------- -*/
1778 : /* CALLED FROM : TX/RX */
1779 : /*============================================================================== */
1780 : /* NOTE: This function cannot change Q because it works on a band */
1781 : /* of harmonics, instead of the whole DTFS */
1782 : /*============================================================================== */
1783 0 : Word32 DTFS_setEngyHarm_fx(
1784 : Word16 f1_fx, /* i : lower band freq of input to control energy */
1785 : Word16 f2_fx, /* i : upper band freq of input to control energy */
1786 : Word16 g1_fx, /* i : lower band freq of output to control energy */
1787 : Word16 g2_fx, /* i : upper band freq of output to control energy */
1788 : Word32 en2_fx, /* i : Target Energy to set the DTFS to */
1789 : Word16 Qen2_fx, /* i : Input Q format for en2 */
1790 : Word16 *Qa_fx, /* i : Output Q format for x->a */
1791 : DTFS_STRUCTURE *X_fx /* i/o: DTFS to adjust the energy of */
1792 : )
1793 : {
1794 :
1795 0 : Word16 k, count = 0, HalfLag_fx;
1796 : Word16 f_low_fx, f_high_fx, g_low_fx, g_high_fx;
1797 : Word32 L_temp_fx, factor_fx;
1798 : Word32 en1_fx;
1799 : Word32 Lacc;
1800 : Word16 exp, tmp, expa, expb, fraca, fracb, scale;
1801 : Word32 L_tmp;
1802 0 : Word32 Lacc_max = 0;
1803 0 : move32();
1804 0 : Word16 expp = 0;
1805 0 : move16();
1806 :
1807 :
1808 0 : f_low_fx = mult( f1_fx, X_fx->lag_fx ); /* Q0 */
1809 0 : f_high_fx = mult( f2_fx, X_fx->lag_fx ); /* Q0 */
1810 0 : g_low_fx = mult( g1_fx, X_fx->lag_fx ); /* Q0 */
1811 0 : g_high_fx = mult( g2_fx, X_fx->lag_fx ); /* Q0 */
1812 0 : HalfLag_fx = s_min( f_high_fx, shl( X_fx->nH_fx, 1 ) );
1813 :
1814 0 : Lacc = L_deposit_l( 0 );
1815 0 : FOR( k = f_low_fx + 1; k <= HalfLag_fx; k++ )
1816 : {
1817 0 : Lacc = L_mac0_sat( Lacc, X_fx->a_fx[k], X_fx->a_fx[k] ); /* 2*X1.Q */
1818 0 : Lacc_max = L_max( Lacc_max, Lacc );
1819 :
1820 0 : count = add( count, 1 );
1821 : }
1822 :
1823 0 : IF( GE_32( Lacc_max, 2147483647 /*1.Q31*/ ) )
1824 : {
1825 0 : tmp = sub( HalfLag_fx, f_low_fx );
1826 0 : exp = norm_s( tmp );
1827 0 : expp = sub( 15, exp );
1828 :
1829 0 : Lacc = L_deposit_l( 0 );
1830 0 : FOR( k = f_low_fx + 1; k <= HalfLag_fx; k++ )
1831 : {
1832 0 : L_tmp = L_mult_sat( X_fx->a_fx[k], X_fx->a_fx[k] );
1833 0 : Lacc = L_add_sat( Lacc, L_shr( L_tmp, expp ) ); /* 2*X1.Q-expp */
1834 0 : count = add( count, 1 );
1835 : }
1836 : }
1837 :
1838 0 : if ( count <= 0 )
1839 : {
1840 0 : count = 1;
1841 0 : move16();
1842 : }
1843 :
1844 0 : exp = norm_s( count );
1845 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), count ); /* 29 - exp */
1846 0 : en1_fx = L_shl_sat( Mult_32_16( Lacc, tmp ), sub( exp, 14 ) );
1847 0 : test();
1848 0 : IF( en1_fx > 0 && en2_fx > 0 )
1849 : {
1850 : /* factor_fx = sqrt_divide_dp((Word40)en2_fx, en1_fx, sub(Qen2_fx, shl(X_fx->Q, 1)), &temp_fx,1); : Q(temp) */
1851 0 : expa = norm_l( en2_fx );
1852 0 : fraca = extract_h( L_shl( en2_fx, expa ) );
1853 0 : expa = sub( 30, add( expa, Qen2_fx ) );
1854 :
1855 :
1856 0 : expb = norm_l( en1_fx );
1857 0 : fracb = round_fx_sat( L_shl( en1_fx, expb ) );
1858 0 : IF( GE_32( Lacc_max, 2147483647 /*1.Q31*/ ) )
1859 : {
1860 0 : expb = sub( 30, add( expb, sub( shl( X_fx->Q, 1 ), expp ) ) );
1861 : }
1862 : ELSE
1863 : {
1864 0 : expb = sub( 30, add( expb, shl( X_fx->Q, 1 ) ) );
1865 : }
1866 :
1867 0 : scale = shr( sub( fraca, fracb ), 15 );
1868 0 : fracb = shl( fracb, scale );
1869 0 : expb = sub( expb, scale );
1870 :
1871 0 : tmp = div_s( fracb, fraca );
1872 0 : exp = sub( expb, expa );
1873 :
1874 0 : L_tmp = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
1875 :
1876 0 : IF( GE_32( Lacc_max, 2147483647 ) )
1877 : {
1878 0 : factor_fx = L_shr( L_tmp, add( 1, s_min( 2, expp ) ) );
1879 : }
1880 : ELSE
1881 : {
1882 0 : factor_fx = L_shr( L_tmp, 1 );
1883 : }
1884 : }
1885 : ELSE
1886 : {
1887 0 : factor_fx = L_deposit_l( 0 );
1888 : }
1889 :
1890 0 : HalfLag_fx = s_min( g_high_fx, shl( X_fx->nH_fx, 1 ) );
1891 0 : FOR( k = g_low_fx + 1; k <= HalfLag_fx; k++ )
1892 : {
1893 : /*L_temp_fx =(Word32)Mpy_32_16(extract_h(factor_fx),extract_l(factor_fx), X_fx->a_fx[k]); move32(); */ /* Q(temp+X1.Q-15) */
1894 0 : L_temp_fx = Mult_32_16( factor_fx, X_fx->a_fx[k] ); /* Q(temp+X1.Q-15) */
1895 0 : X_fx->a_fx[k] = round_fx_sat( L_temp_fx ); /* Q(temp+X1.Q-15-16)=Q(temp+X1.Q-31); */
1896 0 : move16();
1897 : }
1898 :
1899 0 : IF( GE_32( Lacc_max, 2147483647 /*1.Q31*/ ) )
1900 : {
1901 0 : *Qa_fx = sub( sub( X_fx->Q, add( 1, s_min( 2, expp ) ) ), exp );
1902 0 : move16();
1903 : }
1904 : ELSE
1905 : {
1906 0 : *Qa_fx = sub( sub( X_fx->Q, 1 ), exp );
1907 0 : move16();
1908 : }
1909 :
1910 0 : return en1_fx; /* Q(2*X1.Q) */
1911 : }
1912 : /*===================================================================*/
1913 : /* FUNCTION : cubicPhase_fx () */
1914 : /*-------------------------------------------------------------------*/
1915 : /* PURPOSE : Compute cubic phase track for WI synthesis */
1916 : /*-------------------------------------------------------------------*/
1917 : /* INPUT ARGUMENTS : */
1918 : /* _ (Word16) ph1_fx : initial phase, Q15 (normalized by 2pi) */
1919 : /* _ (Word16) ph2_fx : final phase, Q15 (normalized by 2pi) */
1920 : /* _ (Word16) L1 : previous pitch lag, Q0 */
1921 : /* _ (Word16) L2 : current pitch lag, Q0 */
1922 : /* _ (Word16) N : length of phase track, Q0 */
1923 : /*-------------------------------------------------------------------*/
1924 : /* OUTPUT ARGUMENTS : */
1925 : /* _ (Word32 []) phOut_fx : phase track, Q27 (normalized by 2pi) */
1926 : /*-------------------------------------------------------------------*/
1927 : /* INPUT/OUTPUT ARGUMENTS : */
1928 : /* _ None */
1929 : /*-------------------------------------------------------------------*/
1930 : /* RETURN ARGUMENTS : _ None. */
1931 : /*-------------------------------------------------------------------*/
1932 : /* CALLED FROM : TX/RX */
1933 : /*===================================================================*/
1934 : /* NOTE: This function outputs phase in (-1,1) */
1935 : /*===================================================================*/
1936 0 : static void cubicPhase_fx(
1937 : Word16 ph1_fx, /* i : phase offset */
1938 : Word16 ph2_fx, /* i : phase 2 */
1939 : const Word16 L1, /* i : previous lag */
1940 : const Word16 L2, /* i : current lag */
1941 : Word16 N, /* i : input length */
1942 : Word32 *phOut_fx /* o : cubic phase output */
1943 : )
1944 : {
1945 : Word16 n;
1946 : Word16 n2;
1947 : Word16 f1, f2; /* invert of L1, L2, Q19 */
1948 : Word16 factor, temp;
1949 : Word16 c0, c1, c2, c3; /* cubic polynomial coefficients */
1950 : /* Q33, Q27, Q19, Q15 respectively */
1951 : Word32 Ltemp1, Ltemp2, Ltemp3, Ltemp4, Ltemp;
1952 : Word16 tmp, exp;
1953 : Word32 Lacc;
1954 : Word16 expa, expb, fraca, fracb, scale;
1955 : Word32 L_tmp, L_tmp1;
1956 : Word16 num_flag, den_flag;
1957 : Word32 N2;
1958 : Word16 dbgshft;
1959 0 : num_flag = 0;
1960 0 : move16();
1961 0 : den_flag = 0;
1962 0 : move16();
1963 :
1964 0 : N = sub( N, L2 );
1965 :
1966 0 : exp = norm_s( L1 );
1967 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), L1 );
1968 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
1969 0 : f1 = round_fx( L_tmp );
1970 :
1971 0 : exp = norm_s( L2 );
1972 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), L2 );
1973 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
1974 0 : Ltemp4 = inverse_table[L2];
1975 0 : f2 = round_fx( L_tmp );
1976 :
1977 0 : c3 = ph1_fx;
1978 0 : move16(); /* c3 in Q15 */
1979 0 : c2 = f1;
1980 0 : move16(); /* c2 in Q19 */
1981 :
1982 0 : Ltemp1 = L_sub( ph1_fx, ph2_fx ); /* Ltemp1=ph1_fx-ph2_fx, Q15 */
1983 0 : Ltemp2 = L_add( f1, f2 ); /* Ltemp2=0.5*(f1+f2), Q20 */
1984 0 : temp = extract_l( Ltemp2 ); /* Q20 */
1985 :
1986 0 : IF( temp < 0 )
1987 : {
1988 0 : L_tmp1 = L_shl( L_add( 65536, temp ), 14 ); /* Q30 */
1989 0 : L_tmp = Mult_32_16( L_tmp1, N ); /* 30-15=15 */
1990 0 : Ltemp2 = L_shl( L_tmp, 1 );
1991 : }
1992 : ELSE
1993 : {
1994 0 : Ltemp2 = (Word32) L_mult0( N, temp ); /* Ltemp2=0.5*N*(f1+f2), Q20 */
1995 : }
1996 :
1997 0 : Ltemp1 = L_add( L_shl( Ltemp1, 5 ), Ltemp2 ); /* Ltemp1=ph1_fx - ph2_fx + 0.5*N*(f2+f1), Q16, Q20 */
1998 :
1999 0 : factor = round_fx( L_shr( Ltemp1, 4 ) ); /* factor in Q0 */
2000 :
2001 0 : c1 = sub( f2, f1 ); /* c1=f2-f1, Q19 */
2002 0 : Ltemp1 = L_sub( ph2_fx, ph1_fx ); /* Q15 */
2003 0 : Ltemp2 = L_mult( N, f1 ); /* Ltemp2=N*f1, Q20 */
2004 0 : Ltemp2 = L_sub( L_shl( L_deposit_h( factor ), 4 ), Ltemp2 ); /* Ltemp2=factor-N*f1, Q20 */
2005 0 : Ltemp1 = L_add( Ltemp2, L_shl( Ltemp1, 5 ) ); /* Ltemp1 in Q20 */
2006 :
2007 0 : IF( GT_16( N, 180 ) )
2008 : {
2009 0 : Ltemp2 = L_shl( L_mult0( N, N ), 14 );
2010 0 : Ltemp2 = L_shl( Mult_32_16( Ltemp2, N ), 1 );
2011 :
2012 : /* IF(N%2) */
2013 0 : if ( EQ_16( s_and( N, 1 ), 1 ) )
2014 : {
2015 0 : Ltemp2 = L_add( Ltemp2, 1 );
2016 : }
2017 : }
2018 : ELSE
2019 : {
2020 0 : Ltemp2 = L_shr( L_mult( N, N ), 1 );
2021 0 : Ltemp2 = L_mult0( N, extract_l( Ltemp2 ) ); /* Ltemp2=N^3 */
2022 : }
2023 0 : Ltemp3 = L_mult( N, c1 ); /* Q20 */
2024 0 : Ltemp3 = L_sub( Ltemp3, L_shl( Ltemp1, 1 ) ); /* Ltemp3=N*c1-2*Ltemp1, Q20 */
2025 :
2026 0 : IF( GE_32( L_abs( Ltemp3 ), L_shl_sat( Ltemp2, 8 ) ) )
2027 : {
2028 0 : Lacc = L_add( MIN_32, 0 );
2029 0 : if ( Ltemp3 > 0 )
2030 : {
2031 0 : Lacc = L_add( MAX_32, 0 );
2032 : }
2033 :
2034 0 : c0 = extract_h( Lacc ); /* c0 in Q33 */
2035 : }
2036 : ELSE
2037 : {
2038 0 : expa = norm_l( Ltemp3 );
2039 0 : fraca = extract_h( L_shl( Ltemp3, expa ) );
2040 0 : expa = sub( 30, add( expa, 20 ) );
2041 0 : if ( fraca < 0 )
2042 : {
2043 0 : num_flag = 1;
2044 0 : move16();
2045 : }
2046 :
2047 0 : expb = norm_l( Ltemp2 );
2048 0 : fracb = extract_h( L_shl( Ltemp2, expb ) );
2049 0 : expb = sub( 30, expb );
2050 0 : if ( fracb < 0 )
2051 : {
2052 0 : den_flag = 1;
2053 0 : move16();
2054 : }
2055 :
2056 0 : if ( num_flag )
2057 : {
2058 0 : fraca = negate( fraca );
2059 : }
2060 0 : if ( den_flag )
2061 : {
2062 0 : fracb = negate( fracb );
2063 : }
2064 0 : scale = shr( sub( fracb, fraca ), 15 );
2065 0 : fraca = shl_sat( fraca, scale );
2066 0 : expa = sub( expa, scale );
2067 :
2068 0 : tmp = div_s( fraca, fracb ); /* 15-exp */
2069 0 : exp = sub( expa, expb );
2070 0 : test();
2071 0 : if ( num_flag && !den_flag )
2072 : {
2073 0 : tmp = negate( tmp );
2074 : }
2075 0 : test();
2076 0 : if ( den_flag && !num_flag )
2077 : {
2078 0 : tmp = negate( tmp );
2079 : }
2080 :
2081 0 : Lacc = L_shl_sat( tmp, add( exp, 34 ) );
2082 0 : Lacc = L_add_sat( Lacc, 0x08000 );
2083 0 : c0 = extract_h( Lacc ); /* c0 in Q33 */
2084 : }
2085 :
2086 0 : Ltemp1 = L_mult( N, N ); /* Ltemp1=2*N*N */
2087 0 : Ltemp1 = L_add( Ltemp1, L_shr( Ltemp1, 1 ) ); /* Ltemp1=3*N*N, max is 3*140*140 */
2088 :
2089 : /* patch added for time warping support, where N can be more than 140 */
2090 0 : dbgshft = norm_l( Ltemp1 );
2091 0 : Ltemp1 = L_shl( Ltemp1, dbgshft );
2092 0 : temp = extract_h( Ltemp1 );
2093 0 : Ltemp1 = (Word32) L_shl( (Word32) L_mult0( c0, temp ), sub( 16, dbgshft ) ); /* Ltemp1=3*N*N*c0, Q33 */
2094 : /* Patch end */
2095 :
2096 0 : num_flag = den_flag = 0;
2097 0 : move16();
2098 0 : move16();
2099 0 : Ltemp1 = L_sub( L_shr( L_deposit_h( c1 ), 2 ), Ltemp1 ); /* Ltemp1=c1-3*N*N*c0, Q33 */
2100 :
2101 0 : expa = norm_l( Ltemp1 );
2102 0 : fraca = extract_h( L_shl( Ltemp1, expa ) );
2103 0 : expa = sub( 30, add( expa, 33 ) );
2104 0 : if ( fraca < 0 )
2105 : {
2106 0 : num_flag = 1;
2107 0 : move16();
2108 : }
2109 :
2110 0 : expb = norm_l( N );
2111 0 : fracb = extract_h( L_shl( N, expb ) );
2112 0 : expb = sub( 30, expb );
2113 0 : if ( fracb < 0 )
2114 : {
2115 0 : den_flag = 1;
2116 0 : move16();
2117 : }
2118 :
2119 0 : if ( num_flag )
2120 : {
2121 0 : fraca = negate( fraca );
2122 : }
2123 0 : if ( den_flag )
2124 : {
2125 0 : fracb = negate( fracb );
2126 : }
2127 0 : scale = shr( sub( fracb, fraca ), 15 );
2128 0 : fraca = shl( fraca, scale );
2129 0 : expa = sub( expa, scale );
2130 :
2131 0 : tmp = div_s( fraca, fracb ); /* 15-exp */
2132 0 : exp = sub( expa, expb );
2133 0 : test();
2134 0 : if ( num_flag && !den_flag )
2135 : {
2136 0 : tmp = negate( tmp );
2137 : }
2138 0 : test();
2139 0 : if ( den_flag && !num_flag )
2140 : {
2141 0 : tmp = negate( tmp );
2142 : }
2143 :
2144 0 : Lacc = L_shl( tmp, add( exp, 27 ) );
2145 0 : Lacc = L_add( Lacc, 0x08000 );
2146 0 : c1 = extract_h( Lacc ); /* c1 in Q27 */
2147 :
2148 :
2149 : /* Computation of the phase value at each sample point */
2150 : /* ph[n]= c0*n^3+c1*n^2+c2*n+c3, Q15 */
2151 0 : phOut_fx[0] = L_shl( ph1_fx, 11 ); /* Q27 */
2152 0 : move32();
2153 :
2154 0 : IF( LT_16( N, 181 ) )
2155 : {
2156 0 : FOR( n = 1; n < N; n++ )
2157 : {
2158 : /* phOut_fx[n] = _POLY3(n,coef) ; */
2159 0 : n2 = i_mult2( n, n ); /* n2=n^2 */
2160 0 : Ltemp3 = (Word32) L_mult0( n, (UNS_Word16) n2 ); /* Ltemp3=n^3 */
2161 :
2162 0 : Ltemp3 = L_shl( Mult_32_16( L_shl( Ltemp3, 7 ), c0 ), 2 ); /* Ltemp3=c0*n^3, Q27 */
2163 0 : Ltemp2 = (Word32) L_mult0( c1, (UNS_Word16) n2 ); /* Ltemp2=c1*n^2, Q27 */
2164 0 : Ltemp1 = L_shl( L_mult( c2, n ), 7 ); /* Ltemp1=c2*n, Q27 */
2165 :
2166 0 : Ltemp = L_shl( (Word32) c3, 12 ); /* Q27 */
2167 0 : Ltemp = L_add( Ltemp1, Ltemp ); /* Q27 */
2168 0 : Ltemp = L_add( Ltemp2, Ltemp ); /* Q27 */
2169 0 : Ltemp = L_add( Ltemp3, Ltemp ); /* Q27 */
2170 :
2171 0 : phOut_fx[n] = Ltemp;
2172 0 : move32(); /* Q27 */
2173 : }
2174 : }
2175 : ELSE
2176 : {
2177 :
2178 0 : FOR( n = 1; n < 181; n++ )
2179 : {
2180 : /* phOut_fx[n] = _POLY3(n,coef) ; */
2181 0 : n2 = i_mult2( n, n ); /* n2=n^2 */
2182 0 : Ltemp3 = (Word32) L_mult0( n, (UNS_Word16) n2 ); /* Ltemp3=n^3 */
2183 :
2184 0 : Ltemp3 = L_shl( Mult_32_16( L_shl( Ltemp3, 7 ), c0 ), 2 ); /* Ltemp3=c0*n^3, Q27 */
2185 0 : Ltemp2 = (Word32) L_mult0( c1, (UNS_Word16) n2 ); /* Ltemp2=c1*n^2, Q27 */
2186 0 : Ltemp1 = L_shl( L_mult( c2, n ), 7 ); /* Ltemp1=c2*n, Q27 */
2187 :
2188 0 : Ltemp = L_shl( (Word32) c3, 12 ); /* Q27 */
2189 0 : Ltemp = L_add( Ltemp1, Ltemp ); /* Q27 */
2190 0 : Ltemp = L_add( Ltemp2, Ltemp ); /* Q27 */
2191 0 : Ltemp = L_add( Ltemp3, Ltemp ); /* Q27 */
2192 :
2193 0 : phOut_fx[n] = Ltemp;
2194 0 : move32(); /* Q27 */
2195 : }
2196 :
2197 0 : FOR( n = 181; n < N; n++ )
2198 : {
2199 : /* phOut_fx[n] = _POLY3(n,coef) ; */
2200 0 : N2 = L_shl( L_mult0( n, n ), 14 );
2201 0 : Ltemp3 = L_shl( Mult_32_16( N2, n ), 1 );
2202 :
2203 0 : if ( EQ_16( s_and( N, 1 ), 1 ) )
2204 : {
2205 0 : Ltemp3 = L_add( Ltemp3, 1 );
2206 : }
2207 :
2208 0 : Ltemp3 = L_shl_sat( Mult_32_16( L_shl_sat( Ltemp3, 7 ), c0 ), 2 ); /* Ltemp3=c0*n^3, Q27 */ //?sat
2209 0 : Ltemp2 = L_shl( Mult_32_16( N2, c1 ), 1 );
2210 : /* Ltemp2 = (Word32)L_mult_su(c1,(UNS_Word16)n2); : Ltemp2=c1*n^2, Q27 */
2211 0 : Ltemp1 = L_shl( L_mult( c2, n ), 7 ); /* Ltemp1=c2*n, Q27 */
2212 :
2213 0 : Ltemp = L_shl( (Word32) c3, 12 ); /* Q27 */
2214 0 : Ltemp = L_add( Ltemp1, Ltemp ); /* Q27 */
2215 0 : Ltemp = L_add( Ltemp2, Ltemp ); /* Q27 */
2216 0 : Ltemp = L_add( Ltemp3, Ltemp ); /* Q27 */
2217 :
2218 0 : phOut_fx[n] = Ltemp;
2219 0 : move32(); /* Q27 */
2220 : }
2221 : }
2222 :
2223 0 : tmp = add( N, L2 );
2224 0 : FOR( ; n < tmp; n++ )
2225 : {
2226 0 : Ltemp = L_add( phOut_fx[n - 1], Ltemp4 ); /* Q27 */
2227 0 : phOut_fx[n] = Ltemp;
2228 0 : move32(); /* Q27 */
2229 : }
2230 0 : return;
2231 : }
2232 :
2233 : /*===================================================================*/
2234 : /* FUNCTION : DTFS_to_erb_fx () */
2235 : /*-------------------------------------------------------------------*/
2236 : /* PURPOSE : Convert harmonics to erb bands */
2237 : /*-------------------------------------------------------------------*/
2238 : /* INPUT ARGUMENTS : */
2239 : /* _ (struct DTFS_STRUCTURE) X_fx : prototype in polar domain */
2240 : /* (Word16) lag_fx: length of prototype in time domain*/
2241 : /* (Word16 []) a_fx: amplitude, normalized */
2242 : /* (Word16) Q_fx: norm factor of a */
2243 : /*-------------------------------------------------------------------*/
2244 : /* OUTPUT ARGUMENTS : */
2245 : /* _ (Word16 []) out_fx : erb output, Q13 */
2246 : /*-------------------------------------------------------------------*/
2247 : /* INPUT/OUTPUT ARGUMENTS : */
2248 : /* _ None */
2249 : /*-------------------------------------------------------------------*/
2250 : /* RETURN ARGUMENTS : _ None. */
2251 : /*-------------------------------------------------------------------*/
2252 : /* CALLED FROM : TX/RX */
2253 : /*===================================================================*/
2254 0 : void DTFS_to_erb_fx(
2255 : const DTFS_STRUCTURE X_fx, /* i : DTFS input */
2256 : Word16 *out_fx /* o : ERB output */
2257 : )
2258 :
2259 : {
2260 : Word16 num_erb_fx;
2261 : Word16 i, j, n, count[NUM_ERB_WB];
2262 : Word16 diff_fx;
2263 : Word32 sum_a_fx[NUM_ERB_WB], Ltemp_fx, L_tmp, L_temp;
2264 : Word16 exp, tmp;
2265 : Word16 expa, expb, fraca, fracb, scale;
2266 0 : const Word16 *erb_fx = NULL;
2267 0 : num_erb_fx = NUM_ERB_NB;
2268 0 : move16();
2269 :
2270 0 : test();
2271 0 : test();
2272 0 : IF( EQ_16( X_fx.upper_cut_off_freq_fx, 0x02800 ) || EQ_16( X_fx.upper_cut_off_freq_fx, 4000 ) ) /* 0x2800=0.3125 in Q15 (4000Hz) */
2273 : {
2274 0 : num_erb_fx = NUM_ERB_NB;
2275 0 : move16();
2276 0 : erb_fx = &( erb_NB_fx[0] );
2277 0 : move16();
2278 : }
2279 0 : ELSE IF( EQ_16( X_fx.upper_cut_off_freq_fx, 0x04000 ) || EQ_16( X_fx.upper_cut_off_freq_fx, 6400 ) ) /* 0x4000=0.5 in Q15 (6400Hz) */
2280 : {
2281 0 : num_erb_fx = NUM_ERB_WB;
2282 0 : move16();
2283 0 : erb_fx = &( erb_WB_fx[0] );
2284 0 : move16();
2285 : }
2286 :
2287 :
2288 0 : FOR( i = 0; i < num_erb_fx; i++ )
2289 : {
2290 :
2291 :
2292 0 : count[i] = 0;
2293 0 : move16();
2294 0 : sum_a_fx[i] = L_deposit_l( 0 );
2295 0 : move32();
2296 : }
2297 :
2298 0 : exp = norm_s( X_fx.lag_fx );
2299 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X_fx.lag_fx ); /* 29-exp */
2300 :
2301 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
2302 0 : diff_fx = round_fx( L_tmp );
2303 :
2304 0 : n = s_min( shr( X_fx.lag_fx, 1 ), X_fx.nH_fx );
2305 :
2306 0 : j = 0;
2307 0 : move16();
2308 0 : FOR( i = 0; i <= n; i++ )
2309 : {
2310 0 : Ltemp_fx = L_mult( diff_fx, i ); /* Ltemp=i*diff, Q20 */
2311 0 : FOR( ; j < num_erb_fx; j++ )
2312 : {
2313 0 : IF( LT_32( Ltemp_fx, L_shl( erb_fx[j + 1], 5 ) ) )
2314 : {
2315 0 : sum_a_fx[j] = L_add( sum_a_fx[j], L_deposit_l( X_fx.a_fx[i] ) );
2316 0 : move32(); /* X_fx.Q */
2317 0 : count[j] = add( count[j], 1 );
2318 0 : move16();
2319 0 : BREAK;
2320 : }
2321 : }
2322 : }
2323 : /* Make output in Q13 */
2324 0 : n = sub( 29, X_fx.Q );
2325 0 : j = negate( X_fx.Q );
2326 :
2327 :
2328 0 : FOR( i = 0; i < num_erb_fx; i++ )
2329 : {
2330 0 : out_fx[i] = round_fx_sat( L_shl_sat( sum_a_fx[i], n ) ); /* Q13 */ //?�sat
2331 0 : move16();
2332 0 : IF( GT_16( count[i], 1 ) )
2333 : {
2334 0 : IF( sum_a_fx[i] < 0 )
2335 : {
2336 0 : L_temp = L_negate( sum_a_fx[i] );
2337 : }
2338 : ELSE
2339 : {
2340 0 : L_temp = L_add( sum_a_fx[i], 0 );
2341 : }
2342 :
2343 0 : expb = norm_l( L_temp );
2344 0 : fracb = round_fx( L_shl( L_temp, expb ) );
2345 0 : expb = sub( 30, add( expb, X_fx.Q ) );
2346 :
2347 :
2348 0 : expa = norm_l( count[i] );
2349 0 : fraca = extract_h( L_shl( count[i], expa ) );
2350 0 : expa = sub( 30, expa );
2351 :
2352 0 : scale = shr( sub( fraca, fracb ), 15 );
2353 0 : fracb = shl( fracb, scale );
2354 0 : expb = sub( expb, scale );
2355 :
2356 0 : tmp = div_s( fracb, fraca );
2357 0 : exp = sub( expb, expa );
2358 0 : L_tmp = L_shl_sat( tmp, add( exp, 14 ) );
2359 0 : out_fx[i] = round_fx_sat( L_tmp );
2360 0 : move16();
2361 : }
2362 : }
2363 0 : return;
2364 : }
2365 : /*===================================================================*/
2366 : /* FUNCTION : erb_slot_fx () */
2367 : /*-------------------------------------------------------------------*/
2368 : /* PURPOSE : Allocate harmonics in ERB bins */
2369 : /*-------------------------------------------------------------------*/
2370 : /* INPUT ARGUMENTS : */
2371 : /* _ (Word16) lag_fx : pitch lag, Q0 */
2372 : /*-------------------------------------------------------------------*/
2373 : /* OUTPUT ARGUMENTS : */
2374 : /* _ (Word16 []) out_fx : number of harmonics in the ERB bins, Q0 */
2375 : /* _ (Word16 []) mfreq_fx : frequency bounds of the ERB bins, Q15 */
2376 : /*-------------------------------------------------------------------*/
2377 : /* INPUT/OUTPUT ARGUMENTS : */
2378 : /* _ None */
2379 : /*-------------------------------------------------------------------*/
2380 : /* RETURN ARGUMENTS : _ None. */
2381 : /*-------------------------------------------------------------------*/
2382 : /* CALLED FROM : TX/RX */
2383 : /*===================================================================*/
2384 : /* NOTE: Frequency is normalized by 12800, i.e. 1=12800Hz */
2385 : /*===================================================================*/
2386 0 : void erb_slot_fx(
2387 : Word16 lag_fx, /* i : input lag */
2388 : Word16 *out_fx, /* o : ERB slots */
2389 : Word16 *mfreq_fx, /* o : ERB frequencies */
2390 : Word16 num_erb_fx /* i : number of ERBs */
2391 : )
2392 : {
2393 : Word16 i, j, n;
2394 : Word16 diff_fx;
2395 : Word16 upper_cut_off_freq_fx;
2396 : Word32 Ltemp_fx;
2397 : Word32 mf_fx[NUM_ERB_WB];
2398 : Word16 nH_band_fx;
2399 : Word16 exp, tmp;
2400 : Word32 L_tmp1, L_tmp;
2401 : Word16 fraca, fracb, expa, expb, scale;
2402 0 : const Word16 *erb_fx = NULL; /*move16(); */
2403 :
2404 0 : upper_cut_off_freq_fx = 4000;
2405 0 : move16();
2406 :
2407 0 : IF( EQ_16( num_erb_fx, NUM_ERB_NB ) )
2408 : {
2409 0 : upper_cut_off_freq_fx = 4000;
2410 0 : move16();
2411 0 : erb_fx = &( erb_NB_fx[0] );
2412 : }
2413 0 : ELSE IF( EQ_16( num_erb_fx, NUM_ERB_WB ) )
2414 : {
2415 0 : upper_cut_off_freq_fx = 6400;
2416 0 : move16();
2417 0 : erb_fx = &( erb_WB_fx[0] );
2418 : }
2419 :
2420 0 : exp = norm_s( lag_fx );
2421 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), lag_fx ); /* Q29-exp */
2422 0 : L_tmp1 = L_mult( 12800, tmp ); /* Q(30-exp) */
2423 0 : diff_fx = extract_h( L_shl( L_tmp1, sub( exp, 14 ) ) ); /* Q0 */
2424 :
2425 0 : exp = norm_s( diff_fx );
2426 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), diff_fx ); /* Q29-exp */
2427 0 : L_tmp1 = L_mult( upper_cut_off_freq_fx, tmp ); /* Q(30-exp) */
2428 0 : nH_band_fx = round_fx( L_shl( L_tmp1, sub( exp, 14 ) ) ); /* Q0 */
2429 :
2430 0 : FOR( i = 0; i < num_erb_fx; i++ )
2431 : {
2432 0 : out_fx[i] = 0;
2433 0 : move16();
2434 0 : mf_fx[i] = 0;
2435 0 : move16();
2436 : }
2437 :
2438 :
2439 0 : L_tmp = L_mult0( diff_fx, nH_band_fx ); /* Q0 */
2440 :
2441 :
2442 0 : IF( GE_32( L_sub( upper_cut_off_freq_fx, L_tmp ), L_deposit_l( diff_fx ) ) ) /* Q0 compare */
2443 : {
2444 0 : nH_band_fx = add( nH_band_fx, 1 ); /* Q0 */
2445 : }
2446 :
2447 0 : n = s_min( shr( lag_fx, 1 ), nH_band_fx );
2448 0 : exp = norm_s( lag_fx );
2449 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), lag_fx ); /* Q29-exp */
2450 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
2451 0 : diff_fx = round_fx( L_tmp );
2452 :
2453 0 : j = 0;
2454 0 : move16();
2455 0 : FOR( i = 0; i <= n; i++ )
2456 : {
2457 0 : Ltemp_fx = L_mult( diff_fx, i ); /* Ltemp=i*diff, Q20 */
2458 : /* freq=round32_16(L_shl(Ltemp,11)); : freq=i*diff, Q15 */
2459 :
2460 0 : IF( EQ_16( num_erb_fx, NUM_ERB_NB ) )
2461 : {
2462 0 : Ltemp_fx = L_min( Ltemp_fx, 0x050000 ); /* 0x50000=0.3125 in Q20 (4000Hz) */
2463 : }
2464 0 : ELSE IF( EQ_16( num_erb_fx, NUM_ERB_WB ) )
2465 : {
2466 0 : Ltemp_fx = L_min( Ltemp_fx, 0x080000 ); /* 0x80000=0.5 in Q20 (6400Hz) */
2467 : }
2468 :
2469 0 : FOR( ; j < num_erb_fx; j++ )
2470 : {
2471 0 : IF( LT_32( Ltemp_fx, L_shl( erb_fx[j + 1], 5 ) ) )
2472 : {
2473 0 : mf_fx[j] = L_add( mf_fx[j], Ltemp_fx );
2474 0 : move32();
2475 0 : out_fx[j] = add( out_fx[j], 1 );
2476 0 : move16();
2477 0 : BREAK;
2478 : }
2479 : }
2480 : }
2481 0 : FOR( j = 0; j < num_erb_fx; j++ )
2482 : {
2483 0 : mfreq_fx[j] = round_fx_sat( L_shl_sat( mf_fx[j], 11 ) ); /* Q15 */
2484 0 : move16();
2485 0 : IF( GT_16( out_fx[j], 1 ) )
2486 : {
2487 0 : expb = norm_l( mf_fx[j] );
2488 0 : fracb = round_fx_sat( L_shl_sat( mf_fx[j], expb ) );
2489 0 : expb = sub( 30, add( expb, 20 ) );
2490 :
2491 :
2492 0 : expa = norm_l( out_fx[j] );
2493 0 : fraca = extract_h( L_shl( out_fx[j], expa ) );
2494 0 : expa = sub( 30, expa );
2495 :
2496 0 : scale = shr( sub( fraca, fracb ), 15 );
2497 0 : fracb = shl( fracb, scale );
2498 0 : expb = sub( expb, scale );
2499 :
2500 0 : tmp = div_s( fracb, fraca );
2501 0 : exp = sub( expb, expa );
2502 0 : L_tmp = L_shl_sat( tmp, add( exp, 16 ) );
2503 :
2504 0 : mfreq_fx[j] = round_fx_sat( L_tmp );
2505 0 : move16();
2506 : }
2507 : }
2508 0 : return;
2509 : }
2510 : /*===================================================================*/
2511 : /* FUNCTION : DTFS_erb_inv_fx () */
2512 : /*-------------------------------------------------------------------*/
2513 : /* PURPOSE : Convert erb into harmonics */
2514 : /*-------------------------------------------------------------------*/
2515 : /* INPUT ARGUMENTS : */
2516 : /* _ (Word16 []) in_fx : erb output, Q13 */
2517 : /* _ (Word16 []) slot_fx : number of harmonics in the ERB bins, Q0 */
2518 : /* _ (Word16 []) mfreq_fx : frequency bounds of the ERB bins, Q15 */
2519 : /* _ (struct DTFS_STRUCTURE) (Word16) lag_fx: length of */
2520 : /* prototype in time domain */
2521 : /*-------------------------------------------------------------------*/
2522 : /* OUTPUT ARGUMENTS : */
2523 : /* _ (struct DTFS_STRUCTURE) : prototype in polar domain */
2524 : /* (Word16 []) a_fx: amplitude, normalized */
2525 : /* (Word16) Q: norm factor of a */
2526 : /*-------------------------------------------------------------------*/
2527 : /* INPUT/OUTPUT ARGUMENTS : */
2528 : /* _ None */
2529 : /*-------------------------------------------------------------------*/
2530 : /* RETURN ARGUMENTS : _ None. */
2531 : /*-------------------------------------------------------------------*/
2532 : /* CALLED FROM : TX/RX */
2533 : /*===================================================================*/
2534 :
2535 0 : void DTFS_erb_inv_fx(
2536 : Word16 *in_fx, /* i : ERB inpt */
2537 : Word16 *slot_fx, /* i : ERB slots filled based on lag */
2538 : Word16 *mfreq_fx, /* i : erb frequence edges */
2539 : DTFS_STRUCTURE *X_fx, /* o : DTFS after erb-inv */
2540 : Word16 num_erb_fx /* i : Number of ERB bands */
2541 : )
2542 : {
2543 :
2544 0 : Word16 i, j, m_fx = 0, n, HalfLag_fx;
2545 : Word16 diff_fx; /* 1/lag, Q19 */
2546 0 : Word16 d1, d2, q[MAXLAG_WI], min_q = 0;
2547 : Word16 d1h, d1l, d2h, d2l;
2548 : Word16 freq_fx, f_fx[NUM_ERB_WB + 2], amp_fx[NUM_ERB_WB + 2];
2549 0 : Word16 upper_cut_off_freq_fx = 0;
2550 : Word32 Ltemp_fx, Ltemp2_fx;
2551 : Word32 Lacc_fx;
2552 : Word16 exp, tmp;
2553 0 : move16();
2554 0 : move16();
2555 0 : move16();
2556 :
2557 0 : IF( EQ_16( num_erb_fx, NUM_ERB_NB ) )
2558 : {
2559 0 : upper_cut_off_freq_fx = 0x02800;
2560 0 : move16(); /* 0x2800=0.3125 in Q15 (4000Hz) */
2561 : }
2562 0 : ELSE IF( EQ_16( num_erb_fx, NUM_ERB_WB ) )
2563 : {
2564 0 : upper_cut_off_freq_fx = 0x04000;
2565 0 : move16(); /* 0x4000=0.5 in Q15 (6400Hz) */
2566 : }
2567 :
2568 0 : f_fx[m_fx] = 0;
2569 0 : move16();
2570 0 : amp_fx[m_fx] = 0;
2571 0 : move16();
2572 0 : m_fx = add( m_fx, 1 );
2573 :
2574 0 : FOR( i = 0; i < num_erb_fx; i++ )
2575 : {
2576 0 : IF( slot_fx[i] != 0 )
2577 : {
2578 0 : f_fx[m_fx] = mfreq_fx[i];
2579 0 : move16();
2580 0 : amp_fx[m_fx] = in_fx[i];
2581 0 : move16();
2582 0 : m_fx = add( m_fx, 1 );
2583 : }
2584 : }
2585 0 : f_fx[m_fx] = upper_cut_off_freq_fx;
2586 0 : move16();
2587 0 : amp_fx[m_fx] = 0;
2588 0 : move16();
2589 0 : m_fx = add( m_fx, 1 );
2590 :
2591 0 : exp = norm_s( X_fx->lag_fx );
2592 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X_fx->lag_fx ); /* 29-exp */
2593 0 : diff_fx = shl( tmp, sub( exp, 10 ) ); /* Q19 */
2594 :
2595 0 : min_q = EVS_SW_MAX;
2596 0 : move16();
2597 0 : X_fx->a_fx[0] = 0;
2598 0 : move16();
2599 :
2600 0 : HalfLag_fx = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
2601 :
2602 0 : j = 1;
2603 0 : move16();
2604 0 : FOR( i = 1; i <= HalfLag_fx; i++ )
2605 : {
2606 0 : Ltemp_fx = L_mult( diff_fx, i ); /* Ltemp=i*diff, Q20 */
2607 0 : freq_fx = round_fx( L_shl( Ltemp_fx, 11 ) ); /* Q15 */
2608 :
2609 0 : freq_fx = s_min( freq_fx, upper_cut_off_freq_fx ); /* 0x4000 is 0.5 in Q15 */
2610 :
2611 0 : FOR( ; j < m_fx; j++ )
2612 : {
2613 0 : d1 = sub( f_fx[j], freq_fx );
2614 :
2615 0 : IF( d1 >= 0 )
2616 : {
2617 0 : d2 = sub( freq_fx, f_fx[j - 1] );
2618 0 : Ltemp_fx = L_mac( L_mult( amp_fx[j], d2 ), amp_fx[j - 1], d1 ); /* Q29 */
2619 0 : d2 = sub( f_fx[j], f_fx[j - 1] ); /* Q15 */
2620 : /* Ltemp2_fx=invert_dp((Word40)d2, 4, &n,1); : Ltemp2=1/d2, Q(61-15-n) */
2621 0 : exp = norm_s( d2 );
2622 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), d2 ); /* 29-exp */
2623 : /* L_tmp = L_shr(tmp,8); */
2624 0 : Ltemp2_fx = L_shl( tmp, 16 );
2625 0 : n = add( exp, 16 );
2626 :
2627 0 : d1h = extract_h( Ltemp_fx );
2628 0 : d1l = extract_l( Ltemp_fx );
2629 0 : d2h = extract_h( Ltemp2_fx );
2630 0 : d2l = extract_l( Ltemp2_fx );
2631 0 : Ltemp_fx = (Word32) L_mult0( d1h, d2l );
2632 0 : Lacc_fx = L_mac0( (Word32) Ltemp_fx, d2h, d1l );
2633 0 : Ltemp_fx = L_add( (Word32) L_shr( Lacc_fx, 15 ), L_mult( d1h, d2h ) ); /* 46-n+29-31 */
2634 0 : d2h = norm_l( Ltemp_fx ); /* d2h is 0 IF Ltemp=0 */
2635 0 : if ( Ltemp_fx == 0 )
2636 : {
2637 0 : d2h = 31;
2638 0 : move16();
2639 : }
2640 0 : X_fx->a_fx[i] = round_fx_sat( L_shl_sat( Ltemp_fx, d2h ) ); /* Q(28-n+d2h) */
2641 0 : move16();
2642 0 : q[i] = add( sub( 28, n ), d2h );
2643 0 : move16();
2644 0 : min_q = s_min( min_q, q[i] );
2645 :
2646 0 : BREAK;
2647 : }
2648 : }
2649 : }
2650 : /* block normalize a[i] */
2651 0 : FOR( i = 1; i <= HalfLag_fx; i++ )
2652 : {
2653 0 : X_fx->a_fx[i] = shl( X_fx->a_fx[i], sub( min_q, q[i] ) );
2654 0 : move16();
2655 : }
2656 :
2657 0 : X_fx->Q = min_q;
2658 0 : move16();
2659 0 : return;
2660 : }
2661 :
2662 :
2663 : /*===================================================================*/
2664 : /* FUNCTION : erb_add_fx () */
2665 : /*-------------------------------------------------------------------*/
2666 : /* PURPOSE : Reconstruct current erb amplitude for QPPP */
2667 : /*-------------------------------------------------------------------*/
2668 : /* INPUT ARGUMENTS : */
2669 : /* _ (Word16) pl_fx : previous pitch lag, Q0 */
2670 : /* _ (Word16) l_fx : current pitch lag, Q0 */
2671 : /* _ (Word16 []) prev_erb_fx : Previous erb amplitude, Q13 */
2672 : /* _ (Word16 []) index_fx: quantized differential erb index */
2673 : /*-------------------------------------------------------------------*/
2674 : /* OUTPUT ARGUMENTS : */
2675 : /* _ (Word16 []) curr_erb_fx : Current erb amplitude, Q13 */
2676 : /*-------------------------------------------------------------------*/
2677 : /* INPUT/OUTPUT ARGUMENTS : */
2678 : /* _ None */
2679 : /*-------------------------------------------------------------------*/
2680 : /* RETURN ARGUMENTS : _ None. */
2681 : /*-------------------------------------------------------------------*/
2682 : /* CALLED FROM : TX/RX */
2683 : /*===================================================================*/
2684 0 : void erb_add_fx(
2685 : Word16 *curr_erb_fx, /* i/o: current ERB */
2686 : Word16 l_fx, /* i : current lag */
2687 : const Word16 *prev_erb_fx, /* i : previous ERB */
2688 : Word16 pl_fx, /* i : previous lag */
2689 : const Word16 *index_fx, /* i : ERB index */
2690 : Word16 num_erb_fx /* i : number of ERBs */
2691 : )
2692 : {
2693 : Word16 i;
2694 : Word16 pslot_fx[NUM_ERB_WB], cslot_fx[NUM_ERB_WB], t_prev_erb_fx[NUM_ERB_WB];
2695 : Word16 tmp_fx, tmp2_fx, tmp_loop;
2696 0 : const Word16 *AmpCB1_fx = NULL; /*move16(); */
2697 :
2698 0 : IF( EQ_16( num_erb_fx, NUM_ERB_NB ) )
2699 : {
2700 0 : AmpCB1_fx = AmpCB1_NB_fx;
2701 0 : move16();
2702 : }
2703 0 : ELSE IF( EQ_16( num_erb_fx, NUM_ERB_WB ) )
2704 : {
2705 0 : AmpCB1_fx = AmpCB1_WB_fx;
2706 0 : move16();
2707 : }
2708 :
2709 0 : erb_slot_fx( l_fx, cslot_fx, t_prev_erb_fx, num_erb_fx );
2710 0 : erb_slot_fx( pl_fx, pslot_fx, t_prev_erb_fx, num_erb_fx );
2711 :
2712 0 : FOR( i = 0; i < num_erb_fx; i++ )
2713 : {
2714 0 : t_prev_erb_fx[i] = prev_erb_fx[i];
2715 0 : move16(); /* Q13 */
2716 : }
2717 :
2718 :
2719 0 : IF( GT_16( pl_fx, l_fx ) )
2720 : {
2721 0 : tmp_fx = t_prev_erb_fx[0];
2722 0 : move16(); /* Q13 */
2723 0 : FOR( i = 0; i < num_erb_fx; i++ )
2724 : {
2725 :
2726 0 : IF( pslot_fx[i] != 0 )
2727 : {
2728 0 : tmp_fx = t_prev_erb_fx[i];
2729 0 : move16(); /* Q13 */
2730 : }
2731 : ELSE
2732 : {
2733 0 : t_prev_erb_fx[i] = tmp_fx;
2734 0 : move16(); /* Q13 */
2735 : }
2736 : }
2737 : }
2738 0 : ELSE IF( GT_16( l_fx, pl_fx ) )
2739 : {
2740 0 : tmp_fx = t_prev_erb_fx[( num_erb_fx - 1 )]; /* Q13 */
2741 0 : FOR( i = ( num_erb_fx - 1 ); i >= 0; i-- )
2742 : {
2743 :
2744 0 : IF( pslot_fx[i] != 0 )
2745 : {
2746 0 : tmp_fx = t_prev_erb_fx[i];
2747 0 : move16(); /* Q13 */
2748 : }
2749 : ELSE
2750 : {
2751 0 : t_prev_erb_fx[i] = tmp_fx;
2752 0 : move16(); /* Q13 */
2753 : }
2754 : }
2755 : }
2756 :
2757 0 : tmp_fx = add( shl( index_fx[0], 3 ), shl( index_fx[0], 1 ) ); /* tmp_fx=10*index_fx[0] */
2758 0 : FOR( i = 1; i < 11; i++ )
2759 : {
2760 :
2761 0 : IF( cslot_fx[i] != 0 )
2762 : {
2763 0 : curr_erb_fx[i] = add( AmpCB1_fx[( ( tmp_fx + i ) - 1 )], t_prev_erb_fx[i] );
2764 0 : move16(); /* Q13+Q13=Q13 */
2765 0 : curr_erb_fx[i] = s_max( 0, curr_erb_fx[i] );
2766 0 : move16();
2767 : }
2768 : ELSE
2769 : {
2770 0 : curr_erb_fx[i] = 0;
2771 0 : move16();
2772 : }
2773 : }
2774 :
2775 0 : tmp_fx = add( shl( index_fx[1], 3 ), index_fx[1] ); /* tmp=9*index[1] */
2776 0 : tmp2_fx = mult( shl( index_fx[1], 6 ), 5632 ); /* temp=11*index_fx[1] */
2777 0 : tmp_loop = sub( num_erb_fx, 2 );
2778 0 : FOR( i = 11; i < tmp_loop; i++ )
2779 : {
2780 :
2781 0 : IF( cslot_fx[i] != 0 )
2782 : {
2783 0 : IF( EQ_16( num_erb_fx, NUM_ERB_NB ) )
2784 : {
2785 0 : curr_erb_fx[i] = add( AmpCB2_NB_fx[( ( tmp_fx + i ) - 11 )], t_prev_erb_fx[i] ); /* Q13+Q13=Q13 */
2786 0 : move16();
2787 0 : curr_erb_fx[i] = s_max( 0, curr_erb_fx[i] );
2788 0 : move16();
2789 : }
2790 0 : ELSE IF( EQ_16( num_erb_fx, NUM_ERB_WB ) )
2791 : {
2792 0 : curr_erb_fx[i] = add( AmpCB2_WB_fx[( ( tmp2_fx + i ) - 11 )], t_prev_erb_fx[i] ); /* Q13 */
2793 0 : move16();
2794 0 : curr_erb_fx[i] = s_max( 0, curr_erb_fx[i] );
2795 0 : move16();
2796 : }
2797 : }
2798 : ELSE
2799 : {
2800 0 : curr_erb_fx[i] = 0;
2801 0 : move16();
2802 : }
2803 : }
2804 0 : return;
2805 : }
2806 :
2807 :
2808 : /*===================================================================*/
2809 : /* FUNCTION : struct quant_target_fx() */
2810 : /*-------------------------------------------------------------------*/
2811 : /* PURPOSE : Get weighting and target for power quantization */
2812 : /*-------------------------------------------------------------------*/
2813 : /* INPUT ARGUMENTS : */
2814 : /* _ (Word16 []) curr_lpc: LPC coefficients, Q12 */
2815 : /* _ (Word16 []) sin_tab: sine table based on lag, Q15 */
2816 : /* _ (Word16 []) cos_tab: cosine table based on lag, Q15 */
2817 : /*-------------------------------------------------------------------*/
2818 : /* OUTPUT ARGUMENTS : */
2819 : /* _ (Word16[]) w: Weighting for power quantization, Q15 */
2820 : /* _ (Word16[]) target: Power of 2 bands for quantization, Q11 */
2821 : /*-------------------------------------------------------------------*/
2822 : /* INPUT/OUTPUT ARGUMENTS : */
2823 : /* _ (struct DTFS_fx) X : prototype in polar domain */
2824 : /* (Word16) lag: length of prototype in time domain */
2825 : /* (Word16 []) a: amplitude of harmonics, normalized */
2826 : /* (Word16) Q: norm factor of a */
2827 : /*-------------------------------------------------------------------*/
2828 : /* RETURN ARGUMENTS : */
2829 : /* _ None */
2830 : /*-------------------------------------------------------------------*/
2831 : /* CALLED FROM : TX */
2832 : /*===================================================================*/
2833 : /* NOTE: This function is used by quant_cw_fx and quant_cw_memless_fx*/
2834 : /*===================================================================*/
2835 0 : void quant_target_fx( DTFS_STRUCTURE *X_fx, const Word16 *curr_lpc, Word16 *w, Word16 *target, Word16 *sin_tab, Word16 *cos_tab )
2836 : {
2837 : Word16 tmp, n, exp;
2838 : Word16 Ql, Qh;
2839 : Word32 Ltemp1, Ltemp, Ltemp2, logLag, Ltemp3, Lacc;
2840 :
2841 0 : tmp = sub( shl( X_fx->Q, 1 ), 13 ); /* tmp=2Q-13, Q factor of getSpEngy... output */
2842 0 : Ltemp3 = L_shl( L_mult( tmp, 24660 ), 9 ); /* Ltemp3=(2Q-13)*10log10(2), Q23, 24660=10log10(2) in Q13 */
2843 :
2844 0 : Ltemp1 = getSpEngyFromResAmp_fx( X_fx, 0, 2828, curr_lpc, sin_tab, cos_tab );
2845 0 : Ltemp1 = log10_fx( Ltemp1 );
2846 :
2847 : /* subtract 10log10(2)*(2Q-13), Q23 */
2848 0 : Ltemp1 = L_sub_sat( Ltemp1, Ltemp3 );
2849 0 : Ltemp1 = L_max( 0, Ltemp1 );
2850 0 : Ltemp2 = getSpEngyFromResAmp_fx( X_fx, 2828, X_fx->upper_cut_off_freq_fx, curr_lpc, sin_tab, cos_tab );
2851 0 : Ltemp2 = log10_fx( Ltemp2 ); /* Ltemp1=10log10(eng_hb), Q23, need to adjust for Q factor of energy (2Q-13) */
2852 0 : Ltemp2 = L_sub_sat( Ltemp2, Ltemp3 ); /* Ltemp2 in Q23 */
2853 :
2854 0 : Ltemp2 = L_max( 0, Ltemp2 );
2855 :
2856 : /* Getting the Speech Domain Energy LOG Ratio */
2857 :
2858 0 : Lacc = L_add( Ltemp1, Ltemp2 );
2859 0 : exp = norm_l( Lacc );
2860 0 : tmp = round_fx( L_shl( Lacc, exp ) );
2861 0 : exp = sub( sub( 30, exp ), 23 );
2862 0 : IF( tmp )
2863 : {
2864 0 : tmp = div_s( 16384, tmp ); /* 15+exp1 */
2865 : }
2866 : ELSE
2867 : {
2868 0 : tmp = 0;
2869 0 : move16();
2870 : }
2871 0 : Ltemp = L_deposit_h( tmp );
2872 :
2873 0 : tmp = round_fx( Ltemp ); /* tmp in Q(22-n) */
2874 0 : Ltemp1 = Mult_32_16( Ltemp1, tmp ); /* Q(30-n) */
2875 0 : n = sub( 8, exp );
2876 0 : w[0] = round_fx_sat( L_shl_sat( Ltemp1, n ) ); /* w[0] in Q15 */
2877 0 : move16();
2878 0 : Ltemp2 = Mult_32_16( Ltemp2, tmp );
2879 0 : w[1] = round_fx_sat( L_shl_sat( Ltemp2, n ) ); /* w[1] in Q15 */
2880 0 : move16();
2881 :
2882 0 : logLag = log10_fx( X_fx->lag_fx ); /* logLag=10*log10(lag), Q23 */
2883 0 : Ltemp3 = L_shl( L_mult( shl( X_fx->Q, 1 ), 24660 ), 9 ); /* Ltemp3=2Q*10log10(2), Q23 */
2884 : /* Process low band */
2885 0 : Ltemp = DTFS_setEngyHarm_fx( 236, 2828, 0, 2828, 1, 0, &Ql, X_fx ); /* Ql is norm factor of low band a[], Ltemp is energy in 2Q */
2886 : /* Compensate for Q factor of energy to get log10(lag*eng) */
2887 0 : Ltemp = log10_fx( Ltemp ); /* Ltemp=10log10(eng), Q23 */
2888 0 : Ltemp = L_add( L_sub( Ltemp, Ltemp3 ), logLag ); /* Ltemp=10*log10(lag*eng), Q23 */
2889 :
2890 0 : target[0] = round_fx( L_shl( Mult_32_16( Ltemp, 0x6666 ), 1 ) ); /* Q11 */
2891 0 : move16();
2892 :
2893 : /* Process high band */
2894 0 : Ltemp = DTFS_setEngyHarm_fx( 2828, X_fx->upper_cut_off_freq_of_interest_fx, 2828, X_fx->upper_cut_off_freq_fx, 1, 0, &Qh, X_fx );
2895 0 : Ltemp = log10_fx( Ltemp );
2896 0 : Ltemp = L_add( L_sub( Ltemp, Ltemp3 ), logLag ); /* Ltemp=10*log10(lag*eng), Q23 */
2897 0 : target[1] = round_fx( L_shl( Mult_32_16( Ltemp, 0x6666 ), 1 ) ); /* Q11 */
2898 0 : move16();
2899 :
2900 : /* Need to unify the Q factors of both bands */
2901 0 : X_fx->Q = s_min( Ql, Qh ); /* set Q factor to be the smaller one */
2902 0 : move16();
2903 0 : n = sub( Ql, Qh ); /* compare band Q factors */
2904 :
2905 0 : IF( n < 0 )
2906 : {
2907 0 : rshiftHarmBand_fx( X_fx, 2828, X_fx->upper_cut_off_freq_fx, n );
2908 : }
2909 0 : ELSE IF( n > 0 )
2910 : {
2911 0 : rshiftHarmBand_fx( X_fx, 0, 2828, sub( Qh, Ql ) );
2912 : }
2913 0 : }
2914 :
2915 : /*==========================================================================*/
2916 : /* FUNCTION : WIsyn_fx () */
2917 : /*--------------------------------------------------------------------------*/
2918 : /* PURPOSE : */
2919 : /*--------------------------------------------------------------------------*/
2920 : /* INPUT ARGUMENTS : */
2921 : /* _ (struct DTFS_STRUCTURE) PREVCW_FX: a/b in PREVCW_FX.Q */
2922 : /* _ (struct DTFS_fx *) CURRCW_DTFS_out: a/b in CURRCW_DTFS_out->Q */
2923 : /* _ (Word16 *) curr_lpc_fx: lpc coefficients in Q12 */
2924 : /* _ (Word16 *) ph_offset_fx: in Q15, normalized by 2pi */
2925 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
2926 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
2927 : /* _ (Word16 *) N: length, Q0 */
2928 : /*--------------------------------------------------------------------------*/
2929 : /* OUTPUT ARGUMENTS : */
2930 : /* _ (Word16 *) out_fx: Q0 */
2931 : /*--------------------------------------------------------------------------*/
2932 : /* INPUT/OUTPUT ARGUMENTS : */
2933 : /* _ (struct DTFS_STRUCTURE *) CURRCW_DTFS_out: a/b in CURRCW_DTFS_out->Q*/
2934 : /* _ (Word16 *) ph_offset_fx: in Q15, normalized by 2pi */
2935 : /*--------------------------------------------------------------------------*/
2936 : /* RETURN ARGUMENTS : _ None. */
2937 : /*==========================================================================*/
2938 0 : ivas_error WIsyn_fx(
2939 : DTFS_STRUCTURE PREVCW_FX, /* i : Prev frame DTFS */
2940 : DTFS_STRUCTURE *CURRCW_DTFS_out, /* i/o: Curr frame DTFS */
2941 : const Word16 *curr_lpc_fx, /* i : LPC */
2942 : Word16 *ph_offset_fx, /* i/o: Phase offset to line up at end of frame */
2943 : Word16 *out_fx, /* o : Waveform Interpolated time domain signal */
2944 : const Word16 N, /* i : Number of output samples to generate */
2945 : const Word16 FR_flag, /* i : called for post-smoothing in FR */
2946 : Word16 *S_fx,
2947 : Word16 *C_fx,
2948 : Word16 *pf_temp1,
2949 : Word16 *pf_temp2,
2950 : Word16 *pf_temp,
2951 : Word16 *pf_n2 )
2952 : {
2953 : Word16 i;
2954 0 : UWord16 I = 1, flag = 0;
2955 0 : move16();
2956 0 : move16();
2957 : /* Word16 *phase_fx = (Word16*)malloc(sizeof(Word16) * (2*N)); new operator used size 2*N */
2958 : Word32 phase_fx[WI_FX_phase_fx];
2959 : Word16 alignment_fx;
2960 : Word16 tmp_fx, temp;
2961 : Word32 L_temp;
2962 : Word16 exp, tmp;
2963 : Word32 L_tmp;
2964 : /* prev_lsp_fx; */
2965 :
2966 :
2967 : DTFS_STRUCTURE *CURRCW_DTFS_FX;
2968 : ivas_error error;
2969 :
2970 0 : error = IVAS_ERR_OK;
2971 0 : move16();
2972 :
2973 0 : IF( ( error = DTFS_new_fx( &CURRCW_DTFS_FX ) ) != IVAS_ERR_OK )
2974 : {
2975 0 : return IVAS_ERROR( error, "Error creating new DTFS structure\n" );
2976 : }
2977 0 : IF( LT_16( PREVCW_FX.Q, CURRCW_DTFS_out->Q ) )
2978 : {
2979 0 : temp = sub( CURRCW_DTFS_out->Q, PREVCW_FX.Q );
2980 0 : tmp = s_min( shr( CURRCW_DTFS_out->lag_fx, 1 ), CURRCW_DTFS_out->nH_fx );
2981 0 : FOR( i = 0; i <= tmp; i++ )
2982 : {
2983 0 : CURRCW_DTFS_out->a_fx[i] = shr( CURRCW_DTFS_out->a_fx[i], temp );
2984 0 : move16();
2985 0 : CURRCW_DTFS_out->b_fx[i] = shr( CURRCW_DTFS_out->b_fx[i], temp );
2986 0 : move16();
2987 : }
2988 0 : CURRCW_DTFS_out->Q = PREVCW_FX.Q;
2989 0 : move16();
2990 : }
2991 :
2992 :
2993 0 : IF( LT_16( CURRCW_DTFS_out->Q, PREVCW_FX.Q ) )
2994 : {
2995 0 : temp = sub( PREVCW_FX.Q, CURRCW_DTFS_out->Q );
2996 0 : tmp = s_min( shr( PREVCW_FX.lag_fx, 1 ), PREVCW_FX.nH_fx );
2997 0 : FOR( i = 0; i <= tmp; i++ )
2998 : {
2999 0 : PREVCW_FX.a_fx[i] = shr( PREVCW_FX.a_fx[i], temp );
3000 0 : move16();
3001 0 : PREVCW_FX.b_fx[i] = shr( PREVCW_FX.b_fx[i], temp );
3002 0 : move16();
3003 : }
3004 0 : PREVCW_FX.Q = CURRCW_DTFS_out->Q;
3005 0 : move16();
3006 : }
3007 :
3008 0 : DTFS_copy_fx( CURRCW_DTFS_FX, *CURRCW_DTFS_out );
3009 :
3010 : /* Calculating the expected alignment shift */
3011 0 : alignment_fx = mult_r( *ph_offset_fx, shl( PREVCW_FX.lag_fx, 7 ) ); /* confirmed I<2 by smv12.org, Q7 */
3012 :
3013 :
3014 0 : IF( EQ_16( flag, 1 ) )
3015 0 : alignment_fx = extract_l( L_shr( L_mult( alignment_fx, I ), 1 ) ); /* Q7 */
3016 :
3017 : /* Calculating the expected alignment shift */
3018 0 : find_rem( (Word16) N, shr( add( PREVCW_FX.lag_fx, CURRCW_DTFS_FX->lag_fx ), 1 ), &temp );
3019 0 : temp = add( shl( temp, 7 ), alignment_fx ); /* Q7 */
3020 :
3021 :
3022 0 : IF( temp < 0 )
3023 : {
3024 0 : temp = add( temp, shl( CURRCW_DTFS_FX->lag_fx, 7 ) ); /* Q7 */
3025 : }
3026 :
3027 0 : find_rem( temp, shl( CURRCW_DTFS_FX->lag_fx, 7 ), &tmp_fx ); /* Q7 */
3028 0 : IF( FR_flag == 0 )
3029 : {
3030 0 : alignment_fx = DTFS_alignment_weight_fx( &PREVCW_FX, *CURRCW_DTFS_FX, tmp_fx, curr_lpc_fx, curr_lpc_fx, S_fx,
3031 : C_fx, pf_temp1, pf_temp2, pf_temp, pf_n2 ); /* Q7 */
3032 : }
3033 : ELSE
3034 : {
3035 0 : alignment_fx = DTFS_alignment_full_fx( PREVCW_FX, *CURRCW_DTFS_FX, *ph_offset_fx, S_fx, C_fx, FR_flag );
3036 :
3037 : /*alignment_fx is in Q1, we make it Q7*/
3038 0 : alignment_fx = shl( alignment_fx, 6 );
3039 : }
3040 :
3041 :
3042 0 : IF( GE_16( alignment_fx, shl( CURRCW_DTFS_FX->lag_fx, 7 ) ) )
3043 : {
3044 0 : temp = sub( alignment_fx, shl( CURRCW_DTFS_FX->lag_fx, 7 ) );
3045 0 : tmp = shl( CURRCW_DTFS_FX->lag_fx, 7 );
3046 0 : exp = norm_s( tmp );
3047 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), tmp ); /* 22-exp */
3048 0 : L_tmp = L_shl( L_mult( temp, tmp ), add( exp, 1 ) );
3049 0 : tmp_fx = round_fx( L_tmp );
3050 : }
3051 0 : ELSE IF( alignment_fx < 0 )
3052 : {
3053 0 : temp = negate( alignment_fx );
3054 0 : tmp = shl( CURRCW_DTFS_FX->lag_fx, 7 );
3055 0 : exp = norm_s( tmp );
3056 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), tmp ); /* 22-exp */
3057 0 : L_tmp = L_shl( L_mult( temp, tmp ), add( exp, 1 ) );
3058 0 : tmp_fx = negate( round_fx( L_tmp ) );
3059 : }
3060 : ELSE
3061 : {
3062 0 : temp = alignment_fx;
3063 0 : move16();
3064 0 : tmp = shl( CURRCW_DTFS_FX->lag_fx, 7 );
3065 0 : exp = norm_s( tmp );
3066 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), tmp ); /* 22-exp */
3067 0 : L_tmp = L_shl( L_mult( temp, tmp ), add( exp, 1 ) );
3068 0 : tmp_fx = round_fx( L_tmp );
3069 : }
3070 :
3071 0 : DTFS_phaseShift_fx( CURRCW_DTFS_FX, alignment_fx, CURRCW_DTFS_FX->lag_fx, S_fx, C_fx ); /* Qmin */
3072 0 : DTFS_phaseShift_fx( CURRCW_DTFS_out, alignment_fx, CURRCW_DTFS_out->lag_fx, S_fx, C_fx ); /* Qmin */
3073 :
3074 : /* Compute the cubic phase track and transform to 1-D signal */
3075 0 : cubicPhase_fx( *ph_offset_fx, tmp_fx, PREVCW_FX.lag_fx, CURRCW_DTFS_FX->lag_fx, N, phase_fx );
3076 :
3077 0 : temp = shr( add( PREVCW_FX.lag_fx, CURRCW_DTFS_FX->lag_fx ), 1 ); /* Q0 */
3078 :
3079 0 : IF( FR_flag == 0 )
3080 : {
3081 0 : DTFS_transform_fx( PREVCW_FX, *CURRCW_DTFS_FX, phase_fx, out_fx, N, 0 );
3082 : }
3083 : ELSE
3084 : {
3085 0 : DTFS_transform_fx( PREVCW_FX, *CURRCW_DTFS_FX, phase_fx, out_fx, N, 1 );
3086 : }
3087 :
3088 :
3089 : /* Adjust the phase offset and wrap it between 0 and 2pi */
3090 :
3091 : // PMT("NEED a complete verification of mem flow and assignment")
3092 :
3093 :
3094 0 : IF( EQ_16( flag, 2 ) )
3095 : {
3096 0 : L_temp = L_shr( L_mult( tmp_fx, I ), 1 ); /* Q15 */
3097 : }
3098 : ELSE
3099 : {
3100 0 : L_temp = L_deposit_l( tmp_fx ); /* Q15 */
3101 : }
3102 :
3103 :
3104 0 : FOR( ; L_temp < 0; L_temp += 0x8000L )
3105 : {
3106 : /* empty loop */
3107 : }
3108 0 : L_temp = L_and( L_temp, 0x7fff );
3109 0 : move16(); /* fraction part */
3110 0 : *ph_offset_fx = extract_l( L_temp );
3111 0 : move16();
3112 :
3113 : /* free(phase_fx) ; */
3114 0 : free( CURRCW_DTFS_FX );
3115 0 : return error;
3116 : }
3117 :
3118 :
3119 : /*===================================================================*/
3120 : /* FUNCTION : ppp_extract_pitch_period_fx () */
3121 : /*-------------------------------------------------------------------*/
3122 : /* PURPOSE : Update background noise estimate, signal energy */
3123 : /* estimate, and band snrs */
3124 : /*-------------------------------------------------------------------*/
3125 : /* INPUT ARGUMENTS : */
3126 : /* _ (Word16 []) in : residual input, Q0 */
3127 : /* _ (Word16) l : pitch lag, Q0 */
3128 : /*-------------------------------------------------------------------*/
3129 : /* OUTPUT ARGUMENTS : */
3130 : /* _ (Word16 []) out : pitch period prototype, Q0 */
3131 : /* _ (Word16*) out_of_bound : pitch lag, Q0 */
3132 : /*-------------------------------------------------------------------*/
3133 : /* INPUT/OUTPUT ARGUMENTS : */
3134 : /* _ None */
3135 : /*-------------------------------------------------------------------*/
3136 : /* RETURN ARGUMENTS : */
3137 : /* _ (Word16) spike_near_edge : Q0 */
3138 : /*===================================================================*/
3139 0 : Word16 ppp_extract_pitch_period_fx(
3140 : const Word16 *in, /* i : input residual */
3141 : Word16 *out, /* o : output residual */
3142 : Word16 l, /* i : lag */
3143 : Word16 *out_of_bound, /* o : out of bound flag */
3144 : Word16 Qres
3145 :
3146 : )
3147 : {
3148 : Word16 i, j, k;
3149 0 : Word16 spike = 0, range;
3150 0 : move16();
3151 0 : Word16 max = 0;
3152 0 : move16();
3153 0 : const Word16 *ptr = in + L_FRAME - l;
3154 0 : Word32 en1 = 0, Lacc, L_tmp;
3155 0 : move32();
3156 0 : Word16 spike_near_edge = 0, scale;
3157 0 : move16();
3158 : Word16 pos_max, neg_max;
3159 0 : Word16 spike_pos = 0, spike_neg = 0;
3160 0 : move16();
3161 0 : move16();
3162 : Word16 x, tmp, expa, fraca, expb, fracb, scale1, exp;
3163 :
3164 0 : pos_max = -0x8000L;
3165 0 : move16();
3166 0 : neg_max = 0;
3167 0 : move16();
3168 0 : *out_of_bound = 0;
3169 0 : move16();
3170 :
3171 0 : en1 = L_deposit_l( 0 );
3172 0 : FOR( i = 0; i < l; i++ )
3173 : {
3174 0 : x = abs_s( ptr[i] );
3175 :
3176 0 : IF( GT_16( x, max ) )
3177 : {
3178 0 : max = x;
3179 0 : move16();
3180 0 : spike = i;
3181 0 : move16();
3182 : }
3183 0 : en1 = L_mac0_sat( en1, ptr[i], ptr[i] ); /* Q0 */
3184 : }
3185 :
3186 0 : IF( ptr[spike] > 0 )
3187 : {
3188 0 : spike_pos = spike;
3189 0 : move16();
3190 : /* search for neg spike around the pos spike */
3191 0 : FOR( j = spike - 10; j < spike + 10; j++ )
3192 : {
3193 0 : k = add( j, l ) % l;
3194 0 : move16();
3195 :
3196 0 : IF( LT_16( ptr[k], neg_max ) )
3197 : {
3198 0 : neg_max = ptr[k];
3199 0 : move16();
3200 0 : spike_neg = k;
3201 0 : move16();
3202 : }
3203 : }
3204 : }
3205 0 : ELSE IF( ptr[spike] < 0 )
3206 : {
3207 0 : spike_neg = spike;
3208 0 : move16();
3209 : /* search for pos spike around the neg spike */
3210 0 : FOR( j = spike - 10; j < spike + 10; j++ )
3211 : {
3212 0 : k = add( j, l ) % l;
3213 0 : move16();
3214 :
3215 0 : IF( GT_16( ptr[k], pos_max ) )
3216 : {
3217 0 : pos_max = ptr[k];
3218 0 : move16();
3219 0 : spike_pos = k;
3220 0 : move16();
3221 : }
3222 : }
3223 : }
3224 :
3225 0 : test();
3226 0 : IF( ( LE_16( sub( sub( l, 1 ), s_max( spike_pos, spike_neg ) ), 2 ) ) || ( LE_16( s_min( spike_pos, spike_neg ), 2 ) ) )
3227 : {
3228 0 : *out_of_bound = 1;
3229 0 : move16();
3230 0 : return spike_near_edge;
3231 : }
3232 :
3233 :
3234 0 : tmp = extract_l( ( L_max( L_mult( CUTFREE_REL_RANGE_Q2, l ), CUTFREE_ABS_RANGE_Q3 ) ) ); /* Q3 */
3235 :
3236 0 : IF( tmp > 0 )
3237 : {
3238 0 : tmp = add( tmp, 4 ); /* Q3 */
3239 : }
3240 : ELSE
3241 : {
3242 0 : tmp = sub( tmp, 4 ); /* Q3 */
3243 : }
3244 0 : range = shr( tmp, 3 ); /* Q0 */
3245 :
3246 0 : test();
3247 0 : IF( ( LT_16( spike, range ) ) || ( GE_16( add( spike, range ), l ) ) )
3248 : {
3249 : /* need to grab from one lag before
3250 : ensure that there is no array bound read */
3251 :
3252 0 : IF( LT_16( sub( L_FRAME, l ), l ) )
3253 : {
3254 0 : *out_of_bound = 1;
3255 0 : move16();
3256 0 : return spike_near_edge;
3257 : }
3258 0 : spike_near_edge = 1;
3259 0 : move16();
3260 : }
3261 :
3262 0 : IF( LT_16( spike, range ) )
3263 : {
3264 0 : tmp = add( l, sub( spike, range ) );
3265 0 : FOR( i = 0; i < tmp; i++ )
3266 : {
3267 0 : out[i] = ptr[i];
3268 0 : move16();
3269 : }
3270 : /* Grab Remaining From One Lag Before */
3271 0 : ptr -= l;
3272 0 : FOR( ; i < l; i++ )
3273 : {
3274 0 : out[i] = ptr[i];
3275 0 : move16();
3276 : }
3277 : }
3278 0 : ELSE IF( GE_16( add( spike, range ), l ) )
3279 : {
3280 0 : tmp = sub( spike, range );
3281 0 : FOR( i = 0; i < tmp; i++ )
3282 : {
3283 0 : out[i] = ptr[i];
3284 0 : move16();
3285 : }
3286 : /* Grab Remaining From One Lag Before */
3287 0 : IF( ptr - l + i >= in )
3288 : {
3289 0 : FOR( ptr -= l; i < l; i++ )
3290 : {
3291 0 : out[i] = ptr[i];
3292 0 : move16();
3293 : }
3294 : }
3295 : ELSE
3296 : {
3297 0 : FOR( ; i < l; i++ )
3298 : {
3299 0 : out[i] = ptr[i];
3300 0 : move16();
3301 : }
3302 : }
3303 : }
3304 : ELSE
3305 : {
3306 0 : FOR( i = 0; i < l; i++ )
3307 : {
3308 0 : out[i] = ptr[i];
3309 0 : move16();
3310 : }
3311 : }
3312 : /* Energy adjustment added to eliminate artifacts at the end of voicing */
3313 0 : Lacc = L_deposit_l( 0 );
3314 0 : FOR( i = 0; i < l; i++ )
3315 : {
3316 0 : Lacc = L_mac0_sat( Lacc, out[i], out[i] ); /* Q0 */
3317 : }
3318 :
3319 :
3320 0 : IF( LT_32( en1, Lacc ) )
3321 : {
3322 : /* Ltmp=sqrt_divide_dp(en1, Lacc, 0, &n,1); */
3323 : /* scale=round_fx(L_shl(Ltmp,sub(31,n))); in Q15 */
3324 0 : expa = norm_l( en1 );
3325 0 : fraca = extract_h( L_shl( en1, expa ) );
3326 0 : expa = sub( 30, add( expa, Qres ) );
3327 :
3328 :
3329 0 : expb = norm_l( Lacc );
3330 0 : fracb = round_fx_sat( L_shl_sat( Lacc, expb ) );
3331 0 : expb = sub( 30, add( expb, Qres ) );
3332 :
3333 0 : scale1 = shr( sub( fraca, fracb ), 15 );
3334 0 : fracb = shl( fracb, scale1 );
3335 0 : expb = sub( expb, scale1 );
3336 :
3337 0 : tmp = div_s( fracb, fraca );
3338 0 : exp = sub( expb, expa );
3339 :
3340 0 : L_tmp = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
3341 0 : scale = round_fx( L_tmp ); /* Q15 */
3342 :
3343 0 : FOR( i = 0; i < l; i++ )
3344 : {
3345 0 : out[i] = mult_r( out[i], scale );
3346 0 : move16();
3347 : }
3348 : }
3349 0 : return spike_near_edge;
3350 : }
3351 : /*===========================================================================*/
3352 : /* FUNCTION : DTFS_peaktoaverage_fx () */
3353 : /*---------------------------------------------------------------------------*/
3354 : /* PURPOSE : */
3355 : /*---------------------------------------------------------------------------*/
3356 : /* INPUT ARGUMENTS : */
3357 : /* _ (struct DTFS_STRUCTURE) X_fx: a_fx/b_fx in Q(X_fx.Q), lag in Q0 */
3358 : /*---------------------------------------------------------------------------*/
3359 : /* OUTPUT ARGUMENTS : */
3360 : /* _ (Word32 *) pos_fx : in *Qpos */
3361 : /* _ (Word32 *) neg_fx : in *Qneg */
3362 : /* _ (Word16 *) Qpos: Q valule of output *pos_fx */
3363 : /* _ (Word16 *) Qneg: Q valule of output *neg_fx */
3364 : /*---------------------------------------------------------------------------*/
3365 : /* INPUT/OUTPUT ARGUMENTS : */
3366 : /* _ None */
3367 : /*---------------------------------------------------------------------------*/
3368 : /* RETURN ARGUMENTS : _ None. */
3369 : /*===========================================================================*/
3370 0 : void DTFS_peaktoaverage_fx( DTFS_STRUCTURE X_fx, Word32 *pos_fx, Word16 *Qpos, Word32 *neg_fx, Word16 *Qneg )
3371 : {
3372 : Word32 L_sum;
3373 0 : Word32 maxPosEn_fx = 0, maxNegEn_fx = 0, L_temp;
3374 0 : move32();
3375 0 : move32();
3376 : Word16 i;
3377 : Word16 time_fx[256];
3378 : Word16 expa, expb, fraca, fracb, scale;
3379 : Word16 exp, tmp;
3380 : Word32 L_tmp;
3381 0 : L_sum = DTFS_getEngy_P2A_fx( &X_fx ); /* 2Q */
3382 0 : DTFS_fast_fs_inv_fx( &X_fx, time_fx, 256, 8 );
3383 :
3384 0 : FOR( i = 0; i < 256; i++ )
3385 : {
3386 0 : L_temp = L_mult_sat( time_fx[i], time_fx[i] ); /* Q(1) */
3387 0 : IF( time_fx[i] >= 0 )
3388 : {
3389 0 : if ( GT_32( L_temp, maxPosEn_fx ) )
3390 : {
3391 0 : maxPosEn_fx = L_temp; /* Q(1) */
3392 0 : move32();
3393 : }
3394 : }
3395 : ELSE
3396 : {
3397 0 : if ( GT_32( L_temp, maxNegEn_fx ) )
3398 : {
3399 0 : maxNegEn_fx = L_temp; /* Q(1) */
3400 0 : move32();
3401 : }
3402 : }
3403 : }
3404 :
3405 :
3406 0 : IF( L_sum == 0 )
3407 : {
3408 0 : *pos_fx = *neg_fx = 0;
3409 0 : move32();
3410 0 : move32();
3411 : }
3412 : ELSE
3413 : {
3414 0 : IF( maxPosEn_fx == 0 )
3415 : {
3416 0 : *pos_fx = 0;
3417 0 : move32();
3418 0 : *Qpos = 31;
3419 0 : move16();
3420 : }
3421 : ELSE
3422 : {
3423 0 : expa = norm_l( maxPosEn_fx );
3424 0 : fraca = extract_h( L_shl( maxPosEn_fx, expa ) );
3425 0 : expa = sub( 30, add( expa, 1 ) );
3426 :
3427 :
3428 0 : expb = norm_l( L_sum );
3429 0 : fracb = round_fx_sat( L_shl( L_sum, expb ) );
3430 :
3431 0 : expb = sub( 30, add( expb, shl( X_fx.Q, 1 ) ) );
3432 :
3433 :
3434 0 : scale = shr( sub( fraca, fracb ), 15 );
3435 0 : fracb = shl_sat( fracb, scale );
3436 0 : expb = sub( expb, scale );
3437 :
3438 0 : tmp = div_s( fracb, fraca );
3439 0 : exp = sub( expb, expa );
3440 :
3441 0 : L_tmp = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
3442 0 : *pos_fx = L_tmp;
3443 0 : move32();
3444 0 : *Qpos = sub( 31, exp );
3445 0 : move16();
3446 : }
3447 :
3448 0 : IF( maxNegEn_fx == 0 )
3449 : {
3450 0 : *neg_fx = 0;
3451 0 : move32();
3452 0 : *Qneg = 31;
3453 0 : move16();
3454 : }
3455 : ELSE
3456 : {
3457 :
3458 0 : expa = norm_l( maxNegEn_fx );
3459 0 : fraca = extract_h( L_shl( maxNegEn_fx, expa ) );
3460 0 : expa = sub( Q29, expa ); // 30 - expa - 1;
3461 :
3462 :
3463 0 : expb = norm_l( L_sum );
3464 0 : fracb = round_fx_sat( L_shl( L_sum, expb ) );
3465 0 : expb = sub( 30, add( expb, shl( X_fx.Q, 1 ) ) );
3466 :
3467 :
3468 0 : scale = shr( sub( fraca, fracb ), 15 );
3469 0 : fracb = shl_sat( fracb, scale );
3470 0 : expb = sub( expb, scale );
3471 :
3472 0 : tmp = div_s( fracb, fraca );
3473 0 : exp = sub( expb, expa );
3474 :
3475 0 : L_tmp = Isqrt_lc( L_deposit_h( tmp ), &exp ); /* Q(31-exp) */
3476 0 : *neg_fx = L_tmp;
3477 0 : move32();
3478 0 : *Qneg = sub( Q31, exp );
3479 0 : move16();
3480 : }
3481 : }
3482 0 : }
3483 :
3484 : /*===================================================================*/
3485 : /* FUNCTION : struct DTFS_fx:: rshiftHarmBand_fx() */
3486 : /*-------------------------------------------------------------------*/
3487 : /* PURPOSE : Right-shift harmonics in band to align Q factor */
3488 : /*-------------------------------------------------------------------*/
3489 : /* INPUT ARGUMENTS : */
3490 : /* _ (Word16) lband: lower band boundary, Q15 */
3491 : /* _ (Word16) hband: upper band boundary, Q15 */
3492 : /* _ (Word16) shift: right shift value, Q0 (must be <0) */
3493 : /*-------------------------------------------------------------------*/
3494 : /* OUTPUT ARGUMENTS : */
3495 : /* _ None */
3496 : /*-------------------------------------------------------------------*/
3497 : /* INPUT/OUTPUT ARGUMENTS : */
3498 : /* _ (struct DTFS_fx) X : prototype in polar domain */
3499 : /* (Word16) lag: length of prototype in time domain */
3500 : /* (Word16 []) a: amplitude of harmonics, normalized */
3501 : /*-------------------------------------------------------------------*/
3502 : /* RETURN ARGUMENTS : _ None. */
3503 : /*-------------------------------------------------------------------*/
3504 : /* CALLED FROM : TX/RX */
3505 : /*===================================================================*/
3506 : /* NOTE: This function should be called after two consecutive calls */
3507 : /* to setEngyHarm_fx, because the norm factor outputs from */
3508 : /* the two previous calls might be different */
3509 : /*===================================================================*/
3510 0 : void rshiftHarmBand_fx( DTFS_STRUCTURE *X_fx, Word16 lband_fx, Word16 hband_fx, Word16 shift_fx )
3511 : {
3512 : Word16 k_fx, HalfLag_fx;
3513 : Word16 low_fx, high_fx;
3514 :
3515 0 : low_fx = mult( lband_fx, X_fx->lag_fx ); /* low=lband*lag, Q0 */
3516 0 : high_fx = mult( hband_fx, X_fx->lag_fx ); /* high=hband*lag, Q0 */
3517 0 : HalfLag_fx = s_min( high_fx, shl( X_fx->nH_fx, 1 ) );
3518 :
3519 0 : FOR( k_fx = low_fx + 1; k_fx <= HalfLag_fx; k_fx++ )
3520 : {
3521 0 : X_fx->a_fx[k_fx] = shl_r( X_fx->a_fx[k_fx], shift_fx );
3522 0 : move16(); /* right shift and round */
3523 : }
3524 0 : }
3525 :
3526 : /*===================================================================*/
3527 : /* FUNCTION : GetSinCosTab_fx () */
3528 : /*-------------------------------------------------------------------*/
3529 : /* PURPOSE : Compute sine & cosine table given pitch lag, */
3530 : /* by interpolating the 512-entry cosine table. */
3531 : /* sin(2pi/4L*n) & cos(2pi/4L*n) for n=0,1,... 4L-1 */
3532 : /*-------------------------------------------------------------------*/
3533 : /* INPUT ARGUMENTS : */
3534 : /* _ (Word16) L : Pitch lag, Q0 */
3535 : /*-------------------------------------------------------------------*/
3536 : /* OUTPUT ARGUMENTS : */
3537 : /* _ (Word16 []) sinTab, Q15 : sin(2pi/4L*n), n=0,1,...,4L-1 */
3538 : /* _ (Word16 []) cosTab, Q15 : cos(2pi/4L*n), n=0,1,...,4L-1 */
3539 : /*-------------------------------------------------------------------*/
3540 : /* INPUT/OUTPUT ARGUMENTS : */
3541 : /* _ None */
3542 : /*-------------------------------------------------------------------*/
3543 : /* RETURN ARGUMENTS : _ None. */
3544 : /*-------------------------------------------------------------------*/
3545 : /* CALLED FROM : TX/RX */
3546 : /*===================================================================*/
3547 : /* NOTE: This function interpolates cos_table for better accuracy */
3548 : /*===================================================================*/
3549 0 : void GetSinCosTab_fx( Word16 L, Word16 *sinTab, Word16 *cosTab )
3550 : {
3551 : Word16 i, L4;
3552 : Word16 dl, t1, w, dt;
3553 : /* Word16 t2; */
3554 : Word32 invL; /* 1/4L in Q25 */
3555 : Word32 Ltemp, Lw;
3556 : Word32 L_tmp;
3557 :
3558 0 : invL = L_shr( inverse_table[L], 4 );
3559 :
3560 0 : L4 = shl( L, 2 );
3561 0 : Lw = L_deposit_l( 0 );
3562 0 : FOR( i = 0; i < L4; i++ )
3563 : {
3564 0 : w = extract_h( Lw ); /* w in Q9 */
3565 0 : dl = extract_l( Lw ); /* dl has 16 bits left-over */
3566 0 : w = s_and( w, 511 );
3567 : /* t1=cos_table[w%512]; */
3568 0 : t1 = cos_table[w];
3569 0 : move16();
3570 : /* t2=cos_table[add(w,1)%512]; */
3571 0 : dt = cos_diff_table[w];
3572 0 : move16();
3573 : /*t2=cos_table[s_and(add(w,1),511)]; */
3574 : /*dt = sub(t2,t1); */ /* Q15 */
3575 :
3576 0 : IF( dl < 0 )
3577 : {
3578 0 : L_tmp = L_add( 65536, dl );
3579 0 : Ltemp = imult3216( L_tmp, dt );
3580 : }
3581 : ELSE
3582 : {
3583 0 : Ltemp = (Word32) L_mult0( dt, dl ); /* Ltemp in Q31 */
3584 : }
3585 0 : cosTab[i] = add( t1, (Word16) L_shr( Ltemp, 16 ) );
3586 0 : move16(); /* interpolated cos(i*2pi/4L) */
3587 :
3588 0 : Ltemp = L_add( Lw, 0x1800000 ); /* add 0.75 in Q25, which is 3pi/2 to convert sin to cos */
3589 0 : w = extract_h( Ltemp ); /* w is equivalent cos index */
3590 : /*dl = extract_l(Ltemp); */ /* dl is 16 bit left-over for interpolation */
3591 0 : w = s_and( w, 511 );
3592 :
3593 :
3594 : /* t1=cos_table[w%512]; */
3595 0 : t1 = cos_table[w];
3596 0 : move16();
3597 : /* t2=cos_table[add(w,1)%512]; */
3598 0 : dt = cos_diff_table[w];
3599 0 : move16();
3600 : /*t2=cos_table[s_and(add(w,1),511)];move16(); */
3601 : /*dt = sub(t2,t1); */ /* dt=t2-t1, Q15 */
3602 :
3603 0 : IF( dl < 0 )
3604 : {
3605 0 : L_tmp = L_add( 65536, dl );
3606 0 : Ltemp = imult3216( L_tmp, dt );
3607 : }
3608 : ELSE
3609 : {
3610 0 : Ltemp = L_mult0( dt, dl ); /* Ltemp in Q31 */
3611 : }
3612 0 : sinTab[i] = add( t1, (Word16) L_shr( Ltemp, 16 ) );
3613 0 : move16(); /* t1 is interpolated cos(w) */
3614 :
3615 0 : Lw = L_add( Lw, invL ); /* Lw=(i+1)/4L, Q25 */
3616 : }
3617 0 : }
3618 :
3619 :
3620 : /*
3621 : * The decimation-in-time complex FFT/IFFT is implemented below.
3622 : * The input complex numbers are presented as real part followed by
3623 : * imaginary part for each sample. The counters are therefore
3624 : * incremented by two to access the complex valued samples.
3625 : */
3626 0 : static void c_fft_wi_fx( Word16 *farray_ptr_fx, Word16 size, Word16 stage, Word16 isign )
3627 : {
3628 :
3629 : Word16 i, j, k, ii, jj, kk, ji, kj;
3630 : Word32 ftmp_fx, ftmp_real_fx, ftmp_imag_fx;
3631 : Word16 tmp, tmp1, tmp2, temp_sand;
3632 : Word16 n_2, K;
3633 : Word16 ii_table[8];
3634 :
3635 0 : K = 0;
3636 0 : move16();
3637 :
3638 0 : FOR( k = 256; k > 0; k -= size )
3639 : {
3640 0 : K = add( K, 1 ); /* K=512/size */
3641 : }
3642 0 : n_2 = shr( size, 1 );
3643 0 : FOR( i = 1; i <= stage; i++ )
3644 : {
3645 0 : ii_table[i - 1] = shr( size, i );
3646 : }
3647 :
3648 : /* Rearrange the input array in bit reversed order */
3649 0 : j = 0;
3650 0 : move16();
3651 0 : FOR( i = 0; i < size - 2; i = i + 2 )
3652 : {
3653 :
3654 0 : IF( GT_16( j, i ) )
3655 : {
3656 0 : ftmp_fx = *( farray_ptr_fx + i );
3657 0 : *( farray_ptr_fx + i ) = *( farray_ptr_fx + j );
3658 0 : *( farray_ptr_fx + j ) = (Word16) ftmp_fx;
3659 0 : move32();
3660 0 : move16();
3661 0 : move16();
3662 :
3663 0 : ftmp_fx = *( farray_ptr_fx + add( i, 1 ) );
3664 0 : *( farray_ptr_fx + add( i, 1 ) ) = *( farray_ptr_fx + add( j, 1 ) );
3665 0 : *( farray_ptr_fx + add( j, 1 ) ) = (Word16) ftmp_fx;
3666 0 : move32();
3667 0 : move16();
3668 0 : move16();
3669 : }
3670 :
3671 0 : k = n_2;
3672 0 : move16();
3673 0 : WHILE( ( j >= k ) )
3674 : {
3675 0 : j = sub( j, k );
3676 0 : k = shr( k, 1 );
3677 : }
3678 0 : j = add( j, k );
3679 : }
3680 :
3681 : /* The FFT part */
3682 :
3683 :
3684 0 : IF( EQ_16( isign, 1 ) )
3685 : {
3686 0 : FOR( i = 0; i < stage; i++ ) /* i is stage counter */
3687 : {
3688 0 : jj = shl( 2, i ); /* FFT size */
3689 0 : kk = shl( jj, 1 ); /* 2 * FFT size */
3690 :
3691 0 : move16();
3692 :
3693 0 : ii = ii_table[i]; /* 2 * number of FFT's */
3694 :
3695 0 : FOR( j = 0; j < jj; j = j + 2 ) /* j is sample counter */
3696 : {
3697 0 : ji = i_mult( j, ii ); /* ji is phase table index */
3698 :
3699 0 : FOR( k = j; k < size; k = k + kk ) /* k is butterfly top */
3700 : {
3701 0 : kj = add( k, jj ); /* kj is butterfly bottom */
3702 0 : temp_sand = s_and( ( add( i_mult( ji, K ), 384 ) ), 511 );
3703 : /* Butterfly computations */
3704 : /* ftmp_real_fx = L_sub(L_mult(*(farray_ptr_fx + kj), cos_table[ji*K]), */
3705 : /* L_mult(*(farray_ptr_fx + kj + 1), cos_table[(ji*K+384)%512])); */
3706 0 : ftmp_real_fx = L_msu( L_mult( *( farray_ptr_fx + kj ), cos_table[( ji * K )] ),
3707 0 : *( farray_ptr_fx + add( kj, 1 ) ), cos_table[temp_sand] );
3708 :
3709 : /* ftmp_imag_fx = L_add(L_mult(*(farray_ptr_fx + kj + 1), cos_table[ji*K]), */
3710 : /* L_mult(*(farray_ptr_fx + kj), cos_table[(ji*K+384)%512])); */
3711 0 : ftmp_imag_fx = L_mac( L_mult( *( farray_ptr_fx + add( kj, 1 ) ), cos_table[( ji * K )] ),
3712 0 : *( farray_ptr_fx + kj ), cos_table[temp_sand] );
3713 :
3714 0 : tmp1 = round_fx( ftmp_real_fx );
3715 0 : tmp2 = round_fx( ftmp_imag_fx );
3716 :
3717 0 : tmp = sub( *( farray_ptr_fx + k ), tmp1 );
3718 0 : *( farray_ptr_fx + kj ) = shr( tmp, 1 );
3719 0 : move16();
3720 :
3721 0 : tmp = sub( *( farray_ptr_fx + add( k, 1 ) ), tmp2 );
3722 0 : *( farray_ptr_fx + add( kj, 1 ) ) = shr( tmp, 1 );
3723 0 : move16();
3724 :
3725 0 : tmp = add( *( farray_ptr_fx + k ), tmp1 );
3726 0 : *( farray_ptr_fx + k ) = shr( tmp, 1 );
3727 0 : move16();
3728 :
3729 0 : tmp = add( *( farray_ptr_fx + add( k, 1 ) ), tmp2 );
3730 0 : *( farray_ptr_fx + add( k, 1 ) ) = shr( tmp, 1 );
3731 0 : move16();
3732 : }
3733 : }
3734 : }
3735 :
3736 : /* The IFFT part */
3737 : }
3738 : ELSE
3739 : {
3740 0 : FOR( i = 0; i < stage; i++ ) /* i is stage counter */
3741 : {
3742 0 : jj = shl( 2, i ); /* FFT size */
3743 0 : kk = shl( jj, 1 ); /* 2 * FFT size */
3744 0 : ii = ii_table[i]; /* 2 * number of FFT's */
3745 :
3746 0 : FOR( j = 0; j < jj; j = j + 2 ) /* j is sample counter */
3747 : {
3748 0 : ji = i_mult( j, ii ); /* ji is phase table index */
3749 :
3750 0 : FOR( k = j; k < size; k = k + kk ) /* k is butterfly top */
3751 : {
3752 0 : kj = add( k, jj ); /* kj is butterfly bottom */
3753 0 : temp_sand = s_and( ( add( i_mult( ji, K ), 384 ) ), 511 );
3754 : /* Butterfly computations */
3755 : /* ftmp_real_fx = L_add(L_mult(*(farray_ptr_fx + kj), cos_table[ji*K]), */
3756 : /* L_mult(*(farray_ptr_fx + kj + 1), cos_table[(ji*K+384)%512])); */
3757 0 : ftmp_real_fx = L_mac( L_mult( *( farray_ptr_fx + kj ), cos_table[( ji * K )] ),
3758 0 : *( farray_ptr_fx + kj + 1 ), cos_table[temp_sand] );
3759 :
3760 : /* ftmp_imag_fx = L_sub(L_mult(*(farray_ptr_fx + kj + 1), cos_table[ji*K]), */
3761 : /* L_mult(*(farray_ptr_fx + kj), cos_table[(ji*K+384)%512])); */
3762 0 : ftmp_imag_fx = L_msu( L_mult( *( farray_ptr_fx + add( kj, 1 ) ), cos_table[( ji * K )] ),
3763 0 : *( farray_ptr_fx + kj ), cos_table[temp_sand] );
3764 :
3765 0 : tmp1 = round_fx( ftmp_real_fx );
3766 0 : tmp2 = round_fx( ftmp_imag_fx );
3767 :
3768 0 : *( farray_ptr_fx + kj ) = sub_sat( *( farray_ptr_fx + k ), tmp1 );
3769 0 : move16();
3770 0 : *( farray_ptr_fx + kj + 1 ) = sub_sat( *( farray_ptr_fx + k + 1 ), tmp2 );
3771 0 : move16();
3772 0 : *( farray_ptr_fx + k ) = add_sat( *( farray_ptr_fx + k ), tmp1 );
3773 0 : move16();
3774 0 : *( farray_ptr_fx + k + 1 ) = add_sat( *( farray_ptr_fx + k + 1 ), tmp2 );
3775 0 : move16();
3776 : }
3777 : }
3778 : }
3779 : }
3780 :
3781 0 : } /* end of c_fft () */
3782 :
3783 :
3784 0 : void r_fft_4_fx( Word16 *farray_ptr_fx /*Q0*/, Word16 size, Word16 stage, Word16 isign )
3785 : {
3786 :
3787 : Word16 ftmp1_real_fx, ftmp1_imag_fx, ftmp2_real_fx, ftmp2_imag_fx;
3788 : Word32 Lftmp1_real_fx, Lftmp1_imag_fx;
3789 : Word16 i, j, temp_sand;
3790 : Word32 Ltmp1_fx, Ltmp2_fx;
3791 : Word16 n_2, k, K;
3792 :
3793 0 : n_2 = shr( size, 1 );
3794 0 : K = 0;
3795 0 : move16();
3796 :
3797 0 : FOR( k = 256; k > 0; k -= size )
3798 : {
3799 0 : K = add( K, 1 ); /* K=512/size */
3800 : }
3801 :
3802 : /* The FFT part */
3803 0 : IF( EQ_16( isign, 1 ) )
3804 : {
3805 : /* Perform the complex FFT */
3806 0 : c_fft_wi_fx( farray_ptr_fx, size, stage, isign );
3807 :
3808 : /* First, handle the DC and foldover frequencies */
3809 0 : ftmp1_real_fx = *farray_ptr_fx;
3810 0 : ftmp2_real_fx = *( farray_ptr_fx + 1 );
3811 0 : *farray_ptr_fx = add( ftmp1_real_fx, ftmp2_real_fx );
3812 0 : move16();
3813 0 : *( farray_ptr_fx + 1 ) = sub( ftmp1_real_fx, ftmp2_real_fx );
3814 0 : move16();
3815 :
3816 : /* Now, handle the remaining positive frequencies */
3817 0 : j = sub( size, 2 );
3818 0 : FOR( i = 2; i <= n_2; i = i + 2 )
3819 : {
3820 0 : ftmp1_real_fx = add( *( farray_ptr_fx + i ), *( farray_ptr_fx + j ) );
3821 0 : ftmp1_imag_fx = sub( *( farray_ptr_fx + i + 1 ), *( farray_ptr_fx + j + 1 ) );
3822 0 : ftmp2_real_fx = add( *( farray_ptr_fx + i + 1 ), *( farray_ptr_fx + j + 1 ) );
3823 0 : ftmp2_imag_fx = sub( *( farray_ptr_fx + j ), *( farray_ptr_fx + i ) );
3824 :
3825 0 : Lftmp1_real_fx = L_deposit_h( ftmp1_real_fx );
3826 0 : Lftmp1_imag_fx = L_deposit_h( ftmp1_imag_fx );
3827 0 : temp_sand = s_and( add( i_mult( i, K ), 384 ), 511 );
3828 : /* Ltmp1_fx = L_sub(L_mult(ftmp2_real_fx, cos_table[i*K]), L_mult(ftmp2_imag_fx, cos_table[(i*K+384)%512])); */
3829 0 : Ltmp1_fx = L_msu( L_mult( ftmp2_real_fx, cos_table[( i * K )] ), ftmp2_imag_fx, cos_table[temp_sand] );
3830 0 : *( farray_ptr_fx + i ) = round_fx( L_shr( L_add( Lftmp1_real_fx, Ltmp1_fx ), 1 ) );
3831 0 : move16();
3832 :
3833 : /* Ltmp1_fx = L_add(L_mult(ftmp2_imag_fx, cos_table[i*K]), L_mult(ftmp2_real_fx, cos_table[(i*K+384)%512])); */
3834 0 : Ltmp1_fx = L_mac( L_mult( ftmp2_imag_fx, cos_table[( i * K )] ), ftmp2_real_fx, cos_table[temp_sand] );
3835 0 : *( farray_ptr_fx + i + 1 ) = round_fx( L_shr( L_add( Lftmp1_imag_fx, Ltmp1_fx ), 1 ) );
3836 0 : move16();
3837 :
3838 : /* Ltmp1_fx = L_add(L_mult(ftmp2_real_fx, cos_table[j*K]), L_mult(ftmp2_imag_fx, cos_table[(j*K+384)%512])); */
3839 0 : Ltmp1_fx = L_mac( L_mult( ftmp2_real_fx, cos_table[( j * K )] ), ftmp2_imag_fx, cos_table[temp_sand] );
3840 0 : *( farray_ptr_fx + j ) = round_fx( L_shr( L_add( Lftmp1_real_fx, Ltmp1_fx ), 1 ) );
3841 0 : move16();
3842 :
3843 : /* Ltmp1_fx = L_add(L_negate(L_mult(ftmp2_imag_fx, cos_table[j*K])), L_mult(ftmp2_real_fx, cos_table[(j*K+384)%512])); */
3844 0 : Ltmp1_fx = L_msu( L_mult( ftmp2_real_fx, cos_table[temp_sand] ), ftmp2_imag_fx, cos_table[( j * K )] );
3845 0 : Ltmp2_fx = L_sub( Ltmp1_fx, Lftmp1_imag_fx );
3846 0 : *( farray_ptr_fx + j + 1 ) = round_fx( L_shr( Ltmp2_fx, 1 ) );
3847 0 : move16();
3848 0 : j = sub( size, i );
3849 : }
3850 : }
3851 : ELSE
3852 : {
3853 :
3854 : /* First, handle the DC and foldover frequencies */
3855 :
3856 0 : move16();
3857 0 : move16();
3858 :
3859 0 : ftmp1_real_fx = *farray_ptr_fx;
3860 0 : ftmp2_real_fx = *( farray_ptr_fx + 1 );
3861 0 : *farray_ptr_fx = shr( add( ftmp1_real_fx, ftmp2_real_fx ), 1 );
3862 0 : move16();
3863 0 : *( farray_ptr_fx + 1 ) = shr( sub( ftmp1_real_fx, ftmp2_real_fx ), 1 );
3864 0 : move16();
3865 :
3866 : /* Now, handle the remaining positive frequencies */
3867 0 : FOR( i = 2; i <= n_2; i += 2 )
3868 : {
3869 0 : j = sub( size, i );
3870 :
3871 0 : ftmp1_real_fx = add( *( farray_ptr_fx + i ), *( farray_ptr_fx + j ) );
3872 0 : ftmp1_imag_fx = sub( *( farray_ptr_fx + i + 1 ), *( farray_ptr_fx + j + 1 ) );
3873 0 : ftmp2_real_fx = negate( add( *( farray_ptr_fx + j + 1 ), *( farray_ptr_fx + i + 1 ) ) );
3874 0 : ftmp2_imag_fx = negate( sub( *( farray_ptr_fx + j ), *( farray_ptr_fx + i ) ) );
3875 :
3876 0 : Lftmp1_real_fx = L_deposit_h( ftmp1_real_fx );
3877 0 : Lftmp1_imag_fx = L_deposit_h( ftmp1_imag_fx );
3878 0 : temp_sand = s_and( ( add( i_mult( i, K ), 384 ) ), 511 );
3879 : /* Ltmp1_fx = L_add(L_mult(ftmp2_real_fx, cos_table[i*K]), L_mult(ftmp2_imag_fx, cos_table[(i*K+384)%512])); */
3880 0 : Ltmp1_fx = L_mac( L_mult( ftmp2_real_fx, cos_table[( i * K )] ), ftmp2_imag_fx, cos_table[temp_sand] );
3881 0 : *( farray_ptr_fx + i ) = round_fx( L_shr( L_add( Lftmp1_real_fx, Ltmp1_fx ), 1 ) );
3882 0 : move16();
3883 :
3884 : /* Ltmp1_fx = L_sub(L_mult(ftmp2_imag_fx, cos_table[i*K]), L_mult(ftmp2_real_fx, cos_table[(i*K+384)%512])); */
3885 0 : Ltmp1_fx = L_msu( L_mult( ftmp2_imag_fx, cos_table[( i * K )] ), ftmp2_real_fx, cos_table[temp_sand] );
3886 0 : *( farray_ptr_fx + i + 1 ) = round_fx( L_shr( L_add( Lftmp1_imag_fx, Ltmp1_fx ), 1 ) );
3887 0 : move16();
3888 :
3889 : /* Ltmp1_fx = L_sub(L_mult(ftmp2_real_fx, cos_table[j*K]), L_mult(ftmp2_imag_fx, cos_table[(j*K+384)%512])); */
3890 0 : Ltmp1_fx = L_msu( L_mult( ftmp2_real_fx, cos_table[( j * K )] ), ftmp2_imag_fx, cos_table[temp_sand] );
3891 0 : *( farray_ptr_fx + j ) = round_fx( L_shr( L_add( Lftmp1_real_fx, Ltmp1_fx ), 1 ) );
3892 0 : move16();
3893 :
3894 : /* Ltmp1_fx = L_negate(L_add(L_mult(ftmp2_imag_fx, cos_table[j*K]), L_mult(ftmp2_real_fx, cos_table[(j*K+384)%512]))); */
3895 0 : Ltmp1_fx = L_negate( L_mac( L_mult( ftmp2_imag_fx, cos_table[( j * K )] ), ftmp2_real_fx, cos_table[temp_sand] ) );
3896 0 : Ltmp2_fx = L_sub( Ltmp1_fx, Lftmp1_imag_fx );
3897 0 : *( farray_ptr_fx + j + 1 ) = round_fx( L_shr( Ltmp2_fx, 1 ) );
3898 0 : move16();
3899 : }
3900 :
3901 : /* Perform the complex IFFT */
3902 0 : c_fft_wi_fx( farray_ptr_fx, size, stage, isign );
3903 : }
3904 0 : }
3905 :
3906 :
3907 : /*===================================================================*/
3908 : /* FUNCTION : struct DTFS_fx::copy_phase_fx () */
3909 : /*-------------------------------------------------------------------*/
3910 : /* PURPOSE : Retain the amplitudes of a prototype X2, but copy*/
3911 : /* the phases of another prototype X1 of same length*/
3912 : /* over to make a new prototype */
3913 : /*-------------------------------------------------------------------*/
3914 : /* INPUT ARGUMENTS : */
3915 : /* _ (struct DTFS_fx) X1 : prototype in Cartesian domain */
3916 : /* (Word16) X1.lag: length of prototype in time domain*/
3917 : /* (Word16 []) X1.a,b: re/im of harmonics, normalized */
3918 : /* (Word16) X1.Q: norm factor of X2.a/b */
3919 : /* _ (struct DTFS_fx) X2 : prototype in polar domain */
3920 : /* (Word16) X2.lag: should be same as X1.lag */
3921 : /* (Word16 []) X2.a:amplitude of harmonics, normalized*/
3922 : /* (Word16 []) X2.b: phase of harmonics, don't care */
3923 : /* (Word16) X2.Q: norm factor of X2.a */
3924 : /*-------------------------------------------------------------------*/
3925 : /* OUTPUT ARGUMENTS : */
3926 : /* _ None */
3927 : /*-------------------------------------------------------------------*/
3928 : /* INPUT/OUTPUT ARGUMENTS : */
3929 : /* _ None */
3930 : /*-------------------------------------------------------------------*/
3931 : /* RETURN ARGUMENTS : */
3932 : /* _ (struct DTFS_fx) X : prototype in Cartesian domain */
3933 : /* The amplitudes of this prototype are from X2 */
3934 : /* and the phases are from X1 */
3935 : /*-------------------------------------------------------------------*/
3936 : /* CALLED FROM : TX/RX */
3937 : /*===================================================================*/
3938 : /* X.a[k]=2*X2.a[k]/sqrt(X1.a[k]^2+X1.b[k]^2)*X1.a[k] */
3939 : /* X.b[k]=2*X2.a[k]/sqrt(X1.a[k]^2+X1.b[k]^2)*X1.b[k] */
3940 : /*===================================================================*/
3941 0 : void copy_phase_fx( DTFS_STRUCTURE *X1_fx, DTFS_STRUCTURE X2_fx, DTFS_STRUCTURE *retX_fx )
3942 : {
3943 : /* DTFS_fx X; */
3944 : Word16 k, q, sn, cn, HalfLag;
3945 : Word16 d1h, d1l;
3946 : Word32 Ltemp_fx, L_tmp;
3947 : Word32 Lacc_fx;
3948 : Word16 exp, tmp, exp1;
3949 :
3950 0 : retX_fx->lag_fx = X1_fx->lag_fx;
3951 0 : move16();
3952 0 : retX_fx->Q = sub( X2_fx.Q, 1 ); /* equivalent to 2x MIN_FX(shr(sub(X_fx->lag_fx,1),1),X_fx->nH_fx) */
3953 0 : move16();
3954 0 : HalfLag = s_min( shr( X1_fx->lag_fx, 1 ), X1_fx->nH_fx );
3955 :
3956 0 : FOR( k = 1; k <= HalfLag; k++ )
3957 : {
3958 0 : Lacc_fx = L_mult_sat( X1_fx->a_fx[k], X1_fx->a_fx[k] );
3959 0 : Lacc_fx = L_mac_sat( Lacc_fx, X1_fx->b_fx[k], X1_fx->b_fx[k] ); /* 2*Q+1 */
3960 :
3961 0 : exp = norm_l( Lacc_fx );
3962 0 : tmp = extract_h( L_shl( Lacc_fx, exp ) );
3963 0 : exp1 = sub( sub( 30, exp ), 2 * X1_fx->Q + 1 );
3964 :
3965 0 : IF( tmp )
3966 : {
3967 0 : tmp = div_s( 16384, tmp ); /* 15+exp1 */
3968 : }
3969 : ELSE
3970 : {
3971 0 : tmp = 0;
3972 0 : move16();
3973 : }
3974 0 : tmp = shr( tmp, 1 );
3975 0 : q = sub( add( add( 15, exp1 ), 16 ), 1 );
3976 :
3977 0 : IF( tmp )
3978 : {
3979 0 : exp = norm_s( tmp );
3980 0 : tmp = shl( tmp, exp );
3981 0 : tmp = div_s( 16384, tmp );
3982 0 : L_tmp = L_deposit_h( tmp );
3983 0 : Ltemp_fx = Isqrt_lc( L_tmp, &exp ); /* Q(31-exp) */
3984 : }
3985 : ELSE
3986 : {
3987 0 : Ltemp_fx = 0;
3988 0 : move32();
3989 : }
3990 :
3991 0 : IF( s_and( q, 1 ) )
3992 : {
3993 0 : Ltemp_fx = Mult_32_16( Ltemp_fx, 23170 ); /* 23170 is 1/sqrt(2) in Q15 */
3994 : }
3995 :
3996 0 : q = shr( q, 1 ); /* Ltemp_fx in Q(q+16) */
3997 :
3998 0 : d1h = extract_h( Ltemp_fx );
3999 0 : d1l = extract_l( Ltemp_fx );
4000 0 : Ltemp_fx = L_mult0( X1_fx->b_fx[k], d1l );
4001 0 : Ltemp_fx = L_add_sat( L_shr( Ltemp_fx, 15 ), L_mult_sat( X1_fx->b_fx[k], d1h ) ); /* sin(w) in Q(q+16+Q-15) */
4002 0 : sn = round_fx_sat( L_shl_sat( Ltemp_fx, sub( 30, add( q, X1_fx->Q ) ) ) ); /* Q15 */
4003 0 : retX_fx->b_fx[k] = mult_r_sat( X2_fx.a_fx[k], sn ); /* X2_fx.Q */
4004 0 : move16();
4005 :
4006 0 : Ltemp_fx = L_mult0( X1_fx->a_fx[k], d1l );
4007 0 : Ltemp_fx = L_add_sat( L_shr( Ltemp_fx, 15 ), L_mult_sat( X1_fx->a_fx[k], d1h ) ); /* cos(w) in Q(q+Q+1) */
4008 0 : cn = round_fx_sat( L_shl_sat( Ltemp_fx, sub( 30, add( q, X1_fx->Q ) ) ) ); /* Q15 */
4009 0 : retX_fx->a_fx[k] = mult_r_sat( X2_fx.a_fx[k], cn ); /* X2_fx.Q */
4010 0 : move16();
4011 : }
4012 0 : k = sub( k, 1 );
4013 :
4014 :
4015 0 : IF( s_and( X1_fx->lag_fx, 1 ) == 0 )
4016 : {
4017 0 : retX_fx->a_fx[k] = shr( retX_fx->a_fx[k], 1 );
4018 0 : move16();
4019 0 : retX_fx->b_fx[k] = shr( retX_fx->b_fx[k], 1 );
4020 0 : move16();
4021 : }
4022 0 : }
4023 :
4024 :
4025 : /*===================================================================*/
4026 : /* FUNCTION : getSpEngyFromResAmp_fx () */
4027 : /*-------------------------------------------------------------------*/
4028 : /* PURPOSE : Get band energy */
4029 : /*-------------------------------------------------------------------*/
4030 : /* INPUT ARGUMENTS : */
4031 : /* _ (struct DTFS_STRUCTURE) X_fx : prototype in polar domain */
4032 : /* (Word16) lag: length of prototype in time domain */
4033 : /* (Word16 []) a: amplitude of harmonics, normalized */
4034 : /* (Word16) Q: norm factor of a */
4035 : /* _ (Word16 []) curr_lpc: LPC coefficients, Q12 */
4036 : /* _ (Word16) lband: lower frequency bound, Q15 */
4037 : /* _ (Word16) hband: upper frequency bound, Q15 */
4038 : /* _ (Word16 []) sin_tab: sine table based on lag, Q15 */
4039 : /* _ (Word16 []) cos_tab: cosine table based on lag, Q15 */
4040 : /*-------------------------------------------------------------------*/
4041 : /* OUTPUT ARGUMENTS : */
4042 : /* _ None */
4043 : /*-------------------------------------------------------------------*/
4044 : /* INPUT/OUTPUT ARGUMENTS : */
4045 : /* _ None */
4046 : /*-------------------------------------------------------------------*/
4047 : /* RETURN ARGUMENTS : */
4048 : /* _ (Word32) en: energy of the specified frequency band, */
4049 : /* Q factor is 2Q-13 */
4050 : /*-------------------------------------------------------------------*/
4051 : /* CALLED FROM : TX */
4052 : /*===================================================================*/
4053 0 : Word32 getSpEngyFromResAmp_fx( DTFS_STRUCTURE *X_fx, Word16 lband, Word16 hband, const Word16 *curr_lpc, Word16 *sin_tab, Word16 *cos_tab )
4054 : {
4055 : Word16 i, k, k4, n, M_fx, HalfLag;
4056 : Word16 fdiff, freq;
4057 : Word32 Ltemp;
4058 : Word32 Lacc;
4059 : Word32 Re, Im; /* Q27 */
4060 : Word32 en;
4061 : Word16 exp, tmp, expa, fraca, expb, fracb, scale;
4062 : Word32 L_tmp;
4063 0 : en = L_deposit_l( 0 );
4064 :
4065 0 : if ( EQ_16( hband, X_fx->upper_cut_off_freq_fx ) )
4066 : {
4067 0 : hband = 0x2803;
4068 0 : move16(); /* 4001.0/12800 in Q15 */
4069 : }
4070 0 : M_fx = shl( X_fx->lag_fx, 2 ); /* M_fx=4*lag */
4071 :
4072 : /* Ltemp=invert_dp(X_fx->lag_fx, 4, &n,1); : Ltemp=1/lag, Q(61-n) */
4073 : /* fdiff=round_fx(L_shl(Ltemp,sub(n,26))); : fdiff=1/lag, Q19 */
4074 :
4075 0 : exp = norm_s( X_fx->lag_fx );
4076 0 : tmp = div_s( shl( 1, sub( 14, exp ) ), X_fx->lag_fx );
4077 0 : L_tmp = L_shl( tmp, add( exp, 6 ) );
4078 0 : fdiff = round_fx( L_tmp );
4079 :
4080 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_4kHz_fx );
4081 0 : FOR( k = 0; k <= HalfLag; k++ )
4082 : {
4083 0 : Ltemp = L_mult( fdiff, k ); /* Ltemp=k*fdiff, Q20 */
4084 0 : freq = extract_h( L_shl( Ltemp, 11 ) ); /* k*fdiff in Q15 */
4085 :
4086 0 : test();
4087 0 : IF( LT_16( freq, hband ) && GE_16( freq, lband ) )
4088 : {
4089 0 : Lacc = L_add( 0x10000000, 0 ); /* Re=1.0, Q28 */
4090 0 : k4 = shl( k, 2 ); /* k4=4*k */
4091 :
4092 0 : n = k4;
4093 0 : move16();
4094 0 : FOR( i = 0; i < M + 1; i++ )
4095 : {
4096 : /* Compute Re */
4097 0 : Lacc = L_mac_sat( Lacc, curr_lpc[i], cos_tab[n % M_fx] ); /* Q28 */
4098 0 : n = add( n, k4 ); /* n=4*i*k */
4099 : }
4100 0 : Re = L_shr( Lacc, 1 ); /* Q27 */
4101 :
4102 0 : n = k4;
4103 0 : move16();
4104 0 : Lacc = L_deposit_l( 0 );
4105 0 : FOR( i = 0; i < M + 1; i++ )
4106 : {
4107 : /* Compute Im */
4108 0 : Lacc = L_msu_sat( Lacc, curr_lpc[i], sin_tab[n % M_fx] ); /* Q28 */
4109 0 : n = add( n, k4 ); /* n=4*i*k */
4110 : }
4111 0 : Im = L_shr( Lacc, 1 ); /* Q27 */
4112 : /* Lacc=L_add(L_mult_ll(Re,Re),(Word32)L_mult_ll(Im,Im)); : Lacc=Re^2+Im^2 in Q23 */
4113 0 : Lacc = L_add_sat( Mult_32_32( Re, Re ), Mult_32_32( Im, Im ) ); /* Lacc=Re^2+Im^2 in Q23 */
4114 0 : Ltemp = L_mult0( X_fx->a_fx[k], X_fx->a_fx[k] ); /* 2*a[k]^2 in 2Q */
4115 : /* Ltemp=(Word32)L_sat32_40(divide_dp(Ltemp,Lacc,-19,1)); : Ltemp in Q(2Q-13) */
4116 :
4117 0 : if ( Lacc < 0 )
4118 : {
4119 0 : Lacc = L_negate( Lacc );
4120 : }
4121 :
4122 0 : IF( Lacc )
4123 : {
4124 :
4125 0 : expa = norm_l( Lacc );
4126 0 : fraca = extract_h( L_shl( Lacc, expa ) );
4127 0 : expa = sub( 30, add( expa, 23 ) );
4128 :
4129 :
4130 0 : expb = norm_l( Ltemp );
4131 0 : fracb = round_fx( L_shl( Ltemp, expb ) );
4132 0 : expb = sub( 30, add( expb, shl( X_fx->Q, 1 ) ) );
4133 :
4134 0 : scale = shr( sub( fraca, fracb ), 15 );
4135 0 : fracb = shl( fracb, scale );
4136 0 : expb = sub( expb, scale );
4137 :
4138 0 : tmp = div_s( fracb, fraca ); /* 15-exp */
4139 0 : exp = sub( expb, expa );
4140 0 : Ltemp = L_shl_sat( tmp, sub( add( shl( X_fx->Q, 1 ), exp ), 27 ) );
4141 : }
4142 : ELSE
4143 : {
4144 0 : Ltemp = L_deposit_l( 0 );
4145 : }
4146 :
4147 0 : test();
4148 0 : IF( X_fx->lag_fx % 2 == 0 && EQ_16( k, shr( X_fx->lag_fx, 1 ) ) )
4149 0 : en = L_add( en, L_shr( Ltemp, 1 ) );
4150 : ELSE
4151 0 : en = L_add( en, Ltemp ); /* en in 2Q-13 */
4152 : }
4153 : }
4154 0 : return ( en ); /* en in 2Q-13 */
4155 : }
4156 : /*===================================================================*/
4157 : /* FUNCTION : DTFS_poleFilter_fx() */
4158 : /*-------------------------------------------------------------------*/
4159 : /* PURPOSE : pole filtering */
4160 : /*-------------------------------------------------------------------*/
4161 : /* INPUT ARGUMENTS : */
4162 : /* _ (Word16) lpc[] : lpc coefficients in Q12 */
4163 : /* _ (Word16) N : lpc order */
4164 : /* _ (Word16) X_fx->lag_fx: in Q0 */
4165 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
4166 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
4167 : /*-------------------------------------------------------------------*/
4168 : /* OUTPUT ARGUMENTS : */
4169 : /* _ None */
4170 : /*-------------------------------------------------------------------*/
4171 : /* INPUT/OUTPUT ARGUMENTS : */
4172 : /* _ (Word16) X_fx->a_fx[] : in Q(X_fx->Q) */
4173 : /* _ (Word16) X_fx->b_fx[] : in Q(X_fx->Q) */
4174 : /* _ (Word16) X_fx->Q: in Q0 */
4175 : /*-------------------------------------------------------------------*/
4176 : /* RETURN ARGUMENTS : _ None. */
4177 : /*-------------------------------------------------------------------*/
4178 : /* CALLED FROM : TX/RX */
4179 : /*===================================================================*/
4180 0 : void DTFS_poleFilter_fx( DTFS_STRUCTURE *X_fx, Word16 *LPC, Word16 N, Word16 *S_fx, Word16 *C_fx )
4181 : {
4182 : Word16 temp, temp1, temp2, HalfLag;
4183 : Word32 sum1_fx, sum2_fx;
4184 : Word32 sum, L_temp1, L_temp2;
4185 : Word16 k, n, na, nb;
4186 : Word16 Qmin, Qab[MAXLAG_WI];
4187 : Word16 exp, tmp;
4188 0 : Qmin = 32767;
4189 0 : move16();
4190 0 : HalfLag = s_min( shr( X_fx->lag_fx, 1 ), X_fx->nH_fx );
4191 0 : FOR( k = 0; k <= HalfLag; k++ )
4192 : {
4193 0 : temp2 = k;
4194 0 : move16();
4195 : /* Calculate sum1 and sum2 */
4196 0 : sum1_fx = L_deposit_h( 4096 ); /* 1: Q(12+15+1) */
4197 :
4198 0 : sum2_fx = L_deposit_l( 0 );
4199 0 : FOR( n = 0; n < N; n++ )
4200 : {
4201 0 : sum1_fx = L_mac_sat( sum1_fx, LPC[n], C_fx[shl( temp2, 2 ) % shl( X_fx->lag_fx, 2 )] ); /* Q(12+15+1) */
4202 0 : sum2_fx = L_mac_sat( sum2_fx, LPC[n], S_fx[shl( temp2, 2 ) % shl( X_fx->lag_fx, 2 )] ); /* Q(12+15+1) */
4203 0 : temp2 = add( temp2, k );
4204 : }
4205 :
4206 0 : temp1 = round_fx_sat( sum1_fx ); /* Q(12+15+1-16)=Q(12) */
4207 0 : temp2 = round_fx_sat( sum2_fx ); /* Q(12) */
4208 : /* Calculate the circular convolution */
4209 0 : sum = L_mac_sat( L_mult_sat( temp1, temp1 ), temp2, temp2 ); /* Q(12+12+1)=Q(25) */
4210 :
4211 0 : L_temp1 = L_mult( temp1, X_fx->a_fx[k] );
4212 0 : L_temp1 = L_mac_sat( L_temp1, temp2, X_fx->b_fx[k] ); /* Q(12+Q+1)=Q(13+Q) */
4213 0 : L_temp2 = L_mult( temp1, X_fx->b_fx[k] );
4214 0 : L_temp2 = L_msu_sat( L_temp2, temp2, X_fx->a_fx[k] ); /* Q(12+Q+1)=Q(13+Q) */
4215 0 : IF( sum )
4216 : {
4217 0 : exp = norm_l( sum );
4218 0 : temp1 = exp;
4219 0 : tmp = extract_h( L_shl( sum, exp ) );
4220 0 : exp = sub( sub( 30, exp ), 25 );
4221 0 : tmp = div_s( 16384, tmp ); /* Q(15+exp) */
4222 0 : sum = L_shl( tmp, 16 );
4223 0 : temp = round_fx( sum );
4224 : }
4225 : ELSE
4226 : {
4227 0 : sum = 0;
4228 0 : move16();
4229 0 : temp = 0;
4230 0 : move16();
4231 0 : temp1 = 0;
4232 0 : move16();
4233 : }
4234 0 : sum1_fx = Mult_32_16( L_temp1, temp ); /* Q(13+Q+20-temp1-15)=Q(Q-temp1+18) */
4235 0 : sum2_fx = Mult_32_16( L_temp2, temp ); /* Q(Q-temp1+18) */
4236 :
4237 : /* normalization */
4238 0 : na = norm_l( sum1_fx );
4239 0 : if ( sum1_fx == 0 )
4240 : {
4241 0 : na = 31;
4242 0 : move16();
4243 : }
4244 0 : nb = norm_l( sum2_fx );
4245 0 : if ( sum2_fx == 0 )
4246 : {
4247 0 : nb = 31;
4248 0 : move16();
4249 : }
4250 :
4251 0 : if ( LT_16( na, nb ) )
4252 : {
4253 0 : nb = na;
4254 0 : move16();
4255 : }
4256 0 : nb = sub( nb, 1 ); /* leave one more sign bit */
4257 0 : X_fx->a_fx[k] = round_fx( L_shl( sum1_fx, nb ) ); /* Q(Q-temp1+22+nb-16)=Q(Q-temp1+nb+2) */
4258 0 : move16();
4259 0 : X_fx->b_fx[k] = round_fx( L_shl( sum2_fx, nb ) ); /* Q(Q-temp1+nb+2) */
4260 0 : move16();
4261 :
4262 0 : Qab[k] = add( sub( add( nb, 2 ), temp1 ), X_fx->Q );
4263 :
4264 0 : if ( LT_16( Qab[k], Qmin ) )
4265 : {
4266 0 : Qmin = Qab[k];
4267 0 : move16();
4268 : }
4269 : }
4270 : /* bring to the same Q */
4271 0 : FOR( k = 0; k <= HalfLag; k++ )
4272 : {
4273 0 : X_fx->a_fx[k] = shl( X_fx->a_fx[k], sub( Qmin, Qab[k] ) );
4274 0 : move16(); /* Q(Q+Qab[k]+Qmin-Qab[k])=Q(Q+Qmin) */
4275 0 : X_fx->b_fx[k] = shl( X_fx->b_fx[k], sub( Qmin, Qab[k] ) );
4276 0 : move16(); /* Q(Q+Qab[k]+Qmin-Qab[k])=Q(Q+Qmin) */
4277 : }
4278 :
4279 0 : X_fx->Q = Qmin;
4280 0 : move16();
4281 0 : }
4282 : /*===================================================================*/
4283 : /* FUNCTION : poleFilter_setup_fx() */
4284 : /*-------------------------------------------------------------------*/
4285 : /* PURPOSE : Sets up pole filtering LPC dependent intermediate*/
4286 : /* values to be used by poleFilter */
4287 : /*-------------------------------------------------------------------*/
4288 : /* INPUT ARGUMENTS : */
4289 : /* _ (Word16) lpc[] : lpc coefficients in Q12 */
4290 : /* _ (Word16) N : lpc order */
4291 : /* _ (Word16) lag : in Q0 */
4292 : /* _ (Word16 *) S_fx: sin(2pi*n/(4*lag)) table, Q15 */
4293 : /* _ (Word16 *) C_fx: cos(2pi*n/(4*lag)) table, Q15 */
4294 : /*-------------------------------------------------------------------*/
4295 : /* OUTPUT ARGUMENTS : */
4296 : /* _ None */
4297 : /*-------------------------------------------------------------------*/
4298 : /* INPUT/OUTPUT ARGUMENTS : */
4299 : /* _ None */
4300 : /*-------------------------------------------------------------------*/
4301 : /* RETURN ARGUMENTS : _ None. */
4302 : /*-------------------------------------------------------------------*/
4303 : /* CALLED FROM : TX/RX */
4304 : /*===================================================================*/
4305 0 : void poleFilter_setup_fx( const Word16 *LPC, Word16 N, DTFS_STRUCTURE X_fx, Word16 *S_fx, Word16 *C_fx, Word16 *pf_temp1, Word16 *pf_temp2, Word16 *pf_temp, Word16 *pf_n2_temp1 )
4306 : {
4307 : Word16 temp1, temp2, HalfLag;
4308 : Word32 sum1_fx, sum2_fx;
4309 : Word32 sum;
4310 : Word16 k, n, n1, n2;
4311 : Word16 exp, tmp;
4312 0 : HalfLag = s_min( shr( X_fx.lag_fx, 1 ), X_fx.nH_fx );
4313 :
4314 0 : FOR( k = 0; k <= HalfLag; k++ )
4315 : {
4316 0 : temp2 = k;
4317 0 : move16();
4318 : /* Calculate sum1 and sum2 */
4319 0 : sum1_fx = L_deposit_h( 4096 ); /* 1: Q(12+15+1) */
4320 :
4321 0 : sum2_fx = L_deposit_l( 0 );
4322 0 : FOR( n = 0; n < N; n++ )
4323 : {
4324 0 : sum1_fx = L_mac_sat( sum1_fx, LPC[n], C_fx[( 4 * temp2 ) % ( 4 * X_fx.lag_fx )] ); /* Q(12+15+1) */
4325 0 : sum2_fx = L_mac_sat( sum2_fx, LPC[n], S_fx[( 4 * temp2 ) % ( 4 * X_fx.lag_fx )] ); /* Q(12+15+1) */
4326 0 : temp2 = add( temp2, k );
4327 : }
4328 :
4329 0 : n1 = norm_l( sum1_fx );
4330 0 : if ( sum1_fx == 0 )
4331 : {
4332 0 : n1 = 31;
4333 0 : move16();
4334 : }
4335 :
4336 0 : n2 = norm_l( sum2_fx );
4337 0 : if ( sum2_fx == 0 )
4338 : {
4339 0 : n2 = 31;
4340 0 : move16();
4341 : }
4342 :
4343 0 : if ( LT_16( n1, n2 ) )
4344 : {
4345 0 : n2 = n1;
4346 0 : move16();
4347 : }
4348 0 : n2 = sub( n2, 1 );
4349 0 : temp1 = pf_temp1[k] = round_fx( (Word32) L_shl( sum1_fx, n2 ) ); /* Q(12+15+1+n2-16)=Q(12+n2) */
4350 0 : move16();
4351 0 : move16();
4352 0 : temp2 = pf_temp2[k] = round_fx( (Word32) L_shl( sum2_fx, n2 ) ); /* Q(12+n2) */
4353 0 : move16();
4354 0 : move16();
4355 :
4356 : /* Calculate the circular convolution */
4357 0 : sum = L_mac( L_mult( temp1, temp1 ), temp2, temp2 ); /* Q(12+n2+12+n2+1)=Q(25+2*n2) */
4358 :
4359 0 : exp = norm_l( sum );
4360 0 : tmp = extract_h( L_shl( sum, exp ) );
4361 0 : exp = sub( sub( 30, exp ), add( 25, shl( n2, 1 ) ) );
4362 0 : tmp = div_s( 16384, tmp ); /* Q(15+exp) */
4363 0 : sum = tmp;
4364 :
4365 0 : pf_n2_temp1[k] = add( n2, exp );
4366 0 : move16();
4367 0 : pf_temp[k] = (Word16) sum;
4368 0 : move16(); /* Q15+exp */
4369 : }
4370 0 : }
4371 :
4372 :
4373 : /*=================================================================================*/
4374 : /* FUNCTION : DTFS_getEngy_band_wb_fx (Word16 lband, Word16 hband) */
4375 : /*---------------------------------------------------------------------------------*/
4376 : /* PURPOSE : compute the energy of X1.a[k] and X2.b[k] */
4377 : /* Get DTFS energy in the specified range from lband to hband. */
4378 : /* This function is different to "DTFS_getEngy_band" as this can calculate */
4379 : /* lband, hband \in [1,6400] where "DTFS_getEngy_band" only upperlimited to*/
4380 : /* 4Khz. Possibility: modify ""DTFS_getEngy_band"" and get rid of this */
4381 : /* function. */
4382 : /*---------------------------------------------------------------------------------*/
4383 : /* INPUT ARGUMENTS : */
4384 : /* _ (struct DTFS_STRUCTURE) X_fx : a_fx/b_fx in X_fx.Q, lag in Q0 */
4385 : /* _ (Word16) lband: Q0 */
4386 : /* _ (Word16) hband: Q0 */
4387 : /*---------------------------------------------------------------------------------*/
4388 : /* OUTPUT ARGUMENTS : */
4389 : /* _ (Word32) en_fx : 2*X1.Q+1 */
4390 : /*---------------------------------------------------------------------------------*/
4391 : /* INPUT/OUTPUT ARGUMENTS : */
4392 : /* _ None */
4393 : /*---------------------------------------------------------------------------------*/
4394 : /* RETURN ARGUMENTS : _ None. */
4395 : /*---------------------------------------------------------------------------------*/
4396 : /* CALLED FROM : TX */
4397 : /*=================================================================================*/
4398 0 : Word32 DTFS_getEngy_band_wb_fx( DTFS_STRUCTURE X_fx, Word16 lband, Word16 hband )
4399 : {
4400 : Word16 k, lk, hk, HalfLag;
4401 : Word32 freq_fx, L_lband, L_hband;
4402 0 : Word32 en_fx = 0;
4403 :
4404 0 : L_lband = L_mult( lband, X_fx.lag_fx );
4405 0 : L_hband = L_mult( hband, X_fx.lag_fx );
4406 0 : HalfLag = shr( sub( X_fx.lag_fx, 1 ), 1 );
4407 :
4408 : /* get lband and hband */
4409 0 : FOR( k = 1; k <= HalfLag; k++ )
4410 : {
4411 0 : freq_fx = L_mult( k, 12800 );
4412 :
4413 0 : IF( GE_32( freq_fx, L_lband ) )
4414 : {
4415 0 : BREAK;
4416 : }
4417 : }
4418 0 : lk = k;
4419 0 : move16();
4420 0 : FOR( k = 1; k <= HalfLag; k++ )
4421 : {
4422 0 : freq_fx = L_mult( k, 12800 );
4423 0 : IF( GE_32( freq_fx, L_hband ) )
4424 : {
4425 0 : BREAK;
4426 : }
4427 : }
4428 0 : hk = k;
4429 0 : move16();
4430 :
4431 0 : FOR( k = lk; k < hk; k++ )
4432 : {
4433 0 : en_fx = L_mac0( en_fx, X_fx.a_fx[k], X_fx.a_fx[k] ); /* 2*X1.Q+1 */
4434 0 : en_fx = L_mac0( en_fx, X_fx.b_fx[k], X_fx.b_fx[k] );
4435 : }
4436 0 : en_fx = L_shr( en_fx, 1 ); /* 2*X1.Q+1 */
4437 :
4438 0 : IF( lband == 0 )
4439 : {
4440 0 : en_fx = L_mac0( en_fx, X_fx.a_fx[0], X_fx.a_fx[0] ); /* 2*X1.Q+1 */
4441 : }
4442 :
4443 : /* IF ((X_fx.lag_fx%2 == 0) && (hband == X_fx.upper_cut_off_freq_fx)) */
4444 0 : test();
4445 0 : IF( ( s_and( X_fx.lag_fx, 1 ) == 0 ) && EQ_16( hband, X_fx.upper_cut_off_freq_fx ) )
4446 : {
4447 0 : en_fx = L_mac0( en_fx, X_fx.a_fx[k], X_fx.a_fx[k] );
4448 0 : en_fx = L_mac0( en_fx, X_fx.b_fx[k], X_fx.b_fx[k] );
4449 : }
4450 :
4451 0 : return en_fx; /* 2*X1.Q+1 */
4452 : }
|
