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 : /*************************************************************************/
13 :
14 :
15 0 : void process_ltpf_coder_fx(Word16 *bits, Word16 ol_pitch, Word16 ltpf_enable, Word16 *mem_in_exp, Word16 mem_in[],
16 : Word16 mem_in_len, Word16 param[], Word16 *xin, Word16 len, Word16 *mem_normcorr,
17 : Word16 *mem_mem_normcorr, Word16 ol_normcorr, Word16 *mem_ltpf_on, Word16 *mem_ltpf_pitch,
18 : Word16 xin_exp, Word16 frame_dms, Word8 *scratchBuffer
19 : , Word16 hrmode
20 : )
21 : {
22 : Word16 pitch_index, scale0, scale1, scale2, *x, x_exp, shift, prod_exp, ltpf_pitch;
23 : Word32 L_tmp, cor_max32, sum0, sum1, sum2, prod, inv;
24 : Word32 *ac32;
25 : Word16 *ac, *currFrame, *predFrame;
26 : Word16 min_pitch, max_pitch, ac_min_pitch, ac_max_pitch, ac_max;
27 : Word16 pitch, pitch_res, min_pitch_fr, pitch_int, pitch_fr, norm_corr, ltpf_active;
28 : Counter n, m, fr;
29 : Word16 tmp, acflen;
30 :
31 : #ifdef DYNMEM_COUNT
32 : Dyn_Mem_In("process_ltpf_coder_fx", sizeof(struct {
33 : Word16 pitch_index, scale0, scale1, scale2, *x, x_exp, shift, prod_exp, ltpf_pitch;
34 : Word32 L_tmp, cor_max32, sum0, sum1, sum2, prod, inv;
35 : Word32 *ac32;
36 : Word16 *ac, *currFrame, *predFrame;
37 : Word16 min_pitch, max_pitch, ac_min_pitch, ac_max_pitch, ac_max;
38 : Word16 pitch, pitch_res, min_pitch_fr, pitch_int, pitch_fr, norm_corr, ltpf_active;
39 : Counter n, m, fr;
40 : Word16 tmp, acflen;
41 : }));
42 : #endif
43 :
44 :
45 :
46 0 : ac32 = (Word32 *)scratchAlign(scratchBuffer, 0); /* Size = 4 * 17 = 68 bytes; */
47 0 : ac = (Word16 *)scratchAlign(ac32, sizeof(*ac32) * 17); /* Size = 2 * 17 = 34 bytes */
48 0 : currFrame = (Word16 *)scratchAlign(scratchBuffer, 0); /* Size = 2 * 128 = 256 bytes */
49 0 : predFrame = (Word16 *)scratchAlign(currFrame, sizeof(*currFrame) * LEN_12K8); /* Size = 2 * 128 = 256 bytes */
50 : /* Buffers 'overlap' since they are not used at the same time */ /* Total size used = 512 bytes */
51 :
52 0 : ltpf_active = 0; move16();
53 0 : norm_corr = 0; move16();
54 :
55 : /* Input buffer */
56 0 : x = mem_in + mem_in_len;
57 :
58 0 : basop_memmove(x, xin, (len + 1) * sizeof(Word16));
59 :
60 0 : ASSERT(mem_in_len + len + 1 <= LTPF_MEMIN_LEN + LEN_12K8 + 1);
61 :
62 : /* Scaling */
63 0 : scale0 = sub(getScaleFactor16_0(mem_in, mem_in_len), 3);
64 0 : *mem_in_exp = sub(*mem_in_exp, scale0); move16();
65 0 : scale1 = sub(getScaleFactor16_0(x, len + 1), 3);
66 0 : x_exp = sub(xin_exp, scale1);
67 0 : scale2 = sub(*mem_in_exp, x_exp);
68 0 : IF (scale2 > 0)
69 : {
70 0 : Scale_sig(x, len + 1, sub(scale1, scale2));
71 0 : Scale_sig(mem_in, mem_in_len, scale0);
72 0 : x_exp = *mem_in_exp; move16();
73 : }
74 : ELSE
75 : {
76 0 : Scale_sig(x, len + 1, scale1);
77 0 : Scale_sig(mem_in, mem_in_len, add(scale0, scale2));
78 0 : *mem_in_exp = x_exp; move16();
79 : }
80 :
81 0 : Word32 normCorrTh = 0;
82 0 : if (hrmode) {
83 0 : normCorrTh = 13107;
84 : } else {
85 0 : normCorrTh = 19660;
86 : }
87 :
88 0 : IF (sub(ol_normcorr, normCorrTh) > 0)
89 : {
90 : /* Autocorrelation Bounds */
91 0 : min_pitch = sub(ol_pitch, 4);
92 0 : max_pitch = add(ol_pitch, 4);
93 0 : min_pitch = s_max(min_pitch, MIN_PITCH_12K8);
94 0 : max_pitch = s_min(max_pitch, MAX_PITCH_12K8);
95 0 : ac_min_pitch = sub(min_pitch, 4);
96 0 : ac_max_pitch = add(max_pitch, 4);
97 0 : acflen = len; move16();
98 0 : if (sub(frame_dms, 25) == 0)
99 : {
100 0 : acflen = shl(len, 1);
101 0 : x = x - len;
102 : }
103 :
104 : /* Compute norm */
105 0 : sum1 = L_mac0(1, x[0], x[0]);
106 0 : sum2 = L_mac0(1, x[-ac_min_pitch], x[-ac_min_pitch]);
107 0 : FOR (m = 1; m < acflen; m++)
108 : {
109 0 : sum1 = L_mac0(sum1, x[m], x[m]);
110 0 : sum2 = L_mac0(sum2, x[m - ac_min_pitch], x[m - ac_min_pitch]);
111 : }
112 0 : scale1 = norm_l(sum1);
113 0 : sum1 = L_shl_pos(sum1, scale1);
114 :
115 : /* Compute Autocorrelation */
116 0 : FOR (n = ac_min_pitch; n <= ac_max_pitch; n++)
117 : {
118 0 : sum0 = L_mac0(0L, x[0], x[0 - n]);
119 0 : FOR (m = 1; m < acflen; m++)
120 : {
121 0 : sum0 = L_mac0(sum0, x[m], x[m - n]);
122 : }
123 0 : if (n > ac_min_pitch)
124 : {
125 0 : sum2 = L_msu0(sum2, x[acflen - 1 - (n - 1)], x[acflen - 1 - (n - 1)]);
126 0 : sum2 = L_mac0_sat(sum2, x[-n], x[-n]);
127 : }
128 0 : scale2 = norm_l(sum2);
129 0 : L_tmp = L_shl_pos(sum2, scale2);
130 0 : prod = Mpy_32_32_lc3plus(sum1, L_tmp);
131 0 : shift = norm_l(prod);
132 0 : prod = L_shl_pos(prod, shift);
133 0 : prod_exp = sub(62, add(add(scale1, scale2), shift));
134 0 : inv = Isqrt_lc3plus(prod, &prod_exp);
135 0 : scale0 = norm_l(sum0);
136 0 : sum0 = L_shl_pos(sum0, scale0);
137 0 : prod = Mpy_32_32_lc3plus(sum0, inv);
138 0 : prod_exp = add(sub(31, scale0), prod_exp);
139 0 : test();
140 0 : IF (prod == 0 || sub(norm_l(prod), prod_exp) >= 0)
141 : {
142 0 : ac[n - ac_min_pitch] = s_max(0, round_fx_sat(L_shl_sat(prod, prod_exp))); move16();
143 : }
144 : ELSE
145 : {
146 0 : ac[n - ac_min_pitch] = 32767; move16();
147 : }
148 : }
149 :
150 : /* Find maximum */
151 0 : ac_max = ac[min_pitch - ac_min_pitch]; move16();
152 0 : pitch = min_pitch; move16();
153 0 : FOR (n = min_pitch + 1; n <= max_pitch; n++)
154 : {
155 0 : tmp = sub_sat(ac[n - ac_min_pitch], ac_max);
156 0 : if (tmp > 0)
157 : {
158 0 : pitch = n; move16();
159 : }
160 0 : ac_max = s_max(ac_max, ac[n - ac_min_pitch]);
161 : }
162 0 : pitch_int = pitch; move16();
163 0 : pitch_fr = 0; move16();
164 0 : pitch_index = add(pitch_int, 283);
165 :
166 : /* If the pitch is low -> estimate a fractional part */
167 0 : IF (sub(pitch, RES2_PITCH_12K8) < 0)
168 : {
169 0 : IF (sub(pitch, RES4_PITCH_12K8) < 0)
170 : {
171 0 : pitch_res = 1; move16();
172 0 : min_pitch_fr = -3; move16();
173 : }
174 : ELSE
175 : {
176 0 : pitch_res = 2; move16();
177 0 : min_pitch_fr = -2; move16();
178 : }
179 0 : if (sub(pitch, min_pitch) == 0)
180 : {
181 0 : min_pitch_fr = 0;
182 : }
183 0 : cor_max32 = MIN_32;
184 0 : FOR (fr = min_pitch_fr; fr < 4; fr += pitch_res)
185 : {
186 0 : sum0 = L_mult0(ac[pitch_int - ac_min_pitch - 4], ltpf_ac_interp_filt[fr + 3][0]);
187 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch - 3], ltpf_ac_interp_filt[fr + 3][1]);
188 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch - 2], ltpf_ac_interp_filt[fr + 3][2]);
189 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch - 1], ltpf_ac_interp_filt[fr + 3][3]);
190 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch + 0], ltpf_ac_interp_filt[fr + 3][4]);
191 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch + 1], ltpf_ac_interp_filt[fr + 3][5]);
192 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch + 2], ltpf_ac_interp_filt[fr + 3][6]);
193 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch + 3], ltpf_ac_interp_filt[fr + 3][7]);
194 0 : sum0 = L_mac0(sum0, ac[pitch_int - ac_min_pitch + 4], ltpf_ac_interp_filt[fr + 3][8]);
195 :
196 0 : L_tmp = L_sub_sat(sum0, cor_max32);
197 0 : if (L_tmp > 0)
198 : {
199 0 : pitch_fr = fr; move16();
200 : }
201 0 : cor_max32 = L_max(cor_max32, sum0);
202 : }
203 0 : IF (pitch_fr < 0)
204 : {
205 0 : pitch_int = sub(pitch_int, 1);
206 0 : pitch_fr = add(pitch_fr, 4);
207 : }
208 0 : IF (sub(pitch_int, 127) >= 0)
209 : {
210 0 : pitch_index = add(add(shl_pos(pitch_int, 1), shr_pos(pitch_fr, 1)), 126);
211 : }
212 : ELSE
213 : {
214 0 : pitch_index = sub(add(shl_pos(pitch_int, 2), pitch_fr), 128);
215 : }
216 : }
217 0 : ltpf_pitch = add(shl_pos(pitch_int, 2), pitch_fr);
218 :
219 : /* Filter current and predicted frame */
220 :
221 0 : FOR (n = 0; n < acflen; n++)
222 : {
223 0 : sum0 = L_mult(x[n + 1], inter_filter[0][0][0]);
224 0 : sum0 = L_mac(sum0, x[n], inter_filter[0][0][1]);
225 0 : currFrame[n] = mac_r(sum0, x[n - 1], inter_filter[0][0][2]);
226 :
227 0 : sum0 = L_mult(x[n - pitch_int + 1], inter_filter[0][pitch_fr][0]);
228 0 : sum0 = L_mac(sum0, x[n - pitch_int], inter_filter[0][pitch_fr][1]);
229 0 : sum0 = L_mac(sum0, x[n - pitch_int - 1], inter_filter[0][pitch_fr][2]);
230 0 : predFrame[n] = mac_r(sum0, x[n - pitch_int - 2], inter_filter[0][pitch_fr][3]);
231 : }
232 :
233 : /* Normalized Correlation */
234 0 : sum0 = L_mult0(currFrame[0], predFrame[0]);
235 0 : sum1 = L_mac0(1, predFrame[0], predFrame[0]);
236 0 : sum2 = L_mac0(1, currFrame[0], currFrame[0]);
237 0 : for (m = 1; m < acflen; m++)
238 : {
239 0 : sum0 = L_mac0(sum0, currFrame[m], predFrame[m]);
240 0 : sum1 = L_mac0(sum1, predFrame[m], predFrame[m]);
241 0 : sum2 = L_mac0(sum2, currFrame[m], currFrame[m]);
242 : }
243 :
244 0 : scale1 = norm_l(sum1);
245 0 : scale2 = norm_l(sum2);
246 0 : sum1 = L_shl_pos(sum1, scale1);
247 0 : sum2 = L_shl_pos(sum2, scale2);
248 0 : prod = Mpy_32_32_lc3plus(sum1, sum2);
249 0 : shift = norm_l(prod);
250 0 : prod = L_shl_pos(prod, shift);
251 0 : prod_exp = sub(62, add(add(scale1, scale2), shift));
252 0 : inv = Isqrt_lc3plus(prod, &prod_exp);
253 0 : scale0 = norm_l(sum0);
254 0 : sum0 = L_shl_pos(sum0, scale0);
255 0 : prod = Mpy_32_32_lc3plus(sum0, inv);
256 0 : prod_exp = add(sub(31, scale0), prod_exp);
257 0 : test();
258 0 : IF (prod == 0 || sub(norm_l(prod), prod_exp) >= 0)
259 : {
260 0 : norm_corr = s_max(0, round_fx_sat(L_shl_sat(prod, prod_exp))); move16();
261 : }
262 : ELSE
263 : {
264 0 : norm_corr = 32767; move16();
265 : }
266 0 : if (norm_corr < 0)
267 : {
268 0 : norm_corr = 0;
269 : }
270 :
271 0 : IF (sub(ltpf_enable, 1) == 0)
272 : {
273 0 : test(); test(); test(); test();
274 : /* Decision if lptf active */
275 0 : IF ((*mem_ltpf_on == 0 && sub(*mem_normcorr, 30802) > 0 && sub(norm_corr, 30802) > 0 &&
276 : (sub(frame_dms, 100) == 0 || sub(*mem_mem_normcorr, 30802) > 0)) ||
277 : (sub(*mem_ltpf_on, 1) == 0 && sub(norm_corr, 29491) > 0) ||
278 : (sub(*mem_ltpf_on, 1) == 0 && sub(abs_s(sub(ltpf_pitch, *mem_ltpf_pitch)), 8) < 0 &&
279 : add(sub(norm_corr, *mem_normcorr), 3277) > 0 && sub(norm_corr, 27525) > 0))
280 : {
281 0 : ltpf_active = 1; move16();
282 : }
283 : }
284 :
285 0 : param[0] = 1; move16();
286 0 : param[1] = ltpf_active; move16();
287 0 : param[2] = pitch_index; move16();
288 0 : *bits = 11; move16();
289 : }
290 : ELSE
291 : {
292 0 : norm_corr = ol_normcorr; move16();
293 0 : param[0] = 0; move16();
294 0 : param[1] = 0; move16();
295 0 : param[2] = 0; move16();
296 0 : *bits = 1; move16();
297 0 : ltpf_pitch = 0; move16();
298 : }
299 :
300 : /* Update memory */
301 0 : FOR (n = 0; n < mem_in_len; n++)
302 : {
303 0 : mem_in[n] = mem_in[n + len]; move16();
304 : }
305 :
306 0 : *mem_mem_normcorr = *mem_normcorr; move16();
307 0 : *mem_normcorr = norm_corr; move16();
308 0 : *mem_ltpf_on = ltpf_active; move16();
309 0 : *mem_ltpf_pitch = ltpf_pitch; move16();
310 :
311 : #ifdef DYNMEM_COUNT
312 : Dyn_Mem_Out();
313 : #endif
314 0 : }
315 :
|
