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 0 : void processEstimateGlobalGain_fx(Word32 x[], Word16 x_e, Word16 lg, Word16 nbitsSQ,
13 : #ifdef ENABLE_HR_MODE
14 : Word32 *gain,
15 : #else
16 : Word16 *gain,
17 : #endif
18 : Word16 *gain_e,
19 : Word16 *quantizedGain, Word16 *quantizedGainMin, Word16 quantizedGainOff,
20 : Word32 *targetBitsOff, Word16 *old_targetBits, Word16 old_specBits,
21 : Word8 *scratchBuffer
22 : #ifdef ENABLE_HR_MODE
23 : , Word16 hrmode, Word16 regBits, Word16 frame_dms
24 : #endif
25 : )
26 : {
27 :
28 : Word16 lg_4, tmp16, iszero, s;
29 : Word32 ener, tmp32, x_max;
30 : Word32 target, fac, offset;
31 : Word32 *en;
32 : Counter iter, i;
33 : Word32 diff, diff2;
34 : #ifdef ENABLE_HR_MODE
35 0 : Word16 *en_exp = NULL;
36 : Word32 M0, M1;
37 : #endif
38 :
39 : #ifdef DYNMEM_COUNT
40 : Dyn_Mem_In("processEstimateGlobalGain_fx", sizeof(struct {
41 : Word16 lg_4, s, tmp16, iszero;
42 : Word32 ener, tmp32, x_max;
43 : Word32 target, fac, offset;
44 : Word32 *en;
45 : Counter i, iter;
46 : Word32 diff, diff2;
47 : }));
48 : #endif
49 :
50 0 : en = (Word32 *)scratchAlign(scratchBuffer,
51 : 0); /* Size = MAX_LEN bytes */
52 :
53 : #ifdef ENABLE_HR_MODE
54 0 : if (hrmode)
55 : {
56 0 : M0 = 1;
57 0 : M1 = 1; /* Regularization factor; needs´to be 1e-5, but 1e-5 is 0 in Q15 */
58 0 : en_exp = (Word16 *) scratchAlign(en, sizeof(*en) * MAX_LEN);
59 : }
60 : #endif
61 0 : IF (*old_targetBits < 0)
62 : {
63 0 : *targetBitsOff = 0;
64 0 : move16();
65 : }
66 : ELSE
67 : {
68 0 : tmp32 = L_add(*targetBitsOff, L_deposit_h(sub(*old_targetBits, old_specBits)));
69 0 : tmp32 = L_min((40 << 16), L_max(-(40 << 16), tmp32));
70 0 : *targetBitsOff = L_add(Mpy_32_16_lc3plus(*targetBitsOff, 26214), Mpy_32_16_lc3plus(tmp32, 6554));
71 0 : move16();
72 : }
73 :
74 0 : *old_targetBits = nbitsSQ;
75 0 : move16();
76 0 : nbitsSQ = add(nbitsSQ, round_fx(*targetBitsOff));
77 :
78 0 : lg_4 = shr_pos(lg, 2);
79 0 : x_max = 0;
80 0 : move32();
81 :
82 : /* energy of quadruples with 9dB offset */
83 : #ifdef ENABLE_HR_MODE
84 0 : IF (hrmode)
85 : {
86 0 : FOR (i = 0; i < lg_4; i++)
87 : {
88 : Word32 absval;
89 : Word16 idx;
90 : /* normalization */
91 0 : s = 31;
92 0 : move16();
93 :
94 : /* M1 requires a 32x16 mult with Q0 i, resulting in Q15. Keeping both M0 and M1 in same Q */
95 : /* Use Q15 for M0 and M1 calculation */
96 0 : idx = shl(i, 2);
97 :
98 0 : tmp32 = L_abs(x[0]);
99 0 : absval = L_shr(tmp32, 16);
100 0 : M0 = L_add(M0, absval); /* M0 += fabs(x[idx])*/
101 0 : M1 = L_add(M1, L_mult(absval, idx)); /* M1 += i*fabs(x[idx])*/
102 0 : idx = add(idx, 1);
103 :
104 0 : absval = L_abs(x[1]);
105 0 : tmp32 = L_max(tmp32, absval);
106 0 : absval = L_shr(tmp32, 16);
107 0 : M0 = L_add(M0, absval); /* M0 += fabs(x[idx])*/
108 0 : M1 = L_add(M1, L_mult(absval, idx)); /* M1 += idx*fabs(x[idx])*/
109 0 : idx = add(idx, 1);
110 :
111 0 : absval = L_abs(x[2]);
112 0 : tmp32 = L_max(tmp32, absval);
113 0 : absval = L_shr(tmp32, 16);
114 0 : M0 = L_add(M0, absval); /* M0 += fabs(x[idx])*/
115 0 : M1 = L_add(M1, L_mult(absval, idx)); /* M1 += idx*fabs(x[idx])*/
116 0 : idx = add(idx, 1);
117 :
118 0 : absval = L_abs(x[3]);
119 0 : tmp32 = L_max(tmp32, absval);
120 0 : absval = L_shr(tmp32, 16);
121 0 : M0 = L_add(M0, absval); /* M0 += fabs(x[idx])*/
122 0 : M1 = L_add(M1, L_mult(absval, idx)); /* M1 += idx*fabs(x[idx])*/
123 :
124 0 : x_max = L_max(x_max, tmp32);
125 :
126 0 : if (tmp32 != 0)
127 0 : s = norm_l(tmp32);
128 :
129 0 : s = sub(s, 2); /* 2 bits headroom */
130 :
131 : /* calc quadruple energy */
132 0 : ener = L_deposit_l(1);
133 :
134 0 : tmp16 = round_fx(L_shl(x[0], s));
135 0 : ener = L_mac(ener, tmp16, tmp16);
136 :
137 0 : tmp16 = round_fx(L_shl(x[1], s));
138 0 : ener = L_mac(ener, tmp16, tmp16);
139 :
140 0 : tmp16 = round_fx(L_shl(x[2], s));
141 0 : ener = L_mac(ener, tmp16, tmp16);
142 :
143 0 : tmp16 = round_fx(L_shl(x[3], s));
144 0 : ener = L_mac(ener, tmp16, tmp16);
145 :
146 0 : s = shl_pos(sub(x_e, s), 1);
147 0 : if (ener == 1 && s < 0)
148 0 : s = 0;
149 0 : IF (regBits > 0)
150 : {
151 0 : en[i] = ener;
152 0 : en_exp[i] = s;
153 0 : move32();
154 : }
155 : ELSE
156 : {
157 : /* log */
158 0 : tmp32 = L_add(BASOP_Util_Log2_lc3plus(ener), L_shl_pos(L_deposit_l(s), 25)); /* log2, 6Q25 */
159 0 : tmp32 = L_add(L_shr_pos(Mpy_32_16_lc3plus(tmp32, 0x436E), 7),
160 : 0x4E666); /* -> (28/20)*(7+10*tmp32/log2(10)), 16Q15 */
161 0 : en[i] = tmp32;
162 0 : move32();
163 : }
164 :
165 0 : x += 4;
166 : }
167 : }
168 : ELSE
169 : #endif
170 : {
171 0 : FOR (i = 0; i < lg_4; i++)
172 : {
173 : /* normalization */
174 0 : s = 31;
175 0 : move16();
176 :
177 0 : tmp32 = L_abs(x[0]);
178 0 : tmp32 = L_max(tmp32, L_abs(x[1]));
179 0 : tmp32 = L_max(tmp32, L_abs(x[2]));
180 0 : tmp32 = L_max(tmp32, L_abs(x[3]));
181 0 : x_max = L_max(x_max, tmp32);
182 :
183 0 : if (tmp32 != 0)
184 0 : s = norm_l(tmp32);
185 :
186 0 : s = sub(s, 2); /* 2 bits headroom */
187 :
188 : /* calc quadruple energy */
189 0 : ener = L_deposit_l(1);
190 :
191 0 : tmp16 = round_fx(L_shl(x[0], s));
192 0 : ener = L_mac(ener, tmp16, tmp16);
193 :
194 0 : tmp16 = round_fx(L_shl(x[1], s));
195 0 : ener = L_mac(ener, tmp16, tmp16);
196 :
197 0 : tmp16 = round_fx(L_shl(x[2], s));
198 0 : ener = L_mac(ener, tmp16, tmp16);
199 :
200 0 : tmp16 = round_fx(L_shl(x[3], s));
201 0 : ener = L_mac(ener, tmp16, tmp16);
202 :
203 0 : s = shl_pos(sub(x_e, s), 1);
204 0 : if (ener == 1 && s < 0)
205 0 : s = 0;
206 :
207 : /* log */
208 0 : tmp32 = L_add_sat(BASOP_Util_Log2_lc3plus(ener), L_shl_sat(L_deposit_l(s), 25)); /* log2, 6Q25 */
209 : tmp32 =
210 0 : L_add(L_shr_pos(Mpy_32_16_lc3plus(tmp32, 0x436E), 6), 0x9CCCD); /* -> (28/20)*(7+10*tmp32/log2(10)), 15Q16 */
211 0 : en[i] = tmp32;
212 0 : move32();
213 0 : x += 4;
214 : }
215 : }
216 :
217 0 : IF (x_max == 0)
218 : {
219 0 : *quantizedGainMin = quantizedGainOff;
220 0 : move16();
221 0 : *quantizedGain = 0;
222 0 : move16();
223 0 : *old_targetBits = -1;
224 0 : move16();
225 : }
226 : ELSE
227 : {
228 0 : Word32 sub_val = 0xFCDD38F;
229 : /*28 * log10(32767 - 0.375) * (1 - 1e-7) in Q21 */
230 :
231 : #ifdef ENABLE_HR_MODE
232 0 : if (hrmode)
233 : {
234 : /*
235 : Original float code :
236 : rB_offset = 8 * (1 - MIN(M1/M0, 2*frame_ms)/(2*frame_ms)
237 : */
238 : Word16 ratio;
239 0 : Word16 ratio_exp = 0;
240 0 : Word32 regterm = MAX_32; /* 1 in Q31 */
241 : Word32 rB_offset, reg_val;
242 : Word32 ratio_prod;
243 : Word16 n_reg_val;
244 :
245 0 : if (M0 <= 0x7fff)
246 : {
247 0 : Word16 inv_M0 = Inv16_lc3plus(M0, &ratio_exp); /* Inverse in Q(15 - ratio_exp) */
248 0 : ratio = L_shr(Mpy_32_16_lc3plus(M1, inv_M0), 15 - ratio_exp); /* Integer ratio */
249 : }
250 : else
251 : {
252 0 : Word16 M0_h = L_shr(M0, 15);
253 0 : Word32 M1_h = L_shr(M1, 15);
254 :
255 0 : Word16 inv_M0 = Inv16_lc3plus(M0_h, &ratio_exp); /* Inverse in Q(15 - ratio_exp) */
256 0 : ratio = extract_l(L_shr(Mpy_32_16_lc3plus(M1_h, inv_M0), 15 - ratio_exp)); /* Integer ratio */
257 : }
258 :
259 : /*
260 : regterm = MIN (M1 / M0, 2 *frame_ms) / (2 * frame_ms)
261 : regterm = MIN (M1 * (10 / 2) / M0, frame_dms) / frame_dms
262 : regterm = MIN ( ratio * 5, frame_dms) / frame_dms
263 : */
264 :
265 0 : ratio_prod = L_mult(ratio, 5);
266 :
267 0 : if (ratio_prod < frame_dms)
268 : {
269 0 : Word16 mult_factor = 0;
270 :
271 0 : SWITCH (frame_dms) /* 1 / frame_dms in Q15 */
272 : {
273 0 : case 25: mult_factor = 1311; break;
274 0 : case 50: mult_factor = 655; break;
275 0 : case 75: mult_factor = 437; break;
276 0 : case 100: mult_factor = 328; break;
277 : }
278 :
279 : /* ratio_prod < frame_dms. Hence Word16 can be used */
280 :
281 0 : regterm = L_shl(L_mult(extract_l(ratio_prod), mult_factor), 15); /* result in Q31 */
282 : }
283 :
284 0 : rB_offset = L_sub(MAX_32, regterm);
285 : /* Calculation in Q28 to prevent overflow. Subtraction result in Q31, downshift by 3 results in Q28.
286 : Multiplication by 8 is implemented as upshift by 3.
287 : */
288 :
289 : /*
290 : FLOAT code : reg_val = x_max * LC3_POW(2,-regBits - rB_offset);
291 : */
292 0 : Word16 reg_val_e = x_e;
293 :
294 0 : IF(rB_offset > 0)
295 : {
296 0 : Word32 reg_exp = L_negate(L_add(L_shl(regBits, 25), L_shr(rB_offset, 3)));
297 0 : reg_val = Mpy_32_32_lc3plus(x_max, BASOP_Util_InvLog2_lc3plus(reg_exp)); /* Product is in Q31 */
298 : /* reg_val is in Q(31-x_e) */
299 : }
300 : ELSE
301 : {
302 0 : reg_val = x_max;
303 0 : reg_val_e = sub(x_e, regBits);
304 : }
305 :
306 0 : sub_val = 0x183BA045;
307 0 : move16();
308 : /*28 * log10(32768*256 - 2) in Q21 */
309 :
310 : /*
311 : Adding LC3_POW(2, -31) to reg_val.2^-31 * 2^(31-x_e) = 2^-x_e.
312 : If x_e is positive, this is below precision requirements to be used.
313 : */
314 :
315 0 : if (reg_val_e < 0)
316 : {
317 0 : reg_val = L_add_sat(reg_val, L_shl_sat(1, negate(reg_val_e)));
318 : }
319 0 : n_reg_val = norm_l(reg_val);
320 :
321 0 : FOR (i = 0; i < lg_4; i++)
322 : {
323 0 : ener = en[i];
324 0 : move16();
325 0 : s = en_exp[i];
326 0 : move16();
327 :
328 0 : Word16 shift_val = sub(reg_val_e, s);
329 :
330 0 : IF (sub(n_reg_val, shift_val) > 0)
331 : {
332 0 : IF(shift_val > -32)
333 : {
334 0 : ener = L_add(ener, L_shl(reg_val, shift_val)); /* Match q formats */
335 : }
336 : }
337 : ELSE
338 : {
339 0 : IF (sub(shift_val, 32) >= 0 )
340 : {
341 0 : ener = reg_val;
342 : }
343 : ELSE
344 : {
345 0 : ener = L_add_sat(reg_val, L_shr(ener, shift_val));
346 : }
347 0 : s = reg_val_e;
348 : }
349 :
350 0 : tmp32 = L_add(BASOP_Util_Log2_lc3plus(ener), L_shl_pos(L_deposit_l(s), 25)); /* log2, 6Q25 */
351 0 : tmp32 = L_add(L_shr_pos(Mpy_32_32_lc3plus(tmp32, 0x436E439A), 7), 0x4E666); /* -> (28/20)*(7+10*tmp32/log2(10)), 15Q16 */
352 0 : en[i] = tmp32;
353 0 : move32();
354 : }
355 : }
356 : #endif
357 0 : x_max = BASOP_Util_Log2_lc3plus(x_max);
358 : /* Minimum gain */
359 0 : x_max = L_add(x_max, L_shl_pos(L_deposit_l(x_e), 25)); /* log2(x_max) in 6Q25 */
360 0 : x_max = L_sub(
361 : Mpy_32_32_lc3plus(x_max, 0x436E439A),
362 : sub_val); /* 28*log10(x_max/(32768-0.375)) = log2(x_max)*(28/log2(10))-28*log10(32768-0.375) in 10Q21 */
363 : /* 28/log1(10) is in Q27
364 : Mpy_32_32_lc3plus : Q25(x_max) + Q27 + Q1(Mpy_32_32_ss) - Q32 = Q21 */
365 0 : *quantizedGainMin = extract_l(L_shr_pos(L_add(x_max, (1 << 21) + (1 << 11)), 21));
366 0 : move16();
367 0 : ASSERT(*quantizedGainMin <= 255 + quantizedGainOff);
368 0 : *quantizedGainMin = s_max(quantizedGainOff, s_min(add(255, quantizedGainOff), *quantizedGainMin));
369 :
370 0 : offset = L_deposit_h(add(255, quantizedGainOff)); /* -> 127 */
371 :
372 : #ifdef ENABLE_HR_MODE
373 0 : IF(hrmode)
374 : {
375 0 : offset = L_shr_pos(offset, 2);
376 : /* SQ scale: 4 bits / 6 dB per quadruple */
377 0 : target = L_mult(0x3EB8, nbitsSQ); /* -> (28/20) * (1.4) * nbitsSQ */
378 :
379 0 : fac = L_add(0x400000, 0); /* -> 256 */
380 : /* find offset (0 to 127) */
381 0 : FOR (iter = 0; iter < 8; iter++)
382 : {
383 0 : fac = L_shr_pos(fac, 1);
384 0 : offset = L_sub(offset, fac);
385 :
386 0 : ener = L_deposit_l(0);
387 0 : iszero = 1;
388 0 : move16();
389 :
390 0 : FOR (i = lg_4 - 1; i >= 0; i--)
391 : {
392 0 : tmp32 = L_sub(L_shr_pos(en[i], 1), offset);
393 0 : diff = L_sub(tmp32, 0x27333); /* 0x4E666 -> (28/20)*(7) in Q15 */
394 0 : if (diff < 0)
395 : {
396 0 : if (iszero == 0)
397 : {
398 0 : ener = L_add(ener, 0xF1EC); /* 0x1E3D7 -> (28/20)*(2.7) in Q15 */
399 : }
400 : }
401 : else
402 : {
403 0 : ener = L_add(ener, tmp32);
404 0 : iszero = 0;
405 0 : move16();
406 :
407 0 : diff2 = L_sub(tmp32, 0x118000); /* 0x230000 -> (28/20)*(50) */
408 0 : if (diff2 >= 0)
409 : {
410 0 : ener = L_add(ener, diff2);
411 : }
412 : }
413 : }
414 :
415 : /* if ener is above target -> increase offset */
416 0 : test();
417 0 : if (L_sub(ener, target) > 0 && iszero == 0)
418 : {
419 0 : offset = L_add(offset, fac);
420 : }
421 : }
422 0 : tmp16 = extract_h(L_shl(offset, 2));
423 : }
424 : ELSE
425 : #endif
426 : {
427 : /* SQ scale: 4 bits / 6 dB per quadruple */
428 0 : target = L_shl_pos(L_mult(0x7D71, nbitsSQ), 1); /* -> (28/20) * (1.4) * nbitsSQ */
429 0 : fac = L_add(0x1000000, 0); /* -> 256 */
430 :
431 : /* find offset (0 to 127) */
432 0 : FOR (iter = 0; iter < 8; iter++)
433 : {
434 0 : fac = L_shr_pos(fac, 1);
435 0 : offset = L_sub(offset, fac);
436 :
437 0 : ener = L_deposit_l(0);
438 0 : iszero = 1;
439 0 : move16();
440 0 : FOR (i = lg_4 - 1; i >= 0; i--)
441 : {
442 0 : tmp32 = L_sub(en[i], offset);
443 0 : diff = L_sub(tmp32, 0x9CCCD); /* 0x9CCCD -> (28/20)*(7) in Q16 */
444 0 : if (diff < 0)
445 : {
446 0 : if (iszero == 0)
447 : {
448 0 : ener = L_add_sat(ener, 0x3C7AE); /* 0x3C7AE -> (28/20)*(2.7) in Q16 */
449 : }
450 : }
451 : else
452 : {
453 0 : ener = L_add_sat(ener, tmp32);
454 0 : iszero = 0;
455 0 : move16();
456 :
457 0 : diff2 = L_sub(tmp32, 0x460000); /* 0x460000 -> (28/20)*(50) in Q16 */
458 0 : if (diff2 >= 0)
459 : {
460 0 : ener = L_add_sat(ener, diff2);
461 : }
462 : }
463 :
464 : }
465 :
466 : /* if ener is above target -> increase offset */
467 0 : test();
468 0 : if (L_sub(ener, target) > 0 && iszero == 0)
469 : {
470 0 : offset = L_add(offset, fac);
471 : }
472 : }
473 0 : tmp16 = extract_h(offset);
474 :
475 : }
476 :
477 0 : if (sub(tmp16, *quantizedGainMin) < 0)
478 : {
479 0 : *old_targetBits = -1;
480 0 : move16();
481 : }
482 0 : *quantizedGain = sub(s_max(*quantizedGainMin, tmp16), quantizedGainOff);
483 0 : move16();
484 : }
485 :
486 : #ifdef ENABLE_HR_MODE
487 : tmp32 =
488 0 : Mpy_32_16_lc3plus(0x797CD707, L_shl_pos(add(*quantizedGain, quantizedGainOff), 6));
489 : #else
490 : tmp32 =
491 : L_shl_pos(L_mult0(add(*quantizedGain, quantizedGainOff), 0x797D), 7); /* 6Q25; 0x797D -> log2(10)/28 (Q18) */
492 : #endif
493 0 : *gain_e = add(extract_l(L_shr_pos(tmp32, 25)), 1); /* get exponent */
494 : #ifdef ENABLE_HR_MODE
495 0 : *gain = BASOP_Util_InvLog2_lc3plus(L_or(tmp32, (Word32)0xFE000000));
496 : #else
497 : *gain = round_fx(BASOP_Util_InvLog2_lc3plus(L_or(tmp32,(Word32) 0xFE000000)));
498 : #endif
499 :
500 : #ifdef DYNMEM_COUNT
501 : Dyn_Mem_Out();
502 : #endif
503 0 : }
504 :
|
