Line data Source code
1 : /******************************************************************************
2 : * ETSI TS 103 634 V1.5.1 *
3 : * Low Complexity Communication Codec Plus (LC3plus) *
4 : * *
5 : * Copyright licence is solely granted through ETSI Intellectual Property *
6 : * Rights Policy, 3rd April 2019. No patent licence is granted by implication, *
7 : * estoppel or otherwise. *
8 : ******************************************************************************/
9 :
10 : /***************************************************************************\
11 : * contents/description: Main function for Time domain concealment
12 : \***************************************************************************/
13 :
14 : #include "options.h"
15 : #include "wmc_auto.h"
16 : #include <string.h>
17 : #include "functions.h"
18 : #include "constants.h"
19 :
20 : static LC3_INT16 TDC_random_short(LC3_INT16 *seed);
21 : static LC3_FLOAT TDC_get_gainp(const LC3_FLOAT x[], const LC3_FLOAT y[], LC3_INT32 n);
22 : static LC3_FLOAT TDC_get_gainc(const LC3_FLOAT x[], const LC3_FLOAT y[], const LC3_FLOAT *gain_p, const LC3_INT32 n, const LC3_INT32 frame_dms);
23 : static void TDC_LPC_synthesis(const LC3_FLOAT a[], LC3_FLOAT x[], LC3_FLOAT y[], LC3_INT32 l, const LC3_FLOAT mem[], LC3_INT32 lpcorder, LC3_FLOAT *buf);
24 : static void TDC_LPC_residu(const LC3_FLOAT *a, LC3_FLOAT *x, LC3_FLOAT *y, LC3_INT32 l, LC3_INT32 lpcorder);
25 : static void TDC_highPassFiltering(const LC3_INT32 L_buffer, LC3_FLOAT exc2[], const LC3_FLOAT hp_filt[], const LC3_INT32 l_fir_fer);
26 : static void TDC_f_preemph(LC3_FLOAT *signal, const LC3_FLOAT *mu, LC3_INT32 L, LC3_FLOAT *mem);
27 : static void TDC_deemph(LC3_FLOAT *signal, const LC3_FLOAT *mu, LC3_INT32 L, const LC3_FLOAT *mem);
28 : const LC3_FLOAT TDC_high_16[TDC_L_FIR_HP] = { 0.f, -0.0205f, -0.0651f, -0.1256f, -0.1792f, 0.8028f, -0.1792f, -0.1256f, -0.0651f, -0.0205f, 0.f };
29 : const LC3_FLOAT TDC_high_32[TDC_L_FIR_HP] = {-0.0517f, -0.0587f, -0.0820f, -0.1024f, -0.1164f, 0.8786f, -0.1164f, -0.1024f, -0.0820f, -0.0587f, -0.0517f};
30 : const LC3_FLOAT TDC_high_16_harm[TDC_L_FIR_HP] = { 0.0053f, 0.0000f, -0.0440f, 0.0000f, 0.2637f, 0.5500f, 0.2637f, 0.0000f, -0.0440f, 0.0000f, 0.0053f};
31 : const LC3_FLOAT TDC_high_32_harm[TDC_L_FIR_HP] = {-0.0053f, -0.0037f, -0.0140f, 0.0180f, 0.2668f, 0.4991f, 0.2668f, 0.0180f, -0.0140f, -0.0037f, -0.0053f};
32 : static void TDC_levinson(LC3_FLOAT *acf, LC3_INT32 len, LC3_FLOAT *out);
33 : static void TDC_copyFLOAT(const LC3_FLOAT * X, LC3_FLOAT * Z, LC3_INT32 n);
34 : static LC3_FLOAT TDC_dotFLOAT(const LC3_FLOAT * X, const LC3_FLOAT * Y, LC3_INT32 n);
35 : static LC3_FLOAT type_2_alpha_long(LC3_INT32 nbLostFramesInRow, LC3_INT32 frame_dms);
36 : const LC3_INT32 beforeNextIncArray[4][4] = {{0,0,0,1},
37 : {0,1,0,1},
38 : {0,1,1,1},
39 : {1,1,1,1}};
40 : const LC3_INT32 nextIncArray[4][4] = {{1,0,0,0},
41 : {1,0,1,0},
42 : {1,0,1,1},
43 : {1,1,1,1}};
44 :
45 6912 : void processTdcApply_fl(const LC3_INT32 pitch_int,
46 : const LC3_FLOAT *preemphFac,
47 : const LC3_FLOAT* A,
48 : const LC3_INT32 lpc_order,
49 : const LC3_FLOAT* pcmbufHist,
50 : const LC3_INT32 max_len_pcm_plc,
51 : const LC3_INT32 N,
52 : const LC3_INT32 frame_dms,
53 : const LC3_INT32 SampRate,
54 : const LC3_INT32 nbLostFramesInRow,
55 : const LC3_INT32 overlap,
56 : const LC3_FLOAT *stabFac,
57 : LC3_FLOAT harmonicBuf[MAX_PITCH],
58 : LC3_FLOAT synthHist[M],
59 : LC3_INT32* fract,
60 : LC3_INT16* seed,
61 : LC3_FLOAT* gain_c,
62 : LC3_FLOAT* alpha,
63 : LC3_FLOAT* synth
64 : , LC3_UINT8 plc_fadeout_type
65 : , LC3_FLOAT* alpha_type_2_table
66 : )
67 : {
68 : LC3_FLOAT step, step_n;
69 : LC3_INT32 i, len, Tc, nbLostCmpt_loc, nextInc, beforeNextInc;
70 : LC3_FLOAT gain_h, tmp, gain_p;
71 : LC3_FLOAT *exc2, *exc_buf, *exc, *x_pre, *buf, *pt_exc, *pt1_exc, *synthMemPtr;
72 : LC3_FLOAT *harmonicBufPtr;
73 : LC3_FLOAT synth_mem[M];
74 : const LC3_FLOAT *hp_filt, *high_harm;
75 : LC3_FLOAT gainInov;
76 : LC3_FLOAT hpBlendFac;
77 : char *scratchSpace1st, *scratchSpaceTmp;
78 : char scratchSpace[(MAX_LEN_PCM_PLC + MDCT_MEM_LEN_MAX + MAX_LEN_PCM_PLC + 1 + M) * sizeof(LC3_FLOAT)];
79 : LC3_FLOAT alphaPrev;
80 : LC3_FLOAT throttle;
81 : LC3_INT32 frame_dms_idx, nbLostFramesInRow_mod;
82 6912 : LC3_INT32 plc_fadeout_len = 0;
83 :
84 6912 : memset(synth_mem, 0, M * sizeof(LC3_FLOAT));
85 6912 : memset(scratchSpace, 0, (MAX_LEN_PCM_PLC + MDCT_MEM_LEN_MAX + MAX_LEN_PCM_PLC + 1 + M) * sizeof(LC3_FLOAT));
86 :
87 : /* len of synthesized signal */
88 6912 : len = N + overlap;
89 :
90 6912 : nbLostCmpt_loc = floor(frame_dms/100.0 * (nbLostFramesInRow - 1) + 1);
91 6912 : frame_dms_idx = frame_dms / 25 - 1; /* 0,1,2,3 */
92 6912 : nbLostFramesInRow_mod = (nbLostFramesInRow - 1) % 4;
93 :
94 6912 : beforeNextInc = beforeNextIncArray[frame_dms_idx][nbLostFramesInRow_mod];
95 6912 : nextInc = nextIncArray [frame_dms_idx][nbLostFramesInRow_mod];
96 :
97 6912 : if (plc_fadeout_type >= 1){
98 48 : plc_fadeout_len = PLC_FADEOUT_TYPE_1_IN_MS;
99 : }
100 : else{
101 6864 : plc_fadeout_len = PLC_FADEOUT_IN_MS;
102 : }
103 :
104 6912 : if (nbLostCmpt_loc > plc_fadeout_len/10)
105 : {
106 784 : gain_p = 0;
107 784 : *gain_c = 0;
108 784 : *alpha = 0;
109 784 : memset(synth, 0, len * sizeof(LC3_FLOAT));
110 784 : return;
111 : }
112 :
113 6128 : Tc = pitch_int;
114 6128 : if (*fract > 0) {
115 2316 : Tc++;
116 : }
117 :
118 : /*----------------------------------------------------------------
119 : * Buffer Initialization for timeDomainConcealment_Apply
120 : *
121 : * 1st
122 : * |--exc_buf--|--x_pre--|
123 : * | |--exc2--|
124 : * | |--buf (LPC Syn)--|
125 : *
126 : *---------------------------------------------------------------*/
127 :
128 6128 : scratchSpace1st = scratchSpace;
129 6128 : exc_buf = (LC3_FLOAT*)scratchSpace1st; scratchSpace1st += (LC3_INT32)sizeof(LC3_FLOAT) * (Tc + N/2 + len);
130 6128 : exc = exc_buf + (Tc + N/2);
131 :
132 6128 : scratchSpaceTmp = scratchSpace1st;
133 6128 : x_pre = (LC3_FLOAT*)scratchSpaceTmp; scratchSpaceTmp += (LC3_INT32)sizeof(LC3_FLOAT) * (lpc_order + Tc + N/2 + 1);
134 :
135 : /*---------------------------------------------------------------*
136 : * LPC Residual *
137 : *---------------------------------------------------------------*/
138 6128 : if (nbLostFramesInRow == 1)
139 : {
140 : /* copy buffer to pre-emphasis buffer */
141 1016 : TDC_copyFLOAT(&(pcmbufHist[max_len_pcm_plc-(lpc_order+Tc+N/2+1)]), &(x_pre[0]), lpc_order+Tc+N/2+1);
142 :
143 : /* apply pre-emphasis to the signal */
144 1016 : TDC_f_preemph(&(x_pre[1]), preemphFac, lpc_order+Tc+N/2, &x_pre[0]);
145 :
146 : /* copy memory for LPC synth */
147 1016 : TDC_copyFLOAT(&(x_pre[Tc+N/2+1]), synth_mem, lpc_order);
148 :
149 : /* LPC Residual */
150 1016 : TDC_LPC_residu(A, &(x_pre[lpc_order+1]), &(exc[-(Tc+N/2)]), Tc+N/2, lpc_order);
151 : }
152 :
153 : /*---------------------------------------------------------------*
154 : * Calculate gains *
155 : *---------------------------------------------------------------*/
156 6128 : if (nbLostFramesInRow == 1)
157 : {
158 1016 : if (pitch_int == Tc)
159 : {
160 316 : gain_p = TDC_get_gainp( &(x_pre[lpc_order+Tc+1]), &(x_pre[lpc_order+1]), N/2 );
161 : }
162 : else
163 : {
164 700 : tmp = TDC_get_gainp( &(x_pre[lpc_order+Tc+1]), &(x_pre[lpc_order+2]), N/2 );
165 700 : gain_p = TDC_get_gainp( &(x_pre[lpc_order+Tc+1]), &(x_pre[lpc_order+1]), N/2 );
166 :
167 700 : if (tmp > gain_p) {
168 352 : Tc = pitch_int;
169 352 : gain_p = tmp;
170 352 : *fract = 0;
171 : }
172 : }
173 :
174 1016 : if(gain_p < 0.0f)
175 : {
176 58 : gain_p = 0.0f;
177 : }
178 :
179 1016 : if(gain_p > 1.0f)
180 : {
181 218 : gain_p = 1.0f;
182 : }
183 :
184 1016 : *gain_c = 0.0f;
185 :
186 1016 : if (pitch_int == Tc)
187 : {
188 668 : *gain_c = TDC_get_gainc( &(exc[-1]), &(exc[-1-Tc]), &gain_p, N/2, frame_dms );
189 : }
190 : else
191 : {
192 348 : tmp = TDC_get_gainc( &(exc[-1]), &(exc[-1-pitch_int]), &gain_p, N/2, frame_dms );
193 348 : *gain_c = TDC_get_gainc( &(exc[-1]), &(exc[-1-Tc]) , &gain_p, N/2, frame_dms );
194 348 : *gain_c = MIN(*gain_c, tmp);
195 : }
196 : }
197 : else
198 : {
199 5112 : gain_p = *alpha;
200 : }
201 :
202 : /*---------------------------------------------------------------*
203 : * Damping factor *
204 : *---------------------------------------------------------------*/
205 :
206 6128 : alphaPrev = 1;
207 6128 : if (nbLostFramesInRow > 1)
208 : {
209 5112 : alphaPrev = *alpha;
210 : }
211 :
212 6128 : if (plc_fadeout_type == 2){
213 0 : *alpha = alpha_type_2_table[nbLostFramesInRow];
214 : }
215 : else{
216 :
217 6128 : if (nextInc != 0)
218 : {
219 3064 : switch (nbLostCmpt_loc)
220 : {
221 1016 : case 1:
222 1016 : *alpha = (LC3_FLOAT)sqrt(gain_p);
223 1016 : if ( *alpha > 0.98f )
224 : {
225 260 : *alpha = 0.98f;
226 : }
227 756 : else if ( *alpha < 0.925f )
228 : {
229 668 : *alpha = 0.925f;
230 : }
231 1016 : break;
232 1016 : case 2:
233 1016 : *alpha = (0.63f + 0.35f * (*stabFac)) * gain_p;
234 1016 : if ( *alpha < 0.919f )
235 : {
236 707 : *alpha = 0.919f;
237 : }
238 1016 : break;
239 1032 : default:
240 1032 : *alpha = (0.652f + 0.328f * (*stabFac)) * gain_p;
241 : }
242 3064 : }
243 :
244 6128 : if (nbLostCmpt_loc > 3)
245 : {
246 1392 : switch (frame_dms)
247 : {
248 0 : case 25: *alpha *= PLC34_ATTEN_FAC_025; break;
249 1392 : case 50: *alpha *= PLC34_ATTEN_FAC_025; break;
250 0 : case 75: *alpha *= PLC34_ATTEN_FAC_075; break;
251 0 : case 100: *alpha *= PLC34_ATTEN_FAC_100; break;
252 : }
253 4736 : }
254 :
255 6128 : if (nbLostCmpt_loc > 5)
256 : {
257 360 : gain_p = *alpha;
258 : }
259 : }
260 : /*---------------------------------------------------------------*
261 : * Construct the harmonic part *
262 : * Last pitch cycle of the previous frame is repeatedly copied. *
263 : *---------------------------------------------------------------*/
264 :
265 6128 : pt_exc = harmonicBuf;
266 6128 : pt1_exc = exc - Tc;
267 :
268 6128 : if( nbLostFramesInRow == 1 )
269 : {
270 1016 : if (*stabFac >= 1)
271 : {
272 704 : TDC_copyFLOAT(pt1_exc, pt_exc, Tc);
273 : }
274 : else
275 : {
276 : /* These values are necessary for the last five filtered samples */
277 312 : TDC_copyFLOAT(&exc[-Tc], exc, (TDC_L_FIR_HP-1)/2);
278 :
279 312 : high_harm = TDC_high_32_harm;
280 312 : if (SampRate <= 16000)
281 : {
282 0 : high_harm = TDC_high_16_harm;
283 : }
284 :
285 102718 : for( i = 0; i < Tc; i++ )
286 : {
287 102406 : pt_exc[i] = TDC_dotFLOAT(&pt1_exc[i-(TDC_L_FIR_HP-1)/2], high_harm, TDC_L_FIR_HP);
288 : }
289 : }
290 : }
291 :
292 : /*---------------------------------------------------------------*
293 : * Construct the random part of excitation *
294 : *---------------------------------------------------------------*/
295 6128 : scratchSpaceTmp = scratchSpace1st;
296 6128 : exc2 = (LC3_FLOAT*)scratchSpaceTmp; scratchSpaceTmp += (LC3_INT32)sizeof(LC3_FLOAT) * (len + TDC_L_FIR_HP - 1);
297 :
298 2641168 : for (i = 0; i < len + TDC_L_FIR_HP - 1; i++) {
299 2635040 : exc2[i] = (LC3_FLOAT)TDC_random_short(seed);
300 : }
301 :
302 : /* high pass noise */
303 6128 : if (SampRate <= 16000 )
304 : {
305 0 : hp_filt = TDC_high_16;
306 : } else {
307 6128 : hp_filt = TDC_high_32;
308 : }
309 :
310 6128 : if ( nbLostFramesInRow == 1 )
311 : {
312 1016 : TDC_highPassFiltering(len, exc2, hp_filt, TDC_L_FIR_HP);
313 : }
314 : else
315 : {
316 : /* moves from 0 to 1, speed is defined by PLC3_HPBLENDTHROTTLE */
317 5112 : throttle = (LC3_FLOAT)nbLostCmpt_loc / (nbLostCmpt_loc + PLC3_HPBLENDTHROTTLE);
318 5112 : hpBlendFac = (1 - *alpha) * throttle;
319 :
320 2152152 : for (i = 0; i < len; i++)
321 : {
322 2147040 : exc2[i] = hpBlendFac * exc2[i+TDC_L_FIR_HP/2] + (1 - hpBlendFac) * TDC_dotFLOAT(&exc2[i], hp_filt, TDC_L_FIR_HP );
323 : }
324 : }
325 :
326 : /* normalize energy */
327 6128 : gainInov = 1.0f / (LC3_FLOAT)sqrt(TDC_dotFLOAT( exc2, exc2, N ) / (LC3_FLOAT)N + 0.01f );
328 6128 : gainInov *= (1.1f - 0.75* gain_p);
329 :
330 : /* gains */
331 6128 : gain_h = alphaPrev;
332 6128 : tmp = *gain_c * *alpha / alphaPrev;
333 :
334 : /* update steps */
335 6128 : step = (1.0f/(LC3_FLOAT)N) * (gain_h - *alpha);
336 6128 : step_n = (1.0f/(LC3_FLOAT)N) * (*gain_c - tmp);
337 :
338 : /*---------------------------------------------------------------*
339 : * Construct the total excitation *
340 : *---------------------------------------------------------------*/
341 6128 : harmonicBufPtr = harmonicBuf + ((nbLostFramesInRow - 1) * N) % Tc;
342 :
343 2579888 : for ( i = 0; i < len; i++ ) {
344 : /* harmonic */
345 2573760 : if (harmonicBufPtr - harmonicBuf >= Tc) {
346 8888 : harmonicBufPtr = harmonicBuf;
347 : }
348 2573760 : exc[i] = *harmonicBufPtr++;
349 2573760 : exc[i] *= gain_h;
350 :
351 : /* random */
352 2573760 : exc2[i] *= *gain_c * gainInov;
353 :
354 : /* total */
355 2573760 : exc[i] = exc[i] + exc2[i];
356 :
357 : /* update */
358 2573760 : gain_h -= step;
359 2573760 : gain_h = MAX(gain_h, 0);
360 2573760 : *gain_c -= step_n;
361 2573760 : *gain_c = MAX(*gain_c, 0);
362 : }
363 :
364 6128 : *gain_c = tmp;
365 :
366 : /*----------------------------------------------------------*
367 : * Compute the synthesis speech *
368 : *----------------------------------------------------------*/
369 6128 : buf = (LC3_FLOAT*)scratchSpace1st; scratchSpace1st += (LC3_INT32)sizeof(LC3_FLOAT) * (len + lpc_order);
370 6128 : synthMemPtr = synth_mem;
371 6128 : if (nbLostFramesInRow != 1)
372 : {
373 5112 : synthMemPtr = synthHist;
374 : }
375 :
376 6128 : TDC_LPC_synthesis(A,
377 : &exc[0],
378 : synth,
379 : len,
380 : synthMemPtr,
381 : lpc_order,
382 : buf);
383 :
384 6128 : TDC_copyFLOAT(&synth[N-lpc_order], synthHist, lpc_order);
385 :
386 : /*----------------------------------------------------------*
387 : * Deemphasis *
388 : *----------------------------------------------------------*/
389 6128 : TDC_deemph( synth, preemphFac, len, &pcmbufHist[max_len_pcm_plc-1] );
390 :
391 : /*----------------------------------------------------------*
392 : * Fade to zero *
393 : *----------------------------------------------------------*/
394 6128 : if (beforeNextInc != 0)
395 : {
396 3064 : if (nbLostCmpt_loc == plc_fadeout_len/10)
397 : {
398 172 : gain_h = 1;
399 172 : step = 1.0f/(LC3_FLOAT)N;
400 41452 : for ( i = 0; i < N; i++ ) {
401 41280 : synth[i] *= gain_h;
402 41280 : gain_h -= step;
403 : }
404 172 : memset(&synth[N], 0, overlap * sizeof(LC3_FLOAT));
405 : }
406 : }
407 : }
408 :
409 : /* Take only real part */
410 6912 : void processTdcInverseOdft_fl(LC3_FLOAT *in, LC3_INT32 n_bands, LC3_FLOAT *out, LC3_INT32 lpc_order)
411 : {
412 : LC3_INT32 i, j, k;
413 : LC3_FLOAT buf[2*MAX_BANDS_NUMBER_PLC];
414 : Complex sum;
415 : Complex res;
416 :
417 : /* Buffer for ifft */
418 6912 : j = 0;
419 283392 : for (i = 0; i < n_bands - 1; i += 2)
420 : {
421 276480 : buf[j] = in[i];
422 276480 : j++;
423 : }
424 :
425 283392 : for (i = n_bands - 1; i > 0; i -= 2)
426 : {
427 276480 : buf[j] = in[i];
428 276480 : j++;
429 : }
430 :
431 559872 : for (i = 0; i < n_bands; i++)
432 : {
433 552960 : buf[j] = in[i];
434 552960 : j++;
435 : }
436 :
437 : /* ifft */
438 559872 : for (j = 0; j < n_bands; j++)
439 : {
440 552960 : sum.r = 0, sum.i = 0;
441 552960 : res.r = 0, res.i = 0;
442 44789760 : for (k = 0; k < n_bands; k++)
443 : {
444 44236800 : res = cexpi((2 * M_PI_LC3PLUS * (LC3_FLOAT) (j * k)) / (LC3_FLOAT) n_bands);
445 44236800 : res.r = res.r * buf[k];
446 44236800 : res.i = res.i * buf[k];
447 44236800 : sum = cadd(sum, res);
448 : }
449 :
450 552960 : res = cexpi((LC3_FLOAT) j * M_PI_LC3PLUS / (2.0 * (LC3_FLOAT) n_bands));
451 552960 : out[j] = (sum.r * res.r - sum.i * res.i);
452 : }
453 :
454 6912 : out[0] = out[0] * 1.0001;
455 6912 : if (out[0] == 0)
456 : {
457 0 : out[0] = 1;
458 0 : zero_float(&out[1], lpc_order);
459 : }
460 6912 : }
461 :
462 6912 : void processTdcPreemphasis_fl(LC3_FLOAT *in, LC3_FLOAT *pre_emph_factor, LC3_INT32 n_bands)
463 : {
464 : LC3_INT32 i;
465 :
466 559872 : for (i = 0; i < n_bands; i++)
467 : {
468 552960 : in[i] = in[i] * (1.0 - 2.0 * (*pre_emph_factor) * LC3_COS(2.0 * M_PI_LC3PLUS * (0.5 + (LC3_FLOAT) i) / (2.0 * (LC3_FLOAT) n_bands)) + (*pre_emph_factor) * (*pre_emph_factor));
469 : }
470 6912 : }
471 :
472 6912 : void processTdcLpcEstimation_fl(LC3_FLOAT *r, LC3_INT32 fs_idx, LC3_INT32 len, LC3_FLOAT *A, LC3_INT32 frame_dms)
473 : {
474 : LC3_INT32 i;
475 : const LC3_FLOAT *lpc_array;
476 :
477 6912 : lpc_array = plc_tdc_lpc_all[fs_idx];
478 :
479 6912 : if (fs_idx == 0 && frame_dms == 25)
480 : {
481 0 : lpc_array = plc_tdc_lpc_8_25ms;
482 : }
483 :
484 : /* r[0] = r[0] * 1 */
485 117504 : for (i = 1; i < len; i++)
486 : {
487 110592 : r[i] = r[i] * lpc_array[i];
488 : }
489 :
490 6912 : TDC_levinson(r, len - 1, A);
491 6912 : }
492 :
493 : /** random
494 : *
495 : * Parameters:
496 : * seed I/O: seed for random number
497 : *
498 : * Function:
499 : * Signed 16 bits random generator.
500 : *
501 : * Returns:
502 : * random number
503 : */
504 2635040 : static LC3_INT16 TDC_random_short(LC3_INT16 *seed)
505 : {
506 2635040 : *seed = (LC3_INT16) (*seed * 12821L + 16831L);
507 2635040 : return(*seed);
508 : }
509 :
510 1716 : static LC3_FLOAT TDC_get_gainp( /* output: gain of pitch */
511 : const LC3_FLOAT x[], /* input : input signal */
512 : const LC3_FLOAT y[], /* input : shifted input signal */
513 : LC3_INT32 n /* input : vector length */
514 : )
515 : {
516 : LC3_FLOAT corr, ener;
517 : LC3_INT16 i;
518 :
519 1716 : corr = 0; ener = 1e-6f;
520 :
521 207636 : for (i = 0; i < n; i++)
522 : {
523 205920 : corr += x[i]*y[i];
524 205920 : ener += y[i]*y[i];
525 : }
526 :
527 1716 : return(corr/ener);
528 : }
529 :
530 1364 : static LC3_FLOAT TDC_get_gainc( /* output: gain of code */
531 : const LC3_FLOAT x[], /* input : input signal */
532 : const LC3_FLOAT y[], /* input : shifted input signal */
533 : const LC3_FLOAT *gain_p, /* input : gain of pitch */
534 : const LC3_INT32 n, /* input : vector length */
535 : const LC3_INT32 frame_dms /* input : frame length in dms */
536 : )
537 : {
538 : LC3_FLOAT gain_c;
539 : LC3_FLOAT gain_c_max;
540 : LC3_INT16 i;
541 :
542 1364 : gain_c = 0; gain_c_max = 0;
543 :
544 165044 : for (i = 0; i < n; i++)
545 : {
546 163680 : gain_c += ( x[-i] - *gain_p * y[-i] ) * ( x[-i] - *gain_p * y[-i] );
547 : }
548 :
549 1364 : if (frame_dms < 100)
550 : {
551 165044 : for (i = 0; i < n; i++)
552 : {
553 163680 : gain_c_max += (x[-i] * x[-i]);
554 : }
555 1364 : gain_c = MIN(gain_c, gain_c_max);
556 : }
557 :
558 1364 : gain_c = (LC3_FLOAT)sqrt(gain_c / n );
559 :
560 1364 : return gain_c;
561 : }
562 :
563 1016 : static void TDC_highPassFiltering(const LC3_INT32 L_buffer, /* i: buffer length */
564 : LC3_FLOAT exc2[], /* i/o: unvoiced excitation before the high pass filtering */
565 : const LC3_FLOAT hp_filt[], /* i: high pass filter coefficients */
566 : const LC3_INT32 l_fir_fer) /* i: high pass filter length */
567 : {
568 : LC3_INT32 i;
569 :
570 427736 : for( i=0 ; i< L_buffer; i++ ) {
571 426720 : exc2[i] = TDC_dotFLOAT(&exc2[i], hp_filt, l_fir_fer);
572 : }
573 1016 : }
574 :
575 6128 : static void TDC_LPC_synthesis(
576 : const LC3_FLOAT a[],
577 : LC3_FLOAT x[],
578 : LC3_FLOAT y[],
579 : LC3_INT32 l,
580 : const LC3_FLOAT mem[],
581 : LC3_INT32 lpcorder,
582 : LC3_FLOAT *buf
583 : )
584 : {
585 : LC3_FLOAT s, *yy;
586 : LC3_INT32 i, j;
587 :
588 : /* copy initial filter states into synthesis buffer */
589 104176 : for (i=0; i < lpcorder; i++)
590 : {
591 98048 : buf[i] = mem[i];
592 : }
593 6128 : yy = &buf[i];
594 :
595 2579888 : for (i = 0; i < l; i++)
596 : {
597 2573760 : s = x[i];
598 43753920 : for (j = 1; j <= lpcorder; j++)
599 : {
600 41180160 : s -= a[j] * yy[i- j];
601 : }
602 2573760 : y[i] = s;
603 2573760 : yy[i] = y[i];
604 : }
605 :
606 6128 : return;
607 : }
608 :
609 :
610 : /** TDC_LPC_residu
611 : *
612 : * Parameters:
613 : * a I: LP filter coefficients (Q12)
614 : * x I: input signal (usually speech)
615 : * y O: output signal (usually residual)
616 : * l I: size of filtering
617 : * lpcorder I: Order of LP filter
618 : *
619 : * Function:
620 : * Compute the LP residual by filtering the input speech through A(z).
621 : *
622 : * Returns:
623 : * void
624 : */
625 1016 : static void TDC_LPC_residu(const LC3_FLOAT *a, LC3_FLOAT *x, LC3_FLOAT *y, LC3_INT32 l, LC3_INT32 lpcorder)
626 : {
627 : LC3_FLOAT s;
628 : LC3_INT32 i, j;
629 :
630 490900 : for (i = 0; i < l; i++)
631 : {
632 489884 : s = x[i];
633 8328028 : for (j = 1; j <= lpcorder; j++)
634 : {
635 7838144 : s += a[j] * x[i - j];
636 : }
637 489884 : y[i] = s;
638 : }
639 :
640 1016 : return;
641 : }
642 :
643 :
644 : /** TDC_f_preemph
645 : *
646 : * Parameters:
647 : * signal I/O: signal
648 : * mu I: preemphasis factor
649 : * L I: vector size
650 : * mem I: memory (x[-1])
651 : *
652 : * Function:
653 : * Filtering through 1 - mu z^-1
654 : *
655 : *
656 : * Returns:
657 : * void
658 : */
659 :
660 1016 : static void TDC_f_preemph(LC3_FLOAT *signal, const LC3_FLOAT *mu, LC3_INT32 L, LC3_FLOAT *mem)
661 : {
662 : LC3_INT32 i;
663 :
664 506140 : for (i = L - 1; i > 0; i--)
665 : {
666 505124 : signal[i] = signal[i] - *mu * signal[i - 1];
667 : }
668 :
669 1016 : signal[0] -= *mu * (*mem);
670 :
671 1016 : return;
672 : }
673 :
674 : /*
675 : * TDC_deemph
676 : *
677 : * Parameters:
678 : * signal I/O: signal
679 : * mu I: deemphasis factor
680 : * L I: vector size
681 : * mem I: memory (signal[-1])
682 : *
683 : * Function:
684 : * Filtering through 1/(1-mu z^-1)
685 : * Signal is divided by 2.
686 : *
687 : * Returns:
688 : * void
689 : */
690 6128 : static void TDC_deemph(LC3_FLOAT *signal, const LC3_FLOAT *mu, LC3_INT32 L, const LC3_FLOAT *mem)
691 : {
692 : LC3_INT32 i;
693 :
694 6128 : signal[0] = signal[0] + *mu * (*mem);
695 :
696 2573760 : for (i = 1; i < L; i++)
697 : {
698 2567632 : signal[i] = signal[i] + *mu * signal[i - 1];
699 : }
700 :
701 6128 : return;
702 : }
703 :
704 9176 : static void TDC_copyFLOAT(const LC3_FLOAT * X, LC3_FLOAT * Z, LC3_INT32 n)
705 : {
706 : /* no values to copy */
707 9176 : if ( (n < 1) || (X == Z) ){
708 0 : return;
709 : }
710 : /* If overlapping */
711 9176 : if ( ( (Z > X) && (Z < X+n) ) || ( (Z < X) && (X < Z+n) ) ) {
712 0 : memmove(Z, X, sizeof(LC3_FLOAT)*n);
713 : }
714 : else{
715 9176 : memcpy(Z, X, sizeof(LC3_FLOAT)*n);
716 : }
717 : }
718 :
719 2682294 : static LC3_FLOAT TDC_dotFLOAT(const LC3_FLOAT * X, const LC3_FLOAT * Y, LC3_INT32 n)
720 : {
721 : LC3_FLOAT acc;
722 : LC3_INT32 i;
723 :
724 2682294 : acc = 0;
725 2682294 : if (n) {
726 2682294 : acc = X[0]*Y[0];
727 : }
728 :
729 30908546 : for (i=1; i<n; i++) acc += X[i]*Y[i];
730 :
731 2682294 : return acc;
732 : }
733 :
734 6912 : static void TDC_levinson(LC3_FLOAT *acf, LC3_INT32 len, LC3_FLOAT *out)
735 : {
736 : LC3_FLOAT g, v, sum, buf[M], buf2[M];
737 : LC3_INT32 i, j, k, t;
738 :
739 6912 : g = acf[1] / acf[0];
740 :
741 6912 : out[0] = g;
742 6912 : v = (1.0 - g * g) * acf[0];
743 :
744 110592 : for (t = 1; t < len; t++)
745 : {
746 103680 : sum = 0; j = 0;
747 933120 : for (i = 1; i <= t; i++)
748 : {
749 829440 : sum += out[j] * acf[i];
750 829440 : j++;
751 : }
752 :
753 103680 : g = (acf[t + 1] - sum) / v;
754 :
755 103680 : move_float(buf, out, len);
756 103680 : move_float(buf2, out, len);
757 103680 : out[0] = g;
758 :
759 103680 : j = 1, k = 0;
760 933120 : for (i = t - 1; i >= 0; i--)
761 : {
762 829440 : out[j] = buf[k] - g * buf2[i];
763 829440 : j++; k++;
764 : }
765 :
766 103680 : v = v * (1.0 - g * g);
767 : }
768 :
769 6912 : move_float(buf, out, len);
770 6912 : out[0] = 1;
771 :
772 6912 : j = 1;
773 117504 : for (i = len - 1; i >= 0; i--)
774 : {
775 110592 : out[j] = -buf[i];
776 110592 : j++;
777 : }
778 6912 : }
779 :
780 128300 : static LC3_FLOAT type_2_alpha_long(LC3_INT32 nbLostFramesInRow, LC3_INT32 frame_dms)
781 : {
782 128300 : if (nbLostFramesInRow <= 3*100.0/frame_dms){
783 23640 : return LC3_POW(0.95,(nbLostFramesInRow + (100.0/frame_dms) - 1) * frame_dms/100.0);
784 : }
785 : else {
786 104660 : LC3_INT32 n_shift = (nbLostFramesInRow - 3*100.0/frame_dms) * 50/frame_dms;
787 104660 : return LC3_POW(0.7,(n_shift + 100.0/frame_dms - 1) * frame_dms/100.0);
788 : }
789 : }
790 :
791 128300 : LC3_FLOAT type_2_fadeout(LC3_INT32 nbLostFramesInRow, LC3_INT32 frame_dms)
792 : {
793 128300 : LC3_FLOAT selector = PLC_FADEOUT_TYPE_2_SELECTOR * 2 * 100/frame_dms;
794 128300 : if (selector >= nbLostFramesInRow){
795 128300 : return type_2_alpha_long(nbLostFramesInRow, frame_dms);
796 : }
797 : else {
798 0 : return LC3_POW(0.5,(nbLostFramesInRow + (100.0/frame_dms) - 1) * frame_dms/100.0);
799 : }
800 : }
|
