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 "options.h"
11 : #include "wmc_auto.h"
12 : #include "functions.h"
13 :
14 : static void pvq_dec(LC3_INT k, LC3_INT m, LC3_INT LS_ind, LC3_INT MPVQ_ind, LC3_INT* pulses);
15 : static LC3_INT find_last_indice_le(LC3_INT compare, const LC3_INT* array, LC3_INT len);
16 : static void idct_II(LC3_FLOAT* in, LC3_FLOAT* out, LC3_INT len);
17 :
18 2367553 : void idct_II(LC3_FLOAT* in, LC3_FLOAT* out, LC3_INT len)
19 : {
20 : LC3_INT i;
21 : LC3_FLOAT norm1, sum;
22 :
23 2367553 : norm1 = 0.353553390593274; /* sqrt(2 / 16) */
24 :
25 40248401 : for (i = 0; i < len; i++) {
26 37880848 : sum = mac_loop(in, idct_lookup[i], len);
27 37880848 : out[i] = norm1 * sum;
28 : }
29 2367553 : }
30 :
31 14754117 : static LC3_INT pvq_pulse_search(LC3_FLOAT *xabs, LC3_FLOAT *ener, LC3_FLOAT *corr, LC3_INT *y, LC3_INT start, LC3_INT end)
32 : {
33 : LC3_INT i;
34 : LC3_INT nBest;
35 : LC3_FLOAT bestCorrSq, bestEn;
36 : LC3_FLOAT corrSq, currCorr, currEn;
37 :
38 14754117 : nBest = 0;
39 14754117 : bestCorrSq = 0.0;
40 14754117 : bestEn = 0.0;
41 :
42 14754117 : *ener += 1; // Added once for the entire loop
43 :
44 14754117 : i = start;
45 :
46 14754117 : currCorr = *corr + xabs[i];
47 14754117 : currEn = *ener + (2 * y[i]);
48 :
49 14754117 : corrSq = currCorr * currCorr;
50 :
51 14754117 : bestEn = currEn;
52 14754117 : bestCorrSq = corrSq;
53 14754117 : nBest = i;
54 :
55 : /* Iterative max search as recommended in the spec */
56 212352663 : for (; i < end; i++)
57 : {
58 197598546 : currCorr = *corr + xabs[i];
59 197598546 : currEn = *ener + (2 * y[i]);
60 :
61 197598546 : corrSq = currCorr * currCorr;
62 :
63 197598546 : if ((corrSq * bestEn) > (bestCorrSq * currEn))
64 : {
65 30747475 : bestEn = currEn;
66 30747475 : bestCorrSq = corrSq;
67 30747475 : nBest = i;
68 : }
69 : }
70 :
71 14754117 : *corr += xabs[nBest];
72 14754117 : *ener += (2 * y[nBest]);
73 :
74 14754117 : y[nBest] += 1; /* Add the selected unit pulse */
75 :
76 14754117 : return nBest;
77 : }
78 :
79 4757244 : static void pvq_enc_vec_normalize(LC3_FLOAT *vec, LC3_INT N)
80 : {
81 4757244 : LC3_FLOAT mag = 0.0, norm_fac;
82 : LC3_INT i;
83 :
84 80873148 : for (i = 0; i < N; i++)
85 : {
86 76115904 : mag += (vec[i] * vec[i]);
87 : }
88 :
89 4757244 : norm_fac = 1.0 / LC3_SQRT(mag);
90 :
91 80873148 : for (i = 0; i < N; i++)
92 : {
93 76115904 : vec[i] = vec[i] * norm_fac;
94 : }
95 :
96 4757244 : return;
97 : }
98 :
99 1189311 : static void pvq_enc_search(LC3_FLOAT* x_in, LC3_INT y[4][M])
100 : {
101 : LC3_INT i, N, K, pulse_total, N_setA;
102 : LC3_FLOAT abs_sum, projfac;
103 : LC3_FLOAT xabs[16];
104 : LC3_FLOAT yy, xy;
105 :
106 1189311 : abs_sum = 0.0;
107 :
108 : /* Step 1 : Projection to pyramid N=16, K=6 */
109 1189311 : N = 16;
110 1189311 : K = 6;
111 1189311 : pulse_total = 0;
112 1189311 : N_setA = 10;
113 :
114 1189311 : yy = xy = 0.0f;
115 :
116 20218287 : for (i = 0; i < N; i++)
117 : {
118 19028976 : xabs[i] = LC3_FABS(x_in[i]);
119 19028976 : abs_sum += xabs[i];
120 : }
121 :
122 1189311 : projfac = (K - 1) / abs_sum;
123 :
124 20218287 : for (i = 0; i < N; i++)
125 : {
126 19028976 : y[3][i] = floor(xabs[i] * projfac);
127 :
128 19028976 : pulse_total += y[3][i];
129 :
130 19028976 : yy += (y[3][i] * y[3][i]);
131 19028976 : xy += (xabs[i] * y[3][i]);
132 : }
133 :
134 : /* Step 2: Adding unit pulses up to K = 6 */
135 7946459 : for (; pulse_total < K; pulse_total++)
136 : {
137 6757148 : pvq_pulse_search(xabs, &yy, &xy, y[3], 0, N);
138 : }
139 :
140 : /* Step 3: Adding unit pulses up to K = 8 */
141 1189311 : memcpy(y[2], y[3], sizeof(LC3_INT)*N);
142 1189311 : K = 8;
143 :
144 3567933 : for (; pulse_total < K; pulse_total++)
145 : {
146 2378622 : pvq_pulse_search(xabs, &yy, &xy, y[2], 0, N);
147 : }
148 :
149 1189311 : memcpy(y[1], y[2], sizeof(LC3_INT)*N_setA);
150 :
151 : /* Step 4: Remove unit pulses not belonging to set A */
152 8325177 : for (i = N_setA; i < N; i++)
153 : {
154 7135866 : y[1][i] = 0;
155 : }
156 :
157 : /* Step 5: Update yy and xy terms to reflect y1 */
158 1189311 : yy = 0;
159 1189311 : xy = 0;
160 1189311 : pulse_total = 0;
161 :
162 13082421 : for (i = 0; i < N_setA; i++)
163 : {
164 11893110 : yy += (y[1][i] * y[1][i]);
165 11893110 : xy += (xabs[i] * y[1][i]);
166 :
167 11893110 : pulse_total += y[1][i];
168 : }
169 :
170 : /* Step 6: Add unit pulses until K = 10 over N = 10 */
171 1189311 : K = 10;
172 5618347 : for (; pulse_total < K; pulse_total++)
173 : {
174 4429036 : pvq_pulse_search(xabs, &yy, &xy, y[1], 0, N_setA);
175 : }
176 :
177 1189311 : memcpy(y[0], y[1], sizeof(LC3_INT)*N);
178 :
179 : /* Step 7: Add unit pulses until K = 1 over N = 6 in set B*/
180 1189311 : pvq_pulse_search(xabs, &yy, &xy, y[0], N_setA, N);
181 :
182 : /* Step 8: Add signs to each of the 4 vectors from x */
183 20218287 : for (i = 0; i < N; i++)
184 : {
185 19028976 : if (x_in[i] < 0)
186 : {
187 9389325 : y[0][i] = -y[0][i];
188 9389325 : y[1][i] = -y[1][i];
189 9389325 : y[2][i] = -y[2][i];
190 9389325 : y[3][i] = -y[3][i];
191 : }
192 : }
193 :
194 1189311 : return;
195 : }
196 :
197 21407598 : static inline LC3_FLOAT calc_mse(LC3_FLOAT *t2rot, LC3_FLOAT *y, LC3_FLOAT gain, LC3_INT N)
198 : {
199 : LC3_FLOAT mse;
200 : LC3_INT i;
201 :
202 21407598 : mse = 0.0;
203 :
204 363929166 : for (i = 0; i < N; i++)
205 : {
206 342521568 : LC3_FLOAT err = (t2rot[i] - gain * y[i]);
207 342521568 : mse += (err * err);
208 : }
209 :
210 21407598 : return mse;
211 : }
212 :
213 1189311 : static void sns_quant_adj_gain_shape_search(LC3_FLOAT *t2rot, LC3_INT y[4][M] ,
214 : LC3_INT *gain_idx, LC3_INT *shape_idx, LC3_FLOAT *y_norm, LC3_FLOAT *scq_gain)
215 : {
216 : LC3_INT gidx, sidx;
217 : LC3_FLOAT min_mse, mse;
218 : LC3_INT N;
219 : LC3_FLOAT yCur[4][16];
220 : LC3_INT i;
221 :
222 1189311 : const LC3_INT gain_levels[4] = { 2, 4, 4, 8 };
223 1189311 : const LC3_FLOAT *sns_vq_gains[4] = { sns_vq_reg_adj_gains_fl , sns_vq_reg_lf_adj_gains_fl ,
224 : sns_vq_near_adj_gains_fl , sns_vq_far_adj_gains_fl };
225 :
226 1189311 : min_mse = -1.0;
227 1189311 : N = 16;
228 :
229 :
230 1189311 : *gain_idx = *shape_idx = 0;
231 :
232 5946555 : for (sidx = 0; sidx < 4; sidx++)
233 : {
234 80873148 : for (i = 0; i < N; i++)
235 : {
236 76115904 : yCur[sidx][i] = (LC3_FLOAT)y[sidx][i];
237 : }
238 :
239 : /* Step 9: Normalize the vectors */
240 4757244 : pvq_enc_vec_normalize(yCur[sidx], N);
241 :
242 26164842 : for (gidx = 0; gidx < gain_levels[sidx]; gidx++)
243 : {
244 21407598 : mse = calc_mse(t2rot, yCur[sidx], sns_vq_gains[sidx][gidx], N);
245 :
246 21407598 : if ((mse < min_mse) || (min_mse < 0))
247 : {
248 3098680 : *gain_idx = gidx;
249 3098680 : *shape_idx = sidx;
250 3098680 : min_mse = mse;
251 : }
252 : }
253 : }
254 :
255 20218287 : for (i = 0; i < N; i++)
256 : {
257 19028976 : y_norm[i] = yCur[*shape_idx][i];
258 : }
259 :
260 1189311 : *scq_gain = sns_vq_gains[*shape_idx][*gain_idx];
261 :
262 1189311 : return;
263 : }
264 :
265 18013878 : static void enc_push_sign(LC3_FLOAT val, LC3_UINT32 *next_sign_ind, LC3_INT *index)
266 : {
267 18013878 : if (((*next_sign_ind & 0x80000000U) == 0) && (val != 0)) {
268 7048768 : *index = 2 * (*index) + *next_sign_ind;
269 : }
270 18013878 : if (val < 0) {
271 4119087 : *next_sign_ind = 1;
272 : }
273 18013878 : if (val > 0) {
274 4549380 : *next_sign_ind = 0;
275 : }
276 :
277 18013878 : return;
278 : }
279 :
280 1619699 : static void MPVQ_enum(LC3_INT dim, LC3_INT *sns_vec, LC3_INT *index_val, LC3_INT *lead_sign_ind)
281 : {
282 : LC3_UINT32 next_sign_ind;
283 : LC3_INT k_val_acc;
284 : LC3_INT pos;
285 : LC3_INT index, n;
286 : LC3_INT const *row_ptr;
287 :
288 : /* MPVQ-index composition loop */
289 : LC3_INT tmp_h_row;
290 : LC3_INT tmp_val;
291 :
292 1619699 : next_sign_ind = 0x80000000U;
293 1619699 : k_val_acc = 0;
294 1619699 : pos = dim;
295 1619699 : index = 0;
296 1619699 : n = 0;
297 :
298 1619699 : row_ptr = (LC3_INT const *)&(pvq_enc_A[n]);
299 1619699 : tmp_h_row = row_ptr[0];
300 :
301 19633577 : for (pos--; pos >= 0; pos--)
302 : {
303 18013878 : tmp_val = sns_vec[pos];
304 18013878 : enc_push_sign(tmp_val, &next_sign_ind, &index);
305 :
306 18013878 : index += tmp_h_row;
307 18013878 : k_val_acc += abs(tmp_val);
308 18013878 : if (pos != 0) {
309 16394179 : n += 1; /* switch row in offset table MPVQ_offsets(n, k) */
310 : }
311 18013878 : row_ptr = (LC3_INT const *)&(pvq_enc_A[n]);
312 :
313 18013878 : tmp_h_row = row_ptr[k_val_acc];
314 : }
315 :
316 1619699 : *index_val = index;
317 1619699 : *lead_sign_ind = next_sign_ind;
318 :
319 1619699 : return;
320 : }
321 :
322 2378622 : static LC3_INT MSEsearch (LC3_FLOAT *scf, const LC3_FLOAT sns_CB[8][32])
323 : {
324 : LC3_FLOAT distance, mse;
325 : LC3_INT i, n, ind;
326 :
327 2378622 : ind = 0;
328 :
329 2378622 : distance = (LC3_FLOAT) LC3_CONST_POW_2_100;
330 78494526 : for (i = 0; i < 32; i++) {
331 76115904 : mse = 0;
332 685043136 : for (n = 0; n < 8; n++) {
333 608927232 : mse += (scf[n] - sns_CB[n][i]) * (scf[n] - sns_CB[n][i]);
334 : }
335 :
336 76115904 : if (mse < distance) {
337 10077826 : distance = mse;
338 10077826 : ind = i;
339 : }
340 : }
341 2378622 : return ind;
342 : }
343 :
344 1189311 : void process_snsQuantizesScf_Enc(LC3_FLOAT* env, LC3_INT* index, LC3_FLOAT* envq, Dct2 dct2structSNS)
345 : {
346 : LC3_FLOAT stage2_en1_norm_sub[M];
347 : LC3_INT i, j;
348 : LC3_FLOAT st1_vector[M];
349 : LC3_FLOAT pvq_target_pre[M];
350 : LC3_FLOAT pvq_target[M];
351 : LC3_FLOAT stage2_en1_norm_pre_sub[M];
352 : LC3_INT gain, shape;
353 : LC3_FLOAT scfq_gain;
354 : LC3_INT y[4][M];
355 :
356 : /* Stage 1 split VQ */
357 1189311 : index[0] = MSEsearch(&env[0], sns_LFCB); /* ind_LF */
358 1189311 : index[1] = MSEsearch(&env[8], sns_HFCB); /* ind_HF */
359 :
360 1189311 : j = 8;
361 10703799 : for (i = 0; i < 8; i++, j++) {
362 9514488 : st1_vector[i] = sns_LFCB[i][index[0]];
363 9514488 : st1_vector[j] = sns_HFCB[i][index[1]];
364 : }
365 :
366 : /* STAGE 2 */
367 20218287 : for (i = 0; i < 16; i++) {
368 19028976 : pvq_target_pre[i] = env[i] - st1_vector[i];
369 : }
370 :
371 1189311 : dct2_apply(&dct2structSNS, pvq_target_pre, pvq_target);
372 1189311 : pvq_enc_search(pvq_target, y);
373 1189311 : sns_quant_adj_gain_shape_search(pvq_target, y, &gain, &shape, stage2_en1_norm_pre_sub, &scfq_gain);
374 :
375 : /* Inverse transform */
376 1189311 : idct_II(stage2_en1_norm_pre_sub, stage2_en1_norm_sub, M);
377 :
378 1189311 : index[2] = shape;
379 1189311 : index[3] = gain;
380 :
381 1189311 : if (shape < 2) {
382 599571 : MPVQ_enum(10, y[shape], &index[5], &index[4]);
383 : }
384 : else {
385 589740 : MPVQ_enum(M, y[shape], &index[5], &index[4]);
386 : }
387 :
388 1189311 : if (shape == 0) {
389 : LC3_INT ls_ind, ind;
390 430388 : MPVQ_enum(6, &y[shape][10], &ind, &ls_ind);
391 430388 : index[6] = ind * 2 + ls_ind;
392 : }
393 758923 : else if (shape == 2) {
394 543350 : index[6] = -1;
395 : }
396 : else {
397 215573 : index[6] = -2;
398 : }
399 :
400 20218287 : for (i = 0; i < M; i++) {
401 19028976 : envq[i] = st1_vector[i] + (stage2_en1_norm_sub[i] * scfq_gain);
402 : }
403 1189311 : }
404 :
405 13545181 : LC3_INT find_last_indice_le(LC3_INT compare, const LC3_INT* array, LC3_INT len)
406 : {
407 13545181 : LC3_INT idx = 0, i = 0;
408 :
409 92933643 : for (i = 0; i < len; i++) {
410 79388462 : if (compare >= array[i]) {
411 70206524 : idx++;
412 : }
413 : }
414 :
415 13545181 : if (idx > 0) {
416 13545181 : idx--;
417 : }
418 :
419 13545181 : return idx;
420 : }
421 :
422 1604706 : void pvq_dec(LC3_INT k, LC3_INT m, LC3_INT LS_ind, LC3_INT MPVQ_ind, LC3_INT* pulses)
423 : {
424 1604706 : LC3_INT leading_sign, idx, k_delta = 0, pos;
425 :
426 1604706 : leading_sign = 1 - 2 * LS_ind;
427 :
428 : /* Decoding loop */
429 :
430 15149887 : for (pos = 0; pos < m; pos++) {
431 15149887 : if (MPVQ_ind != 0) {
432 : /* Find last indice */
433 13545181 : idx = find_last_indice_le(MPVQ_ind, &pvq_enc_A[m - pos - 1][0], k + 1);
434 13545181 : MPVQ_ind = MPVQ_ind - pvq_enc_A[m - pos - 1][idx];
435 13545181 : k_delta = k - idx;
436 : } else {
437 1604706 : pulses[pos] = leading_sign * k;
438 1604706 : break;
439 : }
440 :
441 13545181 : if (k_delta != 0) {
442 6986852 : pulses[pos] = leading_sign * k_delta;
443 6986852 : if ((MPVQ_ind % 2) != 0) {
444 3279384 : leading_sign = -1;
445 : } else {
446 3707468 : leading_sign = 1;
447 : }
448 :
449 6986852 : MPVQ_ind = floor(MPVQ_ind / 2);
450 6986852 : k = k - k_delta;
451 : }
452 : }
453 1604706 : }
454 :
455 1178242 : void process_snsQuantizesScf_Dec(LC3_INT* scf_idx, LC3_FLOAT* scf_q)
456 : {
457 : LC3_INT i, submode;
458 1178242 : LC3_INT pulses2[6] = {0}, pulses[M] = {0};
459 1178242 : LC3_FLOAT st2_vector[M], st2_vector_idct[M], sum = 0;
460 :
461 : /* Decode first stage */
462 :
463 10604178 : for (i = 0; i < 8; i++) {
464 9425936 : scf_q[i] = sns_LFCB[i][scf_idx[0]];
465 9425936 : scf_q[i + 8] = sns_HFCB[i][scf_idx[1]];
466 : }
467 :
468 : /* STAGE 2 */
469 : /* Decode submode */
470 :
471 1178242 : submode = scf_idx[2];
472 :
473 : /* Decode pulses */
474 :
475 1178242 : if (submode < 2) {
476 592992 : pvq_dec(10, 10, scf_idx[4], scf_idx[5], pulses);
477 :
478 592992 : if (submode == 0) {
479 426464 : pvq_dec(1, 6, (scf_idx[6] % 2), floor(scf_idx[6] / 2), pulses2);
480 :
481 426464 : move_int(&pulses[10], pulses2, 6);
482 :
483 : } else {
484 166528 : pulses[15] = 0;
485 : }
486 585250 : } else if (submode == 2) {
487 539201 : pvq_dec(8, 16, scf_idx[4], scf_idx[5], pulses);
488 : } else {
489 46049 : pvq_dec(6, 16, scf_idx[4], scf_idx[5], pulses);
490 : }
491 :
492 : /* Normalization */
493 :
494 20030114 : for (i = 0; i < M; i++) {
495 18851872 : sum += pulses[i] * pulses[i];
496 : }
497 :
498 1178242 : sum = 1.0 / LC3_SQRT(sum);
499 :
500 20030114 : for (i = 0; i < M; i++) {
501 18851872 : st2_vector[i] = pulses[i] * sum;
502 : }
503 :
504 : /* Inverse transform */
505 1178242 : idct_II(st2_vector, st2_vector_idct, M);
506 :
507 : /* Gain */
508 : /* Add stage 1 and stage 2 */
509 20030114 : for (i = 0; i < M; i++) {
510 18851872 : scf_q[i] += st2_vector_idct[i] * sns_dec_gains[submode][scf_idx[3]];
511 : }
512 1178242 : }
|
