From 0a7fa4ecf0e700ff356244da33be5bec12008626 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Oct 2024 15:34:47 +0200 Subject: [PATCH] continue USLP support --- src/lib.rs | 2 +- src/uslp/mod.rs | 449 ++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 413 insertions(+), 38 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index b40c9b2..46cc5eb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,9 +76,9 @@ use serde::{Deserialize, Serialize}; pub mod cfdp; pub mod ecss; -pub mod uslp; pub mod seq_count; pub mod time; +pub mod uslp; pub mod util; mod private { diff --git a/src/uslp/mod.rs b/src/uslp/mod.rs index d9824b2..0b2645b 100644 --- a/src/uslp/mod.rs +++ b/src/uslp/mod.rs @@ -1,6 +1,8 @@ -use crate::ByteConversionError; +/// # Support of the CCSDS Unified Space Data Link Protocol (USLP) +use crate::{ByteConversionError, CRC_CCITT_FALSE}; -const USLP_VERSION_NUMBER: u8 = 0b1100; +/// Only this version is supported by the library +pub const USLP_VERSION_NUMBER: u8 = 0b1100; /// Identifies the association of the data contained in the transfer frame. #[derive( @@ -37,31 +39,20 @@ pub enum BypassSequenceControlFlag { #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(u8)] pub enum ProtocolControlCommandFlag { - TfdfContainsUserData, - TfdfContainsProtocolInfo, + TfdfContainsUserData = 0, + TfdfContainsProtocolInfo = 1, } #[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub struct PrimaryHeader { - spacecraft_id: u16, - source_or_dest_field: SourceOrDestField, - vc_id: u8, - map_id: u8, - frame_len: u16, - sequence_control_flag: BypassSequenceControlFlag, - protocol_control_command_flag: ProtocolControlCommandFlag, - ocf_flag: bool, - vc_frame_count_len: u8, - vc_frame_count: u64, -} - -#[derive(Debug)] pub enum UslpError { ByteConversion(ByteConversionError), HeaderIsTruncated, + InvalidProtocolId(u8), + InvalidConstructionRule(u8), InvalidVersionNumber(u8), + InvalidVcid(u8), + InvalidMapId(u8), + ChecksumFailure(u16), } impl From for UslpError { @@ -70,7 +61,82 @@ impl From for UslpError { } } +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct InvalidValueForLen { + value: u64, + len: u8, +} + +#[derive(Debug, Copy, Clone, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct PrimaryHeader { + pub spacecraft_id: u16, + pub source_or_dest_field: SourceOrDestField, + pub vc_id: u8, + pub map_id: u8, + pub frame_len: u16, + pub sequence_control_flag: BypassSequenceControlFlag, + pub protocol_control_command_flag: ProtocolControlCommandFlag, + pub ocf_flag: bool, + vc_frame_count_len: u8, + vc_frame_count: u64, +} + impl PrimaryHeader { + pub fn new( + spacecraft_id: u16, + source_or_dest_field: SourceOrDestField, + vc_id: u8, + map_id: u8, + frame_len: u16, + ) -> Result { + if vc_id > 0b111111 { + return Err(UslpError::InvalidVcid(vc_id)); + } + if map_id > 0b1111 { + return Err(UslpError::InvalidMapId(map_id)); + } + Ok(Self { + spacecraft_id, + source_or_dest_field, + vc_id, + map_id, + frame_len, + sequence_control_flag: BypassSequenceControlFlag::SequenceControlledQoS, + protocol_control_command_flag: ProtocolControlCommandFlag::TfdfContainsUserData, + ocf_flag: false, + vc_frame_count_len: 0, + vc_frame_count: 0, + }) + } + + pub fn set_vc_frame_count( + &mut self, + count_len: u8, + count: u64, + ) -> Result<(), InvalidValueForLen> { + if count > 2_u64.pow(count_len as u32 * 8) - 1 { + return Err(InvalidValueForLen { + value: count, + len: count_len, + }); + } + self.vc_frame_count_len = count_len; + self.vc_frame_count = count; + Ok(()) + } + + pub fn vc_frame_count(&self) -> u64 { + self.vc_frame_count + } + + pub fn vc_frame_count_len(&self) -> u8 { + self.vc_frame_count_len + } + pub fn from_bytes(buf: &[u8]) -> Result { if buf.len() < 4 { return Err(ByteConversionError::FromSliceTooSmall { @@ -138,9 +204,66 @@ impl PrimaryHeader { vc_frame_count, }) } + + pub fn write_to_be_bytes(&self, buf: &mut [u8]) -> Result { + if buf.len() < self.len_header() { + return Err(ByteConversionError::ToSliceTooSmall { + found: buf.len(), + expected: self.len_header(), + }); + } + buf[0] = (USLP_VERSION_NUMBER << 4) | ((self.spacecraft_id >> 12) as u8) & 0b1111; + buf[1] = (self.spacecraft_id >> 4) as u8; + buf[2] = (((self.spacecraft_id & 0b1111) as u8) << 4) + | ((self.source_or_dest_field as u8) << 3) + | (self.vc_id >> 3) & 0b111; + buf[3] = ((self.vc_id & 0b111) << 5) | (self.map_id << 1); + buf[4..6].copy_from_slice(&self.frame_len.to_be_bytes()); + buf[6] = ((self.sequence_control_flag as u8) << 7) + | ((self.protocol_control_command_flag as u8) << 6) + | ((self.ocf_flag as u8) << 3) + | self.vc_frame_count_len; + let mut packet_idx = 7; + for idx in (0..self.vc_frame_count_len).rev() { + buf[packet_idx] = ((self.vc_frame_count >> (idx * 8)) & 0xff) as u8; + packet_idx += 1; + } + Ok(self.len_header()) + } + + #[inline(always)] + pub fn len_header(&self) -> usize { + 7 + self.vc_frame_count_len as usize + } + + #[inline(always)] + pub fn len_frame(&self) -> usize { + // 4.1.2.7.2 + // The field contains a length count C that equals one fewer than the total octets + // in the transfer frame. + self.frame_len as usize + 1 + } } -#[derive(Debug, Copy, Clone, PartialEq, Eq, num_enum::TryFromPrimitive, num_enum::IntoPrimitive)] +/// Custom implementation which skips the check whether the VC frame count length field is equal. +/// Only the actual VC count value is compared. +impl PartialEq for PrimaryHeader { + fn eq(&self, other: &Self) -> bool { + self.spacecraft_id == other.spacecraft_id + && self.source_or_dest_field == other.source_or_dest_field + && self.vc_id == other.vc_id + && self.map_id == other.map_id + && self.frame_len == other.frame_len + && self.sequence_control_flag == other.sequence_control_flag + && self.protocol_control_command_flag == other.protocol_control_command_flag + && self.ocf_flag == other.ocf_flag + && self.vc_frame_count == other.vc_frame_count + } +} + +#[derive( + Debug, Copy, Clone, PartialEq, Eq, num_enum::TryFromPrimitive, num_enum::IntoPrimitive, +)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(u8)] @@ -204,6 +327,13 @@ pub struct TransferFrameDataFieldHeader { } impl TransferFrameDataFieldHeader { + pub fn len_header(&self) -> usize { + if self.construction_rule.applicable_to_fixed_len_tfdz() { + 3 + } else { + 1 + } + } pub fn construction_rule(&self) -> ConstructionRule { self.construction_rule @@ -217,45 +347,290 @@ impl TransferFrameDataFieldHeader { self.fhp_or_lvo } - pub fn from_bytes(buf: &[u8]) -> Option { - if buf.len() < 1 { - return None; + pub fn from_bytes(buf: &[u8]) -> Result { + if buf.is_empty() { + return Err(ByteConversionError::FromSliceTooSmall { + found: 0, + expected: 1, + } + .into()); } - let construction_rule = ConstructionRule::try_from((buf[0] >> 5) & 0b111).ok()?; + let construction_rule = ConstructionRule::try_from((buf[0] >> 5) & 0b111) + .map_err(|e| UslpError::InvalidConstructionRule(e.number))?; let mut fhp_or_lvo = None; if construction_rule.applicable_to_fixed_len_tfdz() { if buf.len() < 3 { - return None; + return Err(ByteConversionError::FromSliceTooSmall { + found: buf.len(), + expected: 3, + } + .into()); } fhp_or_lvo = Some(u16::from_be_bytes(buf[1..3].try_into().unwrap())); } - Some(Self { + Ok(Self { construction_rule, - uslp_protocol_id: (buf[0] & 0b11111).try_into().ok()?, + uslp_protocol_id: UslpProtocolId::try_from(buf[0] & 0b11111) + .map_err(|e| UslpError::InvalidProtocolId(e.number))?, fhp_or_lvo, }) } } +/// Simple USLP transfer frame reader. +/// +/// Currently, only insert zone lengths of 0 are supported. pub struct TransferFrameReader<'buf> { - header: PrimaryHeader, + primary_header: PrimaryHeader, data_field_header: TransferFrameDataFieldHeader, data: &'buf [u8], - operational_control_field: u32, + operational_control_field: Option, } impl<'buf> TransferFrameReader<'buf> { - /// This function assumes an insert zone length of 0 - pub fn from_bytes(buf: &[u8], has_fecf: bool) -> Result { + /// This function assumes an insert zone length of 0. + pub fn from_bytes( + buf: &'buf [u8], + has_fecf: bool, + ) -> Result, UslpError> { let primary_header = PrimaryHeader::from_bytes(buf)?; + if primary_header.len_frame() < buf.len() { + return Err(ByteConversionError::FromSliceTooSmall { + found: buf.len(), + expected: primary_header.len_frame(), + } + .into()); + } + let data_field_header = TransferFrameDataFieldHeader::from_bytes(buf)?; + let data_idx = primary_header.len_header() + data_field_header.len_header(); + let frame_len = primary_header.len_frame(); + let mut operational_control_field = None; + let mut data_len = frame_len - data_idx; + if has_fecf { + data_len -= 2; + } + if primary_header.ocf_flag { + data_len -= 4; + operational_control_field = Some(u32::from_be_bytes( + buf[data_idx + data_len..data_idx + data_len + 4] + .try_into() + .unwrap(), + )); + } + let data_end = data_idx + data_len; + let mut digest = CRC_CCITT_FALSE.digest(); + digest.update(&buf[0..frame_len]); + if digest.finalize() != 0 { + return Err(UslpError::ChecksumFailure(u16::from_be_bytes( + buf[frame_len - 2..frame_len].try_into().unwrap(), + ))); + } Ok(Self { - header: primary_header, - data_field_header: todo!(), - data: todo!(), - operational_control_field: todo!(), + primary_header, + data_field_header, + data: buf[data_idx..data_end].try_into().unwrap(), + operational_control_field, }) } + + pub fn primary_header(&self) -> &PrimaryHeader { + &self.primary_header + } + + pub fn data_field_header(&self) -> &TransferFrameDataFieldHeader { + &self.data_field_header + } + + pub fn data(&self) -> &'buf [u8] { + self.data + } + + pub fn operational_control_field(&self) -> &Option { + &self.operational_control_field + } } #[cfg(test)] -mod tests {} +mod tests { + use super::*; + + fn common_basic_check(buf: &[u8]) { + assert_eq!(buf[0] >> 4, USLP_VERSION_NUMBER); + // First four bits SCID. + assert_eq!(buf[0] & 0b1111, 0b1010); + // Next eight bits SCID. + assert_eq!(buf[1], 0b01011100); + // Last four bits SCID. + assert_eq!(buf[2] >> 4, 0b0011); + assert_eq!((buf[2] >> 3) & 0b1, SourceOrDestField::Dest as u8); + // First three bits VCID. + assert_eq!(buf[2] & 0b111, 0b110); + // Last three bits VCID. + assert_eq!(buf[3] >> 5, 0b101); + // MAP ID + assert_eq!((buf[3] >> 1) & 0b1111, 0b1010); + // End of primary header flag + assert_eq!(buf[3] & 0b1, 0); + assert_eq!(u16::from_be_bytes(buf[4..6].try_into().unwrap()), 0x2345); + } + #[test] + fn test_basic_0() { + let mut buf: [u8; 8] = [0; 8]; + // Should be all zeros after writing. + buf[6] = 0xff; + let primary_header = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b1010, + 0x2345, + ) + .unwrap(); + // Virtual channel count 0. + assert_eq!(primary_header.write_to_be_bytes(&mut buf).unwrap(), 7); + common_basic_check(&buf); + // Bypass / Sequence Control Flag. + assert_eq!( + (buf[6] >> 7) & 0b1, + BypassSequenceControlFlag::SequenceControlledQoS as u8 + ); + // Protocol Control Command Flag. + assert_eq!( + (buf[6] >> 6) & 0b1, + ProtocolControlCommandFlag::TfdfContainsUserData as u8 + ); + // OCF flag. + assert_eq!((buf[6] >> 3) & 0b1, false as u8); + // VCF count length. + assert_eq!(buf[6] & 0b111, 0); + } + + #[test] + fn test_basic_1() { + let mut buf: [u8; 16] = [0; 16]; + // Should be all zeros after writing. + buf[6] = 0xff; + let mut primary_header = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b1010, + 0x2345, + ) + .unwrap(); + primary_header.sequence_control_flag = BypassSequenceControlFlag::ExpeditedQoS; + primary_header.protocol_control_command_flag = + ProtocolControlCommandFlag::TfdfContainsProtocolInfo; + primary_header.ocf_flag = true; + primary_header.set_vc_frame_count(4, 0x12345678).unwrap(); + // Virtual channel count 4. + assert_eq!(primary_header.write_to_be_bytes(&mut buf).unwrap(), 11); + common_basic_check(&buf); + // Bypass / Sequence Control Flag. + assert_eq!( + (buf[6] >> 7) & 0b1, + BypassSequenceControlFlag::ExpeditedQoS as u8 + ); + // Protocol Control Command Flag. + assert_eq!( + (buf[6] >> 6) & 0b1, + ProtocolControlCommandFlag::TfdfContainsProtocolInfo as u8 + ); + // OCF flag. + assert_eq!((buf[6] >> 3) & 0b1, true as u8); + // VCF count length. + assert_eq!(buf[6] & 0b111, 4); + assert_eq!( + u32::from_be_bytes(buf[7..11].try_into().unwrap()), + 0x12345678 + ); + } + + #[test] + fn test_reading_0() { + let mut buf: [u8; 8] = [0; 8]; + let primary_header = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b1010, + 0x2345, + ) + .unwrap(); + assert_eq!(primary_header.write_to_be_bytes(&mut buf).unwrap(), 7); + let parsed_header = PrimaryHeader::from_bytes(&buf).unwrap(); + assert_eq!(parsed_header, primary_header); + } + + #[test] + fn test_reading_1() { + let mut buf: [u8; 16] = [0; 16]; + let mut primary_header = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b1010, + 0x2345, + ) + .unwrap(); + primary_header.sequence_control_flag = BypassSequenceControlFlag::ExpeditedQoS; + primary_header.protocol_control_command_flag = + ProtocolControlCommandFlag::TfdfContainsProtocolInfo; + primary_header.ocf_flag = true; + primary_header.set_vc_frame_count(4, 0x12345678).unwrap(); + assert_eq!(primary_header.write_to_be_bytes(&mut buf).unwrap(), 11); + let parsed_header = PrimaryHeader::from_bytes(&buf).unwrap(); + assert_eq!(parsed_header, primary_header); + } + + #[test] + fn test_invalid_vcid() { + let error = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b1101011, + 0b1010, + 0x2345, + ); + assert!(error.is_err()); + let error = error.unwrap_err(); + matches!(error, UslpError::InvalidVcid(0b1101011)); + } + + #[test] + fn test_invalid_mapid() { + let error = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b10101, + 0x2345, + ); + assert!(error.is_err()); + let error = error.unwrap_err(); + matches!(error, UslpError::InvalidMapId(0b10101)); + } + + #[test] + fn test_invalid_vc_count() { + let mut primary_header = PrimaryHeader::new( + 0b10100101_11000011, + SourceOrDestField::Dest, + 0b110101, + 0b1010, + 0x2345, + ) + .unwrap(); + matches!( + primary_header.set_vc_frame_count(0, 1).unwrap_err(), + InvalidValueForLen { value: 1, len: 0 } + ); + matches!( + primary_header.set_vc_frame_count(1, 256).unwrap_err(), + InvalidValueForLen { value: 256, len: 1 } + ); + } + + #[test] + fn test_frame_parsing() {} +}