first basic verif reporter impl

2022-09-03 18:51:01 +02:00 · 2022-09-03 18:51:01 +02:00 · 38bdc2155f
commit 38bdc2155f
parent a34497bf3c
4 changed files with 222 additions and 45 deletions
--- a/fsrc-core/src/pus/mod.rs
+++ b/fsrc-core/src/pus/mod.rs
@ -1,5 +1,6 @@
 use crate::pool::StoreError;
 use spacepackets::time::TimestampError;
+use spacepackets::ByteConversionError;

 pub mod verification;

@ -7,5 +8,6 @@ pub mod verification;
 pub enum SendStoredTmError<E> {
    SendError(E),
    TimeStampError(TimestampError),
+    ToFromBytesError(ByteConversionError),
    StoreError(StoreError),
 }
--- a/fsrc-core/src/pus/verification.rs
+++ b/fsrc-core/src/pus/verification.rs
@ -3,10 +3,11 @@ use alloc::boxed::Box;
 use alloc::vec;
 use alloc::vec::Vec;
 use core::mem::size_of;
+use spacepackets::ecss::EcssEnumeration;
 use spacepackets::tc::PusTc;
-use spacepackets::time::{CcsdsTimeProvider, TimeWriter, TimestampError};
+use spacepackets::time::{CcsdsTimeProvider, TimeWriter};
 use spacepackets::tm::{PusTm, PusTmSecondaryHeader};
-use spacepackets::SpHeader;
+use spacepackets::{ByteConversionError, SizeMissmatch, SpHeader};
 use spacepackets::{CcsdsPacket, PacketId, PacketSequenceCtrl};
 use std::marker::PhantomData;

@ -75,6 +76,45 @@ impl VerificationReporterCfg {
    }
 }

