sat-rs/satrs-example/src/pus/mod.rs

836 lines
33 KiB
Rust
Raw Normal View History

2023-07-04 15:17:43 +02:00
use crate::tmtc::MpscStoreAndSendError;
2022-11-20 19:54:14 +01:00
use satrs_core::events::EventU32;
2023-02-14 15:53:14 +01:00
use satrs_core::hk::{CollectionIntervalFactor, HkRequest};
2023-02-27 13:44:24 +01:00
use satrs_core::mode::{ModeAndSubmode, ModeRequest};
2023-07-03 01:33:13 +02:00
use satrs_core::objects::ObjectId;
2023-02-14 15:53:14 +01:00
use satrs_core::params::Params;
use satrs_core::pool::{PoolProvider, StoreAddr};
2022-11-20 19:54:14 +01:00
use satrs_core::pus::event_man::{EventRequest, EventRequestWithToken};
2023-07-03 01:33:13 +02:00
use satrs_core::pus::hk;
2023-02-15 11:19:23 +01:00
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,
VerificationToken,
};
2023-07-03 01:33:13 +02:00
use satrs_core::pus::{event, EcssTcSenderCore, GenericTcCheckError, MpscTmtcInStoreSender};
use satrs_core::pus::{mode, EcssTcSender};
2022-11-27 22:05:42 +01:00
use satrs_core::res_code::ResultU16;
2023-07-02 17:40:16 +02:00
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
use satrs_core::spacepackets::ecss::{scheduling, PusServiceId};
use satrs_core::spacepackets::time::CcsdsTimeProvider;
2023-07-04 15:17:43 +02:00
use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
2023-02-15 11:05:32 +01:00
use satrs_core::tmtc::{AddressableId, PusServiceProvider, TargetId};
2023-01-10 17:16:57 +01:00
use satrs_core::{
spacepackets::ecss::PusPacket, spacepackets::tc::PusTc, spacepackets::time::cds::TimeProvider,
spacepackets::time::TimeWriter, spacepackets::SpHeader,
2023-01-10 17:16:57 +01:00
};
use satrs_example::{hk_err, tmtc_err, CustomPusServiceId, TEST_EVENT};
2023-01-27 14:02:46 +01:00
use std::cell::RefCell;
2022-12-19 17:03:26 +01:00
use std::collections::HashMap;
use std::convert::TryFrom;
2023-01-26 10:58:44 +01:00
use std::rc::Rc;
use std::sync::mpsc::{Receiver, Sender};
2022-08-29 01:33:32 +02:00
2023-07-03 18:22:08 +02:00
pub mod test;
2023-07-03 01:33:13 +02:00
// pub trait PusTcRouter {
// type Error;
// fn route_pus_tc(
// &mut self,
// apid: u16,
// service: u8,
// subservice: u8,
// tc: &PusTc,
// );
// }
2023-07-03 18:22:08 +02:00
pub enum PusTcWrapper<'tc> {
PusTc(&'tc PusTc<'tc>),
StoreAddr(StoreAddr),
}
2023-07-03 01:33:13 +02:00
pub type AcceptedTc = (StoreAddr, VerificationToken<TcStateAccepted>);
pub struct PusTcMpscRouter {
2023-07-03 18:22:08 +02:00
pub test_service_receiver: Sender<AcceptedTc>,
pub event_service_receiver: Sender<AcceptedTc>,
pub sched_service_receiver: Sender<AcceptedTc>,
pub hk_service_receiver: Sender<AcceptedTc>,
pub action_service_receiver: Sender<AcceptedTc>,
2023-07-02 20:49:45 +02:00
}
2023-07-03 01:33:13 +02:00
// impl PusTcRouter for PusTcMpscRouter {
// type Error = ();
//
// fn route_pus_tc(&mut self, apid: u16, service: u8, subservice: u8, tc: &PusTc) {
// if apid == PUS_APID {
// if service == PusServiceId::Event as u8 {
// self.event_service_receiver.send_tc(*tc).unwrap();
// }
// if service == PusServiceId::Action as u8 {
// // TODO: Look up object ID and then route the action request to that object.
// self.action_service_receiver.send_tc(*tc).unwrap();
// }
// if service == PusServiceId::Housekeeping as u8 {
// // TODO: Look up object ID and then route the HK request to that object.
// }
// if service == PusServiceId::Scheduling as u8 {
// self.sched_service_receiver.send_tc(*tc).unwrap();
// }
// if service == PusServiceId::Test as u8 {
// self.test_service_receiver.send_tc(*tc).unwrap();
// }
// }
// todo!()
// }
// }
2022-09-03 13:47:25 +02:00
pub struct PusReceiver {
pub tm_helper: PusTmWithCdsShortHelper,
2023-02-15 11:05:32 +01:00
pub tm_args: PusTmArgs,
2023-02-15 11:26:19 +01:00
pub tc_args: PusTcArgs,
stamp_helper: TimeStampHelper,
2022-09-03 13:47:25 +02:00
}
2023-02-04 15:35:18 +01:00
pub struct PusTmArgs {
/// All telemetry is sent with this sender handle.
pub tm_tx: Sender<StoreAddr>,
/// All TM to be sent is stored here
2023-07-04 15:17:43 +02:00
pub tm_store: SharedTmStore,
2023-02-04 15:35:18 +01:00
/// All verification reporting is done with this reporter.
pub verif_reporter: StdVerifReporterWithSender,
/// Sequence count provider for TMs sent from within pus demultiplexer
pub seq_count_provider: SeqCountProviderSyncClonable,
2023-02-04 15:35:18 +01:00
}
2023-02-15 11:26:19 +01:00
impl PusTmArgs {
fn vr(&mut self) -> &mut StdVerifReporterWithSender {
&mut self.verif_reporter
}
}
2023-07-03 01:33:13 +02:00
// #[allow(dead_code)]
// pub struct PusTcHandlerBase {
// pub tc_store: Box<dyn PoolProvider>,
// pub receiver: Receiver<(StoreAddr, VerificationToken<TcStateAccepted>)>,
// pub verif_reporter: StdVerifReporterWithSender,
// pub time_provider: Box<dyn CcsdsTimeProvider>,
// }
//
// pub trait TestHandlerNoPing {
// fn handle_no_ping_tc(&mut self, tc: PusTc);
// }
//
// #[allow(dead_code)]
// pub struct PusTestTcHandler {
// pub base: PusTcHandlerBase,
// handler: Option<Box<dyn TestHandlerNoPing>>,
// }
//
// #[allow(dead_code)]
// pub struct PusScheduleTcHandler {
// pub base: PusTestTcHandler,
// }
//
// impl PusTestTcHandler {
// #[allow(dead_code)]
// pub fn operation(&mut self) {
// let (addr, token) = self.base.receiver.recv().unwrap();
// let data = self.base.tc_store.read(&addr).unwrap();
// let (pus_tc, _len) = PusTc::from_bytes(data).unwrap();
// let stamp: [u8; 7] = [0; 7];
// if pus_tc.subservice() == 1 {
// self.base
// .verif_reporter
// .completion_success(token, Some(&stamp))
// .unwrap();
// } else if let Some(handler) = &mut self.handler {
// handler.handle_no_ping_tc(pus_tc);
// }
// }
// }
2023-02-04 15:35:18 +01:00
pub struct PusTcArgs {
2023-07-03 18:22:08 +02:00
//pub event_request_tx: Sender<EventRequestWithToken>,
2023-07-03 01:33:13 +02:00
/// This routes all telecommands to their respective recipients
pub pus_router: PusTcMpscRouter,
2023-02-16 00:00:08 +01:00
/// Request routing helper. Maps targeted requests to their recipient.
2023-07-03 01:33:13 +02:00
//pub request_map: HashMap<TargetId, Sender<RequestWithToken>>,
2023-02-04 15:35:18 +01:00
/// Required for scheduling of telecommands.
2023-07-03 01:33:13 +02:00
//pub tc_source: PusTcSource,
/// Used to send events from within the TC router
2023-02-13 09:20:00 +01:00
pub event_sender: Sender<(EventU32, Option<Params>)>,
2023-07-03 01:33:13 +02:00
//pub scheduler: Rc<RefCell<PusScheduler>>,
2023-02-04 15:35:18 +01:00
}
struct TimeStampHelper {
stamper: TimeProvider,
time_stamp: [u8; 7],
}
impl TimeStampHelper {
pub fn new() -> Self {
Self {
stamper: TimeProvider::new_with_u16_days(0, 0),
time_stamp: [0; 7],
}
}
pub fn stamp(&self) -> &[u8] {
&self.time_stamp
}
pub fn update_from_now(&mut self) {
self.stamper
.update_from_now()
.expect("Updating timestamp failed");
self.stamper
.write_to_bytes(&mut self.time_stamp)
.expect("Writing timestamp failed");
}
}
2022-09-03 13:47:25 +02:00
impl PusReceiver {
2023-02-04 15:35:18 +01:00
pub fn new(apid: u16, tm_arguments: PusTmArgs, tc_arguments: PusTcArgs) -> Self {
2022-09-03 13:47:25 +02:00
Self {
tm_helper: PusTmWithCdsShortHelper::new(apid),
2023-02-15 11:05:32 +01:00
tm_args: tm_arguments,
2023-02-15 11:26:19 +01:00
tc_args: tc_arguments,
stamp_helper: TimeStampHelper::new(),
2022-09-03 13:47:25 +02:00
}
}
}
2023-07-03 18:22:08 +02:00
impl PusReceiver {
pub fn handle_tc_packet(
2022-09-03 13:47:25 +02:00
&mut self,
2023-07-03 18:22:08 +02:00
store_addr: StoreAddr,
2022-09-03 13:47:25 +02:00
service: u8,
pus_tc: &PusTc,
2023-07-03 18:22:08 +02:00
) -> Result<(), MpscStoreAndSendError> {
2023-02-15 11:05:32 +01:00
let init_token = self.tm_args.verif_reporter.add_tc(pus_tc);
self.stamp_helper.update_from_now();
2023-02-15 11:19:23 +01:00
let accepted_token = self
.tm_args
2023-02-15 11:26:19 +01:00
.vr()
2023-02-15 11:19:23 +01:00
.acceptance_success(init_token, Some(self.stamp_helper.stamp()))
2022-09-10 20:18:19 +02:00
.expect("Acceptance success failure");
let service = PusServiceId::try_from(service);
match service {
Ok(standard_service) => match standard_service {
2023-07-03 01:33:13 +02:00
PusServiceId::Test => self
.tc_args
.pus_router
.test_service_receiver
2023-07-03 18:22:08 +02:00
.send((store_addr, accepted_token))
.unwrap(),
PusServiceId::Housekeeping => self
.tc_args
.pus_router
.hk_service_receiver
.send((store_addr, accepted_token))
.unwrap(),
2023-07-03 01:33:13 +02:00
PusServiceId::Event => self
.tc_args
.pus_router
.event_service_receiver
2023-07-03 18:22:08 +02:00
.send((store_addr, accepted_token))
.unwrap(),
2023-07-03 01:33:13 +02:00
PusServiceId::Scheduling => self
.tc_args
.pus_router
.sched_service_receiver
2023-07-03 18:22:08 +02:00
.send((store_addr, accepted_token))
.unwrap(),
2023-02-15 11:19:23 +01:00
_ => self
.tm_args
.verif_reporter
.start_failure(
accepted_token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::PUS_SERVICE_NOT_IMPLEMENTED,
Some(&[standard_service as u8]),
),
)
.expect("Start failure verification failed"),
},
Err(e) => {
if let Ok(custom_service) = CustomPusServiceId::try_from(e.number) {
match custom_service {
CustomPusServiceId::Mode => {
2023-07-03 18:22:08 +02:00
//self.handle_mode_service(pus_tc, accepted_token)
}
CustomPusServiceId::Health => {}
}
} else {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
accepted_token,
FailParams::new(
Some(self.stamp_helper.stamp()),
&tmtc_err::INVALID_PUS_SUBSERVICE,
Some(&[e.number]),
),
)
.expect("Start failure verification failed")
}
}
2022-09-03 13:47:25 +02:00
}
Ok(())
}
}
2023-07-03 18:22:08 +02:00
// impl PusServiceProvider for PusReceiver {
// type Error = ();
//
// fn handle_pus_tc_packet(
// &mut self,
// service: u8,
// _header: &SpHeader,
// pus_tc: &PusTc,
// ) -> Result<(), Self::Error> {
// let init_token = self.tm_args.verif_reporter.add_tc(pus_tc);
// self.stamp_helper.update_from_now();
// let accepted_token = self
// .tm_args
// .vr()
// .acceptance_success(init_token, Some(self.stamp_helper.stamp()))
// .expect("Acceptance success failure");
// let service = PusServiceId::try_from(service);
// match service {
// Ok(standard_service) => match standard_service {
// PusServiceId::Test => self
// .tc_args
// .pus_router
// .test_service_receiver
// .send_tc(*pus_tc),
// PusServiceId::Housekeeping => {
// self.tc_args.pus_router.hk_service_receiver.send_tc(*pus_tc)
// } //self.handle_hk_request(pus_tc, accepted_token),
// PusServiceId::Event => self
// .tc_args
// .pus_router
// .event_service_receiver
// .send_tc(*pus_tc), //self.handle_event_request(pus_tc, accepted_token),
// PusServiceId::Scheduling => self
// .tc_args
// .pus_router
// .sched_service_receiver
// .send_tc(*pus_tc), //self.handle_scheduled_tc(pus_tc, accepted_token),
// _ => self
// .tm_args
// .verif_reporter
// .start_failure(
// accepted_token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::PUS_SERVICE_NOT_IMPLEMENTED,
// Some(&[standard_service as u8]),
// ),
// )
// .expect("Start failure verification failed"),
// },
// Err(e) => {
// if let Ok(custom_service) = CustomPusServiceId::try_from(e.number) {
// match custom_service {
// CustomPusServiceId::Mode => {
// self.handle_mode_service(pus_tc, accepted_token)
// }
// CustomPusServiceId::Health => {}
// }
// } else {
// self.tm_args
// .verif_reporter
// .start_failure(
// accepted_token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::INVALID_PUS_SUBSERVICE,
// Some(&[e.number]),
// ),
// )
// .expect("Start failure verification failed")
// }
// }
// }
// Ok(())
// }
// }
2022-09-03 13:47:25 +02:00
2023-07-03 01:33:13 +02:00
// impl PusReceiver {
// fn handle_test_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
// match PusPacket::subservice(pus_tc) {
// 1 => {
// info!("Received PUS ping command TC[17,1]");
// info!("Sending ping reply PUS TM[17,2]");
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("Error sending start success");
// let ping_reply = self.tm_helper.create_pus_tm_timestamp_now(
// 17,
// 2,
// None,
// self.tm_args.seq_count_provider.get(),
// );
// let addr = self.tm_args.tm_store.add_pus_tm(&ping_reply);
// self.tm_args
// .tm_tx
// .send(addr)
// .expect("Sending TM to TM funnel failed");
// self.tm_args.seq_count_provider.increment();
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("Error sending completion success");
// }
// 128 => {
// info!("Generating test event");
// self.tc_args
// .event_sender
// .send((TEST_EVENT.into(), None))
// .expect("Sending test event failed");
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("Error sending start success");
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("Error sending completion success");
// }
// _ => {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::INVALID_PUS_SUBSERVICE,
// None,
// ),
// )
// .expect("Sending start failure TM failed");
// }
// }
// }
//
// fn handle_hk_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
// if pus_tc.user_data().is_none() {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// None,
// ),
// )
// .expect("Sending start failure TM failed");
// return;
// }
// let user_data = pus_tc.user_data().unwrap();
// if user_data.len() < 8 {
// let err = if user_data.len() < 4 {
// &hk_err::TARGET_ID_MISSING
// } else {
// &hk_err::UNIQUE_ID_MISSING
// };
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(Some(self.stamp_helper.stamp()), err, None),
// )
// .expect("Sending start failure TM failed");
// return;
// }
// let addressable_id = AddressableId::from_raw_be(user_data).unwrap();
// if !self
// .tc_args
// .request_map
// .contains_key(&addressable_id.target_id)
// {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &hk_err::UNKNOWN_TARGET_ID,
// None,
// ),
// )
// .expect("Sending start failure TM failed");
// return;
// }
// let send_request = |target: TargetId, request: HkRequest| {
// let sender = self
// .tc_args
// .request_map
// .get(&addressable_id.target_id)
// .unwrap();
// sender
// .send(RequestWithToken::new(
// target,
// Request::HkRequest(request),
// token,
// ))
// .unwrap_or_else(|_| panic!("Sending HK request {request:?} failed"));
// };
// if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableHkGeneration as u8 {
// send_request(
// addressable_id.target_id,
// HkRequest::Enable(addressable_id.unique_id),
// );
// } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableHkGeneration as u8 {
// send_request(
// addressable_id.target_id,
// HkRequest::Disable(addressable_id.unique_id),
// );
// } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcGenerateOneShotHk as u8 {
// send_request(
// addressable_id.target_id,
// HkRequest::OneShot(addressable_id.unique_id),
// );
// } else if PusPacket::subservice(pus_tc)
// == hk::Subservice::TcModifyHkCollectionInterval as u8
// {
// if user_data.len() < 12 {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &hk_err::COLLECTION_INTERVAL_MISSING,
// None,
// ),
// )
// .expect("Sending start failure TM failed");
// return;
// }
// send_request(
// addressable_id.target_id,
// HkRequest::ModifyCollectionInterval(
// addressable_id.unique_id,
// CollectionIntervalFactor::from_be_bytes(user_data[8..12].try_into().unwrap()),
// ),
// );
// }
// }
//
// fn handle_event_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
// let send_start_failure = |vr: &mut StdVerifReporterWithSender,
// timestamp: &[u8],
// failure_code: &ResultU16,
// failure_data: Option<&[u8]>| {
// vr.start_failure(
// token,
// FailParams::new(Some(timestamp), failure_code, failure_data),
// )
// .expect("Sending start failure TM failed");
// };
// let send_start_acceptance = |vr: &mut StdVerifReporterWithSender, timestamp: &[u8]| {
// vr.start_success(token, Some(timestamp))
// .expect("Sending start success TM failed")
// };
// if pus_tc.user_data().is_none() {
// send_start_failure(
// &mut self.tm_args.verif_reporter,
// self.stamp_helper.stamp(),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// None,
// );
// return;
// }
// let app_data = pus_tc.user_data().unwrap();
// if app_data.len() < 4 {
// send_start_failure(
// &mut self.tm_args.verif_reporter,
// self.stamp_helper.stamp(),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// None,
// );
// return;
// }
// let event_id = EventU32::from(u32::from_be_bytes(app_data.try_into().unwrap()));
// match PusPacket::subservice(pus_tc).try_into() {
// Ok(event::Subservice::TcEnableEventGeneration) => {
// let start_token = send_start_acceptance(
// &mut self.tm_args.verif_reporter,
// self.stamp_helper.stamp(),
// );
// self.tc_args
// .event_request_tx
// .send(EventRequestWithToken {
// request: EventRequest::Enable(event_id),
// token: start_token,
// })
// .expect("Sending event request failed");
// }
// Ok(event::Subservice::TcDisableEventGeneration) => {
// let start_token = send_start_acceptance(
// &mut self.tm_args.verif_reporter,
// self.stamp_helper.stamp(),
// );
// self.tc_args
// .event_request_tx
// .send(EventRequestWithToken {
// request: EventRequest::Disable(event_id),
// token: start_token,
// })
// .expect("Sending event request failed");
// }
// _ => {
// send_start_failure(
// &mut self.tm_args.verif_reporter,
// self.stamp_helper.stamp(),
// &tmtc_err::INVALID_PUS_SUBSERVICE,
// None,
// );
// }
// }
// }
//
// fn handle_scheduled_tc(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
// let subservice = match pus_11_generic_tc_check(pus_tc) {
// Ok(subservice) => subservice,
// Err(e) => match e {
// GenericTcCheckError::NotEnoughAppData => {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// None,
// ),
// )
// .expect("could not sent verification error");
// return;
// }
// GenericTcCheckError::InvalidSubservice => {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::INVALID_PUS_SUBSERVICE,
// None,
// ),
// )
// .expect("could not sent verification error");
// return;
// }
// },
// };
// match subservice {
// scheduling::Subservice::TcEnableScheduling => {
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("Error sending start success");
//
// let mut scheduler = self.tc_args.scheduler.borrow_mut();
// scheduler.enable();
// if scheduler.is_enabled() {
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("Error sending completion success");
// } else {
// panic!("Failed to enable scheduler");
// }
// }
// scheduling::Subservice::TcDisableScheduling => {
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("Error sending start success");
//
// let mut scheduler = self.tc_args.scheduler.borrow_mut();
// scheduler.disable();
// if !scheduler.is_enabled() {
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("Error sending completion success");
// } else {
// panic!("Failed to disable scheduler");
// }
// }
// scheduling::Subservice::TcResetScheduling => {
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("Error sending start success");
//
// let mut pool = self
// .tc_args
// .tc_source
// .tc_store
// .pool
// .write()
// .expect("Locking pool failed");
//
// let mut scheduler = self.tc_args.scheduler.borrow_mut();
// scheduler
// .reset(pool.as_mut())
// .expect("Error resetting TC Pool");
// drop(scheduler);
//
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("Error sending completion success");
// }
// scheduling::Subservice::TcInsertActivity => {
// let start_token = self
// .tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("error sending start success");
//
// let mut pool = self
// .tc_args
// .tc_source
// .tc_store
// .pool
// .write()
// .expect("locking pool failed");
// let mut scheduler = self.tc_args.scheduler.borrow_mut();
// scheduler
// .insert_wrapped_tc::<TimeProvider>(pus_tc, pool.as_mut())
// .expect("insertion of activity into pool failed");
// drop(scheduler);
//
// self.tm_args
// .verif_reporter
// .completion_success(start_token, Some(self.stamp_helper.stamp()))
// .expect("sending completion success failed");
// }
// _ => {}
// }
// }
//
// fn handle_mode_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
// let mut app_data_len = 0;
// let app_data = pus_tc.user_data();
// if app_data.is_some() {
// app_data_len = pus_tc.user_data().unwrap().len();
// }
// if app_data_len < 4 {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// Some(format!("expected {} bytes, found {}", 4, app_data_len).as_bytes()),
// ),
// )
// .expect("Sending start failure TM failed");
// }
// let app_data = app_data.unwrap();
// let mut invalid_subservice_handler = || {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::INVALID_PUS_SUBSERVICE,
// Some(&[PusPacket::subservice(pus_tc)]),
// ),
// )
// .expect("Sending start failure TM failed");
// };
// let subservice = mode::Subservice::try_from(PusPacket::subservice(pus_tc));
// if let Ok(subservice) = subservice {
// let forward_mode_request = |target_id, mode_request: ModeRequest| match self
// .tc_args
// .request_map
// .get(&target_id)
// {
// None => warn!("not mode request recipient for target ID {target_id} found"),
// Some(sender_to_recipient) => {
// sender_to_recipient
// .send(RequestWithToken::new(
// target_id,
// Request::ModeRequest(mode_request),
// token,
// ))
// .expect("sending mode request failed");
// }
// };
// let mut valid_subservice = true;
// match subservice {
// Subservice::TcSetMode => {
// let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
// let min_len = ModeAndSubmode::raw_len() + 4;
// if app_data_len < min_len {
// self.tm_args
// .verif_reporter
// .start_failure(
// token,
// FailParams::new(
// Some(self.stamp_helper.stamp()),
// &tmtc_err::NOT_ENOUGH_APP_DATA,
// Some(
// format!("expected {min_len} bytes, found {app_data_len}")
// .as_bytes(),
// ),
// ),
// )
// .expect("Sending start failure TM failed");
// }
// // Should never fail after size check
// let mode_submode = ModeAndSubmode::from_be_bytes(
// app_data[4..4 + ModeAndSubmode::raw_len()]
// .try_into()
// .unwrap(),
// )
// .unwrap();
// forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
// }
// Subservice::TcReadMode => {
// let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
// forward_mode_request(target_id, ModeRequest::ReadMode);
// }
// Subservice::TcAnnounceMode => {
// let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
// forward_mode_request(target_id, ModeRequest::AnnounceMode);
// }
// Subservice::TcAnnounceModeRecursive => {
// let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
// forward_mode_request(target_id, ModeRequest::AnnounceModeRecursive);
// }
// _ => {
// warn!("Can not process mode request with subservice {subservice:?}");
// invalid_subservice_handler();
// valid_subservice = false;
// }
// }
// if valid_subservice {
// self.tm_args
// .verif_reporter
// .start_success(token, Some(self.stamp_helper.stamp()))
// .expect("sending start success TM failed");
// }
// } else {
// invalid_subservice_handler();
// }
// }
// }