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