satrs_core/encoding/
ccsds.rs

1#[cfg(feature = "alloc")]
2use alloc::vec::Vec;
3#[cfg(feature = "alloc")]
4use hashbrown::HashSet;
5use spacepackets::PacketId;
6
7use crate::tmtc::ReceivesTcCore;
8
9pub trait PacketIdLookup {
10    fn validate(&self, packet_id: u16) -> bool;
11}
12
13#[cfg(feature = "alloc")]
14impl PacketIdLookup for Vec<u16> {
15    fn validate(&self, packet_id: u16) -> bool {
16        self.contains(&packet_id)
17    }
18}
19
20#[cfg(feature = "alloc")]
21impl PacketIdLookup for HashSet<u16> {
22    fn validate(&self, packet_id: u16) -> bool {
23        self.contains(&packet_id)
24    }
25}
26
27impl PacketIdLookup for [u16] {
28    fn validate(&self, packet_id: u16) -> bool {
29        self.binary_search(&packet_id).is_ok()
30    }
31}
32
33impl PacketIdLookup for &[u16] {
34    fn validate(&self, packet_id: u16) -> bool {
35        self.binary_search(&packet_id).is_ok()
36    }
37}
38
39#[cfg(feature = "alloc")]
40impl PacketIdLookup for Vec<PacketId> {
41    fn validate(&self, packet_id: u16) -> bool {
42        self.contains(&PacketId::from(packet_id))
43    }
44}
45#[cfg(feature = "alloc")]
46impl PacketIdLookup for HashSet<PacketId> {
47    fn validate(&self, packet_id: u16) -> bool {
48        self.contains(&PacketId::from(packet_id))
49    }
50}
51
52impl PacketIdLookup for [PacketId] {
53    fn validate(&self, packet_id: u16) -> bool {
54        self.binary_search(&PacketId::from(packet_id)).is_ok()
55    }
56}
57
58impl PacketIdLookup for &[PacketId] {
59    fn validate(&self, packet_id: u16) -> bool {
60        self.binary_search(&PacketId::from(packet_id)).is_ok()
61    }
62}
63
64/// This function parses a given buffer for tightly packed CCSDS space packets. It uses the
65/// [PacketId] field of the CCSDS packets to detect the start of a CCSDS space packet and then
66/// uses the length field of the packet to extract CCSDS packets.
67///
68/// This function is also able to deal with broken tail packets at the end as long a the parser
69/// can read the full 7 bytes which constitue a space packet header plus one byte minimal size.
70/// If broken tail packets are detected, they are moved to the front of the buffer, and the write
71/// index for future write operations will be written to the `next_write_idx` argument.
72///
73/// The parser will write all packets which were decoded successfully to the given `tc_receiver`
74/// and return the number of packets found. If the [ReceivesTcCore::pass_tc] calls fails, the
75/// error will be returned.
76pub fn parse_buffer_for_ccsds_space_packets<E>(
77    buf: &mut [u8],
78    packet_id_lookup: &(impl PacketIdLookup + ?Sized),
79    tc_receiver: &mut (impl ReceivesTcCore<Error = E> + ?Sized),
80    next_write_idx: &mut usize,
81) -> Result<u32, E> {
82    *next_write_idx = 0;
83    let mut packets_found = 0;
84    let mut current_idx = 0;
85    let buf_len = buf.len();
86    loop {
87        if current_idx + 7 >= buf.len() {
88            break;
89        }
90        let packet_id = u16::from_be_bytes(buf[current_idx..current_idx + 2].try_into().unwrap());
91        if packet_id_lookup.validate(packet_id) {
92            let length_field =
93                u16::from_be_bytes(buf[current_idx + 4..current_idx + 6].try_into().unwrap());
94            let packet_size = length_field + 7;
95            if (current_idx + packet_size as usize) <= buf_len {
96                tc_receiver.pass_tc(&buf[current_idx..current_idx + packet_size as usize])?;
97                packets_found += 1;
98            } else {
99                // Move packet to start of buffer if applicable.
100                if current_idx > 0 {
101                    buf.copy_within(current_idx.., 0);
102                    *next_write_idx = buf.len() - current_idx;
103                }
104            }
105            current_idx += packet_size as usize;
106            continue;
107        }
108        current_idx += 1;
109    }
110    Ok(packets_found)
111}
112
113#[cfg(test)]
114mod tests {
115    use spacepackets::{
116        ecss::{tc::PusTcCreator, WritablePusPacket},
117        PacketId, SpHeader,
118    };
119
120    use crate::encoding::tests::TcCacher;
121
122    use super::parse_buffer_for_ccsds_space_packets;
123
124    const TEST_APID_0: u16 = 0x02;
125    const TEST_APID_1: u16 = 0x10;
126    const TEST_PACKET_ID_0: PacketId = PacketId::const_tc(true, TEST_APID_0);
127    const TEST_PACKET_ID_1: PacketId = PacketId::const_tc(true, TEST_APID_1);
128
129    #[test]
130    fn test_basic() {
131        let mut sph = SpHeader::tc_unseg(TEST_APID_0, 0, 0).unwrap();
132        let ping_tc = PusTcCreator::new_simple(&mut sph, 17, 1, None, true);
133        let mut buffer: [u8; 32] = [0; 32];
134        let packet_len = ping_tc
135            .write_to_bytes(&mut buffer)
136            .expect("writing packet failed");
137        let valid_packet_ids = [TEST_PACKET_ID_0];
138        let mut tc_cacher = TcCacher::default();
139        let mut next_write_idx = 0;
140        let parse_result = parse_buffer_for_ccsds_space_packets(
141            &mut buffer,
142            valid_packet_ids.as_slice(),
143            &mut tc_cacher,
144            &mut next_write_idx,
145        );
146        assert!(parse_result.is_ok());
147        let parsed_packets = parse_result.unwrap();
148        assert_eq!(parsed_packets, 1);
149        assert_eq!(tc_cacher.tc_queue.len(), 1);
150        assert_eq!(
151            tc_cacher.tc_queue.pop_front().unwrap(),
152            buffer[..packet_len]
153        );
154    }
155
156    #[test]
157    fn test_multi_packet() {
158        let mut sph = SpHeader::tc_unseg(TEST_APID_0, 0, 0).unwrap();
159        let ping_tc = PusTcCreator::new_simple(&mut sph, 17, 1, None, true);
160        let action_tc = PusTcCreator::new_simple(&mut sph, 8, 0, None, true);
161        let mut buffer: [u8; 32] = [0; 32];
162        let packet_len_ping = ping_tc
163            .write_to_bytes(&mut buffer)
164            .expect("writing packet failed");
165        let packet_len_action = action_tc
166            .write_to_bytes(&mut buffer[packet_len_ping..])
167            .expect("writing packet failed");
168        let valid_packet_ids = [TEST_PACKET_ID_0];
169        let mut tc_cacher = TcCacher::default();
170        let mut next_write_idx = 0;
171        let parse_result = parse_buffer_for_ccsds_space_packets(
172            &mut buffer,
173            valid_packet_ids.as_slice(),
174            &mut tc_cacher,
175            &mut next_write_idx,
176        );
177        assert!(parse_result.is_ok());
178        let parsed_packets = parse_result.unwrap();
179        assert_eq!(parsed_packets, 2);
180        assert_eq!(tc_cacher.tc_queue.len(), 2);
181        assert_eq!(
182            tc_cacher.tc_queue.pop_front().unwrap(),
183            buffer[..packet_len_ping]
184        );
185        assert_eq!(
186            tc_cacher.tc_queue.pop_front().unwrap(),
187            buffer[packet_len_ping..packet_len_ping + packet_len_action]
188        );
189    }
190
191    #[test]
192    fn test_multi_apid() {
193        let mut sph = SpHeader::tc_unseg(TEST_APID_0, 0, 0).unwrap();
194        let ping_tc = PusTcCreator::new_simple(&mut sph, 17, 1, None, true);
195        sph = SpHeader::tc_unseg(TEST_APID_1, 0, 0).unwrap();
196        let action_tc = PusTcCreator::new_simple(&mut sph, 8, 0, None, true);
197        let mut buffer: [u8; 32] = [0; 32];
198        let packet_len_ping = ping_tc
199            .write_to_bytes(&mut buffer)
200            .expect("writing packet failed");
201        let packet_len_action = action_tc
202            .write_to_bytes(&mut buffer[packet_len_ping..])
203            .expect("writing packet failed");
204        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
205        let mut tc_cacher = TcCacher::default();
206        let mut next_write_idx = 0;
207        let parse_result = parse_buffer_for_ccsds_space_packets(
208            &mut buffer,
209            valid_packet_ids.as_slice(),
210            &mut tc_cacher,
211            &mut next_write_idx,
212        );
213        assert!(parse_result.is_ok());
214        let parsed_packets = parse_result.unwrap();
215        assert_eq!(parsed_packets, 2);
216        assert_eq!(tc_cacher.tc_queue.len(), 2);
217        assert_eq!(
218            tc_cacher.tc_queue.pop_front().unwrap(),
219            buffer[..packet_len_ping]
220        );
221        assert_eq!(
222            tc_cacher.tc_queue.pop_front().unwrap(),
223            buffer[packet_len_ping..packet_len_ping + packet_len_action]
224        );
225    }
226
227    #[test]
228    fn test_split_packet_multi() {
229        let mut sph = SpHeader::tc_unseg(TEST_APID_0, 0, 0).unwrap();
230        let ping_tc = PusTcCreator::new_simple(&mut sph, 17, 1, None, true);
231        sph = SpHeader::tc_unseg(TEST_APID_1, 0, 0).unwrap();
232        let action_tc = PusTcCreator::new_simple(&mut sph, 8, 0, None, true);
233        let mut buffer: [u8; 32] = [0; 32];
234        let packet_len_ping = ping_tc
235            .write_to_bytes(&mut buffer)
236            .expect("writing packet failed");
237        let packet_len_action = action_tc
238            .write_to_bytes(&mut buffer[packet_len_ping..])
239            .expect("writing packet failed");
240        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
241        let mut tc_cacher = TcCacher::default();
242        let mut next_write_idx = 0;
243        let parse_result = parse_buffer_for_ccsds_space_packets(
244            &mut buffer[..packet_len_ping + packet_len_action - 4],
245            valid_packet_ids.as_slice(),
246            &mut tc_cacher,
247            &mut next_write_idx,
248        );
249        assert!(parse_result.is_ok());
250        let parsed_packets = parse_result.unwrap();
251        assert_eq!(parsed_packets, 1);
252        assert_eq!(tc_cacher.tc_queue.len(), 1);
253        // The broken packet was moved to the start, so the next write index should be after the
254        // last segment missing 4 bytes.
255        assert_eq!(next_write_idx, packet_len_action - 4);
256    }
257
258    #[test]
259    fn test_one_split_packet() {
260        let mut sph = SpHeader::tc_unseg(TEST_APID_0, 0, 0).unwrap();
261        let ping_tc = PusTcCreator::new_simple(&mut sph, 17, 1, None, true);
262        let mut buffer: [u8; 32] = [0; 32];
263        let packet_len_ping = ping_tc
264            .write_to_bytes(&mut buffer)
265            .expect("writing packet failed");
266        let valid_packet_ids = [TEST_PACKET_ID_0, TEST_PACKET_ID_1];
267        let mut tc_cacher = TcCacher::default();
268        let mut next_write_idx = 0;
269        let parse_result = parse_buffer_for_ccsds_space_packets(
270            &mut buffer[..packet_len_ping - 4],
271            valid_packet_ids.as_slice(),
272            &mut tc_cacher,
273            &mut next_write_idx,
274        );
275        assert_eq!(next_write_idx, 0);
276        assert!(parse_result.is_ok());
277        let parsed_packets = parse_result.unwrap();
278        assert_eq!(parsed_packets, 0);
279        assert_eq!(tc_cacher.tc_queue.len(), 0);
280    }
281}
satrs_core/encoding/ccsds.rs

satrs_core/encoding/
ccsds.rs
