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 0 : void idct_II(LC3_FLOAT* in, LC3_FLOAT* out, LC3_INT len)
19 : {
20 : LC3_INT i;
21 : LC3_FLOAT norm1, sum;
22 :
23 0 : norm1 = 0.353553390593274; /* sqrt(2 / 16) */
24 :
25 0 : for (i = 0; i < len; i++) {
26 0 : sum = mac_loop(in, idct_lookup[i], len);
27 0 : out[i] = norm1 * sum;
28 : }
29 0 : }
30 :
31 0 : 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 0 : nBest = 0;
39 0 : bestCorrSq = 0.0;
40 0 : bestEn = 0.0;
41 :
42 0 : *ener += 1; // Added once for the entire loop
43 :
44 0 : i = start;
45 :
46 0 : currCorr = *corr + xabs[i];
47 0 : currEn = *ener + (2 * y[i]);
48 :
49 0 : corrSq = currCorr * currCorr;
50 :
51 0 : bestEn = currEn;
52 0 : bestCorrSq = corrSq;
53 0 : nBest = i;
54 :
55 : /* Iterative max search as recommended in the spec */
56 0 : for (; i < end; i++)
57 : {
58 0 : currCorr = *corr + xabs[i];
59 0 : currEn = *ener + (2 * y[i]);
60 :
61 0 : corrSq = currCorr * currCorr;
62 :
63 0 : if ((corrSq * bestEn) > (bestCorrSq * currEn))
64 : {
65 0 : bestEn = currEn;
66 0 : bestCorrSq = corrSq;
67 0 : nBest = i;
68 : }
69 : }
70 :
71 0 : *corr += xabs[nBest];
72 0 : *ener += (2 * y[nBest]);
73 :
74 0 : y[nBest] += 1; /* Add the selected unit pulse */
75 :
76 0 : return nBest;
77 : }
78 :
79 0 : static void pvq_enc_vec_normalize(LC3_FLOAT *vec, LC3_INT N)
80 : {
81 0 : LC3_FLOAT mag = 0.0, norm_fac;
82 : LC3_INT i;
83 :
84 0 : for (i = 0; i < N; i++)
85 : {
86 0 : mag += (vec[i] * vec[i]);
87 : }
88 :
89 0 : norm_fac = 1.0 / LC3_SQRT(mag);
90 :
91 0 : for (i = 0; i < N; i++)
92 : {
93 0 : vec[i] = vec[i] * norm_fac;
94 : }
95 :
96 0 : return;
97 : }
98 :
99 0 : 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 0 : abs_sum = 0.0;
107 :
108 : /* Step 1 : Projection to pyramid N=16, K=6 */
109 0 : N = 16;
110 0 : K = 6;
111 0 : pulse_total = 0;
112 0 : N_setA = 10;
113 :
114 0 : yy = xy = 0.0f;
115 :
116 0 : for (i = 0; i < N; i++)
117 : {
118 0 : xabs[i] = LC3_FABS(x_in[i]);
119 0 : abs_sum += xabs[i];
120 : }
121 :
122 0 : projfac = (K - 1) / abs_sum;
123 :
124 0 : for (i = 0; i < N; i++)
125 : {
126 0 : y[3][i] = floor(xabs[i] * projfac);
127 :
128 0 : pulse_total += y[3][i];
129 :
130 0 : yy += (y[3][i] * y[3][i]);
131 0 : xy += (xabs[i] * y[3][i]);
132 : }
133 :
134 : /* Step 2: Adding unit pulses up to K = 6 */
135 0 : for (; pulse_total < K; pulse_total++)
136 : {
137 0 : pvq_pulse_search(xabs, &yy, &xy, y[3], 0, N);
138 : }
139 :
140 : /* Step 3: Adding unit pulses up to K = 8 */
141 0 : memcpy(y[2], y[3], sizeof(LC3_INT)*N);
142 0 : K = 8;
143 :
144 0 : for (; pulse_total < K; pulse_total++)
145 : {
146 0 : pvq_pulse_search(xabs, &yy, &xy, y[2], 0, N);
147 : }
148 :
149 0 : memcpy(y[1], y[2], sizeof(LC3_INT)*N_setA);
150 :
151 : /* Step 4: Remove unit pulses not belonging to set A */
152 0 : for (i = N_setA; i < N; i++)
153 : {
154 0 : y[1][i] = 0;
155 : }
156 :
157 : /* Step 5: Update yy and xy terms to reflect y1 */
158 0 : yy = 0;
159 0 : xy = 0;
160 0 : pulse_total = 0;
161 :
162 0 : for (i = 0; i < N_setA; i++)
163 : {
164 0 : yy += (y[1][i] * y[1][i]);
165 0 : xy += (xabs[i] * y[1][i]);
166 :
167 0 : pulse_total += y[1][i];
168 : }
169 :
170 : /* Step 6: Add unit pulses until K = 10 over N = 10 */
171 0 : K = 10;
172 0 : for (; pulse_total < K; pulse_total++)
173 : {
174 0 : pvq_pulse_search(xabs, &yy, &xy, y[1], 0, N_setA);
175 : }
176 :
177 0 : 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 0 : 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 0 : for (i = 0; i < N; i++)
184 : {
185 0 : if (x_in[i] < 0)
186 : {
187 0 : y[0][i] = -y[0][i];
188 0 : y[1][i] = -y[1][i];
189 0 : y[2][i] = -y[2][i];
190 0 : y[3][i] = -y[3][i];
191 : }
192 : }
193 :
194 0 : return;
195 : }
196 :
197 0 : 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 0 : mse = 0.0;
203 :
204 0 : for (i = 0; i < N; i++)
205 : {
206 0 : LC3_FLOAT err = (t2rot[i] - gain * y[i]);
207 0 : mse += (err * err);
208 : }
209 :
210 0 : return mse;
211 : }
212 :
213 0 : 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 0 : const LC3_INT gain_levels[4] = { 2, 4, 4, 8 };
223 0 : 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 0 : min_mse = -1.0;
227 0 : N = 16;
228 :
229 :
230 0 : *gain_idx = *shape_idx = 0;
231 :
232 0 : for (sidx = 0; sidx < 4; sidx++)
233 : {
234 0 : for (i = 0; i < N; i++)
235 : {
236 0 : yCur[sidx][i] = (LC3_FLOAT)y[sidx][i];
237 : }
238 :
239 : /* Step 9: Normalize the vectors */
240 0 : pvq_enc_vec_normalize(yCur[sidx], N);
241 :
242 0 : for (gidx = 0; gidx < gain_levels[sidx]; gidx++)
243 : {
244 0 : mse = calc_mse(t2rot, yCur[sidx], sns_vq_gains[sidx][gidx], N);
245 :
246 0 : if ((mse < min_mse) || (min_mse < 0))
247 : {
248 0 : *gain_idx = gidx;
249 0 : *shape_idx = sidx;
250 0 : min_mse = mse;
251 : }
252 : }
253 : }
254 :
255 0 : for (i = 0; i < N; i++)
256 : {
257 0 : y_norm[i] = yCur[*shape_idx][i];
258 : }
259 :
260 0 : *scq_gain = sns_vq_gains[*shape_idx][*gain_idx];
261 :
262 0 : return;
263 : }
264 :
265 0 : static void enc_push_sign(LC3_FLOAT val, LC3_UINT32 *next_sign_ind, LC3_INT *index)
266 : {
267 0 : if (((*next_sign_ind & 0x80000000U) == 0) && (val != 0)) {
268 0 : *index = 2 * (*index) + *next_sign_ind;
269 : }
270 0 : if (val < 0) {
271 0 : *next_sign_ind = 1;
272 : }
273 0 : if (val > 0) {
274 0 : *next_sign_ind = 0;
275 : }
276 :
277 0 : return;
278 : }
279 :
280 0 : 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 0 : next_sign_ind = 0x80000000U;
293 0 : k_val_acc = 0;
294 0 : pos = dim;
295 0 : index = 0;
296 0 : n = 0;
297 :
298 0 : row_ptr = (LC3_INT const *)&(pvq_enc_A[n]);
299 0 : tmp_h_row = row_ptr[0];
300 :
301 0 : for (pos--; pos >= 0; pos--)
302 : {
303 0 : tmp_val = sns_vec[pos];
304 0 : enc_push_sign(tmp_val, &next_sign_ind, &index);
305 :
306 0 : index += tmp_h_row;
307 0 : k_val_acc += abs(tmp_val);
308 0 : if (pos != 0) {
309 0 : n += 1; /* switch row in offset table MPVQ_offsets(n, k) */
310 : }
311 0 : row_ptr = (LC3_INT const *)&(pvq_enc_A[n]);
312 :
313 0 : tmp_h_row = row_ptr[k_val_acc];
314 : }
315 :
316 0 : *index_val = index;
317 0 : *lead_sign_ind = next_sign_ind;
318 :
319 0 : return;
320 : }
321 :
322 0 : 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 0 : ind = 0;
328 :
329 0 : distance = (LC3_FLOAT) LC3_CONST_POW_2_100;
330 0 : for (i = 0; i < 32; i++) {
331 0 : mse = 0;
332 0 : for (n = 0; n < 8; n++) {
333 0 : mse += (scf[n] - sns_CB[n][i]) * (scf[n] - sns_CB[n][i]);
334 : }
335 :
336 0 : if (mse < distance) {
337 0 : distance = mse;
338 0 : ind = i;
339 : }
340 : }
341 0 : return ind;
342 : }
343 :
344 0 : 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 0 : index[0] = MSEsearch(&env[0], sns_LFCB); /* ind_LF */
358 0 : index[1] = MSEsearch(&env[8], sns_HFCB); /* ind_HF */
359 :
360 0 : j = 8;
361 0 : for (i = 0; i < 8; i++, j++) {
362 0 : st1_vector[i] = sns_LFCB[i][index[0]];
363 0 : st1_vector[j] = sns_HFCB[i][index[1]];
364 : }
365 :
366 : /* STAGE 2 */
367 0 : for (i = 0; i < 16; i++) {
368 0 : pvq_target_pre[i] = env[i] - st1_vector[i];
369 : }
370 :
371 0 : dct2_apply(&dct2structSNS, pvq_target_pre, pvq_target);
372 0 : pvq_enc_search(pvq_target, y);
373 0 : sns_quant_adj_gain_shape_search(pvq_target, y, &gain, &shape, stage2_en1_norm_pre_sub, &scfq_gain);
374 :
375 : /* Inverse transform */
376 0 : idct_II(stage2_en1_norm_pre_sub, stage2_en1_norm_sub, M);
377 :
378 0 : index[2] = shape;
379 0 : index[3] = gain;
380 :
381 0 : if (shape < 2) {
382 0 : MPVQ_enum(10, y[shape], &index[5], &index[4]);
383 : }
384 : else {
385 0 : MPVQ_enum(M, y[shape], &index[5], &index[4]);
386 : }
387 :
388 0 : if (shape == 0) {
389 : LC3_INT ls_ind, ind;
390 0 : MPVQ_enum(6, &y[shape][10], &ind, &ls_ind);
391 0 : index[6] = ind * 2 + ls_ind;
392 : }
393 0 : else if (shape == 2) {
394 0 : index[6] = -1;
395 : }
396 : else {
397 0 : index[6] = -2;
398 : }
399 :
400 0 : for (i = 0; i < M; i++) {
401 0 : envq[i] = st1_vector[i] + (stage2_en1_norm_sub[i] * scfq_gain);
402 : }
403 0 : }
404 :
405 0 : LC3_INT find_last_indice_le(LC3_INT compare, const LC3_INT* array, LC3_INT len)
406 : {
407 0 : LC3_INT idx = 0, i = 0;
408 :
409 0 : for (i = 0; i < len; i++) {
410 0 : if (compare >= array[i]) {
411 0 : idx++;
412 : }
413 : }
414 :
415 0 : if (idx > 0) {
416 0 : idx--;
417 : }
418 :
419 0 : return idx;
420 : }
421 :
422 0 : void pvq_dec(LC3_INT k, LC3_INT m, LC3_INT LS_ind, LC3_INT MPVQ_ind, LC3_INT* pulses)
423 : {
424 0 : LC3_INT leading_sign, idx, k_delta = 0, pos;
425 :
426 0 : leading_sign = 1 - 2 * LS_ind;
427 :
428 : /* Decoding loop */
429 :
430 0 : for (pos = 0; pos < m; pos++) {
431 0 : if (MPVQ_ind != 0) {
432 : /* Find last indice */
433 0 : idx = find_last_indice_le(MPVQ_ind, &pvq_enc_A[m - pos - 1][0], k + 1);
434 0 : MPVQ_ind = MPVQ_ind - pvq_enc_A[m - pos - 1][idx];
435 0 : k_delta = k - idx;
436 : } else {
437 0 : pulses[pos] = leading_sign * k;
438 0 : break;
439 : }
440 :
441 0 : if (k_delta != 0) {
442 0 : pulses[pos] = leading_sign * k_delta;
443 0 : if ((MPVQ_ind % 2) != 0) {
444 0 : leading_sign = -1;
445 : } else {
446 0 : leading_sign = 1;
447 : }
448 :
449 0 : MPVQ_ind = floor(MPVQ_ind / 2);
450 0 : k = k - k_delta;
451 : }
452 : }
453 0 : }
454 :
455 0 : void process_snsQuantizesScf_Dec(LC3_INT* scf_idx, LC3_FLOAT* scf_q)
456 : {
457 : LC3_INT i, submode;
458 0 : LC3_INT pulses2[6] = {0}, pulses[M] = {0};
459 0 : LC3_FLOAT st2_vector[M], st2_vector_idct[M], sum = 0;
460 :
461 : /* Decode first stage */
462 :
463 0 : for (i = 0; i < 8; i++) {
464 0 : scf_q[i] = sns_LFCB[i][scf_idx[0]];
465 0 : scf_q[i + 8] = sns_HFCB[i][scf_idx[1]];
466 : }
467 :
468 : /* STAGE 2 */
469 : /* Decode submode */
470 :
471 0 : submode = scf_idx[2];
472 :
473 : /* Decode pulses */
474 :
475 0 : if (submode < 2) {
476 0 : pvq_dec(10, 10, scf_idx[4], scf_idx[5], pulses);
477 :
478 0 : if (submode == 0) {
479 0 : pvq_dec(1, 6, (scf_idx[6] % 2), floor(scf_idx[6] / 2), pulses2);
480 :
481 0 : move_int(&pulses[10], pulses2, 6);
482 :
483 : } else {
484 0 : pulses[15] = 0;
485 : }
486 0 : } else if (submode == 2) {
487 0 : pvq_dec(8, 16, scf_idx[4], scf_idx[5], pulses);
488 : } else {
489 0 : pvq_dec(6, 16, scf_idx[4], scf_idx[5], pulses);
490 : }
491 :
492 : /* Normalization */
493 :
494 0 : for (i = 0; i < M; i++) {
495 0 : sum += pulses[i] * pulses[i];
496 : }
497 :
498 0 : sum = 1.0 / LC3_SQRT(sum);
499 :
500 0 : for (i = 0; i < M; i++) {
501 0 : st2_vector[i] = pulses[i] * sum;
502 : }
503 :
504 : /* Inverse transform */
505 0 : idct_II(st2_vector, st2_vector_idct, M);
506 :
507 : /* Gain */
508 : /* Add stage 1 and stage 2 */
509 0 : for (i = 0; i < M; i++) {
510 0 : scf_q[i] += st2_vector_idct[i] * sns_dec_gains[submode][scf_idx[3]];
511 : }
512 0 : }
|
