fsrc-core/src/pus/mod.rs

@@ -1,7 +1,9 @@
 use crate::pool::StoreError;
+use spacepackets::ecss::PusError;
 use spacepackets::time::TimestampError;
 use spacepackets::ByteConversionError;
 
+#[cfg(feature = "alloc")]
 pub mod verification;
 
 #[derive(Debug, Clone)]
@@ -9,5 +11,6 @@ pub enum SendStoredTmError<E> {
     SendError(E),
     TimeStampError(TimestampError),
     ToFromBytesError(ByteConversionError),
+    PusError(PusError),
     StoreError(StoreError),
 }

fsrc-core/src/pus/verification.rs

@@ -1,16 +1,24 @@
+use crate::pool::{LocalPool, StoreAddr};
 use crate::pus::SendStoredTmError;
-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};
+use spacepackets::time::CcsdsTimeProvider;
 use spacepackets::tm::{PusTm, PusTmSecondaryHeader};
 use spacepackets::{ByteConversionError, SizeMissmatch, SpHeader};
 use spacepackets::{CcsdsPacket, PacketId, PacketSequenceCtrl};
 use std::marker::PhantomData;
 
+use alloc::sync::Arc;
+#[cfg(feature = "std")]
+use std::sync::{mpsc, Mutex};
+
+#[cfg(feature = "std")]
+use std::sync::mpsc::SendError;
+use std::sync::MutexGuard;
+
 #[derive(Debug, Eq, PartialEq, Copy, Clone)]
 pub struct RequestId {
     version_number: u8,
@@ -22,11 +30,23 @@ impl RequestId {
     const SIZE_AS_BYTES: usize = size_of::<u32>();
 
     pub fn to_bytes(&self, buf: &mut [u8]) {
-        let raw = ((self.version_number as u32) << 31)
+        let raw = ((self.version_number as u32) << 29)
             | ((self.packet_id.raw() as u32) << 16)
             | self.psc.raw() as u32;
         buf.copy_from_slice(raw.to_be_bytes().as_slice());
     }
+
+    pub fn from_bytes(buf: &[u8]) -> Option<Self> {
+        if buf.len() < 4 {
+            return None;
+        }
+        let raw = u32::from_be_bytes(buf[0..Self::SIZE_AS_BYTES].try_into().unwrap());
+        Some(Self {
+            version_number: ((raw >> 29) & 0b111) as u8,
+            packet_id: PacketId::from(((raw >> 16) & 0xffff) as u16),
+            psc: PacketSequenceCtrl::from((raw & 0xffff) as u16),
+        })
+    }
 }
 impl RequestId {
     pub fn new(tc: &PusTc) -> Self {
@@ -42,19 +62,73 @@ pub trait VerificationSender<E> {
     fn send_verification_tm(&mut self, tm: PusTm) -> Result<(), SendStoredTmError<E>>;
 }
 
+#[cfg(feature = "std")]
+pub struct StdVerifSender {
+    pub ignore_poison_error: bool,
+    tm_store: Arc<Mutex<LocalPool>>,
+    tx: mpsc::Sender<StoreAddr>,
+}
+
+#[cfg(feature = "std")]
+impl StdVerifSender {
+    pub fn new(tm_store: Arc<Mutex<LocalPool>>, tx: mpsc::Sender<StoreAddr>) -> Self {
+        Self {
+            ignore_poison_error: true,
+            tx,
+            tm_store,
+        }
+    }
+}
+
+#[cfg(feature = "std")]
+#[derive(Debug, Eq, PartialEq)]
+pub enum StdVerifSenderError {
+    PoisonError,
+    SendError(SendError<StoreAddr>),
+}
+
+#[cfg(feature = "std")]
+impl VerificationSender<StdVerifSenderError> for StdVerifSender {
+    fn send_verification_tm(
+        &mut self,
+        tm: PusTm,
+    ) -> Result<(), SendStoredTmError<StdVerifSenderError>> {
+        let operation = |mut mg: MutexGuard<LocalPool>| {
+            let (addr, mut buf) = mg
+                .free_element(tm.len_packed())
+                .map_err(|e| SendStoredTmError::StoreError(e))?;
+            tm.write_to(&mut buf)
+                .map_err(|e| SendStoredTmError::PusError(e))?;
+            self.tx
+                .send(addr)
+                .map_err(|e| SendStoredTmError::SendError(StdVerifSenderError::SendError(e)))?;
+            Ok(())
+        };
+        match self.tm_store.lock() {
+            Ok(lock) => operation(lock),
+            Err(poison_error) => {
+                if self.ignore_poison_error {
+                    operation(poison_error.into_inner())
+                } else {
+                    return Err(SendStoredTmError::SendError(
+                        StdVerifSenderError::PoisonError,
+                    ));
+                }
+            }
+        }
+    }
+}
+
 pub struct VerificationReporter {
     pub apid: u16,
     pub dest_id: u16,
     msg_count: u16,
-    time_stamper: Box<dyn TimeWriter>,
-    time_stamp_buf: Vec<u8>,
     source_data_buf: Vec<u8>,
 }
 
 pub struct VerificationReporterCfg {
     pub apid: u16,
     pub dest_id: u16,
-    pub time_stamper: Box<dyn TimeWriter>,
     pub step_field_width: u8,
     pub failure_code_field_width: u8,
     pub max_fail_data_len: usize,
@@ -62,12 +136,11 @@ pub struct VerificationReporterCfg {
 }
 
 impl VerificationReporterCfg {
-    pub fn new(time_stamper: impl TimeWriter + CcsdsTimeProvider + 'static, apid: u16) -> Self {
+    pub fn new(time_stamper: impl CcsdsTimeProvider, apid: u16) -> Self {
         let max_stamp_len = time_stamper.len_as_bytes();
         Self {
             apid,
             dest_id: 0,
-            time_stamper: Box::new(time_stamper),
             step_field_width: size_of::<u8>() as u8,
             failure_code_field_width: size_of::<u16>() as u8,
             max_fail_data_len: 2 * size_of::<u32>(),
@@ -77,6 +150,7 @@ impl VerificationReporterCfg {
 }
 
 pub struct FailParams<'a, E> {
+    time_stamp: &'a [u8],
     sender: &'a mut dyn VerificationSender<E>,
     failure_code: &'a dyn EcssEnumeration,
     failure_data: &'a [u8],
@@ -85,11 +159,13 @@ pub struct FailParams<'a, E> {
 impl<'a, E> FailParams<'a, E> {
     pub fn new(
         sender: &'a mut impl VerificationSender<E>,
+        time_stamp: &'a [u8],
         failure_code: &'a impl EcssEnumeration,
         failure_data: &'a [u8],
     ) -> Self {
         Self {
             sender,
+            time_stamp,
             failure_code,
             failure_data,
         }
@@ -104,12 +180,13 @@ pub struct FailParamsWithStep<'a, E> {
 impl<'a, E> FailParamsWithStep<'a, E> {
     pub fn new(
         sender: &'a mut impl VerificationSender<E>,
+        time_stamp: &'a [u8],
         failure_code: &'a impl EcssEnumeration,
         failure_data: &'a [u8],
         step: &'a impl EcssEnumeration,
     ) -> Self {
         Self {
-            bp: FailParams::new(sender, failure_code, failure_data),
+            bp: FailParams::new(sender, time_stamp, failure_code, failure_data),
             step,
         }
     }
@@ -121,8 +198,6 @@ impl VerificationReporter {
             apid: cfg.apid,
             dest_id: cfg.dest_id,
             msg_count: 0,
-            time_stamper: cfg.time_stamper,
-            time_stamp_buf: vec![0; cfg.max_stamp_len],
             source_data_buf: vec![
                 0;
                 RequestId::SIZE_AS_BYTES
@@ -133,7 +208,11 @@ impl VerificationReporter {
         }
     }
 
-    pub fn add_tc(&mut self, req_id: RequestId) -> VerificationToken<StateNone> {
+    pub fn add_tc(&mut self, pus_tc: &PusTc) -> VerificationToken<StateNone> {
+        self.add_tc_with_req_id(RequestId::new(pus_tc))
+    }
+
+    pub fn add_tc_with_req_id(&mut self, req_id: RequestId) -> VerificationToken<StateNone> {
         VerificationToken::<StateNone>::new(req_id)
     }
 
@@ -141,9 +220,15 @@ impl VerificationReporter {
         &mut self,
         token: VerificationToken<StateNone>,
         sender: &mut impl VerificationSender<E>,
+        time_stamp: &[u8],
     ) -> Result<VerificationToken<StateAccepted>, SendStoredTmError<E>> {
-        let test: Option<&dyn EcssEnumeration> = None;
-        let tm = self.create_pus_verif_success_tm(1, 1, &token.req_id, test)?;
+        let tm = self.create_pus_verif_success_tm(
+            1,
+            1,
+            &token.req_id,
+            time_stamp,
+            None::<&dyn EcssEnumeration>,
+        )?;
         sender.send_verification_tm(tm)?;
         self.msg_count += 1;
         Ok(VerificationToken {
@@ -160,6 +245,7 @@ impl VerificationReporter {
         let tm = self.create_pus_verif_fail_tm(
             1,
             2,
+            params.time_stamp,
             &token.req_id,
             None::<&dyn EcssEnumeration>,
             params.failure_code,
@@ -173,10 +259,16 @@ impl VerificationReporter {
     pub fn start_success<E>(
         &mut self,
         token: VerificationToken<StateAccepted>,
+        time_stamp: &[u8],
         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>)?;
+        let tm = self.create_pus_verif_success_tm(
+            1,
+            3,
+            &token.req_id,
+            time_stamp,
+            None::<&dyn EcssEnumeration>,
+        )?;
         sender.send_verification_tm(tm)?;
         self.msg_count += 1;
         Ok(VerificationToken {
@@ -193,6 +285,7 @@ impl VerificationReporter {
         let tm = self.create_pus_verif_fail_tm(
             1,
             4,
+            params.time_stamp,
             &token.req_id,
             None::<&dyn EcssEnumeration>,
             params.failure_code,
@@ -207,9 +300,10 @@ impl VerificationReporter {
         &mut self,
         token: &VerificationToken<StateAccepted>,
         sender: &mut impl VerificationSender<E>,
+        time_stamp: &[u8],
         step: impl EcssEnumeration,
     ) -> Result<(), SendStoredTmError<E>> {
-        let tm = self.create_pus_verif_success_tm(1, 5, &token.req_id, Some(&step))?;
+        let tm = self.create_pus_verif_success_tm(1, 5, &token.req_id, time_stamp, Some(&step))?;
         sender.send_verification_tm(tm)?;
         self.msg_count += 1;
         Ok(())
@@ -223,6 +317,7 @@ impl VerificationReporter {
         let tm = self.create_pus_verif_fail_tm(
             1,
             6,
+            params.bp.time_stamp,
             &token.req_id,
             Some(params.step),
             params.bp.failure_code,
@@ -237,9 +332,15 @@ impl VerificationReporter {
         &mut self,
         token: VerificationToken<StateAccepted>,
         sender: &mut impl VerificationSender<E>,
+        time_stamp: &[u8],
     ) -> Result<(), SendStoredTmError<E>> {
-        let tm =
-            self.create_pus_verif_success_tm(1, 7, &token.req_id, None::<&dyn EcssEnumeration>)?;
+        let tm = self.create_pus_verif_success_tm(
+            1,
+            7,
+            &token.req_id,
+            time_stamp,
+            None::<&dyn EcssEnumeration>,
+        )?;
         sender.send_verification_tm(tm)?;
         self.msg_count += 1;
         Ok(())
@@ -253,6 +354,7 @@ impl VerificationReporter {
         let tm = self.create_pus_verif_fail_tm(
             1,
             8,
+            params.time_stamp,
             &token.req_id,
             None::<&dyn EcssEnumeration>,
             params.failure_code,
@@ -263,11 +365,12 @@ impl VerificationReporter {
         Ok(())
     }
 
-    fn create_pus_verif_success_tm<E>(
-        &mut self,
+    fn create_pus_verif_success_tm<'a, E>(
+        &'a mut self,
         service: u8,
         subservice: u8,
         req_id: &RequestId,
+        time_stamp: &'a [u8],
         step: Option<&(impl EcssEnumeration + ?Sized)>,
     ) -> Result<PusTm, SendStoredTmError<E>> {
         let mut source_data_len = size_of::<u32>();
@@ -284,16 +387,20 @@ impl VerificationReporter {
                 .unwrap();
         }
         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::TimeStampError(e))?;
-        Ok(self.create_pus_verif_tm_base(service, subservice, &mut sp_header, source_data_len))
+        Ok(self.create_pus_verif_tm_base(
+            service,
+            subservice,
+            &mut sp_header,
+            time_stamp,
+            source_data_len,
+        ))
     }
 
-    fn create_pus_verif_fail_tm<E>(
-        &mut self,
+    fn create_pus_verif_fail_tm<'a, E>(
+        &'a mut self,
         service: u8,
         subservice: u8,
+        time_stamp: &'a [u8],
         req_id: &RequestId,
         step: Option<&(impl EcssEnumeration + ?Sized)>,
         fail_code: &(impl EcssEnumeration + ?Sized),
@@ -319,10 +426,13 @@ impl VerificationReporter {
         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))
+        Ok(self.create_pus_verif_tm_base(
+            service,
+            subservice,
+            &mut sp_header,
+            time_stamp,
+            source_data_len,
+        ))
     }
 
     fn source_buffer_large_enough<E>(&self, len: usize) -> Result<(), SendStoredTmError<E>> {
@@ -337,11 +447,12 @@ impl VerificationReporter {
         Ok(())
     }
 
-    fn create_pus_verif_tm_base(
-        &mut self,
+    fn create_pus_verif_tm_base<'a>(
+        &'a mut self,
         service: u8,
         subservice: u8,
         sp_header: &mut SpHeader,
+        time_stamp: &'a [u8],
         source_data_len: usize,
     ) -> PusTm {
         let tm_sec_header = PusTmSecondaryHeader::new(
@@ -349,7 +460,7 @@ impl VerificationReporter {
             subservice,
             self.msg_count,
             self.dest_id,
-            &self.time_stamp_buf,
+            time_stamp,
         );
         PusTm::new(
             sp_header,
@@ -380,12 +491,86 @@ impl<STATE> VerificationToken<STATE> {
 
 #[cfg(test)]
 mod tests {
+    use crate::pus::verification::{
+        RequestId, VerificationReporter, VerificationReporterCfg, VerificationSender,
+    };
+    use crate::pus::SendStoredTmError;
+    use alloc::vec::Vec;
+    use spacepackets::ecss::PusPacket;
+    use spacepackets::tc::{PusTc, PusTcSecondaryHeader};
+    use spacepackets::time::{CdsShortTimeProvider, TimeWriter};
+    use spacepackets::tm::{PusTm, PusTmSecondaryHeaderT};
+    use spacepackets::{CcsdsPacket, SpHeader};
+    use std::collections::VecDeque;
 
+    const TEST_APID: u16 = 0x02;
+
+    struct TmInfo {
+        pub subservice: u8,
+        pub apid: u16,
+        pub msg_counter: u16,
+        pub dest_id: u16,
+        pub time_stamp: [u8; 7],
+        pub req_id: RequestId,
+        pub additional_data: Option<Vec<u8>>,
+    }
+
+    #[derive(Default)]
+    struct TestSender {
+        pub service_queue: VecDeque<TmInfo>,
+    }
+
+    impl VerificationSender<()> for TestSender {
+        fn send_verification_tm(&mut self, tm: PusTm) -> Result<(), SendStoredTmError<()>> {
+            assert_eq!(PusPacket::service(&tm), 1);
+            assert!(tm.source_data().is_some());
+            let mut time_stamp = [0; 7];
+            time_stamp.clone_from_slice(tm.time_stamp());
+            let src_data = tm.source_data().unwrap();
+            assert!(src_data.len() >= 4);
+            let req_id = RequestId::from_bytes(&src_data[0..RequestId::SIZE_AS_BYTES]).unwrap();
+            let mut vec = None;
+            if src_data.len() > 4 {
+                let mut new_vec = Vec::new();
+                new_vec.extend_from_slice(&src_data[RequestId::SIZE_AS_BYTES..]);
+                vec = Some(new_vec);
+            }
+            self.service_queue.push_back(TmInfo {
+                subservice: PusPacket::subservice(&tm),
+                apid: tm.apid(),
+                msg_counter: tm.msg_counter(),
+                dest_id: tm.dest_id(),
+                time_stamp,
+                req_id,
+                additional_data: vec,
+            });
+            Ok(())
+        }
+    }
     #[test]
-    pub fn test_basic_type_state() {
-        //let mut reporter = VerificationToken::new();
-        //let mut accepted = reporter.acceptance_success();
-        //let started = accepted.start_success();
-        //started.completion_success();
+    pub fn test_basic() {
+        let time_stamper = CdsShortTimeProvider::default();
+        let cfg = VerificationReporterCfg::new(time_stamper, 0x02);
+        let mut reporter = VerificationReporter::new(cfg);
+        let mut sph = SpHeader::tc(TEST_APID, 0x34, 0).unwrap();
+        let tc_header = PusTcSecondaryHeader::new_simple(17, 1);
+        let pus_tc = PusTc::new(&mut sph, tc_header, None, true);
+        let verif_token = reporter.add_tc(&pus_tc);
+        let req_id = RequestId::new(&pus_tc);
+        let mut stamp_buf = [0; 7];
+        time_stamper.write_to_bytes(&mut stamp_buf).unwrap();
+        let mut sender = TestSender::default();
+        reporter
+            .acceptance_success(verif_token, &mut sender, &stamp_buf)
+            .expect("Sending acceptance success failed");
+        assert_eq!(sender.service_queue.len(), 1);
+        let info = sender.service_queue.pop_front().unwrap();
+        assert_eq!(info.subservice, 1);
+        assert_eq!(info.time_stamp, [0; 7]);
+        assert_eq!(info.dest_id, 0);
+        assert_eq!(info.apid, TEST_APID);
+        assert_eq!(info.msg_counter, 0);
+        assert_eq!(info.additional_data, None);
+        assert_eq!(info.req_id, req_id);
     }
 }

spacepackets

@@ -1 +1 @@
-Subproject commit bb83e67e54619ad160bd3a02eb55993ec39a70df
+Subproject commit c72c5ad4aaca277754e94a16377a7d4a6eff5d8e