diff --git a/satrs-core/src/parsers/ccsds.rs b/satrs-core/src/parsers/ccsds.rs new file mode 100644 index 0000000..949a1cf --- /dev/null +++ b/satrs-core/src/parsers/ccsds.rs @@ -0,0 +1,78 @@ +#[cfg(feature = "alloc")] +use alloc::vec::Vec; +#[cfg(feature = "alloc")] +use hashbrown::HashSet; + +use crate::tmtc::ReceivesTc; + +pub trait PacketIdLookup { + fn validate(&self, apid: u16) -> bool; +} + +#[cfg(feature = "alloc")] +impl PacketIdLookup for Vec { + fn validate(&self, apid: u16) -> bool { + self.contains(&apid) + } +} + +#[cfg(feature = "alloc")] +impl PacketIdLookup for HashSet { + fn validate(&self, apid: u16) -> bool { + self.contains(&apid) + } +} + +impl PacketIdLookup for &[u16] { + fn validate(&self, apid: u16) -> bool { + if self.binary_search(&apid).is_ok() { + return true; + } + false + } +} + +/// This function parses a given buffer for tightly packed CCSDS space packets. It uses the +/// [PacketId] field of the CCSDS packets to detect the start of a CCSDS space packet and then +/// uses the length field of the packet to extract CCSDS packets. +/// +/// This function is also able to deal with broken tail packets at the end as long a the parser +/// can read the full 6 bytes which constitue a space packet header. If broken tail packets are +/// detected, they are moved to the front of the buffer, and the write index for future write +/// operations will be written to the `next_write_idx` argument. +/// +/// The parser will write all packets which were decoded successfully to the given `tc_receiver`. +pub fn parse_buffer_for_ccsds_space_packets( + buf: &mut [u8], + packet_id_lookup: &dyn PacketIdLookup, + tc_receiver: &mut dyn ReceivesTc, + next_write_idx: &mut usize, +) -> Result { + let packets_found = 0; + let mut current_idx = 0; + let buf_len = buf.len(); + loop { + if current_idx + 7 >= buf.len() { + break; + } + let packet_id = u16::from_be_bytes(buf[current_idx..current_idx + 2].try_into().unwrap()); + if packet_id_lookup.validate(packet_id) { + let length_field = + u16::from_be_bytes(buf[current_idx + 4..current_idx + 6].try_into().unwrap()); + let packet_size = length_field + 7; + if (current_idx + packet_size as usize) < buf_len { + tc_receiver.pass_tc(&buf[current_idx..current_idx + packet_size as usize])?; + } else { + // Move packet to start of buffer if applicable. + if current_idx > 0 { + buf.copy_within(current_idx.., 0); + *next_write_idx = current_idx; + } + } + current_idx += packet_size as usize; + continue; + } + current_idx += 1; + } + Ok(packets_found) +} diff --git a/satrs-core/src/parsers.rs b/satrs-core/src/parsers/cobs.rs similarity index 72% rename from satrs-core/src/parsers.rs rename to satrs-core/src/parsers/cobs.rs index cfcd9b8..bf99ce6 100644 --- a/satrs-core/src/parsers.rs +++ b/satrs-core/src/parsers/cobs.rs @@ -1,10 +1,5 @@ -#[cfg(feature = "alloc")] -use alloc::vec::Vec; -use cobs::decode_in_place; -#[cfg(feature = "alloc")] -use hashbrown::HashSet; - use crate::tmtc::ReceivesTc; +use cobs::decode_in_place; /// This function parses a given buffer for COBS encoded packets. The packet structure is /// expected to be like this, assuming a sentinel value of 0 as the packet delimiter: @@ -58,90 +53,18 @@ pub fn parse_buffer_for_cobs_encoded_packets( Ok(packets_found) } -pub trait PacketIdLookup { - fn validate(&self, apid: u16) -> bool; -} - -#[cfg(feature = "alloc")] -impl PacketIdLookup for Vec { - fn validate(&self, apid: u16) -> bool { - self.contains(&apid) - } -} - -#[cfg(feature = "alloc")] -impl PacketIdLookup for HashSet { - fn validate(&self, apid: u16) -> bool { - self.contains(&apid) - } -} - -impl PacketIdLookup for &[u16] { - fn validate(&self, apid: u16) -> bool { - if self.binary_search(&apid).is_ok() { - return true; - } - false - } -} - -/// This function parses a given buffer for tightly packed CCSDS space packets. It uses the -/// [PacketId] field of the CCSDS packets to detect the start of a CCSDS space packet and then -/// uses the length field of the packet to extract CCSDS packets. -/// -/// This function is also able to deal with broken tail packets at the end as long a the parser -/// can read the full 6 bytes which constitue a space packet header. If broken tail packets are -/// detected, they are moved to the front of the buffer, and the write index for future write -/// operations will be written to the `next_write_idx` argument. -/// -/// The parser will write all packets which were decoded successfully to the given `tc_receiver`. -pub fn parse_buffer_for_ccsds_space_packets( - buf: &mut [u8], - packet_id_lookup: &dyn PacketIdLookup, - tc_receiver: &mut dyn ReceivesTc, - next_write_idx: &mut usize, -) -> Result { - let packets_found = 0; - let mut current_idx = 0; - let buf_len = buf.len(); - loop { - if current_idx + 7 >= buf.len() { - break; - } - let packet_id = u16::from_be_bytes(buf[current_idx..current_idx + 2].try_into().unwrap()); - if packet_id_lookup.validate(packet_id) { - let length_field = - u16::from_be_bytes(buf[current_idx + 4..current_idx + 6].try_into().unwrap()); - let packet_size = length_field + 7; - if (current_idx + packet_size as usize) < buf_len { - tc_receiver.pass_tc(&buf[current_idx..current_idx + packet_size as usize])?; - } else { - // Move packet to start of buffer if applicable. - if current_idx > 0 { - buf.copy_within(current_idx.., 0); - *next_write_idx = current_idx; - } - } - current_idx += packet_size as usize; - continue; - } - current_idx += 1; - } - Ok(packets_found) -} - #[cfg(test)] pub(crate) mod tests { use alloc::{collections::VecDeque, vec::Vec}; use cobs::encode; - use crate::tmtc::ReceivesTcCore; + use crate::{ + parsers::tests::{encode_simple_packet, INVERTED_PACKET, SIMPLE_PACKET}, + tmtc::ReceivesTcCore, + }; use super::parse_buffer_for_cobs_encoded_packets; - pub(crate) const SIMPLE_PACKET: [u8; 5] = [1, 2, 3, 4, 5]; - pub(crate) const INVERTED_PACKET: [u8; 5] = [5, 4, 3, 2, 1]; - #[derive(Default)] struct TcCacher { tc_queue: VecDeque>, @@ -156,14 +79,6 @@ pub(crate) mod tests { } } - pub(crate) fn encode_simple_packet(encoded_buf: &mut [u8], current_idx: &mut usize) { - encoded_buf[*current_idx] = 0; - *current_idx += 1; - *current_idx += encode(&SIMPLE_PACKET, &mut encoded_buf[*current_idx..]); - encoded_buf[*current_idx] = 0; - *current_idx += 1; - } - #[test] fn test_parsing_simple_packet() { let mut test_sender = TcCacher::default(); diff --git a/satrs-core/src/parsers/mod.rs b/satrs-core/src/parsers/mod.rs new file mode 100644 index 0000000..c25060a --- /dev/null +++ b/satrs-core/src/parsers/mod.rs @@ -0,0 +1,21 @@ +pub mod ccsds; +pub mod cobs; + +pub use crate::parsers::ccsds::parse_buffer_for_ccsds_space_packets; +pub use crate::parsers::cobs::parse_buffer_for_cobs_encoded_packets; + +#[cfg(test)] +pub(crate) mod tests { + use cobs::encode; + + pub(crate) const SIMPLE_PACKET: [u8; 5] = [1, 2, 3, 4, 5]; + pub(crate) const INVERTED_PACKET: [u8; 5] = [5, 4, 3, 2, 1]; + + pub(crate) fn encode_simple_packet(encoded_buf: &mut [u8], current_idx: &mut usize) { + encoded_buf[*current_idx] = 0; + *current_idx += 1; + *current_idx += encode(&SIMPLE_PACKET, &mut encoded_buf[*current_idx..]); + encoded_buf[*current_idx] = 0; + *current_idx += 1; + } +}