Re-worked TMTC modules

satrs/src/encoding/ccsds.rs

@@ -1,23 +1,52 @@
-use crate::{tmtc::ReceivesTcCore, ValidatorU16Id};
+use spacepackets::{CcsdsPacket, SpHeader};
+
+use crate::{tmtc::PacketSenderRaw, ComponentId};
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub enum SpValidity {
+    Valid,
+    /// The space packet can be assumed to have a valid format, but the packet should
+    /// be skipped.
+    Skip,
+    /// The space packet or space packet header has an invalid format, for example a CRC check
+    /// failed. In that case, the parser loses the packet synchronization and needs to check for
+    /// the start of a new space packet header start again. The space packet header
+    /// [spacepackets::PacketId] can be used as a synchronization marker to detect the start
+    /// of a possible valid packet again.
+    Invalid,
+}
+
+/// Simple trait to allow user code to check the validity of a space packet.
+pub trait SpacePacketValidator {
+    fn validate(&self, sp_header: &SpHeader, raw_buf: &[u8]) -> SpValidity;
+}
 
 /// 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.
+/// [spacepackets::SpHeader] of the CCSDS packets and a user provided [SpacePacketValidator]
+/// to check whether a received space packet is relevant for processing.
 ///
 /// This function is also able to deal with broken tail packets at the end as long a the parser
 /// can read the full 7 bytes which constitue a space packet header plus one byte minimal size.
 /// 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`
-/// and return the number of packets found. If the [ReceivesTcCore::pass_tc] calls fails, the
-/// error will be returned.
-pub fn parse_buffer_for_ccsds_space_packets<E>(
+/// The parses will behave differently based on the [SpValidity] returned from the user provided
+/// [SpacePacketValidator]:
+///
+///  1. [SpValidity::Valid]: The parser will forward all packets to the given `packet_sender` and
+///     return the number of packets found.If the [PacketSenderRaw::send_packet] calls fails, the
+///     error will be returned.
+///  2. [SpValidity::Invalid]: The parser assumes that the synchronization is lost and tries to
+///     find the start of a new space packet header by scanning all the following bytes.
+///  3. [SpValidity::Skip]: The parser skips the packet using the packet length determined from the
+///     space packet header.
+pub fn parse_buffer_for_ccsds_space_packets<SendError>(
     buf: &mut [u8],
-    packet_id_validator: &(impl ValidatorU16Id + ?Sized),
-    tc_receiver: &mut (impl ReceivesTcCore<Error = E> + ?Sized),
+    packet_validator: &(impl SpacePacketValidator + ?Sized),
+    sender_id: ComponentId,
+    packet_sender: &(impl PacketSenderRaw<Error = SendError> + ?Sized),
     next_write_idx: &mut usize,
-) -> Result<u32, E> {
+) -> Result<u32, SendError> {
     *next_write_idx = 0;
     let mut packets_found = 0;
     let mut current_idx = 0;
@@ -26,25 +55,33 @@ pub fn parse_buffer_for_ccsds_space_packets<E>(
         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_validator.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])?;
-                packets_found += 1;
-            } else {
-                // Move packet to start of buffer if applicable.
-                if current_idx > 0 {
-                    buf.copy_within(current_idx.., 0);
-                    *next_write_idx = buf.len() - current_idx;
+        let sp_header = SpHeader::from_be_bytes(&buf[current_idx..]).unwrap().0;
+        // let packet_id = u16::from_be_bytes(buf[current_idx..current_idx + 2].try_into().unwrap());
+        match packet_validator.validate(&sp_header, &buf[current_idx..]) {
+            SpValidity::Valid => {
+                let packet_size = sp_header.total_len();
+                if (current_idx + packet_size) <= buf_len {
+                    packet_sender
+                        .send_packet(sender_id, &buf[current_idx..current_idx + packet_size])?;
+                    packets_found += 1;
+                } else {
+                    // Move packet to start of buffer if applicable.
+                    if current_idx > 0 {
+                        buf.copy_within(current_idx.., 0);
+                        *next_write_idx = buf.len() - current_idx;
+                    }
                 }
+                current_idx += packet_size;
+                continue;
+            }
+            SpValidity::Skip => {
+                current_idx += sp_header.total_len();
+            }
+            // We might have lost sync. Try to find the start of a new space packet header.
+            SpValidity::Invalid => {
+                current_idx += 1;
             }
-            current_idx += packet_size as usize;
-            continue;
         }
-        current_idx += 1;
     }
     Ok(packets_found)
 }
