From c9f4f5c9034da69df2ef2cfc3d18a54dba56554f Mon Sep 17 00:00:00 2001 From: Thomas Karpiniec Date: Thu, 17 Apr 2025 18:21:00 +1000 Subject: [PATCH] Setters for reflector packets --- m17core/src/reflector.rs | 250 +++++++++++++++++++++++++++++---------- 1 file changed, 187 insertions(+), 63 deletions(-) diff --git a/m17core/src/reflector.rs b/m17core/src/reflector.rs index 7f49c36..657d864 100644 --- a/m17core/src/reflector.rs +++ b/m17core/src/reflector.rs @@ -1,23 +1,44 @@ // Based on https://github.com/n7tae/mrefd/blob/master/Packet-Description.md // and the main M17 specification +use crate::address::Address; use crate::protocol::LsfFrame; macro_rules! define_message { - ($t:tt, $sz:tt) => { - pub struct $t([u8; $sz]); + ($t:tt, $sz:tt, $min_sz:tt, $magic:tt) => { + pub struct $t(pub [u8; $sz], pub usize); + impl $t { + pub fn new() -> Self { + let mut bytes = [0u8; $sz]; + bytes[0..4].copy_from_slice($magic); + Self(bytes, $sz) + } + + #[allow(clippy::double_comparisons)] // occurs in some macro invocations + #[allow(clippy::manual_range_contains)] // way more readable, good grief pub fn from_bytes(b: &[u8]) -> Option { - if b.len() != $sz { + let len = b.len(); + if len > $sz || len < $min_sz { return None; } - let mut s = Self([0; $sz]); - s.0[..].copy_from_slice(b); + let mut s = Self([0; $sz], len); + s.0[0..len].copy_from_slice(b); if !s.verify_integrity() { return None; } Some(s) } + + pub fn as_bytes(&self) -> &[u8] { + &self.0[0..self.1] + } + } + + impl Default for $t { + fn default() -> Self { + Self::new() + } } }; } @@ -28,6 +49,13 @@ macro_rules! impl_stream_id { pub fn stream_id(&self) -> u16 { u16::from_be_bytes([self.0[$from], self.0[$from + 1]]) } + + pub fn set_stream_id(&mut self, id: u16) { + let bytes = id.to_be_bytes(); + self.0[$from] = bytes[0]; + self.0[$from + 1] = bytes[1]; + self.recalculate_crc(); + } } }; } @@ -41,6 +69,11 @@ macro_rules! impl_link_setup { frame.recalculate_crc(); frame } + + pub fn set_link_setup_frame(&mut self, lsf: &LsfFrame) { + self.0[$from..($from + 28)].copy_from_slice(&lsf.0[0..28]); + self.recalculate_crc(); + } } }; } @@ -53,6 +86,12 @@ macro_rules! impl_link_setup_frame { frame.0[..].copy_from_slice(&self.0[$from..($from + 30)]); frame } + + pub fn set_link_setup_frame(&mut self, lsf: &LsfFrame) { + debug_assert_eq!(lsf.check_crc(), 0); + self.0[$from..($from + 30)].copy_from_slice(&lsf.0); + self.recalculate_crc(); + } } }; } @@ -69,6 +108,22 @@ macro_rules! impl_frame_number { let frame_num = u16::from_be_bytes([self.0[$from], self.0[$from + 1]]); (frame_num & 0x8000) > 0 } + + pub fn set_frame_number(&mut self, number: u16) { + let existing_eos = u16::from_be_bytes([self.0[$from], self.0[$from + 1]]) & 0x8000; + let new = (existing_eos | (number & 0x7fff)).to_be_bytes(); + self.0[$from] = new[0]; + self.0[$from + 1] = new[1]; + self.recalculate_crc(); + } + + pub fn set_end_of_stream(&mut self, eos: bool) { + let existing_fn = u16::from_be_bytes([self.0[$from], self.0[$from + 1]]) & 0x7fff; + let new = (existing_fn | (if eos { 0x8000 } else { 0 })).to_be_bytes(); + self.0[$from] = new[0]; + self.0[$from + 1] = new[1]; + self.recalculate_crc(); + } } }; } @@ -79,6 +134,11 @@ macro_rules! impl_payload { pub fn payload(&self) -> &[u8] { &self.0[$from..$to] } + + pub fn set_payload(&mut self, bytes: &[u8]) { + self.0[$from..$to].copy_from_slice(bytes); + self.recalculate_crc(); + } } }; } @@ -89,6 +149,17 @@ macro_rules! impl_modules { pub fn modules(&self) -> ModulesIterator { ModulesIterator::new(&self.0[$from..$to]) } + + pub fn set_modules(&mut self, list: &str) { + debug_assert!(list.len() < 27); + let mut idx = $from; + for m in list.chars() { + self.0[idx] = m as u8; + idx += 1; + } + self.0[idx] = 0; + self.recalculate_crc(); + } } }; } @@ -99,6 +170,11 @@ macro_rules! impl_module { pub fn module(&self) -> char { self.0[$at] as char } + + pub fn set_module(&mut self, m: char) { + self.0[$at] = m as u8; + self.recalculate_crc(); + } } }; } @@ -106,9 +182,15 @@ macro_rules! impl_module { macro_rules! impl_address { ($t:ty, $from:tt) => { impl $t { - pub fn address(&self) -> crate::address::Address { + pub fn address(&self) -> Address { crate::address::decode_address(self.0[$from..($from + 6)].try_into().unwrap()) } + + pub fn set_address(&mut self, address: Address) { + let encoded = crate::address::encode_address(&address); + self.0[$from..($from + 6)].copy_from_slice(&encoded); + self.recalculate_crc(); + } } }; } @@ -119,6 +201,14 @@ macro_rules! impl_trailing_crc_verify { pub fn verify_integrity(&self) -> bool { crate::crc::m17_crc(&self.0) == 0 } + + pub fn recalculate_crc(&mut self) { + let len = self.0.len(); + let start_crc = crate::crc::m17_crc(&self.0[0..(len - 2)]).to_be_bytes(); + self.0[len - 2] = start_crc[0]; + self.0[len - 1] = start_crc[1]; + debug_assert!(self.verify_integrity()); + } } }; } @@ -129,6 +219,14 @@ macro_rules! impl_internal_crc { pub fn verify_integrity(&self) -> bool { crate::crc::m17_crc(&self.0[$from..$to]) == 0 } + + pub fn recalculate_crc(&mut self) { + // assume the last two bytes of the range are the CRC + let start_crc = crate::crc::m17_crc(&self.0[$from..($to - 2)]).to_be_bytes(); + self.0[$to - 2] = start_crc[0]; + self.0[$to - 1] = start_crc[1]; + debug_assert!(self.verify_integrity()); + } } }; } @@ -139,6 +237,7 @@ macro_rules! no_crc { pub fn verify_integrity(&self) -> bool { true } + pub fn recalculate_crc(&mut self) {} } }; } @@ -149,6 +248,11 @@ macro_rules! impl_is_relayed { pub fn is_relayed(&self) -> bool { self.0[self.0.len() - 1] != 0 } + + pub fn set_relayed(&mut self, relayed: bool) { + self.0[self.0.len() - 1] = if relayed { 1 } else { 0 }; + self.recalculate_crc(); + } } }; } @@ -194,7 +298,7 @@ pub const MAGIC_PONG: &[u8] = b"PONG"; /// Messages sent from a station/client to a reflector #[allow(clippy::large_enum_variant)] pub enum ClientMessage { - VoiceFull(VoiceFull), + Voice(Voice), VoiceHeader(VoiceHeader), VoiceData(VoiceData), Packet(Packet), @@ -210,14 +314,14 @@ impl ClientMessage { return None; } match &bytes[0..4] { - MAGIC_VOICE => Some(Self::VoiceFull(VoiceFull::from_bytes(&bytes)?)), - MAGIC_VOICE_HEADER => Some(Self::VoiceHeader(VoiceHeader::from_bytes(&bytes)?)), - MAGIC_VOICE_DATA => Some(Self::VoiceData(VoiceData::from_bytes(&bytes)?)), - MAGIC_PACKET => Some(Self::Packet(Packet::from_bytes(&bytes)?)), - MAGIC_PONG => Some(Self::Pong(Pong::from_bytes(&bytes)?)), - MAGIC_CONNECT => Some(Self::Connect(Connect::from_bytes(&bytes)?)), - MAGIC_LISTEN => Some(Self::Listen(Listen::from_bytes(&bytes)?)), - MAGIC_DISCONNECT => Some(Self::Disconnect(Disconnect::from_bytes(&bytes)?)), + MAGIC_VOICE => Some(Self::Voice(Voice::from_bytes(bytes)?)), + MAGIC_VOICE_HEADER => Some(Self::VoiceHeader(VoiceHeader::from_bytes(bytes)?)), + MAGIC_VOICE_DATA => Some(Self::VoiceData(VoiceData::from_bytes(bytes)?)), + MAGIC_PACKET => Some(Self::Packet(Packet::from_bytes(bytes)?)), + MAGIC_PONG => Some(Self::Pong(Pong::from_bytes(bytes)?)), + MAGIC_CONNECT => Some(Self::Connect(Connect::from_bytes(bytes)?)), + MAGIC_LISTEN => Some(Self::Listen(Listen::from_bytes(bytes)?)), + MAGIC_DISCONNECT => Some(Self::Disconnect(Disconnect::from_bytes(bytes)?)), _ => None, } } @@ -226,7 +330,7 @@ impl ClientMessage { /// Messages sent from a reflector to a station/client #[allow(clippy::large_enum_variant)] pub enum ServerMessage { - VoiceFull(VoiceFull), + Voice(Voice), VoiceHeader(VoiceHeader), VoiceData(VoiceData), Packet(Packet), @@ -243,19 +347,19 @@ impl ServerMessage { return None; } match &bytes[0..4] { - MAGIC_VOICE => Some(Self::VoiceFull(VoiceFull::from_bytes(&bytes)?)), - MAGIC_VOICE_HEADER => Some(Self::VoiceHeader(VoiceHeader::from_bytes(&bytes)?)), - MAGIC_VOICE_DATA => Some(Self::VoiceData(VoiceData::from_bytes(&bytes)?)), - MAGIC_PACKET => Some(Self::Packet(Packet::from_bytes(&bytes)?)), - MAGIC_PING => Some(Self::Ping(Ping::from_bytes(&bytes)?)), + MAGIC_VOICE => Some(Self::Voice(Voice::from_bytes(bytes)?)), + MAGIC_VOICE_HEADER => Some(Self::VoiceHeader(VoiceHeader::from_bytes(bytes)?)), + MAGIC_VOICE_DATA => Some(Self::VoiceData(VoiceData::from_bytes(bytes)?)), + MAGIC_PACKET => Some(Self::Packet(Packet::from_bytes(bytes)?)), + MAGIC_PING => Some(Self::Ping(Ping::from_bytes(bytes)?)), MAGIC_DISCONNECT if bytes.len() == 4 => Some(Self::DisconnectAcknowledge( - DisconnectAcknowledge::from_bytes(&bytes)?, + DisconnectAcknowledge::from_bytes(bytes)?, )), - MAGIC_DISCONNECT => Some(Self::ForceDisconnect(ForceDisconnect::from_bytes(&bytes)?)), + MAGIC_DISCONNECT => Some(Self::ForceDisconnect(ForceDisconnect::from_bytes(bytes)?)), MAGIC_ACKNOWLEDGE => Some(Self::ConnectAcknowledge(ConnectAcknowledge::from_bytes( - &bytes, + bytes, )?)), - MAGIC_NACK => Some(Self::ConnectNack(ConnectNack::from_bytes(&bytes)?)), + MAGIC_NACK => Some(Self::ConnectNack(ConnectNack::from_bytes(bytes)?)), _ => None, } } @@ -281,54 +385,58 @@ impl InterlinkMessage { return None; } match &bytes[0..4] { - MAGIC_VOICE => Some(Self::VoiceInterlink(VoiceInterlink::from_bytes(&bytes)?)), + MAGIC_VOICE => Some(Self::VoiceInterlink(VoiceInterlink::from_bytes(bytes)?)), MAGIC_VOICE_HEADER => Some(Self::VoiceHeaderInterlink( - VoiceHeaderInterlink::from_bytes(&bytes)?, + VoiceHeaderInterlink::from_bytes(bytes)?, )), MAGIC_VOICE_DATA => Some(Self::VoiceDataInterlink(VoiceDataInterlink::from_bytes( - &bytes, - )?)), - MAGIC_PACKET => Some(Self::PacketInterlink(PacketInterlink::from_bytes(&bytes)?)), - MAGIC_PING => Some(Self::Ping(Ping::from_bytes(&bytes)?)), - MAGIC_CONNECT => Some(Self::ConnectInterlink(ConnectInterlink::from_bytes( - &bytes, + bytes, )?)), + MAGIC_PACKET => Some(Self::PacketInterlink(PacketInterlink::from_bytes(bytes)?)), + MAGIC_PING => Some(Self::Ping(Ping::from_bytes(bytes)?)), + MAGIC_CONNECT => Some(Self::ConnectInterlink(ConnectInterlink::from_bytes(bytes)?)), MAGIC_ACKNOWLEDGE => Some(Self::ConnectInterlinkAcknowledge( - ConnectInterlinkAcknowledge::from_bytes(&bytes)?, + ConnectInterlinkAcknowledge::from_bytes(bytes)?, )), - MAGIC_NACK => Some(Self::ConnectNack(ConnectNack::from_bytes(&bytes)?)), + MAGIC_NACK => Some(Self::ConnectNack(ConnectNack::from_bytes(bytes)?)), MAGIC_DISCONNECT => Some(Self::DisconnectInterlink(DisconnectInterlink::from_bytes( - &bytes, + bytes, )?)), _ => None, } } } -define_message!(VoiceFull, 54); -impl_stream_id!(VoiceFull, 4); -impl_link_setup!(VoiceFull, 6); -impl_frame_number!(VoiceFull, 34); -impl_payload!(VoiceFull, 36, 52); -impl_trailing_crc_verify!(VoiceFull); +define_message!(Voice, 54, 54, MAGIC_VOICE); +impl_stream_id!(Voice, 4); +impl_link_setup!(Voice, 6); +impl_frame_number!(Voice, 34); +impl_payload!(Voice, 36, 52); +impl_trailing_crc_verify!(Voice); -define_message!(VoiceHeader, 36); +define_message!(VoiceHeader, 36, 36, MAGIC_VOICE_HEADER); impl_stream_id!(VoiceHeader, 4); impl_link_setup!(VoiceHeader, 6); impl_trailing_crc_verify!(VoiceHeader); -define_message!(VoiceData, 26); +define_message!(VoiceData, 26, 26, MAGIC_VOICE_DATA); impl_stream_id!(VoiceData, 4); impl_frame_number!(VoiceData, 6); impl_payload!(VoiceData, 8, 24); impl_trailing_crc_verify!(VoiceData); -define_message!(Packet, 859); +define_message!(Packet, 859, 38, MAGIC_PACKET); impl_link_setup_frame!(Packet, 4); impl Packet { pub fn payload(&self) -> &[u8] { - &self.0[34..] + &self.0[34..self.1] + } + + pub fn set_payload(&mut self, bytes: &[u8]) { + let end = 34 + bytes.len(); + self.0[34..end].copy_from_slice(bytes); + self.1 = end; } pub fn verify_integrity(&self) -> bool { @@ -336,44 +444,48 @@ impl Packet { && self.payload().len() >= 4 && crate::crc::m17_crc(self.payload()) == 0 } + + pub fn recalculate_crc(&mut self) { + // LSF and payload should be confirmed valid before construction + } } -define_message!(Pong, 10); +define_message!(Pong, 10, 10, MAGIC_PONG); impl_address!(Pong, 4); no_crc!(Pong); -define_message!(Connect, 11); +define_message!(Connect, 11, 11, MAGIC_CONNECT); impl_address!(Connect, 4); impl_module!(Connect, 10); no_crc!(Connect); -define_message!(Listen, 11); +define_message!(Listen, 11, 11, MAGIC_LISTEN); impl_address!(Listen, 4); impl_module!(Listen, 10); no_crc!(Listen); -define_message!(Disconnect, 10); +define_message!(Disconnect, 10, 10, MAGIC_DISCONNECT); impl_address!(Disconnect, 4); no_crc!(Disconnect); -define_message!(Ping, 10); +define_message!(Ping, 10, 10, MAGIC_PING); impl_address!(Ping, 4); no_crc!(Ping); -define_message!(DisconnectAcknowledge, 4); +define_message!(DisconnectAcknowledge, 4, 4, MAGIC_DISCONNECT); no_crc!(DisconnectAcknowledge); -define_message!(ForceDisconnect, 10); +define_message!(ForceDisconnect, 10, 10, MAGIC_DISCONNECT); impl_address!(ForceDisconnect, 4); no_crc!(ForceDisconnect); -define_message!(ConnectAcknowledge, 4); +define_message!(ConnectAcknowledge, 4, 4, MAGIC_ACKNOWLEDGE); no_crc!(ConnectAcknowledge); -define_message!(ConnectNack, 4); +define_message!(ConnectNack, 4, 4, MAGIC_NACK); no_crc!(ConnectNack); -define_message!(VoiceInterlink, 55); +define_message!(VoiceInterlink, 55, 55, MAGIC_VOICE); impl_stream_id!(VoiceInterlink, 4); impl_link_setup!(VoiceInterlink, 6); impl_frame_number!(VoiceInterlink, 34); @@ -381,26 +493,34 @@ impl_payload!(VoiceInterlink, 36, 52); impl_internal_crc!(VoiceInterlink, 0, 54); impl_is_relayed!(VoiceInterlink); -define_message!(VoiceHeaderInterlink, 37); +define_message!(VoiceHeaderInterlink, 37, 37, MAGIC_VOICE_HEADER); impl_stream_id!(VoiceHeaderInterlink, 4); impl_link_setup!(VoiceHeaderInterlink, 6); impl_internal_crc!(VoiceHeaderInterlink, 0, 36); impl_is_relayed!(VoiceHeaderInterlink); -define_message!(VoiceDataInterlink, 27); +define_message!(VoiceDataInterlink, 27, 27, MAGIC_VOICE_DATA); impl_stream_id!(VoiceDataInterlink, 4); impl_frame_number!(VoiceDataInterlink, 6); impl_payload!(VoiceDataInterlink, 8, 24); impl_internal_crc!(VoiceDataInterlink, 0, 24); impl_is_relayed!(VoiceDataInterlink); -define_message!(PacketInterlink, 860); +define_message!(PacketInterlink, 860, 39, MAGIC_PACKET); impl_link_setup_frame!(PacketInterlink, 4); impl_is_relayed!(PacketInterlink); impl PacketInterlink { pub fn payload(&self) -> &[u8] { - &self.0[34..(self.0.len() - 1)] + &self.0[34..(self.1 - 1)] + } + + pub fn set_payload(&mut self, bytes: &[u8]) { + let is_relayed = self.is_relayed(); + let end = 34 + bytes.len(); + self.0[34..end].copy_from_slice(bytes); + self.1 = end + 1; + self.set_relayed(is_relayed); } pub fn verify_integrity(&self) -> bool { @@ -408,18 +528,22 @@ impl PacketInterlink { && self.payload().len() >= 4 && crate::crc::m17_crc(self.payload()) == 0 } + + pub fn recalculate_crc(&mut self) { + // LSF and payload should be confirmed valid before construction + } } -define_message!(ConnectInterlink, 37); +define_message!(ConnectInterlink, 37, 37, MAGIC_CONNECT); impl_address!(ConnectInterlink, 4); impl_modules!(ConnectInterlink, 10, 37); no_crc!(ConnectInterlink); -define_message!(ConnectInterlinkAcknowledge, 37); +define_message!(ConnectInterlinkAcknowledge, 37, 37, MAGIC_ACKNOWLEDGE); impl_address!(ConnectInterlinkAcknowledge, 4); impl_modules!(ConnectInterlinkAcknowledge, 10, 37); no_crc!(ConnectInterlinkAcknowledge); -define_message!(DisconnectInterlink, 10); +define_message!(DisconnectInterlink, 10, 10, MAGIC_DISCONNECT); impl_address!(DisconnectInterlink, 4); no_crc!(DisconnectInterlink); -- 2.39.5