continue USLP support

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 {

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,
+    TfdfContainsUserData = 0,
-    TfdfContainsProtocolInfo,
+    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<ByteConversionError> for UslpError {
@@ -70,7 +61,82 @@ impl From<ByteConversionError> 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<Self, UslpError> {
+        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<Self, UslpError> {
         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<usize, ByteConversionError> {
+        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<Self> {
+    pub fn from_bytes(buf: &[u8]) -> Result<Self, UslpError> {
-        if buf.len() < 1 {
+        if buf.is_empty() {
-            return None;
+            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<u32>,
 }
 
 impl<'buf> TransferFrameReader<'buf> {
-    /// This function assumes an insert zone length of 0
+    /// This function assumes an insert zone length of 0.
-    pub fn from_bytes(buf: &[u8], has_fecf: bool) -> Result<Self, UslpError> {
+    pub fn from_bytes(
+        buf: &'buf [u8],
+        has_fecf: bool,
+    ) -> Result<TransferFrameReader<'buf>, 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,
+            primary_header,
-            data_field_header: todo!(),
+            data_field_header,
-            data: todo!(),
+            data: buf[data_idx..data_end].try_into().unwrap(),
-            operational_control_field: todo!(),
+            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<u32> {
+        &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() {}
+}