From 6598d3458fd08e2eaf89415d6ec483cf4ea41225 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 17 Jun 2022 01:45:08 +0200 Subject: [PATCH] ccsds length field calculation --- spacepackets/src/lib.rs | 18 ++++++------ spacepackets/src/tc.rs | 63 +++++++++++++++++++++++++++++++++++------ 2 files changed, 65 insertions(+), 16 deletions(-) diff --git a/spacepackets/src/lib.rs b/spacepackets/src/lib.rs index de8c055..5e83fdf 100644 --- a/spacepackets/src/lib.rs +++ b/spacepackets/src/lib.rs @@ -10,6 +10,8 @@ pub enum PacketError { ToBytesSliceTooSmall(usize), /// The [zerocopy] library failed to write to bytes ToBytesZeroCopyError, + /// CRC16 needs to be calculated first + CrcCalculationMissing, } #[derive(Serialize, Deserialize, Debug, PartialEq, Copy, Clone)] @@ -375,17 +377,9 @@ pub mod zc { ((self.version_packet_id.get() >> 13) as u8) & 0b111 } - fn packet_id_raw(&self) -> u16 { - self.version_packet_id.get() & (!VERSION_MASK) - } fn packet_id(&self) -> PacketId { PacketId::from(self.packet_id_raw()) } - - fn psc_raw(&self) -> u16 { - self.psc.get() - } - fn psc(&self) -> PacketSequenceCtrl { PacketSequenceCtrl::from(self.psc_raw()) } @@ -394,6 +388,14 @@ pub mod zc { fn data_len(&self) -> u16 { self.data_len.get() } + + fn packet_id_raw(&self) -> u16 { + self.version_packet_id.get() & (!VERSION_MASK) + } + + fn psc_raw(&self) -> u16 { + self.psc.get() + } } impl CcsdsPrimaryHeader for SpHeader { diff --git a/spacepackets/src/tc.rs b/spacepackets/src/tc.rs index e1f47ca..730c5fd 100644 --- a/spacepackets/src/tc.rs +++ b/spacepackets/src/tc.rs @@ -1,7 +1,10 @@ use std::mem::size_of; +type CrcType = u16; +/// PUS C secondary header length is fixed +pub const PUC_TC_SECONDARY_HEADER_LEN: usize = size_of::(); pub const PUS_TC_MIN_LEN_WITHOUT_APP_DATA: usize = - size_of::() + size_of::(); + size_of::() + PUC_TC_SECONDARY_HEADER_LEN + size_of::(); pub mod zc { use crate::ecss::{PusError, PusVersion}; @@ -95,10 +98,47 @@ pub mod srd { } } + pub fn len(&self) -> usize { + let mut length = super::PUS_TC_MIN_LEN_WITHOUT_APP_DATA; + if let Some(app_data) = self.app_data { + length += app_data.len(); + } + length + } + + /// Calculate the CCSDS space packet data length field and sets it + pub fn set_ccsds_data_len(&mut self) { + self.sph.data_len = self.len() as u16 - size_of::() as u16 - 1; + } + + pub fn calc_crc16(&mut self) { + let mut digest = CRC_CCITT_FALSE.digest(); + let sph_zc = crate::zc::SpHeader::from(self.sph); + digest.update(sph_zc.as_bytes()); + let pus_tc_header = + super::zc::PusTcDataFieldHeader::try_from(self.data_field_header).unwrap(); + digest.update(pus_tc_header.as_bytes()); + if let Some(app_data) = self.app_data { + digest.update(app_data); + } + self.crc16 = Some(digest.finalize()) + } + + /// This function updates two important internal fields: The CCSDS packet length in the + /// space packet header and the CRC16 field. This function should be called before + /// the TC packet is serialized + pub fn update_packet_fields(&mut self) { + self.set_ccsds_data_len(); + self.calc_crc16(); + } + pub fn copy_to_buf( - &mut self, + &self, slice: &mut (impl AsMut<[u8]> + ?Sized), ) -> Result { + if self.crc16.is_none() { + return Err(PacketError::CrcCalculationMissing); + } let mut_slice = slice.as_mut(); let mut curr_idx = 0; let sph_zc = crate::zc::SpHeader::from(self.sph); @@ -126,10 +166,16 @@ pub mod srd { mut_slice[curr_idx..curr_idx + app_data.len()].copy_from_slice(app_data); curr_idx += app_data.len(); } + mut_slice[curr_idx..curr_idx + 2] + .copy_from_slice(self.crc16.unwrap().to_ne_bytes().as_slice()); + curr_idx += 2; Ok(curr_idx) } - pub fn append_to_vec(&mut self, vec: &mut Vec) -> Result { + pub fn append_to_vec(&self, vec: &mut Vec) -> Result { + if self.crc16.is_none() { + return Err(PacketError::CrcCalculationMissing); + } let sph_zc = crate::zc::SpHeader::from(self.sph); let mut appended_len = super::PUS_TC_MIN_LEN_WITHOUT_APP_DATA; if let Some(app_data) = self.app_data { @@ -143,6 +189,7 @@ pub mod srd { if let Some(app_data) = self.app_data { vec.extend_from_slice(app_data); } + vec.extend_from_slice(self.crc16.unwrap().to_ne_bytes().as_slice()); Ok(appended_len) } } @@ -213,19 +260,19 @@ mod tests { fn test_tc() { let mut sph = SpHeader::tc(0x42, 12).unwrap(); let mut pus_tc = PusTc::new(&mut sph, 1, 1, None); - let out = to_stdvec(&pus_tc).unwrap(); - println!("Vector {:#04x?} with length {}", out, out.len()); - + let _out = to_stdvec(&pus_tc).unwrap(); let mut test_buf = [0; 32]; + pus_tc.update_packet_fields(); + assert_eq!(pus_tc.len(), 13); let size = pus_tc .copy_to_buf(test_buf.as_mut_slice()) .expect("Error writing TC to buffer"); - println!("Test buffer: {:?} with {size} written bytes", test_buf); + println!("Test buffer: {:02x?} with {size} written bytes", test_buf); let mut test_vec = Vec::new(); let size = pus_tc .append_to_vec(&mut test_vec) .expect("Error writing TC to vector"); - println!("Test Vector: {:?} with {size} written bytes", test_vec); + println!("Test Vector: {:02x?} with {size} written bytes", test_vec); } }