fast demod bin

[m17rt] / m17core / src / decode.rs

diff --git a/m17core/src/decode.rs b/m17core/src/decode.rs
old mode 100755 (executable)
new mode 100644 (file)
index 60c210e..3195ed5
--- a/m17core/src/decode.rs
+++ b/m17core/src/decode.rs
@@ -1,8 +1,11 @@
 use crate::{
     bits::BitsMut,
-    fec::{self, p_1, p_2},
+    fec::{self, p_1, p_2, p_3},
     interleave::interleave,
-    protocol::{LsfFrame, StreamFrame, BERT_SYNC, LSF_SYNC, PACKET_SYNC, STREAM_SYNC},
+    protocol::{
+        BERT_SYNC, END_OF_TRANSMISSION, LSF_SYNC, LsfFrame, PACKET_SYNC, PREAMBLE, PacketFrame,
+        PacketFrameCounter, STREAM_SYNC, StreamFrame,
+    },
     random::random_xor,
 };
 use log::debug;
@@ -30,6 +33,8 @@ pub(crate) enum SyncBurst {
     Bert,
     Stream,
     Packet,
+    Preamble,
+    EndOfTransmission,
 }
 
 impl SyncBurst {
@@ -39,6 +44,8 @@ impl SyncBurst {
             Self::Bert => BERT_SYNC,
             Self::Stream => STREAM_SYNC,
             Self::Packet => PACKET_SYNC,
+            Self::Preamble => PREAMBLE,
+            Self::EndOfTransmission => END_OF_TRANSMISSION,
         }
     }
 }
@@ -51,17 +58,18 @@ pub(crate) fn sync_burst_correlation(target: [i8; 8], samples: &[f32]) -> (f32,
     let mut pos_max: f32 = f32::MIN;
     let mut neg_max: f32 = f32::MAX;
     for i in 0..8 {
-        pos_max = pos_max.max(samples[i * 10]);
-        neg_max = neg_max.min(samples[i * 10]);
+        pos_max = pos_max.max(samples[i]);
+        neg_max = neg_max.min(samples[i]);
     }
     let gain = (pos_max - neg_max) / 2.0;
     let shift = pos_max + neg_max;
     if gain < SYNC_MIN_GAIN {
         return (f32::MAX, gain, shift);
     }
+
     let mut diff = 0.0;
     for i in 0..8 {
-        let sym_diff = (((samples[i * 10] - shift) / gain) - target[i] as f32).abs();
+        let sym_diff = (((samples[i] - shift) / gain) - target[i] as f32).abs();
         if sym_diff > SYNC_BIT_THRESHOLD {
             return (f32::MAX, gain, shift);
         }
@@ -83,14 +91,15 @@ pub(crate) fn frame_initial_decode(frame: &[f32] /* length 192 */) -> [u8; 46] {
     interleave(&decoded[2..])
 }
 
-pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<LsfFrame> {
+pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<(LsfFrame, u8)> {
     let deinterleaved = frame_initial_decode(frame);
-    let lsf = match fec::decode(&deinterleaved, 240, p_1) {
-        Some(lsf) => LsfFrame(lsf),
+    debug!("deinterleaved: {:?}", deinterleaved);
+    let (lsf, errors) = match fec::decode(&deinterleaved, 240, p_1) {
+        Some((lsf, errors)) => (LsfFrame(lsf), errors),
         None => return None,
     };
     debug!("full lsf: {:?}", lsf.0);
-    let crc = lsf.crc();
+    let crc = lsf.check_crc();
     debug!("recv crc: {:04X}", crc);
     debug!("destination: {:?}", lsf.destination());
     debug!("source: {:?}", lsf.source());
@@ -99,10 +108,62 @@ pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<LsfFrame> {
     debug!("encryption type: {:?}", lsf.encryption_type());
     debug!("can: {}", lsf.channel_access_number());
     debug!("meta: {:?}", lsf.meta());
-    Some(lsf)
+    Some((lsf, errors))
+}
+
+pub(crate) fn parse_stream(frame: &[f32] /* length 192 */) -> Option<(StreamFrame, u8)> {
+    let deinterleaved = frame_initial_decode(frame);
+    let stream_part = &deinterleaved[12..];
+    let (stream, errors) = fec::decode(stream_part, 144, p_2)?;
+    let frame_num = u16::from_be_bytes([stream[0], stream[1]]);
+    let eos = (frame_num & 0x8000) > 0;
+    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)) = 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,
+                frame_number: frame_num,
+                end_of_stream: eos,
+                stream_data: stream[2..18].try_into().unwrap(),
+            },
+            errors,
+        ))
+    } else {
+        None
+    }
+}
+
+pub(crate) fn parse_packet(frame: &[f32] /* length 192 */) -> Option<(PacketFrame, u8)> {
+    let deinterleaved = frame_initial_decode(frame);
+    let (packet, errors) = fec::decode(&deinterleaved, 206, p_3)?;
+    let final_frame = (packet[25] & 0x80) > 0;
+    let number = (packet[25] >> 2) & 0x1f;
+    let counter = if final_frame {
+        PacketFrameCounter::FinalFrame {
+            payload_len: number as usize,
+        }
+    } else {
+        PacketFrameCounter::Frame {
+            index: number as usize,
+        }
+    };
+    Some((
+        PacketFrame {
+            payload: packet[0..25].try_into().unwrap(),
+            counter,
+        },
+        errors,
+    ))
 }
 
-pub(crate) fn try_lich_decode(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
+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;
@@ -116,28 +177,15 @@ pub(crate) fn try_lich_decode(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
     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..];
-    let stream = match fec::decode(stream_part, 144, p_2) {
-        Some(stream) => stream,
-        None => return None,
-    };
-    let frame_num = u16::from_be_bytes([stream[0], stream[1]]);
-    let eos = (frame_num & 0x8000) > 0;
-    let frame_num = frame_num & 0x7fff; // higher layer has to handle wraparound
-    debug!("frame number: {frame_num}, codec2: {:?}", &stream[2..18]);
+#[cfg(test)]
+mod tests {
+    use super::*;
 
-    if let Some((counter, part)) = try_lich_decode(&deinterleaved[0..12]) {
-        debug!("LICH: received part {counter}");
-        Some(StreamFrame {
-            lich_idx: counter,
-            lich_part: part,
-            frame_number: frame_num,
-            end_of_stream: eos,
-            stream_data: stream[2..18].try_into().unwrap(),
-        })
-    } else {
-        None
+    #[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)));
     }
 }