satrs-core/src/tmtc/tm_helper.rs

@@ -64,7 +64,7 @@ impl PusTmWithCdsShortHelper {
         self.create_pus_tm_common(service, subservice, source_data, seq_count)
     }
 
-    pub fn create_pus_tm_with_stamp<'a>(
+    pub fn create_pus_tm_with_stamper<'a>(
         &'a mut self,
         service: u8,
         subservice: u8,
@@ -76,6 +76,17 @@ impl PusTmWithCdsShortHelper {
         self.create_pus_tm_common(service, subservice, source_data, seq_count)
     }
 
+    pub fn create_pus_tm_with_stamp<'a>(
+        &'a mut self,
+        service: u8,
+        subservice: u8,
+        source_data: Option<&'a [u8]>,
+        timestamp: &'a [u8],
+        seq_count: u16,
+    ) -> PusTm {
+        self.create_pus_tm_common(service, subservice, source_data, seq_count)
+    }
+
     fn create_pus_tm_common<'a>(
         &'a self,
         service: u8,

satrs-example/src/main.rs

@@ -170,9 +170,9 @@ fn main() {
     let mut pus17_handler = PusService17TestHandler::new(
         pus_test_rx,
         tc_store.pool.clone(),
-        PusTmWithCdsShortHelper::new(PUS_APID),
         tm_funnel_tx.clone(),
         tm_store.clone(),
+        PUS_APID,
         verif_reporter.clone(),
     );
     let mut srv_17_wrapper = Service17CustomWrapper {
@@ -345,7 +345,7 @@ fn main() {
         .spawn(move || loop {
             let queue_empty = srv_17_wrapper.perform_operation();
             if queue_empty {
-                thread::sleep(Duration::from_millis(400));
+                thread::sleep(Duration::from_millis(200));
             }
         })
         .unwrap();

satrs-example/src/pus/mod.rs

@@ -11,7 +11,7 @@ use satrs_core::pus::hk;
 use satrs_core::pus::mode::Subservice;
 use satrs_core::pus::scheduling::PusScheduler;
 use satrs_core::pus::verification::{
-    pus_11_generic_tc_check, FailParams, StdVerifReporterWithSender, TcStateAccepted,
+    pus_11_generic_tc_check, FailParams, StdVerifReporterWithSender, TcStateAccepted, TcStateToken,
     VerificationToken,
 };
 use satrs_core::pus::{event, EcssTcSenderCore, GenericTcCheckError, MpscTmtcInStoreSender};
@@ -19,7 +19,7 @@ use satrs_core::pus::{mode, EcssTcSender};
 use satrs_core::res_code::ResultU16;
 use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
 use satrs_core::spacepackets::ecss::{scheduling, PusError, PusServiceId};
-use satrs_core::spacepackets::time::{CcsdsTimeProvider, StdTimestampError};
+use satrs_core::spacepackets::time::{CcsdsTimeProvider, StdTimestampError, TimestampError};
 use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
 use satrs_core::tmtc::{AddressableId, PusServiceProvider, TargetId};
 use satrs_core::{
@@ -31,7 +31,7 @@ use std::cell::RefCell;
 use std::collections::HashMap;
 use std::convert::TryFrom;
 use std::rc::Rc;
-use std::sync::mpsc::{Receiver, SendError, Sender};
+use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
 
 pub mod scheduler;
 pub mod test;
@@ -40,10 +40,9 @@ pub mod test;
 pub enum PusPacketHandlingError {
     PusError(PusError),
     WrongService(u8),
+    NotEnoughAppData(String),
     StoreError(StoreError),
     RwGuardError(String),
-    TimeError(StdTimestampError),
-    TmSendError(String),
     QueueDisconnected,
     OtherError(String),
 }
@@ -54,24 +53,44 @@ impl From<PusError> for PusPacketHandlingError {
     }
 }
 
-impl From<StdTimestampError> for PusPacketHandlingError {
+impl From<StoreError> for PusPacketHandlingError {
+    fn from(value: StoreError) -> Self {
+        Self::StoreError(value)
+    }
+}
+
+#[derive(Debug, Clone)]
+pub enum PartialPusHandlingError {
+    TimeError(StdTimestampError),
+    TmSendError(String),
+    VerificationError,
+}
+impl From<StdTimestampError> for PartialPusHandlingError {
     fn from(value: StdTimestampError) -> Self {
         Self::TimeError(value)
     }
 }
 
-#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+impl From<TimestampError> for PartialPusHandlingError {
+    fn from(value: TimestampError) -> Self {
+        Self::TimeError(StdTimestampError::TimestampError(value))
+    }
+}
+
+#[derive(Debug, Clone)]
 pub enum PusPacketHandlerResult {
     RequestHandled,
+    RequestHandledPartialSuccess(PartialPusHandlingError),
     CustomSubservice(VerificationToken<TcStateAccepted>),
     Empty,
 }
+
 pub struct PusServiceBase {
     tc_rx: Receiver<AcceptedTc>,
     tc_store: SharedPool,
-    tm_helper: PusTmWithCdsShortHelper,
     tm_tx: Sender<StoreAddr>,
     tm_store: SharedTmStore,
+    tm_apid: u16,
     verification_handler: StdVerifReporterWithSender,
     stamp_buf: [u8; 7],
     pus_buf: [u8; 2048],
@@ -82,15 +101,15 @@ impl PusServiceBase {
     pub fn new(
         receiver: Receiver<AcceptedTc>,
         tc_pool: SharedPool,
-        tm_helper: PusTmWithCdsShortHelper,
         tm_tx: Sender<StoreAddr>,
         tm_store: SharedTmStore,
+        tm_apid: u16,
         verification_handler: StdVerifReporterWithSender,
     ) -> Self {
         Self {
             tc_rx: receiver,
             tc_store: tc_pool,
-            tm_helper,
+            tm_apid,
             tm_tx,
             tm_store,
             verification_handler,
@@ -99,6 +118,40 @@ impl PusServiceBase {
             pus_size: 0,
         }
     }
+
+    pub fn handle_next_packet<
+        T: FnOnce(
+            StoreAddr,
+            VerificationToken<TcStateAccepted>,
+        ) -> Result<PusPacketHandlerResult, PusPacketHandlingError>,
+    >(
+        &mut self,
+        handle_one_packet: T,
+    ) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
+        return match self.tc_rx.try_recv() {
+            Ok((addr, token)) => handle_one_packet(addr, token),
+            Err(e) => match e {
+                TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
+                TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
+            },
+        };
+    }
+
+    pub fn update_stamp(&mut self) -> Result<(), PartialPusHandlingError> {
+        let time_provider = TimeProvider::from_now_with_u16_days()
+            .map_err(|e| PartialPusHandlingError::TimeError(e));
+        return if time_provider.is_ok() {
+            // Can not fail, buffer is large enough.
+            time_provider
+                .unwrap()
+                .write_to_bytes(&mut self.stamp_buf)
+                .unwrap();
+            Ok(())
+        } else {
+            self.stamp_buf = [0; 7];
+            Err(time_provider.unwrap_err())
+        };
+    }
 }
 
 // pub trait PusTcRouter {

satrs-example/src/pus/scheduler.rs

@@ -1,4 +1,7 @@
-use crate::pus::{AcceptedTc, PusPacketHandlerResult, PusPacketHandlingError, PusServiceBase};
+use crate::pus::{
+    AcceptedTc, PartialPusHandlingError, PusPacketHandlerResult, PusPacketHandlingError,
+    PusServiceBase,
+};
 use delegate::delegate;
 use satrs_core::pool::{SharedPool, StoreAddr};
 use satrs_core::pus::scheduling::PusScheduler;
@@ -7,9 +10,10 @@ use satrs_core::pus::verification::{
     VerificationToken,
 };
 use satrs_core::pus::GenericTcCheckError;
-use satrs_core::spacepackets::ecss::scheduling;
+use satrs_core::spacepackets::ecss::{scheduling, PusPacket};
 use satrs_core::spacepackets::tc::PusTc;
 use satrs_core::spacepackets::time::cds::TimeProvider;
+use satrs_core::spacepackets::time::TimeWriter;
 use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
 use satrs_example::tmtc_err;
 use std::sync::mpsc::{Receiver, Sender, TryRecvError};
@@ -23,9 +27,9 @@ impl PusService11SchedHandler {
     pub fn new(
         receiver: Receiver<AcceptedTc>,
         tc_pool: SharedPool,
-        tm_helper: PusTmWithCdsShortHelper,
         tm_tx: Sender<StoreAddr>,
         tm_store: SharedTmStore,
+        tm_apid: u16,
         verification_handler: StdVerifReporterWithSender,
         scheduler: PusScheduler,
     ) -> Self {
@@ -33,9 +37,9 @@ impl PusService11SchedHandler {
             psb: PusServiceBase::new(
                 receiver,
                 tc_pool,
-                tm_helper,
                 tm_tx,
                 tm_store,
+                tm_apid,
                 verification_handler,
             ),
             scheduler,
@@ -44,68 +48,36 @@ impl PusService11SchedHandler {
 
     pub fn handle_next_packet(&mut self) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
         return match self.psb.tc_rx.try_recv() {
-            Ok((addr, token)) => {
+            Ok((addr, token)) => self.handle_one_tc(addr, token),
-                if self.handle_one_tc(addr, token)? {
-                    return Ok(PusPacketHandlerResult::RequestHandled);
-                }
-                Ok(PusPacketHandlerResult::CustomSubservice(token))
-            }
             Err(e) => match e {
                 TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
                 TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
             },
         };
     }
-
     pub fn handle_one_tc(
         &mut self,
         addr: StoreAddr,
         token: VerificationToken<TcStateAccepted>,
-    ) -> Result<bool, PusPacketHandlingError> {
+    ) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
-        let time_provider = TimeProvider::from_now_with_u16_days().unwrap();
+        let mut partial_result = self.psb.update_stamp().err();
-        // TODO: Better error handling
         {
             // Keep locked section as short as possible.
-            let mut tc_pool = self.psb.tc_store.write().unwrap();
+            let mut tc_pool = self
+                .psb
+                .tc_store
+                .write()
+                .map_err(|e| PusPacketHandlingError::RwGuardError(format!("{e}")))?;
             let tc_guard = tc_pool.read_with_guard(addr);
             let tc_raw = tc_guard.read().unwrap();
             self.psb.pus_buf[0..tc_raw.len()].copy_from_slice(tc_raw);
         }
         let (tc, tc_size) = PusTc::from_bytes(&self.psb.pus_buf).unwrap();
-        let subservice = match pus_11_generic_tc_check(&tc) {
+        let std_service = scheduling::Subservice::try_from(tc.subservice());
-            Ok(subservice) => subservice,
+        if std_service.is_err() {
-            Err(e) => match e {
+            return Ok(PusPacketHandlerResult::CustomSubservice(token));
-                GenericTcCheckError::NotEnoughAppData => {
+        }
-                    self.psb
+        match std_service.unwrap() {
-                        .verification_handler
-                        .start_failure(
-                            token,
-                            FailParams::new(
-                                Some(&self.psb.stamp_buf),
-                                &tmtc_err::NOT_ENOUGH_APP_DATA,
-                                None,
-                            ),
-                        )
-                        .expect("could not sent verification error");
-                    return;
-                }
-                GenericTcCheckError::InvalidSubservice => {
-                    self.psb
-                        .verification_handler
-                        .start_failure(
-                            token,
-                            FailParams::new(
-                                Some(&self.psb.stamp_buf),
-                                &tmtc_err::INVALID_PUS_SUBSERVICE,
-                                None,
-                            ),
-                        )
-                        .expect("could not sent verification error");
-                    return;
-                }
-            },
-        };
-        match subservice {
             scheduling::Subservice::TcEnableScheduling => {
                 let start_token = self
                     .psb
@@ -175,7 +147,10 @@ impl PusService11SchedHandler {
                     .completion_success(start_token, Some(&self.psb.stamp_buf))
                     .expect("sending completion success failed");
             }
-            _ => {}
+            _ => {
+                return Ok(PusPacketHandlerResult::CustomSubservice(token));
+            }
         }
+        Ok(PusPacketHandlerResult::CustomSubservice(token))
     }
 }

satrs-example/src/pus/test.rs

@@ -1,4 +1,7 @@
-use crate::pus::{AcceptedTc, PusPacketHandlerResult, PusPacketHandlingError, PusServiceBase};
+use crate::pus::{
+    AcceptedTc, PartialPusHandlingError, PusPacketHandlerResult, PusPacketHandlingError,
+    PusServiceBase,
+};
 use delegate::delegate;
 use log::{error, info, warn};
 use satrs_core::events::EventU32;
@@ -13,7 +16,8 @@ use satrs_core::spacepackets::ecss::{PusError, PusPacket};
 use satrs_core::spacepackets::tc::PusTc;
 use satrs_core::spacepackets::time::cds::TimeProvider;
 use satrs_core::spacepackets::time::{StdTimestampError, TimeWriter};
-use satrs_core::spacepackets::tm::PusTm;
+use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
+use satrs_core::spacepackets::SpHeader;
 use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
 use satrs_example::{tmtc_err, TEST_EVENT};
 use std::sync::mpsc::{Receiver, Sender, TryRecvError};
@@ -39,6 +43,13 @@ impl Service17CustomWrapper {
                 info!("Sent ping reply PUS TM[17,2]");
                 handled_pings += 1;
             }
+            PusPacketHandlerResult::RequestHandledPartialSuccess(partial_err) => {
+                warn!(
+                    "Handled PUS ping command with partial success: {:?}",
+                    partial_err
+                );
+                handled_pings += 1;
+            }
             PusPacketHandlerResult::CustomSubservice(token) => {
                 let (buf, _) = self.pus17_handler.pus_tc_buf();
                 let (tc, size) = PusTc::from_bytes(buf).unwrap();
@@ -90,18 +101,18 @@ impl PusService17TestHandler {
     pub fn new(
         receiver: Receiver<AcceptedTc>,
         tc_pool: SharedPool,
-        tm_helper: PusTmWithCdsShortHelper,
         tm_tx: Sender<StoreAddr>,
         tm_store: SharedTmStore,
+        tm_apid: u16,
         verification_handler: StdVerifReporterWithSender,
     ) -> Self {
         Self {
             psb: PusServiceBase::new(
                 receiver,
                 tc_pool,
-                tm_helper,
                 tm_tx,
                 tm_store,
+                tm_apid,
                 verification_handler,
             ),
         }
@@ -117,12 +128,7 @@ impl PusService17TestHandler {
 
     pub fn handle_next_packet(&mut self) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
         return match self.psb.tc_rx.try_recv() {
-            Ok((addr, token)) => {
+            Ok((addr, token)) => self.handle_one_tc(addr, token),
-                if self.handle_one_tc(addr, token)? {
-                    return Ok(PusPacketHandlerResult::RequestHandled);
-                }
-                Ok(PusPacketHandlerResult::CustomSubservice(token))
-            }
             Err(e) => match e {
                 TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
                 TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
@@ -134,7 +140,8 @@ impl PusService17TestHandler {
         &mut self,
         addr: StoreAddr,
         token: VerificationToken<TcStateAccepted>,
-    ) -> Result<bool, PusPacketHandlingError> {
+    ) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
+        let mut partial_result = None;
         {
             // Keep locked section as short as possible.
             let mut tc_pool = self
@@ -143,39 +150,40 @@ impl PusService17TestHandler {
                 .write()
                 .map_err(|e| PusPacketHandlingError::RwGuardError(format!("{e}")))?;
             let tc_guard = tc_pool.read_with_guard(addr);
-            let tc_raw = tc_guard.read().expect("Reading pool guard failed");
+            let tc_raw = tc_guard.read()?;
             self.psb.pus_buf[0..tc_raw.len()].copy_from_slice(tc_raw);
         }
+        let mut partial_error = None;
         let (tc, tc_size) = PusTc::from_bytes(&self.psb.pus_buf)?;
         if tc.service() != 17 {
             return Err(PusPacketHandlingError::WrongService(tc.service()));
         }
         if tc.subservice() == 1 {
-            let time_provider = TimeProvider::from_now_with_u16_days()?;
+            partial_result = self.psb.update_stamp().err();
-            // Can not fail, buffer is large enough.
-            time_provider
-                .write_to_bytes(&mut self.psb.stamp_buf)
-                .unwrap();
             let result = self
                 .psb
                 .verification_handler
-                .start_success(token, Some(&self.psb.stamp_buf));
+                .start_success(token, Some(&self.psb.stamp_buf))
+                .map_err(|e| PartialPusHandlingError::VerificationError);
             let start_token = if result.is_err() {
-                error!("Could not send start success verification");
+                partial_error = Some(result.unwrap_err());
                 None
             } else {
                 Some(result.unwrap())
             };
             // Sequence count will be handled centrally in TM funnel.
-            let ping_reply =
+            let mut reply_header = SpHeader::tm_unseg(self.psb.tm_apid, 0, 0).unwrap();
-                self.psb
+            let tc_header = PusTmSecondaryHeader::new_simple(17, 2, &self.psb.stamp_buf);
-                    .tm_helper
+            let ping_reply = PusTm::new(&mut reply_header, tc_header, None, true);
-                    .create_pus_tm_with_stamp(17, 2, None, &time_provider, 0);
             let addr = self.psb.tm_store.add_pus_tm(&ping_reply);
-            self.psb
+            if let Err(e) = self
+                .psb
                 .tm_tx
                 .send(addr)
-                .map_err(|e| PusPacketHandlingError::TmSendError(format!("{e}")))?;
+                .map_err(|e| PartialPusHandlingError::TmSendError(format!("{e}")))
+            {
+                partial_error = Some(e);
+            }
             if let Some(start_token) = start_token {
                 if self
                     .psb
@@ -183,11 +191,16 @@ impl PusService17TestHandler {
                     .completion_success(start_token, Some(&self.psb.stamp_buf))
                     .is_err()
                 {
-                    error!("Could not send completion success verification");
+                    partial_error = Some(PartialPusHandlingError::VerificationError)
                 }
             }
-            return Ok(true);
+            if partial_error.is_some() {
+                return Ok(PusPacketHandlerResult::RequestHandledPartialSuccess(
+                    partial_error.unwrap(),
+                ));
+            }
+            return Ok(PusPacketHandlerResult::RequestHandled);
         }
-        Ok(false)
+        Ok(PusPacketHandlerResult::CustomSubservice(token))
     }
 }