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

   1 use crate::{
   2     bits::BitsMut,
   3     fec::{self, p_1, p_2},
   4     interleave::interleave,
   5     protocol::{LsfFrame, StreamFrame, BERT_SYNC, LSF_SYNC, PACKET_SYNC, STREAM_SYNC},
   6     random::random_xor,
   7 };
   8 use log::debug;
   9
  10 const PLUS_THREE: [u8; 2] = [0, 1];
  11 const PLUS_ONE: [u8; 2] = [0, 0];
  12 const MINUS_ONE: [u8; 2] = [1, 0];
  13 const MINUS_THREE: [u8; 2] = [1, 1];
  14
  15 fn decode_sample(sample: f32) -> [u8; 2] {
  16     if sample > 0.667 {
  17         PLUS_THREE
  18     } else if sample > 0.0 {
  19         PLUS_ONE
  20     } else if sample > -0.667 {
  21         MINUS_ONE
  22     } else {
  23         MINUS_THREE
  24     }
  25 }
  26
  27 #[derive(Clone, Copy, Debug, PartialEq, Eq)]
  28 pub(crate) enum SyncBurst {
  29     Lsf,
  30     Bert,
  31     Stream,
  32     Packet,
  33 }
  34
  35 impl SyncBurst {
  36     pub(crate) fn target(&self) -> [i8; 8] {
  37         match self {
  38             Self::Lsf => LSF_SYNC,
  39             Self::Bert => BERT_SYNC,
  40             Self::Stream => STREAM_SYNC,
  41             Self::Packet => PACKET_SYNC,
  42         }
  43     }
  44 }
  45
  46 const SYNC_MIN_GAIN: f32 = 16.0;
  47 const SYNC_BIT_THRESHOLD: f32 = 0.3;
  48 pub const SYNC_THRESHOLD: f32 = 100.0;
  49
  50 pub(crate) fn sync_burst_correlation(target: [i8; 8], samples: &[f32]) -> (f32, f32, f32) {
  51     let mut pos_max: f32 = f32::MIN;
  52     let mut neg_max: f32 = f32::MAX;
  53     for i in 0..8 {
  54         pos_max = pos_max.max(samples[i * 10]);
  55         neg_max = neg_max.min(samples[i * 10]);
  56     }
  57     let gain = (pos_max - neg_max) / 2.0;
  58     let shift = pos_max + neg_max;
  59     if gain < SYNC_MIN_GAIN {
  60         return (f32::MAX, gain, shift);
  61     }
  62     let mut diff = 0.0;
  63     for i in 0..8 {
  64         let sym_diff = (((samples[i * 10] - shift) / gain) - target[i] as f32).abs();
  65         if sym_diff > SYNC_BIT_THRESHOLD {
  66             return (f32::MAX, gain, shift);
  67         }
  68         diff += sym_diff;
  69     }
  70     (diff, gain, shift)
  71 }
  72
  73 /// Decode frame and return contents after the sync burst
  74 pub(crate) fn frame_initial_decode(frame: &[f32] /* length 192 */) -> [u8; 46] {
  75     let mut decoded = [0u8; 48];
  76     let mut decoded_bits = BitsMut::new(&mut decoded);
  77     for (idx, s) in frame.iter().enumerate() {
  78         let dibits = decode_sample(*s);
  79         decoded_bits.set_bit(idx * 2, dibits[0]);
  80         decoded_bits.set_bit(idx * 2 + 1, dibits[1]);
  81     }
  82     random_xor(&mut decoded[2..]);
  83     interleave(&decoded[2..])
  84 }
  85
  86 pub(crate) fn parse_lsf(frame: &[f32] /* length 192 */) -> Option<LsfFrame> {
  87     let deinterleaved = frame_initial_decode(frame);
  88     let lsf = match fec::decode(&deinterleaved, 240, p_1) {
  89         Some(lsf) => LsfFrame(lsf),
  90         None => return None,
  91     };
  92     debug!("full lsf: {:?}", lsf.0);
  93     let crc = lsf.crc();
  94     debug!("recv crc: {:04X}", crc);
  95     debug!("destination: {:?}", lsf.destination());
  96     debug!("source: {:?}", lsf.source());
  97     debug!("mode: {:?}", lsf.mode());
  98     debug!("data type: {:?}", lsf.data_type());
  99     debug!("encryption type: {:?}", lsf.encryption_type());
 100     debug!("can: {}", lsf.channel_access_number());
 101     debug!("meta: {:?}", lsf.meta());
 102     Some(lsf)
 103 }
 104
 105 pub(crate) fn try_lich_decode(type2_bits: &[u8]) -> Option<(u8, [u8; 5])> {
 106     let mut decoded = 0u64;
 107     for (input_idx, input_bytes) in type2_bits.chunks(3).enumerate() {
 108         let mut input: u32 = 0;
 109         for (idx, byte) in input_bytes.iter().enumerate() {
 110             input |= (*byte as u32) << (16 - (8 * idx));
 111         }
 112         let (val, _dist) = cai_golay::extended::decode(input)?;
 113         decoded |= (val as u64) << ((3 - input_idx) * 12);
 114     }
 115     let b = decoded.to_be_bytes();
 116     Some((b[7] >> 5, [b[2], b[3], b[4], b[5], b[6]]))
 117 }
 118
 119 pub(crate) fn parse_stream(frame: &[f32] /* length 192 */) -> Option<StreamFrame> {
 120     let deinterleaved = frame_initial_decode(frame);
 121     let stream_part = &deinterleaved[12..];
 122     let stream = match fec::decode(stream_part, 144, p_2) {
 123         Some(stream) => stream,
 124         None => return None,
 125     };
 126     let frame_num = u16::from_be_bytes([stream[0], stream[1]]);
 127     let eos = (frame_num & 0x8000) > 0;
 128     let frame_num = frame_num & 0x7fff; // higher layer has to handle wraparound
 129     debug!("frame number: {frame_num}, codec2: {:?}", &stream[2..18]);
 130
 131     if let Some((counter, part)) = try_lich_decode(&deinterleaved[0..12]) {
 132         debug!("LICH: received part {counter}");
 133         Some(StreamFrame {
 134             lich_idx: counter,
 135             lich_part: part,
 136             frame_number: frame_num,
 137             end_of_stream: eos,
 138             stream_data: stream[2..18].try_into().unwrap(),
 139         })
 140     } else {
 141         None
 142     }
 143 }