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