Skip to main content

audio_codec/
g722.rs

1#[cfg(feature = "plc")]
2use super::plc::PitchConcealer;
3use super::{CodecError, Decoder, Encoder, Sample};
4#[cfg(feature = "plc")]
5use alloc::boxed::Box;
6
7#[cfg(feature = "std")]
8use super::PcmBuf;
9
10pub enum Bitrate {
11    Mode1_64000,
12    Mode2_56000,
13    Mode3_48000,
14}
15
16// Quantization decision thresholds used in the encoder
17const QUANT_DECISION_LEVEL: [i32; 32] = [
18    0, 35, 72, 110, 150, 190, 233, 276, 323, 370, 422, 473, 530, 587, 650, 714, 786, 858, 940,
19    1023, 1121, 1219, 1339, 1458, 1612, 1765, 1980, 2195, 2557, 2919, 0, 0,
20];
21
22// Negative quantization interval indices
23const QUANT_INDEX_NEG: [i32; 32] = [
24    0, 63, 62, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11,
25    10, 9, 8, 7, 6, 5, 4, 0,
26];
27
28// Positive quantization interval indices
29const QUANT_INDEX_POS: [i32; 32] = [
30    0, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39,
31    38, 37, 36, 35, 34, 33, 32, 0,
32];
33
34// Scale factor adaptation table for low band
35const SCALE_FACTOR_ADJUST_LOW: [i32; 8] = [-60, -30, 58, 172, 334, 538, 1198, 3042];
36
37// Mapping from 4 bits of low band code to 3 bits of logarithmic scale factor
38const LOG_SCALE_FACTOR_MAP: [i32; 16] = [0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0];
39
40// Inverse logarithmic base for computing the scale factor
41const INV_LOG_BASE: [i32; 32] = [
42    2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383, 2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834,
43    2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371, 3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008,
44];
45
46// Quantizer multipliers for 4-bit low band
47const QUANT_MULT_LOW_4BIT: [i32; 16] = [
48    0, -20456, -12896, -8968, -6288, -4240, -2584, -1200, 20456, 12896, 8968, 6288, 4240, 2584,
49    1200, 0,
50];
51
52// Quantizer multipliers for 2-bit high band
53const QUANT_MULT_HIGH_2BIT: [i32; 4] = [-7408, -1616, 7408, 1616];
54
55// QMF filter coefficients for band splitting and reconstruction
56const QMF_FILTER_COEFS: [i32; 12] = [3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11];
57
58// Negative high band quantization indices
59const HIGH_QUANT_INDEX_NEG: [i32; 3] = [0, 1, 0];
60
61// Positive high band quantization indices
62const HIGH_QUANT_INDEX_POS: [i32; 3] = [0, 3, 2];
63
64/// Delay through the QMF analysis and synthesis filters, in 16 kHz samples.
65#[cfg(feature = "plc")]
66const QMF_DELAY: usize = 22;
67
68// Scale factor adaptation table for high band
69const SCALE_FACTOR_ADJUST_HIGH: [i32; 3] = [0, -214, 798];
70
71// Mapping from 2 bits of high band code to 2 bits of logarithmic scale factor
72const HIGH_LOG_SCALE_MAP: [i32; 4] = [2, 1, 2, 1];
73
74// Quantizer multipliers for 5-bit quantization (56kbps mode)
75const QUANT_MULT_56K: [i32; 32] = [
76    -280, -280, -23352, -17560, -14120, -11664, -9752, -8184, -6864, -5712, -4696, -3784, -2960,
77    -2208, -1520, -880, 23352, 17560, 14120, 11664, 9752, 8184, 6864, 5712, 4696, 3784, 2960, 2208,
78    1520, 880, 280, -280,
79];
80
81// Quantizer multipliers for 6-bit quantization (64kbps mode)
82const QUANT_MULT_64K: [i32; 64] = [
83    -136, -136, -136, -136, -24808, -21904, -19008, -16704, -14984, -13512, -12280, -11192, -10232,
84    -9360, -8576, -7856, -7192, -6576, -6000, -5456, -4944, -4464, -4008, -3576, -3168, -2776,
85    -2400, -2032, -1688, -1360, -1040, -728, 24808, 21904, 19008, 16704, 14984, 13512, 12280,
86    11192, 10232, 9360, 8576, 7856, 7192, 6576, 6000, 5456, 4944, 4464, 4008, 3576, 3168, 2776,
87    2400, 2032, 1688, 1360, 1040, 728, 432, 136, -432, -136,
88];
89
90impl Bitrate {
91    fn bits_per_sample(&self) -> i32 {
92        match self {
93            Bitrate::Mode1_64000 => 8,
94            Bitrate::Mode2_56000 => 7,
95            Bitrate::Mode3_48000 => 6,
96        }
97    }
98}
99
100/// G.722 ADPCM band state structure used in the codec
101/// Each band (lower and upper) uses an independent state structure
102#[derive(Default, Clone)]
103struct G722Band {
104    /// Current signal prediction value (signal estimate)
105    signal_estimate: i32,
106    /// Pole filter output (result from IIR filter part)
107    pole_filter_output: i32,
108    /// Zero filter output (result from FIR filter part)
109    zero_filter_output: i32,
110    /// Reconstructed signal history [current, previous, previous-1]
111    reconstructed_signal: [i32; 3],
112    /// Pole filter coefficients [unused(0), a1, a2]
113    pole_coefficients: [i32; 3],
114    /// Temporary pole filter coefficients [unused(0), a1', a2']
115    pole_coefficients_temp: [i32; 3],
116    /// Partially reconstructed signal history [current, previous, previous-1]
117    partial_reconstructed: [i32; 3],
118    /// Difference signal history [current, previous, ..., previous-5]
119    difference_signal: [i32; 7],
120    /// Zero filter coefficients [unused(0), b1, b2, ..., b6]
121    zero_coefficients: [i32; 7],
122    /// Temporary zero filter coefficients [unused(0), b1', b2', ..., b6']
123    zero_coefficients_temp: [i32; 7],
124    /// Log scale factor (used for quantization and dequantization)
125    log_scale_factor: i32,
126    /// Quantizer step size (used for adaptive quantization)
127    quantizer_step_size: i32,
128}
129
130#[inline(always)]
131fn saturate(amp: i32) -> i32 {
132    amp.clamp(i16::MIN as i32, i16::MAX as i32)
133}
134
135/// Process Block 4 operations for G.722 ADPCM algorithm
136/// This function performs the predictor adaptation and reconstruction steps
137/// as defined in the G.722 standard
138#[inline]
139fn block4(band: &mut G722Band, d: i32) {
140    // Block 4, RECONS - Reconstruct the signal
141    band.difference_signal[0] = d;
142    band.reconstructed_signal[0] = saturate(band.signal_estimate + d);
143
144    // Block 4, PARREC - Partial reconstruction
145    let partial_rec0 = saturate(band.zero_filter_output + d);
146    band.partial_reconstructed[0] = partial_rec0;
147
148    // Block 4, UPPOL2 - Update second predictor coefficient
149    let s0 = partial_rec0 >> 15;
150    let s1 = band.partial_reconstructed[1] >> 15;
151    let s2 = band.partial_reconstructed[2] >> 15;
152
153    let a1_scaled = saturate(band.pole_coefficients[1] << 2);
154
155    let mut a2_update = if s0 == s1 { -a1_scaled } else { a1_scaled };
156    a2_update = a2_update.min(32767);
157
158    let mut a2_adj = a2_update >> 7;
159    a2_adj += if s0 == s2 { 128 } else { -128 };
160    a2_adj += (band.pole_coefficients[2] * 32512) >> 15;
161
162    band.pole_coefficients_temp[2] = a2_adj.clamp(-12288, 12288);
163
164    // Block 4, UPPOL1 - Update first predictor coefficient
165    let sign_factor = if s0 == s1 { 192 } else { -192 };
166    let leakage = (band.pole_coefficients[1] * 32640) >> 15;
167    band.pole_coefficients_temp[1] = saturate(sign_factor + leakage);
168
169    let limit = saturate(15360 - band.pole_coefficients_temp[2]);
170    band.pole_coefficients_temp[1] = band.pole_coefficients_temp[1].clamp(-limit, limit);
171
172    // Block 4, UPZERO - Update zero section (FIR) coefficients
173    let step_size = if d == 0 { 0 } else { 128 };
174    let sd = d >> 15;
175
176    {
177        macro_rules! update_zero {
178            ($i:expr) => {
179                let sz = band.difference_signal[$i] >> 15;
180                let adj = if sz == sd { step_size } else { -step_size };
181                let leakage = (band.zero_coefficients[$i] * 32640) >> 15;
182                band.zero_coefficients_temp[$i] = saturate(adj + leakage);
183            };
184        }
185        update_zero!(1);
186        update_zero!(2);
187        update_zero!(3);
188        update_zero!(4);
189        update_zero!(5);
190        update_zero!(6);
191    }
192
193    // Block 4, DELAYA - Delay updates for filter memory
194    band.difference_signal[6] = band.difference_signal[5];
195    band.difference_signal[5] = band.difference_signal[4];
196    band.difference_signal[4] = band.difference_signal[3];
197    band.difference_signal[3] = band.difference_signal[2];
198    band.difference_signal[2] = band.difference_signal[1];
199    band.difference_signal[1] = d;
200
201    band.zero_coefficients[1] = band.zero_coefficients_temp[1];
202    band.zero_coefficients[2] = band.zero_coefficients_temp[2];
203    band.zero_coefficients[3] = band.zero_coefficients_temp[3];
204    band.zero_coefficients[4] = band.zero_coefficients_temp[4];
205    band.zero_coefficients[5] = band.zero_coefficients_temp[5];
206    band.zero_coefficients[6] = band.zero_coefficients_temp[6];
207
208    band.reconstructed_signal[2] = band.reconstructed_signal[1];
209    band.reconstructed_signal[1] = band.reconstructed_signal[0];
210    band.partial_reconstructed[2] = band.partial_reconstructed[1];
211    band.partial_reconstructed[1] = partial_rec0;
212
213    band.pole_coefficients[1] = band.pole_coefficients_temp[1];
214    band.pole_coefficients[2] = band.pole_coefficients_temp[2];
215
216    // Block 4, FILTEP - Pole section (IIR) filtering
217    let r1_adj = saturate(band.reconstructed_signal[1] << 1);
218    let pole1 = (band.pole_coefficients[1] * r1_adj) >> 15;
219
220    let r2_adj = saturate(band.reconstructed_signal[2] << 1);
221    let pole2 = (band.pole_coefficients[2] * r2_adj) >> 15;
222
223    band.pole_filter_output = saturate(pole1 + pole2);
224
225    // Block 4, FILTEZ - Zero section (FIR) filtering
226    let mut zero_out = (band.zero_coefficients[1] * saturate(band.difference_signal[1] << 1)) >> 15;
227    zero_out += (band.zero_coefficients[2] * saturate(band.difference_signal[2] << 1)) >> 15;
228    zero_out += (band.zero_coefficients[3] * saturate(band.difference_signal[3] << 1)) >> 15;
229    zero_out += (band.zero_coefficients[4] * saturate(band.difference_signal[4] << 1)) >> 15;
230    zero_out += (band.zero_coefficients[5] * saturate(band.difference_signal[5] << 1)) >> 15;
231    zero_out += (band.zero_coefficients[6] * saturate(band.difference_signal[6] << 1)) >> 15;
232
233    band.zero_filter_output = saturate(zero_out);
234
235    // Block 4, PREDIC - Prediction
236    band.signal_estimate = saturate(band.pole_filter_output + band.zero_filter_output);
237}
238
239pub struct G722Encoder {
240    packed: bool,
241    eight_k: bool,
242    bits_per_sample: i32,
243    x: [i32; 24],
244    band: [G722Band; 2],
245    out_buffer: u32,
246    out_bits: i32,
247}
248
249pub struct G722Decoder {
250    packed: bool,
251    eight_k: bool,
252    bits_per_sample: i32,
253    x: [i32; 24],
254    band: [G722Band; 2],
255    in_buffer: u32,
256    in_bits: i32,
257    /// Packet loss concealment, once enabled.
258    #[cfg(feature = "plc")]
259    plc: Option<Box<PitchConcealer>>,
260}
261
262impl G722Encoder {
263    pub fn new() -> Self {
264        Self::with_options(Bitrate::Mode1_64000, false, false)
265    }
266
267    /// Creates an encoder with specified bitrate and options
268    pub fn with_options(rate: Bitrate, eight_k: bool, packed: bool) -> Self {
269        let mut encoder = Self {
270            packed,
271            eight_k,
272            bits_per_sample: rate.bits_per_sample(),
273            x: [0; 24],
274            band: [G722Band::default(), G722Band::default()],
275            out_buffer: 0,
276            out_bits: 0,
277        };
278
279        // Initialize band states with correct starting values
280        encoder.band[0].log_scale_factor = 32 << 2; // Initial det value for lower band
281        encoder.band[1].log_scale_factor = 8 << 2; // Initial det value for upper band
282
283        encoder
284    }
285
286    /// Encode 16-bit PCM samples into G.722 format
287    /// This function follows the G.722 standard algorithm exactly
288    fn g722_encode_into(&mut self, amp: &[i16], out: &mut [u8]) -> Result<usize, CodecError> {
289        // Cursor over `out`. We track a byte position plus a sub-byte bit
290        // buffer used only in packed mode. The cursor returns an error if
291        // `out` cannot accept another byte.
292        struct OutCursor<'a> {
293            buf: &'a mut [u8],
294            byte_pos: usize,
295            bit_buffer: u32,
296            bit_count: i32,
297        }
298        impl<'a> OutCursor<'a> {
299            #[inline]
300            fn push_byte(&mut self, b: u8) -> Result<(), CodecError> {
301                if self.byte_pos >= self.buf.len() {
302                    return Err(CodecError::BufferTooSmall);
303                }
304                self.buf[self.byte_pos] = b;
305                self.byte_pos += 1;
306                Ok(())
307            }
308        }
309
310        let packed = self.packed;
311        let bits_per_sample = self.bits_per_sample;
312        let mut cursor = OutCursor {
313            buf: out,
314            byte_pos: 0,
315            bit_buffer: self.out_buffer,
316            bit_count: self.out_bits,
317        };
318
319        // Helper closure: write one `bits_per_sample`-bit code.
320        let output_code = |code: i32, cursor: &mut OutCursor<'_>| -> Result<(), CodecError> {
321            if packed {
322                cursor.bit_buffer |= (code as u32) << cursor.bit_count;
323                cursor.bit_count += bits_per_sample;
324                if cursor.bit_count >= 8 {
325                    cursor.push_byte((cursor.bit_buffer & 0xFF) as u8)?;
326                    cursor.bit_count -= 8;
327                    cursor.bit_buffer >>= 8;
328                }
329            } else {
330                cursor.push_byte(code as u8)?;
331            }
332            Ok(())
333        };
334
335        let mut input_idx = 0usize;
336
337        if self.eight_k {
338            while input_idx < amp.len() {
339                // 8kHz mode - Just use input directly with scaling
340                let xlow = amp[input_idx] as i32 >> 1;
341                input_idx += 1;
342
343                // 8kHz mode - only low band matters
344                let code = self.encode_low_band(xlow, true);
345                output_code(code, &mut cursor)?;
346            }
347        } else {
348            // Process all input samples in 16kHz mode
349            // Use chunks_exact(2) for better performance and to avoid bound checks
350            let chunks = amp.chunks_exact(2);
351            let rem = chunks.remainder();
352
353            for chunk in chunks {
354                // Shuffle buffer down to make room for new samples
355                self.x.copy_within(2..24, 0);
356
357                // Add new samples to buffer
358                self.x[22] = chunk[0] as i32;
359                self.x[23] = chunk[1] as i32;
360
361                // Apply QMF filter to split input into bands
362                // Unrolled loop for 12 coefficients
363                let mut sumodd = self.x[0] * QMF_FILTER_COEFS[0];
364                sumodd += self.x[2] * QMF_FILTER_COEFS[1];
365                sumodd += self.x[4] * QMF_FILTER_COEFS[2];
366                sumodd += self.x[6] * QMF_FILTER_COEFS[3];
367                sumodd += self.x[8] * QMF_FILTER_COEFS[4];
368                sumodd += self.x[10] * QMF_FILTER_COEFS[5];
369                sumodd += self.x[12] * QMF_FILTER_COEFS[6];
370                sumodd += self.x[14] * QMF_FILTER_COEFS[7];
371                sumodd += self.x[16] * QMF_FILTER_COEFS[8];
372                sumodd += self.x[18] * QMF_FILTER_COEFS[9];
373                sumodd += self.x[20] * QMF_FILTER_COEFS[10];
374                sumodd += self.x[22] * QMF_FILTER_COEFS[11];
375
376                let mut sumeven = self.x[1] * QMF_FILTER_COEFS[11];
377                sumeven += self.x[3] * QMF_FILTER_COEFS[10];
378                sumeven += self.x[5] * QMF_FILTER_COEFS[9];
379                sumeven += self.x[7] * QMF_FILTER_COEFS[8];
380                sumeven += self.x[9] * QMF_FILTER_COEFS[7];
381                sumeven += self.x[11] * QMF_FILTER_COEFS[6];
382                sumeven += self.x[13] * QMF_FILTER_COEFS[5];
383                sumeven += self.x[15] * QMF_FILTER_COEFS[4];
384                sumeven += self.x[17] * QMF_FILTER_COEFS[3];
385                sumeven += self.x[19] * QMF_FILTER_COEFS[2];
386                sumeven += self.x[21] * QMF_FILTER_COEFS[1];
387                sumeven += self.x[23] * QMF_FILTER_COEFS[0];
388
389                // Scale filter outputs to get low and high bands
390                let xlow = (sumeven + sumodd) >> 14;
391                let xhigh = (sumeven - sumodd) >> 14;
392
393                // 16kHz mode - encode both bands
394                let ilow = self.encode_low_band(xlow, false);
395                let ihigh = self.encode_high_band(xhigh);
396                let code = (ihigh << 6 | ilow) >> (8 - self.bits_per_sample);
397
398                // Output the encoded code
399                output_code(code, &mut cursor)?;
400            }
401
402            if !rem.is_empty() {
403                self.x.copy_within(2..24, 0);
404                self.x[22] = rem[0] as i32;
405                self.x[23] = 0;
406                let mut sumodd = self.x[0] * QMF_FILTER_COEFS[0];
407                sumodd += self.x[2] * QMF_FILTER_COEFS[1];
408                sumodd += self.x[4] * QMF_FILTER_COEFS[2];
409                sumodd += self.x[6] * QMF_FILTER_COEFS[3];
410                sumodd += self.x[8] * QMF_FILTER_COEFS[4];
411                sumodd += self.x[10] * QMF_FILTER_COEFS[5];
412                sumodd += self.x[12] * QMF_FILTER_COEFS[6];
413                sumodd += self.x[14] * QMF_FILTER_COEFS[7];
414                sumodd += self.x[16] * QMF_FILTER_COEFS[8];
415                sumodd += self.x[18] * QMF_FILTER_COEFS[9];
416                sumodd += self.x[20] * QMF_FILTER_COEFS[10];
417                sumodd += self.x[22] * QMF_FILTER_COEFS[11];
418
419                let mut sumeven = self.x[1] * QMF_FILTER_COEFS[11];
420                sumeven += self.x[3] * QMF_FILTER_COEFS[10];
421                sumeven += self.x[5] * QMF_FILTER_COEFS[9];
422                sumeven += self.x[7] * QMF_FILTER_COEFS[8];
423                sumeven += self.x[9] * QMF_FILTER_COEFS[7];
424                sumeven += self.x[11] * QMF_FILTER_COEFS[6];
425                sumeven += self.x[13] * QMF_FILTER_COEFS[5];
426                sumeven += self.x[15] * QMF_FILTER_COEFS[4];
427                sumeven += self.x[17] * QMF_FILTER_COEFS[3];
428                sumeven += self.x[19] * QMF_FILTER_COEFS[2];
429                sumeven += self.x[21] * QMF_FILTER_COEFS[1];
430                sumeven += self.x[23] * QMF_FILTER_COEFS[0];
431
432                let xlow = (sumeven + sumodd) >> 14;
433                let xhigh = (sumeven - sumodd) >> 14;
434                let ilow = self.encode_low_band(xlow, false);
435                let ihigh = self.encode_high_band(xhigh);
436                let code = (ihigh << 6 | ilow) >> (8 - self.bits_per_sample);
437                output_code(code, &mut cursor)?;
438            }
439        }
440
441        // Handle any remaining bits in the output buffer
442        if self.packed && cursor.bit_count > 0 {
443            cursor.push_byte((cursor.bit_buffer & 0xFF) as u8)?;
444        }
445
446        // Persist bit buffer state for streaming across calls.
447        self.out_buffer = cursor.bit_buffer;
448        self.out_bits = cursor.bit_count;
449
450        Ok(cursor.byte_pos)
451    }
452
453    /// Encode low band sample and update state
454    /// Returns the encoded low band bits
455    #[inline]
456    fn encode_low_band(&mut self, xlow: i32, is_eight_k: bool) -> i32 {
457        // Block 1L, SUBTRA - Calculate difference signal
458        let el = saturate(xlow - self.band[0].signal_estimate);
459
460        // Block 1L, QUANTL - Quantize difference signal
461        let wd = el.abs().wrapping_sub((el >> 31) & 1);
462
463        // Find quantization interval using linear search. This early-exits on
464        // the first (typically low) crossing, which beats a binary search on
465        // real speech — measured on-device (2026-10-08).
466        let lsf = self.band[0].log_scale_factor;
467        let mut quantization_idx = 1;
468        while quantization_idx < 30 {
469            let decision_level = (QUANT_DECISION_LEVEL[quantization_idx] * lsf) >> 12;
470            if wd < decision_level {
471                break;
472            }
473            quantization_idx += 1;
474        }
475
476        // Select output bits based on sign
477        let ilow = if el < 0 {
478            QUANT_INDEX_NEG[quantization_idx]
479        } else {
480            QUANT_INDEX_POS[quantization_idx]
481        };
482
483        // Block 2L, INVQAL - Inverse quantize for prediction
484        let ril = ilow >> 2;
485        let wd2 = QUANT_MULT_LOW_4BIT[ril as usize];
486        let dlow = (self.band[0].log_scale_factor * wd2) >> 15;
487
488        // Block 3L, LOGSCL - Update scale factor
489        let il4 = LOG_SCALE_FACTOR_MAP[ril as usize];
490        let mut nb = (self.band[0].quantizer_step_size * 127) >> 7;
491        nb += SCALE_FACTOR_ADJUST_LOW[il4 as usize];
492        self.band[0].quantizer_step_size = nb.clamp(0, 18432);
493
494        // Block 3L, SCALEL - Compute new quantizer scale factor
495        let wd1 = self.band[0].quantizer_step_size >> 6 & 31;
496        let wd2 = 8 - (self.band[0].quantizer_step_size >> 11);
497        let wd3 = if wd2 < 0 {
498            INV_LOG_BASE[wd1 as usize] << -wd2
499        } else {
500            INV_LOG_BASE[wd1 as usize] >> wd2
501        };
502        self.band[0].log_scale_factor = wd3 << 2;
503
504        // Apply predictor adaptation (ADPCM core algorithm)
505        block4(&mut self.band[0], dlow);
506
507        // Return appropriate value based on mode
508        if is_eight_k {
509            ((0xc0 | ilow) >> 8) - self.bits_per_sample
510        } else {
511            ilow
512        }
513    }
514
515    /// Encode high band sample and update state
516    /// Returns the encoded high band bits
517    #[inline]
518    fn encode_high_band(&mut self, xhigh: i32) -> i32 {
519        // Block 1H, SUBTRA - Calculate difference signal
520        let eh = saturate(xhigh - self.band[1].signal_estimate);
521
522        // Block 1H, QUANTH - Quantize difference signal
523        let wd = if eh >= 0 { eh } else { -(eh + 1) };
524        let decision_level = (564 * self.band[1].log_scale_factor) >> 12;
525
526        // Determine quantization level for high band (2-bit)
527        let mih = if wd >= decision_level { 2 } else { 1 };
528        let ihigh = if eh < 0 {
529            HIGH_QUANT_INDEX_NEG[mih as usize]
530        } else {
531            HIGH_QUANT_INDEX_POS[mih as usize]
532        };
533
534        // Block 2H, INVQAH - Inverse quantize for prediction
535        let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
536        let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;
537
538        // Block 3H, LOGSCH - Update scale factor
539        let ih2 = HIGH_LOG_SCALE_MAP[ihigh as usize];
540        let mut nb = (self.band[1].quantizer_step_size * 127) >> 7;
541        nb += SCALE_FACTOR_ADJUST_HIGH[ih2 as usize];
542        self.band[1].quantizer_step_size = nb.clamp(0, 22528);
543
544        // Block 3H, SCALEH - Compute quantizer scale factor
545        let wd1 = self.band[1].quantizer_step_size >> 6 & 31;
546        let wd2 = 10 - (self.band[1].quantizer_step_size >> 11);
547        let wd3 = if wd2 < 0 {
548            INV_LOG_BASE[wd1 as usize] << -wd2
549        } else {
550            INV_LOG_BASE[wd1 as usize] >> wd2
551        };
552        self.band[1].log_scale_factor = wd3 << 2;
553
554        // Apply predictor adaptation (ADPCM core algorithm)
555        block4(&mut self.band[1], dhigh);
556
557        ihigh
558    }
559}
560
561impl G722Decoder {
562    pub fn new() -> Self {
563        Self::with_options(Bitrate::Mode1_64000, false, false)
564    }
565
566    pub fn with_options(rate: Bitrate, packed: bool, eight_k: bool) -> Self {
567        Self {
568            packed,
569            eight_k,
570            bits_per_sample: rate.bits_per_sample(),
571            x: Default::default(),
572            band: Default::default(),
573            in_buffer: 0,
574            in_bits: 0,
575            #[cfg(feature = "plc")]
576            plc: None,
577        }
578    }
579
580    /// Extracts the next G.722 code from the input data stream
581    #[inline]
582    fn extract_code(&mut self, data: &[u8], idx: &mut usize) -> i32 {
583        if self.packed {
584            // When packed, bits are combined across bytes
585            if self.in_bits < self.bits_per_sample {
586                self.in_buffer |= (data[*idx] as u32) << self.in_bits;
587                *idx += 1;
588                self.in_bits += 8;
589            }
590            let code = (self.in_buffer & ((1 << self.bits_per_sample) - 1) as u32) as i32;
591            self.in_buffer >>= self.bits_per_sample;
592            self.in_bits -= self.bits_per_sample;
593            code
594        } else {
595            // Direct byte-based access when not packed
596            let code = data[*idx] as i32;
597            *idx += 1;
598            code
599        }
600    }
601
602    /// Parses the G.722 code into low-band word and high-band index based on bit rate
603    #[inline]
604    fn parse_code(&self, code: i32) -> (i32, i32, i32) {
605        // Returns (wd1, ihigh, wd2) tuple: low-band word, high-band index, and scaled value
606        match self.bits_per_sample {
607            7 => {
608                // 56 kbit/s mode
609                let wd1 = code & 0x1f;
610                let ihigh = (code >> 5) & 0x3;
611                let wd2 = QUANT_MULT_56K[wd1 as usize];
612                (wd1 >> 1, ihigh, wd2)
613            }
614            6 => {
615                // 48 kbit/s mode
616                let wd1 = code & 0xf;
617                let ihigh = (code >> 4) & 0x3;
618                let wd2 = QUANT_MULT_LOW_4BIT[wd1 as usize];
619                (wd1, ihigh, wd2)
620            }
621            _ => {
622                // 64 kbit/s mode (default)
623                let wd1 = code & 0x3f;
624                let ihigh = (code >> 6) & 0x3;
625                let wd2 = QUANT_MULT_64K[wd1 as usize];
626                (wd1 >> 2, ihigh, wd2)
627            }
628        }
629    }
630
631    /// Process the low band component of the G.722 stream
632    #[inline]
633    fn process_low_band(&mut self, wd1: i32, wd2: i32) -> i32 {
634        // Block 5L, LOW BAND INVQBL - Inverse quantization for low band
635        let dequant = (self.band[0].log_scale_factor * wd2) >> 15;
636
637        // Block 5L, RECONS - Reconstruction of low band signal
638        let rlow = self.band[0].signal_estimate + dequant;
639
640        // Block 6L, LIMIT - Limiting to valid range
641        let rlow = rlow.clamp(-16384, 16383);
642
643        // Block 2L, INVQAL - Inverse adaptive quantizer for prediction
644        let wd2 = QUANT_MULT_LOW_4BIT[wd1 as usize];
645        let dlowt = (self.band[0].log_scale_factor * wd2) >> 15;
646
647        // Block 3L, LOGSCL - Compute log scale factor
648        let wd2 = LOG_SCALE_FACTOR_MAP[wd1 as usize];
649        let mut wd1 = (self.band[0].quantizer_step_size * 127) >> 7;
650        wd1 += SCALE_FACTOR_ADJUST_LOW[wd2 as usize];
651        self.band[0].quantizer_step_size = wd1.clamp(0, 18432);
652
653        // Block 3L, SCALEL - Compute quantizer scale factor
654        let wd1 = (self.band[0].quantizer_step_size >> 6) & 31;
655        let wd2 = 8 - (self.band[0].quantizer_step_size >> 11);
656        let wd3 = if wd2 < 0 {
657            INV_LOG_BASE[wd1 as usize] << -wd2
658        } else {
659            INV_LOG_BASE[wd1 as usize] >> wd2
660        };
661        self.band[0].log_scale_factor = wd3 << 2;
662
663        // Apply predictor adaptation
664        block4(&mut self.band[0], dlowt);
665
666        rlow
667    }
668
669    /// Process the high band component of the G.722 stream
670    #[inline]
671    fn process_high_band(&mut self, ihigh: i32) -> i32 {
672        // Block 2H, INVQAH - Inverse quantizer for high band
673        let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
674        let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;
675
676        // Block 5H, RECONS - Reconstruction of high band signal
677        let rhigh = dhigh + self.band[1].signal_estimate;
678
679        // Block 6H, LIMIT - Limiting to valid range
680        let rhigh = rhigh.clamp(-16384, 16383);
681
682        // Block 2H, INVQAH - Adaptation logic
683        let wd2 = HIGH_LOG_SCALE_MAP[ihigh as usize];
684        let mut wd1 = (self.band[1].quantizer_step_size * 127) >> 7;
685        wd1 += SCALE_FACTOR_ADJUST_HIGH[wd2 as usize];
686        self.band[1].quantizer_step_size = wd1.clamp(0, 22528);
687
688        // Block 3H, SCALEH - Compute quantizer scale factor
689        let wd1 = (self.band[1].quantizer_step_size >> 6) & 31;
690        let wd2 = 10 - (self.band[1].quantizer_step_size >> 11);
691        let wd3 = if wd2 < 0 {
692            INV_LOG_BASE[wd1 as usize] << -wd2
693        } else {
694            INV_LOG_BASE[wd1 as usize] >> wd2
695        };
696        self.band[1].log_scale_factor = wd3 << 2;
697
698        // Apply predictor adaptation
699        block4(&mut self.band[1], dhigh);
700
701        rhigh
702    }
703
704    /// Apply QMF synthesis filter to combine low and high band signals
705    #[inline]
706    fn apply_qmf_synthesis(&mut self, rlow: i32, rhigh: i32) -> [i16; 2] {
707        // Shift filter state
708        self.x.copy_within(2..24, 0);
709
710        // Set new filter state values
711        self.x[22] = rlow + rhigh;
712        self.x[23] = rlow - rhigh;
713
714        // Apply QMF synthesis filter (unrolled loop for 12 coefficients)
715        let mut xout2 = self.x[0] * QMF_FILTER_COEFS[0];
716        xout2 += self.x[2] * QMF_FILTER_COEFS[1];
717        xout2 += self.x[4] * QMF_FILTER_COEFS[2];
718        xout2 += self.x[6] * QMF_FILTER_COEFS[3];
719        xout2 += self.x[8] * QMF_FILTER_COEFS[4];
720        xout2 += self.x[10] * QMF_FILTER_COEFS[5];
721        xout2 += self.x[12] * QMF_FILTER_COEFS[6];
722        xout2 += self.x[14] * QMF_FILTER_COEFS[7];
723        xout2 += self.x[16] * QMF_FILTER_COEFS[8];
724        xout2 += self.x[18] * QMF_FILTER_COEFS[9];
725        xout2 += self.x[20] * QMF_FILTER_COEFS[10];
726        xout2 += self.x[22] * QMF_FILTER_COEFS[11];
727
728        let mut xout1 = self.x[1] * QMF_FILTER_COEFS[11];
729        xout1 += self.x[3] * QMF_FILTER_COEFS[10];
730        xout1 += self.x[5] * QMF_FILTER_COEFS[9];
731        xout1 += self.x[7] * QMF_FILTER_COEFS[8];
732        xout1 += self.x[9] * QMF_FILTER_COEFS[7];
733        xout1 += self.x[11] * QMF_FILTER_COEFS[6];
734        xout1 += self.x[13] * QMF_FILTER_COEFS[5];
735        xout1 += self.x[15] * QMF_FILTER_COEFS[4];
736        xout1 += self.x[17] * QMF_FILTER_COEFS[3];
737        xout1 += self.x[19] * QMF_FILTER_COEFS[2];
738        xout1 += self.x[21] * QMF_FILTER_COEFS[1];
739        xout1 += self.x[23] * QMF_FILTER_COEFS[0];
740
741        // Return reconstructed samples with proper scaling
742        [saturate(xout1 >> 11) as i16, saturate(xout2 >> 11) as i16]
743    }
744
745    /// Decodes a G.722 frame into the caller-provided buffer.
746    ///
747    /// Returns the number of samples written to `out`. Returns
748    /// [`CodecError::BufferTooSmall`] if `out` cannot accept all decoded
749    /// samples.
750    pub fn decode_frame_into(
751        &mut self,
752        data: &[u8],
753        out: &mut [Sample],
754    ) -> Result<usize, CodecError> {
755        let mut written = 0usize;
756        let mut idx = 0usize;
757
758        if self.eight_k {
759            while idx < data.len() {
760                let code = self.extract_code(data, &mut idx);
761                let (wd1, _, wd2) = self.parse_code(code);
762                let rlow = self.process_low_band(wd1, wd2);
763                if written >= out.len() {
764                    return Err(CodecError::BufferTooSmall);
765                }
766                out[written] = (rlow << 1) as i16;
767                written += 1;
768            }
769        } else {
770            while idx < data.len() {
771                let code = self.extract_code(data, &mut idx);
772                let (wd1, ihigh, wd2) = self.parse_code(code);
773                let rlow = self.process_low_band(wd1, wd2);
774                let rhigh = self.process_high_band(ihigh);
775                let pcm = self.apply_qmf_synthesis(rlow, rhigh);
776                if out.len() < written + 2 {
777                    return Err(CodecError::BufferTooSmall);
778                }
779                out[written] = pcm[0];
780                out[written + 1] = pcm[1];
781                written += 2;
782            }
783        }
784        #[cfg(feature = "plc")]
785        if let Some(plc) = &mut self.plc {
786            plc.on_decoded(&mut out[..written]);
787        }
788        Ok(written)
789    }
790
791    /// Carry the ADPCM state through a loss along `signal`, the concealment
792    /// at full level: encode it from the decoder's own state (encoder and
793    /// decoder adapt identically on the same codes) and decode the codes,
794    /// discarding the audio. Left alone, the state would stay where the
795    /// loss began while the sender's moved on, and the first packets after
796    /// the loss would decode with a mismatched predictor and step size.
797    #[cfg(feature = "plc")]
798    fn follow(&mut self, signal: &[Sample], plc: &PitchConcealer) {
799        // The decoder's output lags the sender's input by the QMF filter
800        // pair's delay. The sender's state at the end of the lost packet
801        // has seen input that much further on, so encode the concealment
802        // shifted by it: the analysis history ends `delay` samples into
803        // `signal`, and the encoding runs `delay` samples past its end.
804        let delay = if self.eight_k { 0 } else { QMF_DELAY };
805        let n = signal.len();
806        let ahead: [Sample; QMF_DELAY] = core::array::from_fn(|k| plc.peek(k));
807        let at = |i: usize| if i < n { signal[i] } else { ahead[i - n] };
808        let mut encoder = G722Encoder {
809            packed: false,
810            eight_k: self.eight_k,
811            bits_per_sample: self.bits_per_sample,
812            x: [0; 24],
813            band: self.band.clone(),
814            out_buffer: 0,
815            out_bits: 0,
816        };
817        // full level, like `signal`: once the loss is fading, the audio
818        // played before this frame is not
819        let played = plc.lead();
820        for (j, x) in encoder.x.iter_mut().enumerate() {
821            // input-timeline sample j of the history window
822            let i = j + delay;
823            *x = if i < 24 { played[i] } else { at(i - 24) } as i32;
824        }
825        let mut chunk = [0 as Sample; 64];
826        let mut codes = [0u8; 64];
827        let mut i = delay;
828        while i < n + delay {
829            let len = (n + delay - i).min(64);
830            for (k, c) in chunk[..len].iter_mut().enumerate() {
831                *c = at(i + k);
832            }
833            i += len;
834            let Ok(written) = encoder.g722_encode_into(&chunk[..len], &mut codes) else {
835                return;
836            };
837            for &code in &codes[..written] {
838                let (wd1, ihigh, wd2) = self.parse_code(code as i32);
839                let rlow = self.process_low_band(wd1, wd2);
840                if !self.eight_k {
841                    let rhigh = self.process_high_band(ihigh);
842                    self.apply_qmf_synthesis(rlow, rhigh);
843                }
844            }
845        }
846    }
847
848    /// Decodes a G.722 frame into a freshly-allocated `Vec`.
849    ///
850    /// Only available with the `std` feature.
851    #[cfg(feature = "std")]
852    pub fn decode_frame(&mut self, data: &[u8]) -> PcmBuf {
853        let max = self.max_decode_samples(data.len());
854        let mut out = vec![0i16; max];
855        match self.decode_frame_into(data, &mut out) {
856            Ok(n) => {
857                out.truncate(n);
858                out
859            }
860            Err(_) => Vec::new(),
861        }
862    }
863}
864
865impl Default for G722Encoder {
866    fn default() -> Self {
867        Self::new()
868    }
869}
870
871impl Default for G722Decoder {
872    fn default() -> Self {
873        Self::new()
874    }
875}
876
877impl Encoder for G722Encoder {
878    fn encode_into(&mut self, samples: &[Sample], out: &mut [u8]) -> Result<usize, CodecError> {
879        self.g722_encode_into(samples, out)
880    }
881
882    fn max_encode_bytes(&self, n_samples: usize) -> usize {
883        // Non-packed: one byte per 16kHz code. 8kHz mode emits one byte per
884        // sample. Use the larger bound so the caller always allocates enough.
885        if self.eight_k {
886            n_samples + 1
887        } else {
888            n_samples / 2 + 1
889        }
890    }
891
892    fn sample_rate(&self) -> u32 {
893        16000 // G.722 encoding sample rate is 16kHz
894    }
895
896    fn channels(&self) -> u16 {
897        1 // G.722 is mono encoding
898    }
899}
900
901impl Decoder for G722Decoder {
902    fn decode_into(&mut self, data: &[u8], out: &mut [Sample]) -> Result<usize, CodecError> {
903        self.decode_frame_into(data, out)
904    }
905
906    /// Conceals with G.711 Appendix I style pitch repetition, carrying the
907    /// ADPCM state along the concealment (in the spirit of G.722 Appendices
908    /// III/IV, which also keep the decoder state in step through a loss).
909    #[cfg(feature = "plc")]
910    fn conceal_into(&mut self, samples: usize, out: &mut [Sample]) -> Result<usize, CodecError> {
911        // taken out for the loss, so `follow` can decode while reading it
912        let Some(mut plc) = self.plc.take() else {
913            return Ok(0);
914        };
915        let Some(out) = out.get_mut(..samples) else {
916            self.plc = Some(plc);
917            return Err(CodecError::BufferTooSmall);
918        };
919        // once the loss has faded out there's no telling where the sender
920        // is: leave the state where the concealment last heard it
921        let audible = plc.audible();
922        if plc.extrapolate(out) && audible {
923            self.follow(out, &plc);
924        }
925        plc.fade(out);
926        self.plc = Some(plc);
927        Ok(samples)
928    }
929
930    #[cfg(feature = "plc")]
931    fn enable_concealment(&mut self) -> bool {
932        let rate = if self.eight_k { 8000 } else { 16000 };
933        self.plc
934            .get_or_insert_with(|| Box::new(PitchConcealer::new(rate)));
935        true
936    }
937
938    fn max_decode_samples(&self, n_bytes: usize) -> usize {
939        // 16kHz mode: 2 samples per byte. 8kHz mode: 1 sample per byte.
940        // Use the larger bound so callers always size the buffer adequately.
941        if self.eight_k { n_bytes } else { n_bytes * 2 }
942    }
943
944    fn sample_rate(&self) -> u32 {
945        16000
946    }
947
948    fn channels(&self) -> u16 {
949        1
950    }
951}
952
953#[cfg(all(test, feature = "std", feature = "plc"))]
954mod plc_tests {
955    use super::*;
956
957    const FRAME: usize = 320;
958
959    /// Voiced, speech-like: 8 harmonics of a pitch gliding 120 → 160 Hz,
960    /// under a slow amplitude envelope.
961    fn voice(n: usize) -> Vec<Sample> {
962        let mut phase = 0f32;
963        (0..n)
964            .map(|i| {
965                let t = i as f32 / 16000.0;
966                let f0 = 120.0 + 40.0 * (t / 2.0).min(1.0);
967                phase += 2.0 * std::f32::consts::PI * f0 / 16000.0;
968                let env = 0.6 + 0.4 * (2.0 * std::f32::consts::PI * 1.5 * t).sin();
969                let v: f32 = (1..=8).map(|k| (k as f32 * phase).sin() / k as f32).sum();
970                (5000.0 * env * v) as Sample
971            })
972            .collect()
973    }
974
975    fn packets(pcm: &[Sample]) -> Vec<Vec<u8>> {
976        let mut enc = G722Encoder::new();
977        pcm.chunks(FRAME).map(|c| enc.encode(c)).collect()
978    }
979
980    /// Error energy (dB relative to the reference) of `frames` packets
981    /// decoded right after losing `lost` packets at `at`.
982    fn error_after_loss(follow: bool, at: usize, lost: usize, frames: usize) -> f32 {
983        let pcm = voice(FRAME * (at + lost + frames + 2));
984        let pkts = packets(&pcm);
985        let mut reference = G722Decoder::new();
986        let mut dec = G722Decoder::new();
987        dec.enable_concealment();
988        let (mut err, mut sig) = (0f64, 0f64);
989        for (i, p) in pkts.iter().enumerate() {
990            let r = reference.decode(p);
991            if i >= at && i < at + lost {
992                let mut out = vec![0; FRAME];
993                if follow {
994                    dec.conceal_into(FRAME, &mut out).unwrap();
995                } else {
996                    dec.plc.as_mut().unwrap().conceal(&mut out);
997                }
998                continue;
999            }
1000            let d = dec.decode(p);
1001            if i >= at + lost && i < at + lost + frames {
1002                for (a, b) in d.iter().zip(&r) {
1003                    err += (*a as f64 - *b as f64).powi(2);
1004                    sig += (*b as f64).powi(2);
1005                }
1006            }
1007        }
1008        (10.0 * (err / sig).log10()) as f32
1009    }
1010
1011    #[test]
1012    fn concealment_is_audible_and_fades() {
1013        let pkts = packets(&voice(FRAME * 12));
1014        let mut dec = G722Decoder::new();
1015        dec.enable_concealment();
1016        for p in &pkts[..10] {
1017            dec.decode(p);
1018        }
1019        let first = dec.conceal(FRAME);
1020        assert_eq!(first.len(), FRAME);
1021        let rms = |s: &[Sample]| {
1022            (s.iter().map(|x| (*x as f64).powi(2)).sum::<f64>() / s.len() as f64).sqrt()
1023        };
1024        assert!(rms(&first) > 500.0, "rms {}", rms(&first));
1025        let later: Vec<_> = (0..3).map(|_| dec.conceal(FRAME)).collect();
1026        assert!(later[2].iter().all(|s| *s == 0));
1027    }
1028
1029    #[test]
1030    fn following_the_state_through_a_loss_beats_freezing_it() {
1031        // decoded audio right after a loss, against a decoder that lost
1032        // nothing: closer when the state followed the concealment
1033        let (mut better, mut total, mut gain) = (0, 0, 0f32);
1034        for lost in [1, 2] {
1035            for at in 8..60 {
1036                let follow = error_after_loss(true, at, lost, 2);
1037                let frozen = error_after_loss(false, at, lost, 2);
1038                total += 1;
1039                gain += frozen - follow;
1040                if follow < frozen {
1041                    better += 1;
1042                }
1043            }
1044        }
1045        let gain = gain / total as f32;
1046        println!("follow closer in {better}/{total} losses, by {gain:.1} dB on average");
1047        // measured: closer in 75/104, by 2.5 dB on average
1048        assert!(better * 3 >= total * 2, "closer in only {better}/{total}");
1049        assert!(gain > 1.5, "only {gain:.1} dB closer on average");
1050    }
1051}