@@ -53,18 +90,43 @@ pub fn parse_buffer_for_ccsds_space_packets<E>(
 mod tests {
     use spacepackets::{
         ecss::{tc::PusTcCreator, WritablePusPacket},
-        PacketId, SpHeader,
+        CcsdsPacket, PacketId, SpHeader,
     };
 
-    use crate::encoding::tests::TcCacher;
+    use crate::{encoding::tests::TcCacher, ComponentId};
 
-    use super::parse_buffer_for_ccsds_space_packets;
+    use super::{parse_buffer_for_ccsds_space_packets, SpValidity, SpacePacketValidator};
 
+    const PARSER_ID: ComponentId = 0x05;
     const TEST_APID_0: u16 = 0x02;
     const TEST_APID_1: u16 = 0x10;
     const TEST_PACKET_ID_0: PacketId = PacketId::new_for_tc(true, TEST_APID_0);
     const TEST_PACKET_ID_1: PacketId = PacketId::new_for_tc(true, TEST_APID_1);
 
+    #[derive(Default)]
+    struct SimpleVerificator {
+        pub enable_second_id: bool,
+    }
+
+    impl SimpleVerificator {
+        pub fn new_with_second_id() -> Self {
+            Self {
+                enable_second_id: true,
+            }
+        }
+    }
+
+    impl SpacePacketValidator for SimpleVerificator {
+        fn validate(&self, sp_header: &SpHeader, _raw_buf: &[u8]) -> super::SpValidity {
+            if sp_header.packet_id() == TEST_PACKET_ID_0
+                || (self.enable_second_id && sp_header.packet_id() == TEST_PACKET_ID_1)
+            {
+                return SpValidity::Valid;
+            }
+            SpValidity::Skip
+        }
+    }
+
     #[test]
     fn test_basic() {
         let sph = SpHeader::new_from_apid(TEST_APID_0);
@@ -73,23 +135,23 @@ mod tests {
         let packet_len = ping_tc
             .write_to_bytes(&mut buffer)
             .expect("writing packet failed");
-        let valid_packet_ids = [TEST_PACKET_ID_0];
-        let mut tc_cacher = TcCacher::default();
+        let tc_cacher = TcCacher::default();
         let mut next_write_idx = 0;
         let parse_result = parse_buffer_for_ccsds_space_packets(
             &mut buffer,
-            valid_packet_ids.as_slice(),
-            &mut tc_cacher,
+            &SimpleVerificator::default(),
+            PARSER_ID,
+            &tc_cacher,
             &mut next_write_idx,
         );
         assert!(parse_result.is_ok());
         let parsed_packets = parse_result.unwrap();
         assert_eq!(parsed_packets, 1);
-        assert_eq!(tc_cacher.tc_queue.len(), 1);
-        assert_eq!(
-            tc_cacher.tc_queue.pop_front().unwrap(),
-            buffer[..packet_len]
-        );
+        let mut queue = tc_cacher.tc_queue.borrow_mut();
+        assert_eq!(queue.len(), 1);
+        let packet_with_sender = queue.pop_front().unwrap();
+        assert_eq!(packet_with_sender.packet, buffer[..packet_len]);
+        assert_eq!(packet_with_sender.sender_id, PARSER_ID);
     }
 
     #[test]
@@ -104,25 +166,27 @@ mod tests {
         let packet_len_action = action_tc
             .write_to_bytes(&mut buffer[packet_len_ping..])
             .expect("writing packet failed");
-        let valid_packet_ids = [TEST_PACKET_ID_0];
-        let mut tc_cacher = TcCacher::default();
+        let tc_cacher = TcCacher::default();
         let mut next_write_idx = 0;
         let parse_result = parse_buffer_for_ccsds_space_packets(
             &mut buffer,
-            valid_packet_ids.as_slice(),
-            &mut tc_cacher,
+            &SimpleVerificator::default(),
+            PARSER_ID,
+            &tc_cacher,
             &mut next_write_idx,
         );
         assert!(parse_result.is_ok());
         let parsed_packets = parse_result.unwrap();
         assert_eq!(parsed_packets, 2);
-        assert_eq!(tc_cacher.tc_queue.len(), 2);
+        let mut queue = tc_cacher.tc_queue.borrow_mut();
+        assert_eq!(queue.len(), 2);
+        let packet_with_addr = queue.pop_front().unwrap();
+        assert_eq!(packet_with_addr.packet, buffer[..packet_len_ping]);
+        assert_eq!(packet_with_addr.sender_id, PARSER_ID);
+        let packet_with_addr = queue.pop_front().unwrap();
+        assert_eq!(packet_with_addr.sender_id, PARSER_ID);
         assert_eq!(
-            tc_cacher.tc_queue.pop_front().unwrap(),
-            buffer[..packet_len_ping]
-        );
-        assert_eq!(
-            tc_cacher.tc_queue.pop_front().unwrap(),
+            packet_with_addr.packet,
             buffer[packet_len_ping..packet_len_ping + packet_len_action]
         );
     }
@@ -140,25 +204,26 @@ mod tests {
         let packet_len_action = action_tc
             .write_to_bytes(&mut buffer[packet_len_ping..])
             .expect("writing packet failed");
-        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
-        let mut tc_cacher = TcCacher::default();
+        let tc_cacher = TcCacher::default();
         let mut next_write_idx = 0;
+        let verificator = SimpleVerificator::new_with_second_id();
         let parse_result = parse_buffer_for_ccsds_space_packets(
             &mut buffer,
-            valid_packet_ids.as_slice(),
-            &mut tc_cacher,
+            &verificator,
+            PARSER_ID,
+            &tc_cacher,
             &mut next_write_idx,
         );
         assert!(parse_result.is_ok());
         let parsed_packets = parse_result.unwrap();
         assert_eq!(parsed_packets, 2);
-        assert_eq!(tc_cacher.tc_queue.len(), 2);
+        let mut queue = tc_cacher.tc_queue.borrow_mut();
+        assert_eq!(queue.len(), 2);
+        let packet_with_addr = queue.pop_front().unwrap();
+        assert_eq!(packet_with_addr.packet, buffer[..packet_len_ping]);
+        let packet_with_addr = queue.pop_front().unwrap();
         assert_eq!(
-            tc_cacher.tc_queue.pop_front().unwrap(),
-            buffer[..packet_len_ping]
-        );
-        assert_eq!(
-            tc_cacher.tc_queue.pop_front().unwrap(),
+            packet_with_addr.packet,
             buffer[packet_len_ping..packet_len_ping + packet_len_action]
         );
     }
@@ -176,19 +241,22 @@ mod tests {
         let packet_len_action = action_tc
             .write_to_bytes(&mut buffer[packet_len_ping..])
             .expect("writing packet failed");
-        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
-        let mut tc_cacher = TcCacher::default();
+        let tc_cacher = TcCacher::default();
         let mut next_write_idx = 0;
+        let verificator = SimpleVerificator::new_with_second_id();
         let parse_result = parse_buffer_for_ccsds_space_packets(
             &mut buffer[..packet_len_ping + packet_len_action - 4],
-            valid_packet_ids.as_slice(),
-            &mut tc_cacher,
+            &verificator,
+            PARSER_ID,
+            &tc_cacher,
             &mut next_write_idx,
         );
         assert!(parse_result.is_ok());
         let parsed_packets = parse_result.unwrap();
         assert_eq!(parsed_packets, 1);
-        assert_eq!(tc_cacher.tc_queue.len(), 1);
+
+        let queue = tc_cacher.tc_queue.borrow();
+        assert_eq!(queue.len(), 1);
         // The broken packet was moved to the start, so the next write index should be after the
         // last segment missing 4 bytes.
         assert_eq!(next_write_idx, packet_len_action - 4);
@@ -202,19 +270,22 @@ mod tests {
         let packet_len_ping = ping_tc
             .write_to_bytes(&mut buffer)
             .expect("writing packet failed");
-        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
-        let mut tc_cacher = TcCacher::default();
+        let tc_cacher = TcCacher::default();
+
+        let verificator = SimpleVerificator::new_with_second_id();
         let mut next_write_idx = 0;
         let parse_result = parse_buffer_for_ccsds_space_packets(
             &mut buffer[..packet_len_ping - 4],
-            valid_packet_ids.as_slice(),
-            &mut tc_cacher,
+            &verificator,
+            PARSER_ID,
+            &tc_cacher,
             &mut next_write_idx,
         );
         assert_eq!(next_write_idx, 0);
         assert!(parse_result.is_ok());
         let parsed_packets = parse_result.unwrap();
         assert_eq!(parsed_packets, 0);
-        assert_eq!(tc_cacher.tc_queue.len(), 0);
+        let queue = tc_cacher.tc_queue.borrow();
+        assert_eq!(queue.len(), 0);
     }
 }