+pub struct FailParams<'a, E> {
+    sender: &'a mut dyn VerificationSender<E>,
+    failure_code: &'a dyn EcssEnumeration,
+    failure_data: &'a [u8],
+}
+
+impl<'a, E> FailParams<'a, E> {
+    pub fn new(
+        sender: &'a mut impl VerificationSender<E>,
+        failure_code: &'a impl EcssEnumeration,
+        failure_data: &'a [u8],
+    ) -> Self {
+        Self {
+            sender,
+            failure_code,
+            failure_data,
+        }
+    }
+}
+
+pub struct FailParamsWithStep<'a, E> {
+    bp: FailParams<'a, E>,
+    step: &'a dyn EcssEnumeration,
+}
+
+impl<'a, E> FailParamsWithStep<'a, E> {
+    pub fn new(
+        sender: &'a mut impl VerificationSender<E>,
+        failure_code: &'a impl EcssEnumeration,
+        failure_data: &'a [u8],
+        step: &'a impl EcssEnumeration,
+    ) -> Self {
+        Self {
+            bp: FailParams::new(sender, failure_code, failure_data),
+            step,
+        }
+    }
+}
+
 impl VerificationReporter {
    pub fn new(cfg: VerificationReporterCfg) -> Self {
        Self {
@ -102,9 +142,8 @@ impl VerificationReporter {
        token: VerificationToken<StateNone>,
        sender: &mut impl VerificationSender<E>,
    ) -> Result<VerificationToken<StateAccepted>, SendStoredTmError<E>> {
-        let tm = self
-            .create_pus_verif_tm(1, 1, &token.req_id)
-            .map_err(|e| SendStoredTmError::TimeStampError(e))?;
+        let test: Option<&dyn EcssEnumeration> = None;
+        let tm = self.create_pus_verif_success_tm(1, 1, &token.req_id, test)?;
        sender.send_verification_tm(tm)?;
        self.msg_count += 1;
        Ok(VerificationToken {
@ -116,59 +155,195 @@ impl VerificationReporter {
    pub fn acceptance_failure<E>(
        mut self,
        token: VerificationToken<StateNone>,
-        sender: &mut impl VerificationSender<E>,
-        _failure_notice: &[u8],
+        params: FailParams<E>,
    ) -> Result<(), SendStoredTmError<E>> {
-        let tm = self
-            .create_pus_verif_tm(1, 2, &token.req_id)
-            .map_err(|e| SendStoredTmError::TimeStampError(e))?;
+        let tm = self.create_pus_verif_fail_tm(
+            1,
+            2,
+            &token.req_id,
+            None::<&dyn EcssEnumeration>,
+            params.failure_code,
+            params.failure_data,
+        )?;
+        params.sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(())
+    }
+
+    pub fn start_success<E>(
+        &mut self,
+        token: VerificationToken<StateAccepted>,
+        sender: &mut impl VerificationSender<E>,
+    ) -> Result<VerificationToken<StateStarted>, SendStoredTmError<E>> {
+        let tm =
+            self.create_pus_verif_success_tm(1, 3, &token.req_id, None::<&dyn EcssEnumeration>)?;
+        sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(VerificationToken {
+            state: PhantomData,
+            req_id: token.req_id,
+        })
+    }
+
+    pub fn start_failure<E>(
+        &mut self,
+        token: VerificationToken<StateAccepted>,
+        params: FailParams<E>,
+    ) -> Result<(), SendStoredTmError<E>> {
+        let tm = self.create_pus_verif_fail_tm(
+            1,
+            4,
+            &token.req_id,
+            None::<&dyn EcssEnumeration>,
+            params.failure_code,
+            params.failure_data,
+        )?;
+        params.sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(())
+    }
+
+    pub fn step_success<E>(
+        &mut self,
+        token: &VerificationToken<StateAccepted>,
+        sender: &mut impl VerificationSender<E>,
+        step: impl EcssEnumeration,
+    ) -> Result<(), SendStoredTmError<E>> {
+        let tm = self.create_pus_verif_success_tm(1, 5, &token.req_id, Some(&step))?;
        sender.send_verification_tm(tm)?;
        self.msg_count += 1;
        Ok(())
    }

-    pub fn start_success(
+    pub fn step_failure<E>(
        &mut self,
        token: VerificationToken<StateAccepted>,
-    ) -> VerificationToken<StateStarted> {
-        VerificationToken {
-            state: PhantomData,
-            req_id: token.req_id,
-        }
+        params: FailParamsWithStep<E>,
+    ) -> Result<(), SendStoredTmError<E>> {
+        let tm = self.create_pus_verif_fail_tm(
+            1,
+            6,
+            &token.req_id,
+            Some(params.step),
+            params.bp.failure_code,
+            params.bp.failure_data,
+        )?;
+        params.bp.sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(())
    }

-    pub fn start_failure(&mut self, _token: VerificationToken<StateAccepted>) {
-        unimplemented!();
+    pub fn completion_success<E>(
+        &mut self,
+        token: VerificationToken<StateAccepted>,
+        sender: &mut impl VerificationSender<E>,
+    ) -> Result<(), SendStoredTmError<E>> {
+        let tm =
+            self.create_pus_verif_success_tm(1, 7, &token.req_id, None::<&dyn EcssEnumeration>)?;
+        sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(())
    }

-    pub fn step_success(&mut self, _token: &VerificationToken<StateAccepted>) {
-        unimplemented!();
+    pub fn completion_failure<E>(
+        &mut self,
+        token: VerificationToken<StateAccepted>,
+        params: FailParams<E>,
+    ) -> Result<(), SendStoredTmError<E>> {
+        let tm = self.create_pus_verif_fail_tm(
+            1,
+            8,
+            &token.req_id,
+            None::<&dyn EcssEnumeration>,
+            params.failure_code,
+            params.failure_data,
+        )?;
+        params.sender.send_verification_tm(tm)?;
+        self.msg_count += 1;
+        Ok(())
    }

-    pub fn step_failure(&mut self, _token: VerificationToken<StateAccepted>) {
-        unimplemented!();
-    }
-
-    pub fn completion_success(&mut self, _token: VerificationToken<StateAccepted>) {
-        unimplemented!();
-    }
-    pub fn completion_failure(&mut self, _token: VerificationToken<StateAccepted>) {
-        unimplemented!();
-    }
-
-    fn create_pus_verif_tm(
+    fn create_pus_verif_success_tm<E>(
        &mut self,
        service: u8,
        subservice: u8,
        req_id: &RequestId,
-    ) -> Result<PusTm, TimestampError> {
-        let source_data_len = size_of::<u32>();
-        req_id.to_bytes(&mut self.source_data_buf[0..source_data_len]);
+        step: Option<&(impl EcssEnumeration + ?Sized)>,
+    ) -> Result<PusTm, SendStoredTmError<E>> {
+        let mut source_data_len = size_of::<u32>();
+        if let Some(step) = step {
+            source_data_len += step.byte_width() as usize;
+        }
+        self.source_buffer_large_enough(source_data_len)?;
+        let mut idx = 0;
+        req_id.to_bytes(&mut self.source_data_buf[0..RequestId::SIZE_AS_BYTES]);
+        idx += RequestId::SIZE_AS_BYTES;
+        if let Some(step) = step {
+            // Size check was done beforehand
+            step.to_bytes(&mut self.source_data_buf[idx..idx + step.byte_width() as usize])
+                .unwrap();
+        }
        let mut sp_header = SpHeader::tm(self.apid, 0, 0).unwrap();
-        // I think it is okay to panic here. This error should never happen, I consider
-        // this a configuration error.
        self.time_stamper
-            .write_to_bytes(self.time_stamp_buf.as_mut_slice())?;
+            .write_to_bytes(self.time_stamp_buf.as_mut_slice())
+            .map_err(|e| SendStoredTmError::TimeStampError(e))?;
+        Ok(self.create_pus_verif_tm_base(service, subservice, &mut sp_header, source_data_len))
+    }
+
+    fn create_pus_verif_fail_tm<E>(
+        &mut self,
+        service: u8,
+        subservice: u8,
+        req_id: &RequestId,
+        step: Option<&(impl EcssEnumeration + ?Sized)>,
+        fail_code: &(impl EcssEnumeration + ?Sized),
+        fail_data: &[u8],
+    ) -> Result<PusTm, SendStoredTmError<E>> {
+        let mut idx = 0;
+        let mut source_data_len =
+            RequestId::SIZE_AS_BYTES + fail_code.byte_width() as usize + fail_data.len();
+        if let Some(step) = step {
+            source_data_len += step.byte_width() as usize;
+        }
+        self.source_buffer_large_enough(source_data_len)?;
+        req_id.to_bytes(&mut self.source_data_buf[0..RequestId::SIZE_AS_BYTES]);
+        idx += RequestId::SIZE_AS_BYTES;
+        if let Some(step) = step {
+            // Size check done beforehand
+            step.to_bytes(&mut self.source_data_buf[idx..idx + step.byte_width() as usize])
+                .unwrap();
+        }
+        fail_code
+            .to_bytes(&mut self.source_data_buf[idx..idx + fail_code.byte_width() as usize])
+            .map_err(|e| SendStoredTmError::<E>::ToFromBytesError(e))?;
+        idx += fail_code.byte_width() as usize;
+        self.source_data_buf[idx..idx + fail_data.len()].copy_from_slice(fail_data);
+        let mut sp_header = SpHeader::tm(self.apid, 0, 0).unwrap();
+        self.time_stamper
+            .write_to_bytes(self.time_stamp_buf.as_mut_slice())
+            .map_err(|e| SendStoredTmError::<E>::TimeStampError(e))?;
+        Ok(self.create_pus_verif_tm_base(service, subservice, &mut sp_header, source_data_len))
+    }
+
+    fn source_buffer_large_enough<E>(&self, len: usize) -> Result<(), SendStoredTmError<E>> {
+        if len > self.source_data_buf.capacity() {
+            return Err(SendStoredTmError::ToFromBytesError(
+                ByteConversionError::ToSliceTooSmall(SizeMissmatch {
+                    found: self.source_data_buf.capacity(),
+                    expected: len,
+                }),
+            ));
+        }
+        Ok(())
+    }
+
+    fn create_pus_verif_tm_base(
+        &mut self,
+        service: u8,
+        subservice: u8,
+        sp_header: &mut SpHeader,
+        source_data_len: usize,
+    ) -> PusTm {
        let tm_sec_header = PusTmSecondaryHeader::new(
            service,
            subservice,
@ -176,12 +351,12 @@ impl VerificationReporter {
            self.dest_id,
            &self.time_stamp_buf,
        );
-        Ok(PusTm::new(
-            &mut sp_header,
+        PusTm::new(
+            sp_header,
            tm_sec_header,
            Some(&self.source_data_buf[0..source_data_len]),
            true,
-        ))
+        )
    }
 }

--- a/fsrc-core/src/tmtc/ccsds_distrib.rs
+++ b/fsrc-core/src/tmtc/ccsds_distrib.rs
@ -87,7 +87,7 @@
 use crate::tmtc::{ReceivesCcsdsTc, ReceivesTc};
 use alloc::boxed::Box;
 use downcast_rs::Downcast;
-use spacepackets::{CcsdsPacket, PacketError, SizeMissmatch, SpHeader};
+use spacepackets::{ByteConversionError, CcsdsPacket, SizeMissmatch, SpHeader};

 /// Generic trait for a handler or dispatcher object handling CCSDS packets.
 ///
@ -125,7 +125,7 @@ pub struct CcsdsDistributor<E> {
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub enum CcsdsError<E> {
    CustomError(E),
-    PacketError(PacketError),
+    PacketError(ByteConversionError),
 }

 impl<E: 'static> ReceivesCcsdsTc for CcsdsDistributor<E> {
@ -142,7 +142,7 @@ impl<E: 'static> ReceivesTc for CcsdsDistributor<E> {
    fn pass_tc(&mut self, tc_raw: &[u8]) -> Result<(), Self::Error> {
        if tc_raw.len() < 7 {
            return Err(CcsdsError::PacketError(
-                PacketError::FromBytesSliceTooSmall(SizeMissmatch {
+                ByteConversionError::FromSliceTooSmall(SizeMissmatch {
                    found: tc_raw.len(),
                    expected: 7,
                }),
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 96d389a6515f763f179c23742a9f578659ae9509
+Subproject commit bb83e67e54619ad160bd3a02eb55993ec39a70df