1 /******************************************************************************
2  *
3  *  Copyright 2022 Google LLC
4  *
5  *  Licensed under the Apache License, Version 2.0 (the "License");
6  *  you may not use this file except in compliance with the License.
7  *  You may obtain a copy of the License at:
8  *
9  *  http://www.apache.org/licenses/LICENSE-2.0
10  *
11  *  Unless required by applicable law or agreed to in writing, software
12  *  distributed under the License is distributed on an "AS IS" BASIS,
13  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  *  See the License for the specific language governing permissions and
15  *  limitations under the License.
16  *
17  ******************************************************************************/
18 
19 #include "spec.h"
20 #include "bits.h"
21 #include "tables.h"
22 
23 
24 /* ----------------------------------------------------------------------------
25  *  Global Gain / Quantization
26  * -------------------------------------------------------------------------- */
27 
28 /**
29  * Resolve quantized gain index offset
30  * sr, nbytes      Samplerate and size of the frame
31  * return          Gain index offset
32  */
resolve_gain_offset(enum lc3_srate sr,int nbytes)33 static int resolve_gain_offset(enum lc3_srate sr, int nbytes)
34 {
35     int g_off = (nbytes * 8) / (10 * (1 + sr));
36     return 105 + 5*(1 + sr) + LC3_MIN(g_off, 115);
37 }
38 
39 /**
40  * Global Gain Estimation
41  * dt, sr          Duration and samplerate of the frame
42  * x               Spectral coefficients
43  * nbits_budget    Number of bits available coding the spectrum
44  * nbits_off       Offset on the available bits, temporarily smoothed
45  * g_off           Gain index offset
46  * reset_off       Return True when the nbits_off must be reset
47  * g_min           Return lower bound of quantized gain value
48  * return          The quantized gain value
49  */
estimate_gain(enum lc3_dt dt,enum lc3_srate sr,const float * x,int nbits_budget,float nbits_off,int g_off,bool * reset_off,int * g_min)50 LC3_HOT static int estimate_gain(
51     enum lc3_dt dt, enum lc3_srate sr, const float *x,
52     int nbits_budget, float nbits_off, int g_off, bool *reset_off, int *g_min)
53 {
54     int ne = LC3_NE(dt, sr) >> 2;
55     int e[LC3_MAX_NE];
56 
57     /* --- Energy (dB) by 4 MDCT blocks --- */
58 
59     float x2_max = 0;
60 
61     for (int i = 0; i < ne; i++, x += 4) {
62         float x0 = x[0] * x[0];
63         float x1 = x[1] * x[1];
64         float x2 = x[2] * x[2];
65         float x3 = x[3] * x[3];
66 
67         x2_max = fmaxf(x2_max, x0);
68         x2_max = fmaxf(x2_max, x1);
69         x2_max = fmaxf(x2_max, x2);
70         x2_max = fmaxf(x2_max, x3);
71 
72         e[i] = fast_db_q16(fmaxf(x0 + x1 + x2 + x3, 1e-10f));
73     }
74 
75     /* --- Determine gain index --- */
76 
77     int nbits = nbits_budget + nbits_off + 0.5f;
78     int g_int = 255 - g_off;
79 
80     const int k_20_28 = 20.f/28 * 0x1p16f + 0.5f;
81     const int k_2u7 = 2.7f * 0x1p16f + 0.5f;
82     const int k_1u4 = 1.4f * 0x1p16f + 0.5f;
83 
84     for (int i = 128, j, j0 = ne-1, j1 ; i > 0; i >>= 1) {
85         int gn = (g_int - i) * k_20_28;
86         int v = 0;
87 
88         for (j = j0; j >= 0 && e[j] < gn; j--);
89 
90         for (j1 = j; j >= 0; j--) {
91             int e_diff = e[j] - gn;
92 
93             v += e_diff < 0 ? k_2u7 :
94                  e_diff < 43 << 16 ?   e_diff + ( 7 << 16)
95                                    : 2*e_diff - (36 << 16);
96         }
97 
98         if (v > nbits * k_1u4)
99             j0 = j1;
100         else
101             g_int = g_int - i;
102     }
103 
104     /* --- Limit gain index --- */
105 
106     *g_min = x2_max == 0 ? -g_off :
107         ceilf(28 * log10f(sqrtf(x2_max) / (32768 - 0.375f)));
108 
109     *reset_off = g_int < *g_min || x2_max == 0;
110     if (*reset_off)
111         g_int = *g_min;
112 
113     return g_int;
114 }
115 
116 /**
117  * Global Gain Adjustment
118  * sr              Samplerate of the frame
119  * g_idx           The estimated quantized gain index
120  * nbits           Computed number of bits coding the spectrum
121  * nbits_budget    Number of bits available for coding the spectrum
122  * g_idx_min       Minimum gain index value
123  * return          Gain adjust value (-1 to 2)
124  */
adjust_gain(enum lc3_srate sr,int g_idx,int nbits,int nbits_budget,int g_idx_min)125 LC3_HOT static int adjust_gain(enum lc3_srate sr, int g_idx,
126     int nbits, int nbits_budget, int g_idx_min)
127 {
128     /* --- Compute delta threshold --- */
129 
130     const int *t = (const int [LC3_NUM_SRATE][3]){
131         {  80,  500,  850 }, { 230, 1025, 1700 }, { 380, 1550, 2550 },
132         { 530, 2075, 3400 }, { 680, 2600, 4250 }
133     }[sr];
134 
135     int delta, den = 48;
136 
137     if (nbits < t[0]) {
138         delta = 3*(nbits + 48);
139 
140     } else if (nbits < t[1]) {
141         int n0 = 3*(t[0] + 48), range = t[1] - t[0];
142         delta = n0 * range + (nbits - t[0]) * (t[1] - n0);
143         den *= range;
144 
145     } else {
146         delta = LC3_MIN(nbits, t[2]);
147     }
148 
149     delta = (delta + den/2) / den;
150 
151     /* --- Adjust gain --- */
152 
153     if (nbits < nbits_budget - (delta + 2))
154         return -(g_idx > g_idx_min);
155 
156     if (nbits > nbits_budget)
157         return (g_idx < 255) + (g_idx < 254 && nbits >= nbits_budget + delta);
158 
159     return 0;
160 }
161 
162 /**
163  * Unquantize gain
164  * g_int           Quantization gain value
165  * return          Unquantized gain value
166  */
unquantize_gain(int g_int)167 static float unquantize_gain(int g_int)
168 {
169     /* Unquantization gain table :
170      * G[i] = 10 ^ (i / 28) , i = [0..64] */
171 
172     static const float iq_table[] = {
173         1.00000000e+00, 1.08571112e+00, 1.17876863e+00, 1.27980221e+00,
174         1.38949549e+00, 1.50859071e+00, 1.63789371e+00, 1.77827941e+00,
175         1.93069773e+00, 2.09617999e+00, 2.27584593e+00, 2.47091123e+00,
176         2.68269580e+00, 2.91263265e+00, 3.16227766e+00, 3.43332002e+00,
177         3.72759372e+00, 4.04708995e+00, 4.39397056e+00, 4.77058270e+00,
178         5.17947468e+00, 5.62341325e+00, 6.10540230e+00, 6.62870316e+00,
179         7.19685673e+00, 7.81370738e+00, 8.48342898e+00, 9.21055318e+00,
180         1.00000000e+01, 1.08571112e+01, 1.17876863e+01, 1.27980221e+01,
181         1.38949549e+01, 1.50859071e+01, 1.63789371e+01, 1.77827941e+01,
182         1.93069773e+01, 2.09617999e+01, 2.27584593e+01, 2.47091123e+01,
183         2.68269580e+01, 2.91263265e+01, 3.16227766e+01, 3.43332002e+01,
184         3.72759372e+01, 4.04708995e+01, 4.39397056e+01, 4.77058270e+01,
185         5.17947468e+01, 5.62341325e+01, 6.10540230e+01, 6.62870316e+01,
186         7.19685673e+01, 7.81370738e+01, 8.48342898e+01, 9.21055318e+01,
187         1.00000000e+02, 1.08571112e+02, 1.17876863e+02, 1.27980221e+02,
188         1.38949549e+02, 1.50859071e+02, 1.63789371e+02, 1.77827941e+02,
189         1.93069773e+02
190     };
191 
192     float g = iq_table[LC3_ABS(g_int) & 0x3f];
193     for(int n64 = LC3_ABS(g_int) >> 6; n64--; )
194         g *= iq_table[64];
195 
196     return g_int >= 0 ? g : 1 / g;
197 }
198 
199 /**
200  * Spectrum quantization
201  * dt, sr          Duration and samplerate of the frame
202  * g_int           Quantization gain value
203  * x               Spectral coefficients, scaled as output
204  * xq, nq          Output spectral quantized coefficients, and count
205  *
206  * The spectral coefficients `xq` are stored as :
207  *   b0       0:positive or zero  1:negative
208  *   b15..b1  Absolute value
209  */
quantize(enum lc3_dt dt,enum lc3_srate sr,int g_int,float * x,uint16_t * xq,int * nq)210 LC3_HOT static void quantize(enum lc3_dt dt, enum lc3_srate sr,
211     int g_int, float *x, uint16_t *xq, int *nq)
212 {
213     float g_inv = 1 / unquantize_gain(g_int);
214     int ne = LC3_NE(dt, sr);
215 
216     *nq = ne;
217 
218     for (int i = 0; i < ne; i += 2) {
219         uint16_t x0, x1;
220 
221         x[i+0] *= g_inv;
222         x[i+1] *= g_inv;
223 
224         x0 = fminf(fabsf(x[i+0]) + 6.f/16, INT16_MAX);
225         x1 = fminf(fabsf(x[i+1]) + 6.f/16, INT16_MAX);
226 
227         xq[i+0] = (x0 << 1) + ((x0 > 0) & (x[i+0] < 0));
228         xq[i+1] = (x1 << 1) + ((x1 > 0) & (x[i+1] < 0));
229 
230         *nq = x0 || x1 ? ne : *nq - 2;
231     }
232 }
233 
234 /**
235  * Spectrum quantization inverse
236  * dt, sr          Duration and samplerate of the frame
237  * g_int           Quantization gain value
238  * x, nq           Spectral quantized, and count of significants
239  * return          Unquantized gain value
240  */
unquantize(enum lc3_dt dt,enum lc3_srate sr,int g_int,float * x,int nq)241 LC3_HOT static float unquantize(enum lc3_dt dt, enum lc3_srate sr,
242     int g_int, float *x, int nq)
243 {
244     float g = unquantize_gain(g_int);
245     int i, ne = LC3_NE(dt, sr);
246 
247     for (i = 0; i < nq; i++)
248         x[i] = x[i] * g;
249 
250     for ( ; i < ne; i++)
251         x[i] = 0;
252 
253     return g;
254 }
255 
256 
257 /* ----------------------------------------------------------------------------
258  *  Spectrum coding
259  * -------------------------------------------------------------------------- */
260 
261 /**
262  * Resolve High-bitrate mode according size of the frame
263  * sr, nbytes      Samplerate and size of the frame
264  * return          True when High-Rate mode enabled
265  */
resolve_high_rate(enum lc3_srate sr,int nbytes)266 static int resolve_high_rate(enum lc3_srate sr, int nbytes)
267 {
268     return nbytes > 20 * (1 + (int)sr);
269 }
270 
271 /**
272  * Bit consumption
273  * dt, sr, nbytes  Duration, samplerate and size of the frame
274  * x               Spectral quantized coefficients
275  * n               Count of significant coefficients, updated on truncation
276  * nbits_budget    Truncate to stay in budget, when not zero
277  * p_lsb_mode      Return True when LSB's are not AC coded, or NULL
278  * return          The number of bits coding the spectrum
279  *
280  * The spectral coefficients `x` storage is :
281  *   b0       0:positive or zero  1:negative
282  *   b15..b1  Absolute value
283  */
compute_nbits(enum lc3_dt dt,enum lc3_srate sr,int nbytes,const uint16_t * x,int * n,int nbits_budget,bool * p_lsb_mode)284 LC3_HOT static int compute_nbits(
285     enum lc3_dt dt, enum lc3_srate sr, int nbytes,
286     const uint16_t *x, int *n, int nbits_budget, bool *p_lsb_mode)
287 {
288     int ne = LC3_NE(dt, sr);
289 
290     /* --- Mode and rate --- */
291 
292     bool lsb_mode  = nbytes >= 20 * (3 + (int)sr);
293     bool high_rate = resolve_high_rate(sr, nbytes);
294 
295     /* --- Loop on quantized coefficients --- */
296 
297     int nbits = 0, nbits_lsb = 0;
298     uint8_t state = 0;
299 
300     int nbits_end = 0;
301     int n_end = 0;
302 
303     nbits_budget = nbits_budget ? nbits_budget * 2048 : INT_MAX;
304 
305     for (int i = 0, h = 0; h < 2; h++) {
306         const uint8_t (*lut_coeff)[4] = lc3_spectrum_lookup[high_rate][h];
307 
308         for ( ; i < LC3_MIN(*n, (ne + 2) >> (1 - h))
309                 && nbits <= nbits_budget; i += 2) {
310 
311             const uint8_t *lut = lut_coeff[state];
312             uint16_t a = x[i] >> 1, b = x[i+1] >> 1;
313 
314             /* --- Sign values --- */
315 
316             int s = (a > 0) + (b > 0);
317             nbits += s * 2048;
318 
319             /* --- LSB values Reduce to 2*2 bits MSB values ---
320              * Reduce to 2x2 bits MSB values. The LSB's pair are arithmetic
321              * coded with an escape code followed by 1 bit for each values.
322              * The LSB mode does not arthmetic code the first LSB,
323              * add the sign of the LSB when one of pair was at value 1 */
324 
325             int k = 0;
326             int m = (a | b) >> 2;
327 
328             if (m) {
329 
330                 if (lsb_mode) {
331                     nbits += lc3_spectrum_bits[lut[k++]][16] - 2*2048;
332                     nbits_lsb += 2 + (a == 1) + (b == 1);
333                 }
334 
335                 for (m >>= lsb_mode; m; m >>= 1, k++)
336                     nbits += lc3_spectrum_bits[lut[LC3_MIN(k, 3)]][16];
337 
338                 nbits += k * 2*2048;
339                 a >>= k;
340                 b >>= k;
341 
342                 k = LC3_MIN(k, 3);
343             }
344 
345             /* --- MSB values --- */
346 
347             nbits += lc3_spectrum_bits[lut[k]][a + 4*b];
348 
349             /* --- Update state --- */
350 
351             if (s && nbits <= nbits_budget) {
352                 n_end = i + 2;
353                 nbits_end = nbits;
354             }
355 
356             state = (state << 4) + (k > 1 ? 12 + k : 1 + (a + b) * (k + 1));
357         }
358     }
359 
360     /* --- Return --- */
361 
362     *n = n_end;
363 
364     if (p_lsb_mode)
365         *p_lsb_mode = lsb_mode &&
366             nbits_end + nbits_lsb * 2048 > nbits_budget;
367 
368     if (nbits_budget >= INT_MAX)
369         nbits_end += nbits_lsb * 2048;
370 
371     return (nbits_end + 2047) / 2048;
372 }
373 
374 /**
375  * Put quantized spectrum
376  * bits            Bitstream context
377  * dt, sr, nbytes  Duration, samplerate and size of the frame
378  * x               Spectral quantized
379  * nq, lsb_mode    Count of significants, and LSB discard indication
380  *
381  * The spectral coefficients `x` storage is :
382  *   b0       0:positive or zero  1:negative
383  *   b15..b1  Absolute value
384  */
put_quantized(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,int nbytes,const uint16_t * x,int nq,bool lsb_mode)385 LC3_HOT static void put_quantized(lc3_bits_t *bits,
386     enum lc3_dt dt, enum lc3_srate sr, int nbytes,
387     const uint16_t *x, int nq, bool lsb_mode)
388 {
389     int ne = LC3_NE(dt, sr);
390     bool high_rate = resolve_high_rate(sr, nbytes);
391 
392     /* --- Loop on quantized coefficients --- */
393 
394     uint8_t state = 0;
395 
396     for (int i = 0, h = 0; h < 2; h++) {
397         const uint8_t (*lut_coeff)[4] = lc3_spectrum_lookup[high_rate][h];
398 
399         for ( ; i < LC3_MIN(nq, (ne + 2) >> (1 - h)); i += 2) {
400 
401             const uint8_t *lut = lut_coeff[state];
402             uint16_t a = x[i] >> 1, b = x[i+1] >> 1;
403 
404             /* --- LSB values Reduce to 2*2 bits MSB values ---
405              * Reduce to 2x2 bits MSB values. The LSB's pair are arithmetic
406              * coded with an escape code and 1 bits for each values.
407              * The LSB mode discard the first LSB (at this step) */
408 
409             int m = (a | b) >> 2;
410             int k = 0, shr = 0;
411 
412             if (m) {
413 
414                 if (lsb_mode)
415                     lc3_put_symbol(bits,
416                         lc3_spectrum_models + lut[k++], 16);
417 
418                 for (m >>= lsb_mode; m; m >>= 1, k++) {
419                     lc3_put_bit(bits, (a >> k) & 1);
420                     lc3_put_bit(bits, (b >> k) & 1);
421                     lc3_put_symbol(bits,
422                         lc3_spectrum_models + lut[LC3_MIN(k, 3)], 16);
423                 }
424 
425                 a >>= lsb_mode;
426                 b >>= lsb_mode;
427 
428                 shr = k - lsb_mode;
429                 k = LC3_MIN(k, 3);
430             }
431 
432             /* --- Sign values --- */
433 
434             if (a) lc3_put_bit(bits, x[i+0] & 1);
435             if (b) lc3_put_bit(bits, x[i+1] & 1);
436 
437             /* --- MSB values --- */
438 
439             a >>= shr;
440             b >>= shr;
441 
442             lc3_put_symbol(bits, lc3_spectrum_models + lut[k], a + 4*b);
443 
444             /* --- Update state --- */
445 
446             state = (state << 4) + (k > 1 ? 12 + k : 1 + (a + b) * (k + 1));
447         }
448     }
449 }
450 
451 /**
452  * Get quantized spectrum
453  * bits            Bitstream context
454  * dt, sr, nbytes  Duration, samplerate and size of the frame
455  * nq, lsb_mode    Count of significants, and LSB discard indication
456  * xq              Return `nq` spectral quantized coefficients
457  * nf_seed         Return the noise factor seed associated
458  * return          0: Ok  -1: Invalid bitstream data
459  */
get_quantized(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,int nbytes,int nq,bool lsb_mode,float * xq,uint16_t * nf_seed)460 LC3_HOT static int get_quantized(lc3_bits_t *bits,
461     enum lc3_dt dt, enum lc3_srate sr, int nbytes,
462     int nq, bool lsb_mode, float *xq, uint16_t *nf_seed)
463 {
464     int ne = LC3_NE(dt, sr);
465     bool high_rate = resolve_high_rate(sr, nbytes);
466 
467      *nf_seed = 0;
468 
469     /* --- Loop on quantized coefficients --- */
470 
471     uint8_t state = 0;
472 
473     for (int i = 0, h = 0; h < 2; h++) {
474         const uint8_t (*lut_coeff)[4] = lc3_spectrum_lookup[high_rate][h];
475 
476         for ( ; i < LC3_MIN(nq, (ne + 2) >> (1 - h)); i += 2) {
477 
478             const uint8_t *lut = lut_coeff[state];
479 
480             /* --- LSB values ---
481              * Until the symbol read indicates the escape value 16,
482              * read an LSB bit for each values.
483              * The LSB mode discard the first LSB (at this step) */
484 
485             int u = 0, v = 0;
486             int k = 0, shl = 0;
487 
488             unsigned s = lc3_get_symbol(bits, lc3_spectrum_models + lut[k]);
489 
490             if (lsb_mode && s >= 16) {
491                 s = lc3_get_symbol(bits, lc3_spectrum_models + lut[++k]);
492                 shl++;
493             }
494 
495             for ( ; s >= 16 && shl < 14; shl++) {
496                 u |= lc3_get_bit(bits) << shl;
497                 v |= lc3_get_bit(bits) << shl;
498 
499                 k += (k < 3);
500                 s  = lc3_get_symbol(bits, lc3_spectrum_models + lut[k]);
501             }
502 
503             if (s >= 16)
504                 return -1;
505 
506             /* --- MSB & sign values --- */
507 
508             int a = s % 4;
509             int b = s / 4;
510 
511             u |= a << shl;
512             v |= b << shl;
513 
514             xq[i  ] = u && lc3_get_bit(bits) ? -u : u;
515             xq[i+1] = v && lc3_get_bit(bits) ? -v : v;
516 
517             *nf_seed = (*nf_seed + u * i + v * (i+1)) & 0xffff;
518 
519             /* --- Update state --- */
520 
521             state = (state << 4) + (k > 1 ? 12 + k : 1 + (a + b) * (k + 1));
522         }
523     }
524 
525     return 0;
526 }
527 
528 /**
529  * Put residual bits of quantization
530  * bits            Bitstream context
531  * nbits           Maximum number of bits to output
532  * x, n            Spectral quantized, and count of significants
533  * xf              Scaled spectral coefficients
534  *
535  * The spectral coefficients `x` storage is :
536  *   b0       0:positive or zero  1:negative
537  *   b15..b1  Absolute value
538  */
put_residual(lc3_bits_t * bits,int nbits,const uint16_t * x,int n,const float * xf)539 LC3_HOT static void put_residual(
540     lc3_bits_t *bits, int nbits, const uint16_t *x, int n, const float *xf)
541 {
542     for (int i = 0; i < n && nbits > 0; i++) {
543 
544         if (x[i] == 0)
545             continue;
546 
547         float xq = x[i] & 1 ? -(x[i] >> 1) : (x[i] >> 1);
548 
549         lc3_put_bit(bits, xf[i] >= xq);
550         nbits--;
551     }
552 }
553 
554 /**
555  * Get residual bits of quantization
556  * bits            Bitstream context
557  * nbits           Maximum number of bits to output
558  * x, nq           Spectral quantized, and count of significants
559  */
get_residual(lc3_bits_t * bits,int nbits,float * x,int nq)560 LC3_HOT static void get_residual(
561     lc3_bits_t *bits, int nbits, float *x, int nq)
562 {
563     for (int i = 0; i < nq && nbits > 0; i++) {
564 
565         if (x[i] == 0)
566             continue;
567 
568         if (lc3_get_bit(bits) == 0)
569             x[i] -= x[i] < 0 ? 5.f/16 : 3.f/16;
570         else
571             x[i] += x[i] > 0 ? 5.f/16 : 3.f/16;
572 
573         nbits--;
574     }
575 }
576 
577 /**
578  * Put LSB values of quantized spectrum values
579  * bits            Bitstream context
580  * nbits           Maximum number of bits to output
581  * x, n            Spectral quantized, and count of significants
582  *
583  * The spectral coefficients `x` storage is :
584  *   b0       0:positive or zero  1:negative
585  *   b15..b1  Absolute value
586  */
put_lsb(lc3_bits_t * bits,int nbits,const uint16_t * x,int n)587 LC3_HOT static void put_lsb(
588     lc3_bits_t *bits, int nbits, const uint16_t *x, int n)
589 {
590     for (int i = 0; i < n && nbits > 0; i += 2) {
591         uint16_t a = x[i] >> 1, b = x[i+1] >> 1;
592         int a_neg = x[i] & 1, b_neg = x[i+1] & 1;
593 
594         if ((a | b) >> 2 == 0)
595             continue;
596 
597         if (nbits-- > 0)
598             lc3_put_bit(bits, a & 1);
599 
600         if (a == 1 && nbits-- > 0)
601             lc3_put_bit(bits, a_neg);
602 
603         if (nbits-- > 0)
604             lc3_put_bit(bits, b & 1);
605 
606         if (b == 1 && nbits-- > 0)
607             lc3_put_bit(bits, b_neg);
608     }
609 }
610 
611 /**
612  * Get LSB values of quantized spectrum values
613  * bits            Bitstream context
614  * nbits           Maximum number of bits to output
615  * x, nq           Spectral quantized, and count of significants
616  * nf_seed         Update the noise factor seed according
617  */
get_lsb(lc3_bits_t * bits,int nbits,float * x,int nq,uint16_t * nf_seed)618 LC3_HOT static void get_lsb(lc3_bits_t *bits,
619     int nbits, float *x, int nq, uint16_t *nf_seed)
620 {
621     for (int i = 0; i < nq && nbits > 0; i += 2) {
622 
623         float a = fabsf(x[i]), b = fabsf(x[i+1]);
624 
625         if (fmaxf(a, b) < 4)
626             continue;
627 
628         if (nbits-- > 0 && lc3_get_bit(bits)) {
629             if (a) {
630                 x[i] += x[i] < 0 ? -1 : 1;
631                 *nf_seed = (*nf_seed + i) & 0xffff;
632             } else if (nbits-- > 0) {
633                 x[i] = lc3_get_bit(bits) ? -1 : 1;
634                 *nf_seed = (*nf_seed + i) & 0xffff;
635             }
636         }
637 
638         if (nbits-- > 0 && lc3_get_bit(bits)) {
639             if (b) {
640                 x[i+1] += x[i+1] < 0 ? -1 : 1;
641                 *nf_seed = (*nf_seed + i+1) & 0xffff;
642             } else if (nbits-- > 0) {
643                 x[i+1] = lc3_get_bit(bits) ? -1 : 1;
644                 *nf_seed = (*nf_seed + i+1) & 0xffff;
645             }
646         }
647     }
648 }
649 
650 
651 /* ----------------------------------------------------------------------------
652  *  Noise coding
653  * -------------------------------------------------------------------------- */
654 
655 /**
656  * Estimate noise level
657  * dt, bw          Duration and bandwidth of the frame
658  * xq, nq          Quantized spectral coefficients
659  * x               Quantization scaled spectrum coefficients
660  * return          Noise factor (0 to 7)
661  *
662  * The spectral coefficients `x` storage is :
663  *   b0       0:positive or zero  1:negative
664  *   b15..b1  Absolute value
665  */
estimate_noise(enum lc3_dt dt,enum lc3_bandwidth bw,const uint16_t * xq,int nq,const float * x)666 LC3_HOT static int estimate_noise(enum lc3_dt dt, enum lc3_bandwidth bw,
667     const uint16_t *xq, int nq, const float *x)
668 {
669     int bw_stop = (dt == LC3_DT_7M5 ? 60 : 80) * (1 + bw);
670     int w = 2 + dt;
671 
672     float sum = 0;
673     int i, n = 0, z = 0;
674 
675     for (i = 6*(3 + dt) - w; i < LC3_MIN(nq, bw_stop); i++) {
676         z = xq[i] ? 0 : z + 1;
677         if (z > 2*w)
678             sum += fabsf(x[i - w]), n++;
679     }
680 
681     for ( ; i < bw_stop + w; i++)
682         if (++z > 2*w)
683             sum += fabsf(x[i - w]), n++;
684 
685     int nf = n ? 8 - (int)((16 * sum) / n + 0.5f) : 0;
686 
687     return LC3_CLIP(nf, 0, 7);
688 }
689 
690 /**
691  * Noise filling
692  * dt, bw          Duration and bandwidth of the frame
693  * nf, nf_seed     The noise factor and pseudo-random seed
694  * g               Quantization gain
695  * x, nq           Spectral quantized, and count of significants
696  */
fill_noise(enum lc3_dt dt,enum lc3_bandwidth bw,int nf,uint16_t nf_seed,float g,float * x,int nq)697 LC3_HOT static void fill_noise(enum lc3_dt dt, enum lc3_bandwidth bw,
698     int nf, uint16_t nf_seed, float g, float *x, int nq)
699 {
700     int bw_stop = (dt == LC3_DT_7M5 ? 60 : 80) * (1 + bw);
701     int w = 2 + dt;
702 
703     float s = g * (float)(8 - nf) / 16;
704     int i, z = 0;
705 
706     for (i = 6*(3 + dt) - w; i < LC3_MIN(nq, bw_stop); i++) {
707         z = x[i] ? 0 : z + 1;
708         if (z > 2*w) {
709             nf_seed = (13849 + nf_seed*31821) & 0xffff;
710             x[i - w] = nf_seed & 0x8000 ? -s : s;
711         }
712     }
713 
714     for ( ; i < bw_stop + w; i++)
715         if (++z > 2*w) {
716             nf_seed = (13849 + nf_seed*31821) & 0xffff;
717             x[i - w] = nf_seed & 0x8000 ? -s : s;
718         }
719 }
720 
721 /**
722  * Put noise factor
723  * bits            Bitstream context
724  * nf              Noise factor (0 to 7)
725  */
put_noise_factor(lc3_bits_t * bits,int nf)726 static void put_noise_factor(lc3_bits_t *bits, int nf)
727 {
728     lc3_put_bits(bits, nf, 3);
729 }
730 
731 /**
732  * Get noise factor
733  * bits            Bitstream context
734  * return          Noise factor (0 to 7)
735  */
get_noise_factor(lc3_bits_t * bits)736 static int get_noise_factor(lc3_bits_t *bits)
737 {
738     return lc3_get_bits(bits, 3);
739 }
740 
741 
742 /* ----------------------------------------------------------------------------
743  *  Encoding
744  * -------------------------------------------------------------------------- */
745 
746 /**
747  * Bit consumption of the number of coded coefficients
748  * dt, sr          Duration, samplerate of the frame
749  * return          Bit consumpution of the number of coded coefficients
750  */
get_nbits_nq(enum lc3_dt dt,enum lc3_srate sr)751 static int get_nbits_nq(enum lc3_dt dt, enum lc3_srate sr)
752 {
753     int ne = LC3_NE(dt, sr);
754     return 4 + (ne > 32) + (ne > 64) + (ne > 128) + (ne > 256);
755 }
756 
757 /**
758  * Bit consumption of the arithmetic coder
759  * dt, sr, nbytes  Duration, samplerate and size of the frame
760  * return          Bit consumption of bitstream data
761  */
get_nbits_ac(enum lc3_dt dt,enum lc3_srate sr,int nbytes)762 static int get_nbits_ac(enum lc3_dt dt, enum lc3_srate sr, int nbytes)
763 {
764     return get_nbits_nq(dt, sr) + 3 + LC3_MIN((nbytes-1) / 160, 2);
765 }
766 
767 /**
768  * Spectrum analysis
769  */
lc3_spec_analyze(enum lc3_dt dt,enum lc3_srate sr,int nbytes,bool pitch,const lc3_tns_data_t * tns,struct lc3_spec_analysis * spec,float * x,uint16_t * xq,struct lc3_spec_side * side)770 void lc3_spec_analyze(enum lc3_dt dt, enum lc3_srate sr,
771     int nbytes, bool pitch, const lc3_tns_data_t *tns,
772     struct lc3_spec_analysis *spec, float *x,
773     uint16_t *xq, struct lc3_spec_side *side)
774 {
775     bool reset_off;
776 
777     /* --- Bit budget --- */
778 
779     const int nbits_gain = 8;
780     const int nbits_nf = 3;
781 
782     int nbits_budget = 8*nbytes - get_nbits_ac(dt, sr, nbytes) -
783         lc3_bwdet_get_nbits(sr) - lc3_ltpf_get_nbits(pitch) -
784         lc3_sns_get_nbits() - lc3_tns_get_nbits(tns) - nbits_gain - nbits_nf;
785 
786     /* --- Global gain --- */
787 
788     float nbits_off = spec->nbits_off + spec->nbits_spare;
789     nbits_off = fminf(fmaxf(nbits_off, -40), 40);
790     nbits_off = 0.8f * spec->nbits_off + 0.2f * nbits_off;
791 
792     int g_off = resolve_gain_offset(sr, nbytes);
793 
794     int g_min, g_int = estimate_gain(dt, sr,
795         x, nbits_budget, nbits_off, g_off, &reset_off, &g_min);
796 
797     /* --- Quantization --- */
798 
799     quantize(dt, sr, g_int, x, xq, &side->nq);
800 
801     int nbits = compute_nbits(dt, sr, nbytes, xq, &side->nq, 0, NULL);
802 
803     spec->nbits_off = reset_off ? 0 : nbits_off;
804     spec->nbits_spare = reset_off ? 0 : nbits_budget - nbits;
805 
806     /* --- Adjust gain and requantize --- */
807 
808     int g_adj = adjust_gain(sr, g_off + g_int,
809         nbits, nbits_budget, g_off + g_min);
810 
811     if (g_adj)
812         quantize(dt, sr, g_adj, x, xq, &side->nq);
813 
814     side->g_idx = g_int + g_adj + g_off;
815     nbits = compute_nbits(dt, sr, nbytes,
816         xq, &side->nq, nbits_budget, &side->lsb_mode);
817 }
818 
819 /**
820  * Put spectral quantization side data
821  */
lc3_spec_put_side(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,const struct lc3_spec_side * side)822 void lc3_spec_put_side(lc3_bits_t *bits,
823     enum lc3_dt dt, enum lc3_srate sr, const struct lc3_spec_side *side)
824 {
825     int nbits_nq = get_nbits_nq(dt, sr);
826 
827     lc3_put_bits(bits, LC3_MAX(side->nq >> 1, 1) - 1, nbits_nq);
828     lc3_put_bits(bits, side->lsb_mode, 1);
829     lc3_put_bits(bits, side->g_idx, 8);
830 }
831 
832 /**
833  * Encode spectral coefficients
834  */
lc3_spec_encode(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,enum lc3_bandwidth bw,int nbytes,const uint16_t * xq,const lc3_spec_side_t * side,const float * x)835 void lc3_spec_encode(lc3_bits_t *bits,
836     enum lc3_dt dt, enum lc3_srate sr, enum lc3_bandwidth bw, int nbytes,
837     const uint16_t *xq, const lc3_spec_side_t *side, const float *x)
838 {
839     bool lsb_mode = side->lsb_mode;
840     int nq = side->nq;
841 
842     put_noise_factor(bits, estimate_noise(dt, bw, xq, nq, x));
843 
844     put_quantized(bits, dt, sr, nbytes, xq, nq, lsb_mode);
845 
846     int nbits_left = lc3_get_bits_left(bits);
847 
848     if (lsb_mode)
849         put_lsb(bits, nbits_left, xq, nq);
850     else
851         put_residual(bits, nbits_left, xq, nq, x);
852 }
853 
854 
855 /* ----------------------------------------------------------------------------
856  *  Decoding
857  * -------------------------------------------------------------------------- */
858 
859 /**
860  * Get spectral quantization side data
861  */
lc3_spec_get_side(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,struct lc3_spec_side * side)862 int lc3_spec_get_side(lc3_bits_t *bits,
863     enum lc3_dt dt, enum lc3_srate sr, struct lc3_spec_side *side)
864 {
865     int nbits_nq = get_nbits_nq(dt, sr);
866     int ne = LC3_NE(dt, sr);
867 
868     side->nq = (lc3_get_bits(bits, nbits_nq) + 1) << 1;
869     side->lsb_mode = lc3_get_bit(bits);
870     side->g_idx = lc3_get_bits(bits, 8);
871 
872     return side->nq > ne ? (side->nq = ne), -1 : 0;
873 }
874 
875 /**
876  * Decode spectral coefficients
877  */
lc3_spec_decode(lc3_bits_t * bits,enum lc3_dt dt,enum lc3_srate sr,enum lc3_bandwidth bw,int nbytes,const lc3_spec_side_t * side,float * x)878 int lc3_spec_decode(lc3_bits_t *bits,
879     enum lc3_dt dt, enum lc3_srate sr, enum lc3_bandwidth bw,
880     int nbytes, const lc3_spec_side_t *side, float *x)
881 {
882     bool lsb_mode = side->lsb_mode;
883     int nq = side->nq;
884     int ret = 0;
885 
886     int nf = get_noise_factor(bits);
887     uint16_t nf_seed;
888 
889     if ((ret = get_quantized(bits, dt, sr, nbytes,
890                     nq, lsb_mode, x, &nf_seed)) < 0)
891         return ret;
892 
893     int nbits_left = lc3_get_bits_left(bits);
894 
895     if (lsb_mode)
896         get_lsb(bits, nbits_left, x, nq, &nf_seed);
897     else
898         get_residual(bits, nbits_left, x, nq);
899 
900     int g_int = side->g_idx - resolve_gain_offset(sr, nbytes);
901     float g = unquantize(dt, sr, g_int, x, nq);
902 
903     if (nq > 2 || x[0] || x[1] || side->g_idx > 0 || nf < 7)
904         fill_noise(dt, bw, nf, nf_seed, g, x, nq);
905 
906     return 0;
907 }
908