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 : #include "functions.h"
11 :
12 : static void ltpf_synth_filter(Word16 *synth_ltp, Word16 *synth, Word16 length, Word16 pitch_int, Word16 pitch_fr,
13 : Word16 gain, Word16 scale_fac_idx, Word16 fs_idx,
14 : Word16 fade /* 0=normal, +1=fade-in, -1=fade-out */);
15 :
16 : /*************************************************************************/
17 :
18 :
19 0 : void process_ltpf_decoder_fx(Word16 *x_e, Word16 L_frame, Word16 old_x_len, Word16 fs_idx, Word16 old_y_len,
20 : Word16 *old_e, Word16 *x_in, Word16 *old_x, Word16 *y_out, Word16 *old_y, Word16 ltpf,
21 : Word16 ltpf_active, Word16 pitch_index, Word16 *old_pitch_int, Word16 *old_pitch_fr,
22 : Word16 *old_gain, Word16 *mem_ltpf_active, Word16 scale_fac_idx, Word16 bfi,
23 : Word16 concealMethod,
24 : Word16 damping, Word16 *old_scale_fac_idx,
25 : Word32 *rel_pitch_change, Word16 hrmode, Word16 frame_dms,
26 : Word8 *scratchBuffer)
27 : {
28 : Counter i;
29 : Word16 gain, s, s0, s1, pitch, pitch_int, pitch_fr, N4, N34;
30 : Word16 *x, *y;
31 : Word16 *z;
32 : Word32 tmp32, pitch_delta;
33 : #ifdef DYNMEM_COUNT
34 : Dyn_Mem_In("process_ltpf_decoder_fx", sizeof(struct {
35 : Counter i;
36 : Word16 gain, s, s0, s1, pitch, pitch_int, pitch_fr, N4, N34;
37 : Word16 *x, *y;
38 : Word16 *z;
39 : Word32 tmp32, pitch_delta;
40 : }));
41 : #endif
42 :
43 0 : z = (Word16 *)scratchAlign(scratchBuffer, 0); /* Size = MAX_LEN / 4 + 10 */
44 :
45 :
46 0 : test();
47 0 : IF ((sub(bfi, 1) == 0) && (sub(concealMethod, LC3_CON_TEC_NS_STD) == 0))
48 : {
49 0 : ltpf = 0; move16();
50 0 : ltpf_active = 0; move16();
51 0 : pitch_int = 0; move16();
52 0 : pitch_fr = 0; move16();
53 0 : gain = 0; move16();
54 : }
55 :
56 : /* Filter parameters */
57 0 : IF (sub(bfi, 1) != 0)
58 : {
59 0 : IF (ltpf == 0)
60 : {
61 0 : pitch_int = 0; move16();
62 0 : pitch_fr = 0; move16();
63 : }
64 : ELSE
65 : {
66 : /* Decode pitch */
67 0 : IF (sub(pitch_index, 380) < 0)
68 : {
69 0 : pitch_int = shr_pos(add(pitch_index, 64), 2);
70 0 : pitch_fr = add(sub(pitch_index, shl_pos(pitch_int, 2)), 128);
71 : }
72 0 : ELSE IF (sub(pitch_index, 440) < 0)
73 : {
74 0 : pitch_int = shr_pos(sub(pitch_index, 126), 1);
75 0 : pitch_fr = sub(sub(shl_pos(pitch_index, 1), shl_pos(pitch_int, 2)), 252);
76 : }
77 : ELSE
78 : {
79 0 : pitch_int = sub(pitch_index, 283);
80 0 : pitch_fr = 0; move16();
81 : }
82 0 : pitch = add(shl_pos(pitch_int, 2), pitch_fr);
83 : #ifdef ENABLE_HR_MODE
84 0 : IF (sub(fs_idx, 5) == 0)
85 : {
86 0 : pitch = round_fx(L_shl_pos(L_mult(shl_pos(pitch, 2), pitch_scale[4]), 1));
87 : }
88 : ELSE
89 : #endif
90 : {
91 0 : pitch = mult_r(shl_pos(pitch, 2), pitch_scale[fs_idx]);
92 : }
93 0 : pitch_int = shr_pos(pitch, 2);
94 0 : pitch_fr = sub(pitch, shl_pos(pitch_int, 2));
95 : }
96 :
97 : /* Decode gain */
98 0 : if (scale_fac_idx < 0)
99 : {
100 0 : ltpf_active = 0;
101 0 : ASSERT(!(*old_scale_fac_idx < 0 && *mem_ltpf_active == 1));
102 : }
103 0 : IF (ltpf_active == 0)
104 : {
105 0 : gain = 0; move16();
106 : }
107 : ELSE
108 : {
109 0 : ASSERT(scale_fac_idx >= 0);
110 0 : gain = gain_scale_fac[scale_fac_idx]; move16();
111 : }
112 : }
113 0 : ELSE IF (sub(concealMethod, LC3_CON_TEC_NS_STD) != 0)
114 : {
115 : /* fix to avoid not initialized filtering for concelament
116 : might be necessary in case of bit errors or rate switching */
117 0 : if (scale_fac_idx < 0) {
118 0 : if (*mem_ltpf_active && *old_scale_fac_idx>=0)
119 : {
120 0 : scale_fac_idx = *old_scale_fac_idx;
121 : }
122 : }
123 :
124 0 : ltpf_active = *mem_ltpf_active; move16();
125 :
126 0 : if ((sub(concealMethod, LC3_CON_TEC_PHASE_ECU) == 0))
127 : { /* always start fade off to save filtering WMOPS for the remaining 7.5 ms */
128 0 : assert(bfi == 1);
129 0 : ltpf_active = 0; move16(); /*always start fade off , still maintain *mem_ltpf_active */
130 : }
131 :
132 0 : pitch_int = *old_pitch_int;
133 0 : pitch_fr = *old_pitch_fr;
134 0 : gain = mult_r(*old_gain, damping);
135 : }
136 :
137 0 : test(); test();
138 0 : IF (ltpf_active == 0 && *mem_ltpf_active == 0)
139 : {
140 : /* LTPF inactive */
141 :
142 0 : basop_memmove(y_out, x_in, L_frame * sizeof(Word16));
143 :
144 : /* Update */
145 0 : s = sub(*old_e, *x_e);
146 0 : IF (s > 0)
147 : {
148 0 : basop_memmove(old_y, &old_y[L_frame], (old_y_len - L_frame) * sizeof(Word16));
149 :
150 0 : IF (sub(s, 15) > 0)
151 : {
152 0 : basop_memset(&old_y[old_y_len - L_frame], 0, (L_frame) * sizeof(Word16));
153 :
154 0 : basop_memset(old_x, 0, (old_x_len) * sizeof(Word16));
155 : }
156 : ELSE
157 : {
158 0 : FOR (i = 0; i < L_frame; i++)
159 : {
160 0 : old_y[i + old_y_len - L_frame] = shr(x_in[i], s); move16();
161 : }
162 0 : FOR (i = 0; i < old_x_len; i++)
163 : {
164 0 : old_x[i] = shr(x_in[i + L_frame - old_x_len], s); move16();
165 : }
166 : }
167 : }
168 : ELSE
169 : {
170 0 : IF (sub(s, -15) < 0)
171 : {
172 0 : basop_memset(old_y, 0, (old_y_len - L_frame) * sizeof(Word16));
173 : }
174 : ELSE
175 : {
176 0 : FOR (i = 0; i < old_y_len - L_frame; i++)
177 : {
178 0 : old_y[i] = shl(old_y[i + L_frame], s); move16();
179 : }
180 : }
181 :
182 0 : basop_memmove(&old_y[old_y_len - L_frame], x_in, (L_frame) * sizeof(Word16));
183 :
184 0 : basop_memmove(old_x, &x_in[L_frame - old_x_len], (old_x_len) * sizeof(Word16));
185 :
186 0 : *old_e = *x_e; move16();
187 : }
188 :
189 0 : if (bfi == 0 && sub(hrmode,1) == 0 && (sub(frame_dms,50) == 0 || sub(frame_dms,25) == 0)){
190 0 : pitch_delta = abs_s(add(sub(*old_pitch_int,pitch_int) , shr_pos(sub(*old_pitch_fr, pitch_fr),2))); //int_old -int_new + (fr_old-fr_new) / 4.0));
191 0 : tmp32 = BASOP_Util_Divide3216_Scale_lc3plus(pitch_delta, MAX(add(*old_pitch_int, shr_pos(*old_pitch_fr,2)), 1),&s0);// = pitch_delta *2^15 / MAX(pitch_fl_c_old, 1);
192 0 : *rel_pitch_change = L_shl_pos(tmp32,s0+16);
193 : }
194 :
195 0 : *old_pitch_int = pitch_int; move16();
196 0 : *old_pitch_fr = pitch_fr; move16();
197 0 : *old_gain = 0; move16();
198 0 : *mem_ltpf_active = 0; move16();
199 : }
200 : ELSE
201 : {
202 : /* Input/Output buffers */
203 0 : x = old_x + old_x_len;
204 0 : y = old_y + old_y_len;
205 :
206 : #ifdef ENABLE_HR_MODE
207 0 : assert(fs_idx < 5 && "Ltpf not supported for 96kHz!\n");
208 : #endif
209 :
210 0 : N4 = ltpf_overlap_len[fs_idx]; move16();
211 0 : N34 = sub(L_frame, N4); move16();
212 :
213 : /* Input */
214 0 : basop_memmove(x, x_in, (L_frame) * sizeof(Word16));
215 :
216 : /* Scaling */
217 0 : s0 = sub(s_min(getScaleFactor16_0(old_x, old_x_len), getScaleFactor16_0(old_y, old_y_len)), 1);
218 0 : *old_e = sub(*old_e, s0); move16();
219 0 : s1 = sub(getScaleFactor16(x, L_frame), 1);
220 0 : *x_e = sub(*x_e, s1); move16();
221 0 : s = sub(*old_e, *x_e);
222 0 : IF (s > 0)
223 : {
224 0 : Scale_sig(x, L_frame, sub(s1, s));
225 0 : Scale_sig(old_x, old_x_len, s0);
226 0 : Scale_sig(old_y, old_y_len, s0);
227 0 : *x_e = *old_e; move16();
228 : }
229 : ELSE
230 : {
231 0 : Scale_sig(x, L_frame, s1);
232 0 : Scale_sig(old_x, old_x_len, add(s0, s));
233 0 : Scale_sig(old_y, old_y_len, add(s0, s));
234 0 : *old_e = *x_e; move16();
235 : }
236 :
237 : /* Filtering */
238 0 : IF (ltpf_active == 0)
239 : {
240 0 : ltpf_synth_filter(y, x, N4, *old_pitch_int, *old_pitch_fr, *old_gain, *old_scale_fac_idx, fs_idx,
241 : -1);
242 : }
243 0 : ELSE IF (*mem_ltpf_active == 0)
244 : {
245 0 : ltpf_synth_filter(y, x, N4, pitch_int, pitch_fr, gain, scale_fac_idx, fs_idx, 1);
246 : }
247 0 : ELSE IF (sub(pitch_int, *old_pitch_int) == 0 && sub(*old_pitch_fr, pitch_fr) == 0)
248 : {
249 0 : ltpf_synth_filter(y, x, N4, pitch_int, pitch_fr, gain, scale_fac_idx, fs_idx, 0);
250 : }
251 : ELSE
252 : {
253 0 : ltpf_synth_filter(y, x, N4, *old_pitch_int, *old_pitch_fr, *old_gain, *old_scale_fac_idx, fs_idx,
254 : -1);
255 0 : basop_memmove(z, y - tilt_filter_len[fs_idx], (N4 + tilt_filter_len[fs_idx]) * sizeof(Word16));
256 0 : ltpf_synth_filter(y, z + tilt_filter_len[fs_idx], N4, pitch_int, pitch_fr, gain, scale_fac_idx,
257 : fs_idx, 1);
258 : }
259 0 : IF (ltpf_active > 0)
260 : {
261 0 : ltpf_synth_filter(y + N4, x + N4, N34, pitch_int, pitch_fr, gain,
262 : scale_fac_idx, fs_idx, 0);
263 : }
264 : ELSE
265 : {
266 0 : basop_memmove(&y[N4], &x[N4], N34 * sizeof(Word16));
267 : }
268 :
269 : /* Output */
270 0 : basop_memmove(y_out, y, (L_frame) * sizeof(Word16));
271 :
272 : /* Update */
273 0 : basop_memmove(old_x, &old_x[L_frame], (old_x_len) * sizeof(Word16));
274 :
275 0 : basop_memmove(old_y, &old_y[L_frame], (old_y_len) * sizeof(Word16));
276 :
277 0 : *old_pitch_int = pitch_int; move16();
278 0 : *old_pitch_fr = pitch_fr; move16();
279 0 : *old_gain = gain; move16();
280 0 : *mem_ltpf_active = ltpf_active; move16();
281 : }
282 :
283 0 : *old_scale_fac_idx = scale_fac_idx; move16();
284 :
285 : #ifdef DYNMEM_COUNT
286 : Dyn_Mem_Out();
287 : #endif
288 0 : }
289 :
290 :
291 0 : static void ltpf_synth_filter(Word16 *synth_ltp, Word16 *synth, Word16 length, Word16 pitch_int, Word16 pitch_fr,
292 : Word16 gain, Word16 scale_fac_idx, Word16 fs_idx,
293 : Word16 fade /* 0=normal, +1=fade-in, -1=fade-out */)
294 : {
295 : Word16 *x0;
296 : Word16 *y0;
297 : Word32 s;
298 : Word16 alpha, step;
299 : Word16 i, k;
300 : Counter j, l;
301 :
302 : #ifdef DYNMEM_COUNT
303 : Dyn_Mem_In("ltpf_synth_filter", sizeof(struct {
304 : Word16 *x0;
305 : Word16 *y0;
306 : Word32 s;
307 : Word16 alpha, step;
308 : Word16 i, k;
309 : Counter j, l;
310 : }));
311 : #endif
312 :
313 0 : ASSERT(scale_fac_idx >= 0);
314 :
315 0 : step = 0; /* initialize just to avoid compiler warning */
316 0 : alpha = 0; /* initialize just to avoid compiler warning */
317 0 : x0 = &synth_ltp[-pitch_int + inter_filter_shift[fs_idx]];
318 0 : y0 = synth;
319 :
320 0 : alpha = 0; move16();
321 0 : IF (fade != 0)
322 : {
323 0 : if (fade < 0)
324 : {
325 0 : alpha = 0x7FFF; move16();
326 : }
327 :
328 : /* step = 1.f/(float)(length); */
329 0 : if (sub(length, 20) == 0)
330 : {
331 0 : step = 1638 /*1.f/20.f Q15*/; move16();
332 : }
333 0 : if (sub(length, 40) == 0)
334 : {
335 0 : step = 819 /*1.f/40.f Q15*/; move16();
336 : }
337 0 : if (sub(length, 60) == 0)
338 : {
339 0 : step = 546 /*1.f/60.f Q15*/; move16();
340 : }
341 0 : if (sub(length, 80) == 0)
342 : {
343 0 : step = 409 /*1.f/80.f Q15*/; move16();
344 : }
345 0 : if (sub(length, 120) == 0)
346 : {
347 0 : step = 273 /*1.f/120.f Q15*/; move16();
348 : }
349 :
350 0 : if (fade < 0)
351 0 : step = negate(step);
352 : }
353 :
354 0 : FOR (j = 0; j < length; j++)
355 : {
356 0 : s = L_mult(x0[0], inter_filter[fs_idx][pitch_fr][0]);
357 0 : FOR (l = 1; l < inter_filter_len[fs_idx]; l++)
358 : {
359 0 : s = L_mac(s, x0[-l], inter_filter[fs_idx][pitch_fr][l]);
360 : }
361 0 : FOR (l = 0; l < tilt_filter_len[fs_idx]; l++)
362 : {
363 0 : s = L_msu(s, y0[-l], tilt_filter[fs_idx][scale_fac_idx][l]);
364 : }
365 :
366 0 : i = msu_r(s, y0[-l], tilt_filter[fs_idx][scale_fac_idx][l]);
367 :
368 0 : k = mult_r(gain, i);
369 :
370 0 : if (fade != 0)
371 0 : k = mult_r(k, alpha);
372 :
373 0 : synth_ltp[j] = add(synth[j], k); move16();
374 :
375 0 : if (fade != 0)
376 0 : alpha = add(alpha, step);
377 :
378 0 : x0++;
379 0 : y0++;
380 : }
381 :
382 : #ifdef DYNMEM_COUNT
383 : Dyn_Mem_Out();
384 : #endif
385 0 : }
386 :
|
