Encoding LICH and round trip tests

diff --git a/m17core/src/decode.rs b/m17core/src/decode.rs
index de131246189bee791e9a0267cd394d2cd07229b0..7ec222495806ac03830bf5e2b4a7c43ab7f2aa30 100755 (executable)
--- a/m17core/src/decode.rs
+++ b/m17core/src/decode.rs
@@ -106,20 +106,6 @@ pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<LsfFrame> {
     Some(lsf)
 }
 
-pub(crate) fn try_lich_decode(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
-    let mut decoded = 0u64;
-    for (input_idx, input_bytes) in type2_bits.chunks(3).enumerate() {
-        let mut input: u32 = 0;
-        for (idx, byte) in input_bytes.iter().enumerate() {
-            input |= (*byte as u32) << (16 - (8 * idx));
-        }
-        let (val, _dist) = cai_golay::extended::decode(input)?;
-        decoded |= (val as u64) << ((3 - input_idx) * 12);
-    }
-    let b = decoded.to_be_bytes();
-    Some((b[7] >> 5, [b[2], b[3], b[4], b[5], b[6]]))
-}
-
 pub(crate) fn parse_stream(frame: &[f32] /* length 192 */) -> Option<StreamFrame> {
     let deinterleaved = frame_initial_decode(frame);
     let stream_part = &deinterleaved[12..];
@@ -132,8 +118,11 @@ pub(crate) fn parse_stream(frame: &[f32] /* length 192 */) -> Option<StreamFrame
     let frame_num = frame_num & 0x7fff; // higher layer has to handle wraparound
     debug!("frame number: {frame_num}, codec2: {:?}", &stream[2..18]);
 
-    if let Some((counter, part)) = try_lich_decode(&deinterleaved[0..12]) {
-        debug!("LICH: received part {counter}");
+    if let Some((counter, part)) = decode_lich(&deinterleaved[0..12]) {
+        debug!(
+            "LICH: received part {counter} part {part:?} from raw {:?}",
+            &deinterleaved[0..12]
+        );
         Some(StreamFrame {
             lich_idx: counter,
             lich_part: part,
@@ -168,3 +157,30 @@ pub(crate) fn parse_packet(frame: &[f32] /* length 192 */) -> Option<PacketFrame
         counter,
     })
 }
+
+pub(crate) fn decode_lich(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
+    let mut decoded = 0u64;
+    for (input_idx, input_bytes) in type2_bits.chunks(3).enumerate() {
+        let mut input: u32 = 0;
+        for (idx, byte) in input_bytes.iter().enumerate() {
+            input |= (*byte as u32) << (16 - (8 * idx));
+        }
+        let (val, _dist) = cai_golay::extended::decode(input)?;
+        decoded |= (val as u64) << ((3 - input_idx) * 12);
+    }
+    let b = decoded.to_be_bytes();
+    Some((b[7] >> 5, [b[2], b[3], b[4], b[5], b[6]]))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_lich_decode() {
+        let input = [221, 82, 162, 16, 85, 200, 5, 14, 254, 4, 13, 153];
+        let expected_counter = 2;
+        let expected_part = [221, 81, 5, 5, 0];
+        assert_eq!(decode_lich(&input), Some((expected_counter, expected_part)));
+    }
+}

diff --git a/m17core/src/encode.rs b/m17core/src/encode.rs
index 70fe012c6e07de03b1b1153d4d3771a88e8a1c18..0b79e98e8425b1a535bf5c2d7da12f59f50fc89b 100644 (file)
--- a/m17core/src/encode.rs
+++ b/m17core/src/encode.rs
@@ -1,3 +1,122 @@
-use crate::protocol::LsfFrame;
+use crate::{
+    bits::Bits,
+    fec::{self, p_1},
+    interleave::interleave,
+    protocol::{LsfFrame, PacketFrame, StreamFrame, LSF_SYNC},
+    random::random_xor,
+};
 
-pub(crate) fn encode_lsf(frame: &LsfFrame) {}
+pub(crate) fn encode_lsf(frame: &LsfFrame) -> [f32; 192] {
+    let type3 = fec::encode(&frame.0, 240, p_1);
+    let mut interleaved = interleave(&type3);
+    random_xor(&mut interleaved);
+    let mut out = [0f32; 192];
+    for (val, o) in LSF_SYNC.iter().zip(out.iter_mut()) {
+        *o = *val as f32;
+    }
+    let bits = Bits::new(&interleaved);
+    let mut out_bits = bits.iter();
+    for o in out[8..].iter_mut() {
+        *o = match (out_bits.next().unwrap(), out_bits.next().unwrap()) {
+            (0, 1) => 1.0,
+            (0, 0) => 1.0 / 3.0,
+            (1, 0) => -1.0 / 3.0,
+            (1, 1) => -1.0,
+            _ => unreachable!(),
+        };
+    }
+    out
+}
+
+/*pub(crate) fn encode_stream(frame: &StreamFrame) -> [f32; 192] {
+    let type3 = fec::encode(&frame.0, 240, p_1);
+    let mut interleaved = interleave(&type3);
+    random_xor(&mut interleaved);
+    let mut out = [0f32; 192];
+    for (val, o) in LSF_SYNC.iter().zip(out.iter_mut()) {
+        *o = *val as f32;
+    }
+    let bits = Bits::new(&interleaved);
+    let mut out_bits = bits.iter();
+    for o in out[8..].iter_mut() {
+        *o = match (out_bits.next().unwrap(), out_bits.next().unwrap()) {
+            (0, 1) => 1.0,
+            (0, 0) => 1.0 / 3.0,
+            (1, 0) => -1.0 / 3.0,
+            (1, 1) => -1.0,
+            _ => unreachable!(),
+        };
+    }
+    out
+}*/
+
+/*pub(crate) fn encode_packet(frame: &PacketFrame) -> [f32; 192] {
+    let type3 = fec::encode(&frame.0, 240, p_1);
+    let mut interleaved = interleave(&type3);
+    random_xor(&mut interleaved);
+    let mut out = [0f32; 192];
+    for (val, o) in LSF_SYNC.iter().zip(out.iter_mut()) {
+        *o = *val as f32;
+    }
+    let bits = Bits::new(&interleaved);
+    let mut out_bits = bits.iter();
+    for o in out[8..].iter_mut() {
+        *o = match (out_bits.next().unwrap(), out_bits.next().unwrap()) {
+            (0, 1) => 1.0,
+            (0, 0) => 1.0 / 3.0,
+            (1, 0) => -1.0 / 3.0,
+            (1, 1) => -1.0,
+            _ => unreachable!(),
+        };
+    }
+    out
+}*/
+
+pub(crate) fn encode_lich(counter: u8, part: [u8; 5]) -> [u8; 12] {
+    let mut out = [0u8; 12];
+    let to_encode = [
+        ((part[0] as u16) << 4) | ((part[1] as u16) >> 4),
+        ((part[1] as u16 & 0x000f) << 8) | part[2] as u16,
+        ((part[3] as u16) << 4) | ((part[4] as u16) >> 4),
+        ((part[4] as u16 & 0x000f) << 8) | ((counter as u16) << 5),
+    ];
+    for (i, o) in to_encode.into_iter().zip(out.chunks_mut(3)) {
+        let encoded = cai_golay::extended::encode(i).to_be_bytes();
+        o[0..3].copy_from_slice(&encoded[1..4]);
+    }
+    out
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn lsf_round_trip() {
+        let lsf = LsfFrame([
+            255, 255, 255, 255, 255, 255, 0, 0, 0, 159, 221, 81, 5, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0, 0, 0, 131, 53,
+        ]);
+        let encoded = encode_lsf(&lsf);
+        let decoded = crate::decode::parse_lsf(&encoded);
+        assert_eq!(decoded, Some(lsf));
+    }
+
+    #[test]
+    fn lich_encode() {
+        let input = [221, 81, 5, 5, 0];
+        let counter = 2;
+        let expected_output = [221, 82, 162, 16, 85, 200, 5, 14, 254, 4, 13, 153];
+        assert_eq!(encode_lich(counter, input), expected_output);
+    }
+
+    #[test]
+    fn lich_round_trip() {
+        let input = [1, 255, 0, 90, 10];
+        let counter = 0;
+        assert_eq!(
+            crate::decode::decode_lich(&encode_lich(counter, input.clone())),
+            Some((counter, input))
+        );
+    }
+}