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
16const 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
22const 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
28const 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
34const SCALE_FACTOR_ADJUST_LOW: [i32; 8] = [-60, -30, 58, 172, 334, 538, 1198, 3042];
36
37const LOG_SCALE_FACTOR_MAP: [i32; 16] = [0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0];
39
40const 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
46const 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
52const QUANT_MULT_HIGH_2BIT: [i32; 4] = [-7408, -1616, 7408, 1616];
54
55const QMF_FILTER_COEFS: [i32; 12] = [3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11];
57
58const HIGH_QUANT_INDEX_NEG: [i32; 3] = [0, 1, 0];
60
61const HIGH_QUANT_INDEX_POS: [i32; 3] = [0, 3, 2];
63
64#[cfg(feature = "plc")]
66const QMF_DELAY: usize = 22;
67
68const SCALE_FACTOR_ADJUST_HIGH: [i32; 3] = [0, -214, 798];
70
71const HIGH_LOG_SCALE_MAP: [i32; 4] = [2, 1, 2, 1];
73
74const 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
81const 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#[derive(Default, Clone)]
103struct G722Band {
104 signal_estimate: i32,
106 pole_filter_output: i32,
108 zero_filter_output: i32,
110 reconstructed_signal: [i32; 3],
112 pole_coefficients: [i32; 3],
114 pole_coefficients_temp: [i32; 3],
116 partial_reconstructed: [i32; 3],
118 difference_signal: [i32; 7],
120 zero_coefficients: [i32; 7],
122 zero_coefficients_temp: [i32; 7],
124 log_scale_factor: i32,
126 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#[inline]
139fn block4(band: &mut G722Band, d: i32) {
140 band.difference_signal[0] = d;
142 band.reconstructed_signal[0] = saturate(band.signal_estimate + d);
143
144 let partial_rec0 = saturate(band.zero_filter_output + d);
146 band.partial_reconstructed[0] = partial_rec0;
147
148 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 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 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 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 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 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 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 #[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 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 encoder.band[0].log_scale_factor = 32 << 2; encoder.band[1].log_scale_factor = 8 << 2; encoder
284 }
285
286 fn g722_encode_into(&mut self, amp: &[i16], out: &mut [u8]) -> Result<usize, CodecError> {
289 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 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 let xlow = amp[input_idx] as i32 >> 1;
341 input_idx += 1;
342
343 let code = self.encode_low_band(xlow, true);
345 output_code(code, &mut cursor)?;
346 }
347 } else {
348 let chunks = amp.chunks_exact(2);
351 let rem = chunks.remainder();
352
353 for chunk in chunks {
354 self.x.copy_within(2..24, 0);
356
357 self.x[22] = chunk[0] as i32;
359 self.x[23] = chunk[1] as i32;
360
361 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 let xlow = (sumeven + sumodd) >> 14;
391 let xhigh = (sumeven - sumodd) >> 14;
392
393 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_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 if self.packed && cursor.bit_count > 0 {
443 cursor.push_byte((cursor.bit_buffer & 0xFF) as u8)?;
444 }
445
446 self.out_buffer = cursor.bit_buffer;
448 self.out_bits = cursor.bit_count;
449
450 Ok(cursor.byte_pos)
451 }
452
453 #[inline]
456 fn encode_low_band(&mut self, xlow: i32, is_eight_k: bool) -> i32 {
457 let el = saturate(xlow - self.band[0].signal_estimate);
459
460 let wd = el.abs().wrapping_sub((el >> 31) & 1);
462
463 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 let ilow = if el < 0 {
478 QUANT_INDEX_NEG[quantization_idx]
479 } else {
480 QUANT_INDEX_POS[quantization_idx]
481 };
482
483 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 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 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 block4(&mut self.band[0], dlow);
506
507 if is_eight_k {
509 ((0xc0 | ilow) >> 8) - self.bits_per_sample
510 } else {
511 ilow
512 }
513 }
514
515 #[inline]
518 fn encode_high_band(&mut self, xhigh: i32) -> i32 {
519 let eh = saturate(xhigh - self.band[1].signal_estimate);
521
522 let wd = if eh >= 0 { eh } else { -(eh + 1) };
524 let decision_level = (564 * self.band[1].log_scale_factor) >> 12;
525
526 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 let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
536 let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;
537
538 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 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 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 #[inline]
582 fn extract_code(&mut self, data: &[u8], idx: &mut usize) -> i32 {
583 if self.packed {
584 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 let code = data[*idx] as i32;
597 *idx += 1;
598 code
599 }
600 }
601
602 #[inline]
604 fn parse_code(&self, code: i32) -> (i32, i32, i32) {
605 match self.bits_per_sample {
607 7 => {
608 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 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 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 #[inline]
633 fn process_low_band(&mut self, wd1: i32, wd2: i32) -> i32 {
634 let dequant = (self.band[0].log_scale_factor * wd2) >> 15;
636
637 let rlow = self.band[0].signal_estimate + dequant;
639
640 let rlow = rlow.clamp(-16384, 16383);
642
643 let wd2 = QUANT_MULT_LOW_4BIT[wd1 as usize];
645 let dlowt = (self.band[0].log_scale_factor * wd2) >> 15;
646
647 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 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 block4(&mut self.band[0], dlowt);
665
666 rlow
667 }
668
669 #[inline]
671 fn process_high_band(&mut self, ihigh: i32) -> i32 {
672 let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
674 let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;
675
676 let rhigh = dhigh + self.band[1].signal_estimate;
678
679 let rhigh = rhigh.clamp(-16384, 16383);
681
682 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 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 block4(&mut self.band[1], dhigh);
700
701 rhigh
702 }
703
704 #[inline]
706 fn apply_qmf_synthesis(&mut self, rlow: i32, rhigh: i32) -> [i16; 2] {
707 self.x.copy_within(2..24, 0);
709
710 self.x[22] = rlow + rhigh;
712 self.x[23] = rlow - rhigh;
713
714 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 [saturate(xout1 >> 11) as i16, saturate(xout2 >> 11) as i16]
743 }
744
745 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 #[cfg(feature = "plc")]
798 fn follow(&mut self, signal: &[Sample], plc: &PitchConcealer) {
799 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 let played = plc.lead();
820 for (j, x) in encoder.x.iter_mut().enumerate() {
821 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 #[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 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 }
895
896 fn channels(&self) -> u16 {
897 1 }
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 #[cfg(feature = "plc")]
910 fn conceal_into(&mut self, samples: usize, out: &mut [Sample]) -> Result<usize, CodecError> {
911 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 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 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 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 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 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 assert!(better * 3 >= total * 2, "closer in only {better}/{total}");
1049 assert!(gain > 1.5, "only {gain:.1} dB closer on average");
1050 }
1051}