code.octet-stream.net Git - m17rt/blob - m17core/src/decode.rs

   1 use crate::{
   2     bits::BitsMut,
   3     fec::{self, p_1, p_2, p_3},
   4     interleave::interleave,
   5     protocol::{
   6         LsfFrame, PacketFrame, PacketFrameCounter, StreamFrame, BERT_SYNC, END_OF_TRANSMISSION,
   7         LSF_SYNC, PACKET_SYNC, PREAMBLE, STREAM_SYNC,
   8     },
   9     random::random_xor,
  10 };
  11 use log::debug;
  12
  13 const PLUS_THREE: [u8; 2] = [0, 1];
  14 const PLUS_ONE: [u8; 2] = [0, 0];
  15 const MINUS_ONE: [u8; 2] = [1, 0];
  16 const MINUS_THREE: [u8; 2] = [1, 1];
  17
  18 fn decode_sample(sample: f32) -> [u8; 2] {
  19     if sample > 0.667 {
  20         PLUS_THREE
  21     } else if sample > 0.0 {
  22         PLUS_ONE
  23     } else if sample > -0.667 {
  24         MINUS_ONE
  25     } else {
  26         MINUS_THREE
  27     }
  28 }
  29
  30 #[derive(Clone, Copy, Debug, PartialEq, Eq)]
  31 pub(crate) enum SyncBurst {
  32     Lsf,
  33     Bert,
  34     Stream,
  35     Packet,
  36     Preamble,
  37     EndOfTransmission,
  38 }
  39
  40 impl SyncBurst {
  41     pub(crate) fn target(&self) -> [i8; 8] {
  42         match self {
  43             Self::Lsf => LSF_SYNC,
  44             Self::Bert => BERT_SYNC,
  45             Self::Stream => STREAM_SYNC,
  46             Self::Packet => PACKET_SYNC,
  47             Self::Preamble => PREAMBLE,
  48             Self::EndOfTransmission => END_OF_TRANSMISSION,
  49         }
  50     }
  51 }
  52
  53 const SYNC_MIN_GAIN: f32 = 16.0;
  54 const SYNC_BIT_THRESHOLD: f32 = 0.3;
  55 pub const SYNC_THRESHOLD: f32 = 100.0;
  56
  57 pub(crate) fn sync_burst_correlation(target: [i8; 8], samples: &[f32]) -> (f32, f32, f32) {
  58     let mut pos_max: f32 = f32::MIN;
  59     let mut neg_max: f32 = f32::MAX;
  60     for i in 0..8 {
  61         pos_max = pos_max.max(samples[i]);
  62         neg_max = neg_max.min(samples[i]);
  63     }
  64     let gain = (pos_max - neg_max) / 2.0;
  65     let shift = pos_max + neg_max;
  66     if gain < SYNC_MIN_GAIN {
  67         return (f32::MAX, gain, shift);
  68     }
  69
  70     let mut diff = 0.0;
  71     for i in 0..8 {
  72         let sym_diff = (((samples[i] - shift) / gain) - target[i] as f32).abs();
  73         if sym_diff > SYNC_BIT_THRESHOLD {
  74             return (f32::MAX, gain, shift);
  75         }
  76         diff += sym_diff;
  77     }
  78     (diff, gain, shift)
  79 }
  80
  81 /// Decode frame and return contents after the sync burst
  82 pub(crate) fn frame_initial_decode(frame: &[f32] /* length 192 */) -> [u8; 46] {
  83     let mut decoded = [0u8; 48];
  84     let mut decoded_bits = BitsMut::new(&mut decoded);
  85     for (idx, s) in frame.iter().enumerate() {
  86         let dibits = decode_sample(*s);
  87         decoded_bits.set_bit(idx * 2, dibits[0]);
  88         decoded_bits.set_bit(idx * 2 + 1, dibits[1]);
  89     }
  90     random_xor(&mut decoded[2..]);
  91     interleave(&decoded[2..])
  92 }
  93
  94 pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<LsfFrame> {
  95     let deinterleaved = frame_initial_decode(frame);
  96     debug!("deinterleaved: {:?}", deinterleaved);
  97     let lsf = match fec::decode(&deinterleaved, 240, p_1) {
  98         Some(lsf) => LsfFrame(lsf),
  99         None => return None,
 100     };
 101     debug!("full lsf: {:?}", lsf.0);
 102     let crc = lsf.check_crc();
 103     debug!("recv crc: {:04X}", crc);
 104     debug!("destination: {:?}", lsf.destination());
 105     debug!("source: {:?}", lsf.source());
 106     debug!("mode: {:?}", lsf.mode());
 107     debug!("data type: {:?}", lsf.data_type());
 108     debug!("encryption type: {:?}", lsf.encryption_type());
 109     debug!("can: {}", lsf.channel_access_number());
 110     debug!("meta: {:?}", lsf.meta());
 111     Some(lsf)
 112 }
 113
 114 pub(crate) fn parse_stream(frame: &[f32] /* length 192 */) -> Option<StreamFrame> {
 115     let deinterleaved = frame_initial_decode(frame);
 116     let stream_part = &deinterleaved[12..];
 117     let stream = match fec::decode(stream_part, 144, p_2) {
 118         Some(stream) => stream,
 119         None => return None,
 120     };
 121     let frame_num = u16::from_be_bytes([stream[0], stream[1]]);
 122     let eos = (frame_num & 0x8000) > 0;
 123     let frame_num = frame_num & 0x7fff; // higher layer has to handle wraparound
 124     debug!("frame number: {frame_num}, codec2: {:?}", &stream[2..18]);
 125
 126     if let Some((counter, part)) = decode_lich(&deinterleaved[0..12]) {
 127         debug!(
 128             "LICH: received part {counter} part {part:?} from raw {:?}",
 129             &deinterleaved[0..12]
 130         );
 131         Some(StreamFrame {
 132             lich_idx: counter,
 133             lich_part: part,
 134             frame_number: frame_num,
 135             end_of_stream: eos,
 136             stream_data: stream[2..18].try_into().unwrap(),
 137         })
 138     } else {
 139         None
 140     }
 141 }
 142
 143 pub(crate) fn parse_packet(frame: &[f32] /* length 192 */) -> Option<PacketFrame> {
 144     let deinterleaved = frame_initial_decode(frame);
 145     let packet = match fec::decode(&deinterleaved, 206, p_3) {
 146         Some(packet) => packet,
 147         None => return None,
 148     };
 149     // TODO: the spec is inconsistent about which bit in packet[25] is EOF
 150     // https://github.com/M17-Project/M17_spec/issues/147
 151     let final_frame = (packet[25] & 0x80) > 0;
 152     let number = (packet[25] >> 2) & 0x1f;
 153     let counter = if final_frame {
 154         PacketFrameCounter::FinalFrame {
 155             payload_len: number as usize,
 156         }
 157     } else {
 158         PacketFrameCounter::Frame {
 159             index: number as usize,
 160         }
 161     };
 162     Some(PacketFrame {
 163         payload: packet[0..25].try_into().unwrap(),
 164         counter,
 165     })
 166 }
 167
 168 pub(crate) fn decode_lich(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
 169     let mut decoded = 0u64;
 170     for (input_idx, input_bytes) in type2_bits.chunks(3).enumerate() {
 171         let mut input: u32 = 0;
 172         for (idx, byte) in input_bytes.iter().enumerate() {
 173             input |= (*byte as u32) << (16 - (8 * idx));
 174         }
 175         let (val, _dist) = cai_golay::extended::decode(input)?;
 176         decoded |= (val as u64) << ((3 - input_idx) * 12);
 177     }
 178     let b = decoded.to_be_bytes();
 179     Some((b[7] >> 5, [b[2], b[3], b[4], b[5], b[6]]))
 180 }
 181
 182 #[cfg(test)]
 183 mod tests {
 184     use super::*;
 185
 186     #[test]
 187     fn test_lich_decode() {
 188         let input = [221, 82, 162, 16, 85, 200, 5, 14, 254, 4, 13, 153];
 189         let expected_counter = 2;
 190         let expected_part = [221, 81, 5, 5, 0];
 191         assert_eq!(decode_lich(&input), Some((expected_counter, expected_part)));
 192     }
 193 }