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

619 lines
24 KiB
Rust
Raw Normal View History

2022-12-21 10:23:32 +01:00
use crate::requests::{Request, RequestWithToken};
use crate::tmtc::{PusTcSource, TmStore};
use log::{info, warn};
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-15 11:19:23 +01:00
use satrs_core::mode::{ModeAndSubmode, ModeCommand, ModeRequest};
2023-02-14 15:53:14 +01:00
use satrs_core::params::Params;
use satrs_core::pool::StoreAddr;
2022-11-20 19:54:14 +01:00
use satrs_core::pus::event_man::{EventRequest, EventRequestWithToken};
2022-12-20 15:33:00 +01:00
use satrs_core::pus::hk;
2023-02-15 11:05:32 +01:00
use satrs_core::pus::mode;
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,
};
use satrs_core::pus::{event, GenericTcCheckError};
2022-11-27 22:05:42 +01:00
use satrs_core::res_code::ResultU16;
use satrs_core::spacepackets::ecss::{scheduling, PusServiceId};
2022-11-20 19:54:14 +01:00
use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
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;
2022-12-19 17:03:26 +01:00
use std::sync::mpsc::Sender;
2022-08-29 01:33:32 +02:00
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
pub tm_store: TmStore,
/// All verification reporting is done with this reporter.
pub verif_reporter: StdVerifReporterWithSender,
}
2023-02-15 11:26:19 +01:00
impl PusTmArgs {
fn vr(&mut self) -> &mut StdVerifReporterWithSender {
&mut self.verif_reporter
}
}
2023-02-04 15:35:18 +01:00
pub struct PusTcArgs {
pub event_request_tx: Sender<EventRequestWithToken>,
2023-02-16 00:00:08 +01:00
/// Request routing helper. Maps targeted requests to their recipient.
2023-02-15 11:05:32 +01:00
pub request_map: HashMap<TargetId, Sender<RequestWithToken>>,
2023-02-04 15:35:18 +01:00
/// Required for scheduling of telecommands.
pub tc_source: PusTcSource,
2023-02-13 09:20:00 +01:00
pub event_sender: Sender<(EventU32, Option<Params>)>,
2023-02-04 15:35:18 +01:00
pub scheduler: Rc<RefCell<PusScheduler>>,
}
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
}
}
}
impl PusServiceProvider for PusReceiver {
type Error = ();
fn handle_pus_tc_packet(
&mut self,
service: u8,
_header: &SpHeader,
pus_tc: &PusTc,
) -> Result<(), Self::Error> {
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 {
PusServiceId::Test => self.handle_test_service(pus_tc, accepted_token),
PusServiceId::Housekeeping => self.handle_hk_request(pus_tc, accepted_token),
PusServiceId::Event => self.handle_event_request(pus_tc, accepted_token),
PusServiceId::Scheduling => self.handle_scheduled_tc(pus_tc, accepted_token),
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 => {
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(())
}
}
impl PusReceiver {
2022-11-13 21:07:16 +01:00
fn handle_test_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
2023-02-13 09:20:00 +01:00
match PusPacket::subservice(pus_tc) {
1 => {
info!("Received PUS ping command TC[17,1]");
info!("Sending ping reply PUS TM[17,2]");
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-02-13 09:20:00 +01:00
.expect("Error sending start success");
let ping_reply = self.tm_helper.create_pus_tm_timestamp_now(17, 2, None);
2023-02-15 11:05:32 +01:00
let addr = self.tm_args.tm_store.add_pus_tm(&ping_reply);
2023-02-15 11:19:23 +01:00
self.tm_args
.tm_tx
2023-02-13 09:20:00 +01:00
.send(addr)
.expect("Sending TM to TM funnel failed");
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-02-13 09:20:00 +01:00
.expect("Error sending completion success");
}
128 => {
info!("Generating test event");
2023-02-15 22:30:32 +01:00
self.tc_args
.event_sender
2023-02-14 15:53:14 +01:00
.send((TEST_EVENT.into(), None))
.expect("Sending test event failed");
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-02-13 09:20:00 +01:00
.expect("Error sending start success");
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-02-13 09:20:00 +01:00
.expect("Error sending completion success");
}
_ => {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
2023-02-13 09:20:00 +01:00
.start_failure(
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
2023-02-13 09:20:00 +01:00
&tmtc_err::INVALID_PUS_SUBSERVICE,
None,
),
)
.expect("Sending start failure TM failed");
}
2022-09-03 13:47:25 +02:00
}
}
2022-11-19 12:16:51 +01:00
2022-12-20 15:33:00 +01:00
fn handle_hk_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
if pus_tc.user_data().is_none() {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
2022-12-20 15:33:00 +01:00
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::NOT_ENOUGH_APP_DATA,
None,
),
2022-12-20 15:33:00 +01:00
)
.expect("Sending start failure TM failed");
2022-12-20 15:45:09 +01:00
return;
2022-12-20 15:33:00 +01:00
}
let user_data = pus_tc.user_data().unwrap();
2022-12-20 15:45:09 +01:00
if user_data.len() < 8 {
let err = if user_data.len() < 4 {
&hk_err::TARGET_ID_MISSING
} else {
&hk_err::UNIQUE_ID_MISSING
};
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
token,
2023-02-15 11:19:23 +01:00
FailParams::new(Some(self.stamp_helper.stamp()), err, None),
)
2022-12-20 15:45:09 +01:00
.expect("Sending start failure TM failed");
return;
}
2022-12-20 17:23:11 +01:00
let addressable_id = AddressableId::from_raw_be(user_data).unwrap();
2023-02-15 22:30:32 +01:00
if !self
.tc_args
.request_map
.contains_key(&addressable_id.target_id)
{
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
2022-12-20 15:45:09 +01:00
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&hk_err::UNKNOWN_TARGET_ID,
None,
),
2022-12-20 15:45:09 +01:00
)
.expect("Sending start failure TM failed");
return;
}
2022-12-20 17:23:11 +01:00
let send_request = |request: HkRequest| {
2023-02-15 22:30:32 +01:00
let sender = self
.tc_args
.request_map
.get(&addressable_id.target_id)
.unwrap();
2022-12-20 15:45:09 +01:00
sender
2022-12-21 10:23:32 +01:00
.send(RequestWithToken(Request::HkRequest(request), token))
2023-01-26 23:31:09 +01:00
.unwrap_or_else(|_| panic!("Sending HK request {request:?} failed"));
2022-12-20 17:23:11 +01:00
};
2023-02-04 15:48:41 +01:00
if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableHkGeneration as u8 {
2022-12-22 09:26:00 +01:00
send_request(HkRequest::Enable(addressable_id));
2023-02-04 15:48:41 +01:00
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableHkGeneration as u8 {
2022-12-22 09:26:00 +01:00
send_request(HkRequest::Disable(addressable_id));
2022-12-20 15:45:09 +01:00
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcGenerateOneShotHk as u8 {
2022-12-22 09:26:00 +01:00
send_request(HkRequest::OneShot(addressable_id));
2023-02-04 15:48:41 +01:00
} else if PusPacket::subservice(pus_tc)
== hk::Subservice::TcModifyHkCollectionInterval as u8
2022-12-20 15:45:09 +01:00
{
2022-12-20 17:41:07 +01:00
if user_data.len() < 12 {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
2022-12-20 17:41:07 +01:00
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
2022-12-20 17:41:07 +01:00
&hk_err::COLLECTION_INTERVAL_MISSING,
None,
),
)
.expect("Sending start failure TM failed");
return;
}
send_request(HkRequest::ModifyCollectionInterval(
2022-12-22 09:26:00 +01:00
addressable_id,
2022-12-20 17:41:07 +01:00
CollectionIntervalFactor::from_be_bytes(user_data[8..12].try_into().unwrap()),
));
2022-12-20 15:33:00 +01:00
}
}
2023-01-25 10:15:21 +01:00
2022-12-19 17:03:26 +01:00
fn handle_event_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
2023-02-15 11:05:32 +01:00
let send_start_failure = |vr: &mut StdVerifReporterWithSender,
timestamp: &[u8],
2022-11-27 19:24:18 +01:00
failure_code: &ResultU16,
2022-11-19 12:16:51 +01:00
failure_data: Option<&[u8]>| {
2023-02-15 11:19:23 +01:00
vr.start_failure(
token,
FailParams::new(Some(timestamp), failure_code, failure_data),
)
.expect("Sending start failure TM failed");
2022-11-19 12:16:51 +01:00
};
2023-02-15 11:19:23 +01:00
let send_start_acceptance = |vr: &mut StdVerifReporterWithSender, timestamp: &[u8]| {
vr.start_success(token, Some(timestamp))
2022-11-19 12:16:51 +01:00
.expect("Sending start success TM failed")
};
if pus_tc.user_data().is_none() {
send_start_failure(
2023-02-15 11:19:23 +01:00
&mut self.tm_args.verif_reporter,
self.stamp_helper.stamp(),
2022-12-20 15:45:09 +01:00
&tmtc_err::NOT_ENOUGH_APP_DATA,
2022-11-19 12:16:51 +01:00
None,
);
return;
}
let app_data = pus_tc.user_data().unwrap();
if app_data.len() < 4 {
send_start_failure(
2023-02-15 11:19:23 +01:00
&mut self.tm_args.verif_reporter,
self.stamp_helper.stamp(),
2022-12-20 15:45:09 +01:00
&tmtc_err::NOT_ENOUGH_APP_DATA,
2022-11-19 12:16:51 +01:00
None,
);
return;
}
let event_id = EventU32::from(u32::from_be_bytes(app_data.try_into().unwrap()));
match PusPacket::subservice(pus_tc).try_into() {
2023-02-04 15:35:18 +01:00
Ok(event::Subservice::TcEnableEventGeneration) => {
2023-02-15 11:19:23 +01:00
let start_token = send_start_acceptance(
&mut self.tm_args.verif_reporter,
self.stamp_helper.stamp(),
);
2023-02-15 22:30:32 +01:00
self.tc_args
.event_request_tx
2022-11-19 12:16:51 +01:00
.send(EventRequestWithToken {
request: EventRequest::Enable(event_id),
token: start_token,
})
.expect("Sending event request failed");
}
2023-02-04 15:35:18 +01:00
Ok(event::Subservice::TcDisableEventGeneration) => {
2023-02-15 11:19:23 +01:00
let start_token = send_start_acceptance(
&mut self.tm_args.verif_reporter,
self.stamp_helper.stamp(),
);
2023-02-15 22:30:32 +01:00
self.tc_args
.event_request_tx
2022-11-19 12:16:51 +01:00
.send(EventRequestWithToken {
request: EventRequest::Disable(event_id),
token: start_token,
})
.expect("Sending event request failed");
}
_ => {
send_start_failure(
2023-02-15 11:19:23 +01:00
&mut self.tm_args.verif_reporter,
self.stamp_helper.stamp(),
2022-12-20 15:45:09 +01:00
&tmtc_err::INVALID_PUS_SUBSERVICE,
2022-11-19 12:16:51 +01:00
None,
);
}
}
}
2023-01-25 10:15:21 +01:00
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 => {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::NOT_ENOUGH_APP_DATA,
None,
),
)
.expect("could not sent verification error");
return;
}
GenericTcCheckError::InvalidSubservice => {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::INVALID_PUS_SUBSERVICE,
None,
),
)
.expect("could not sent verification error");
return;
}
},
};
match subservice {
scheduling::Subservice::TcEnableScheduling => {
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-01-25 10:15:21 +01:00
.expect("Error sending start success");
2023-01-26 10:58:44 +01:00
2023-02-15 11:26:19 +01:00
let mut scheduler = self.tc_args.scheduler.borrow_mut();
2023-01-26 10:58:44 +01:00
scheduler.enable();
if scheduler.is_enabled() {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-01-25 10:15:21 +01:00
.expect("Error sending completion success");
} else {
2023-01-26 10:58:44 +01:00
panic!("Failed to enable scheduler");
2023-01-25 10:15:21 +01:00
}
2023-01-26 10:58:44 +01:00
}
scheduling::Subservice::TcDisableScheduling => {
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-01-26 10:58:44 +01:00
.expect("Error sending start success");
2023-01-27 14:02:46 +01:00
2023-02-15 11:26:19 +01:00
let mut scheduler = self.tc_args.scheduler.borrow_mut();
2023-01-26 10:58:44 +01:00
scheduler.disable();
if !scheduler.is_enabled() {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-01-25 10:15:21 +01:00
.expect("Error sending completion success");
} else {
2023-01-26 10:58:44 +01:00
panic!("Failed to disable scheduler");
2023-01-25 10:15:21 +01:00
}
2023-01-26 10:58:44 +01:00
}
scheduling::Subservice::TcResetScheduling => {
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-01-25 10:15:21 +01:00
.expect("Error sending start success");
2023-01-27 14:02:46 +01:00
let mut pool = self
2023-02-15 11:26:19 +01:00
.tc_args
2023-01-27 14:02:46 +01:00
.tc_source
.tc_store
.pool
.write()
.expect("Locking pool failed");
2023-02-15 11:26:19 +01:00
let mut scheduler = self.tc_args.scheduler.borrow_mut();
2023-01-27 14:02:46 +01:00
scheduler
.reset(pool.as_mut())
.expect("Error resetting TC Pool");
drop(scheduler);
2023-01-27 14:02:46 +01:00
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-01-27 14:02:46 +01:00
.expect("Error sending completion success");
2023-01-26 10:58:44 +01:00
}
scheduling::Subservice::TcInsertActivity => {
2023-02-15 11:19:23 +01:00
let start_token = self
.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
2023-02-15 11:05:32 +01:00
.expect("error sending start success");
2023-01-27 14:02:46 +01:00
let mut pool = self
2023-02-15 11:26:19 +01:00
.tc_args
2023-01-27 14:02:46 +01:00
.tc_source
.tc_store
.pool
.write()
2023-02-15 11:05:32 +01:00
.expect("locking pool failed");
2023-02-15 11:26:19 +01:00
let mut scheduler = self.tc_args.scheduler.borrow_mut();
2023-01-27 14:02:46 +01:00
scheduler
.insert_wrapped_tc::<TimeProvider>(pus_tc, pool.as_mut())
2023-02-15 11:05:32 +01:00
.expect("insertion of activity into pool failed");
drop(scheduler);
2023-01-27 14:02:46 +01:00
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.completion_success(start_token, Some(self.stamp_helper.stamp()))
2023-02-15 11:05:32 +01:00
.expect("sending completion success failed");
2023-01-26 10:58:44 +01:00
}
2023-02-14 16:00:59 +01:00
_ => {}
2023-01-25 10:15:21 +01:00
}
}
2023-02-15 11:05:32 +01:00
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 {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
2023-02-15 11:05:32 +01:00
.start_failure(
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::NOT_ENOUGH_APP_DATA,
Some(format!("expected {} bytes, found {}", 4, app_data_len).as_bytes()),
2023-02-15 11:05:32 +01:00
),
)
.expect("Sending start failure TM failed");
}
let app_data = app_data.unwrap();
2023-02-15 23:32:19 +01:00
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");
};
2023-02-15 11:05:32 +01:00
let subservice = mode::Subservice::try_from(PusPacket::subservice(pus_tc));
if let Ok(subservice) = subservice {
2023-02-15 22:41:11 +01:00
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(Request::ModeRequest(mode_request), token))
.expect("sending mode request failed");
}
};
2023-02-15 23:32:19 +01:00
let mut valid_subservice = true;
2023-02-15 11:05:32 +01:00
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 {
2023-02-15 11:19:23 +01:00
self.tm_args
.verif_reporter
.start_failure(
2023-02-15 11:05:32 +01:00
token,
FailParams::new(
2023-02-15 11:19:23 +01:00
Some(self.stamp_helper.stamp()),
&tmtc_err::NOT_ENOUGH_APP_DATA,
Some(
2023-02-15 22:41:11 +01:00
format!("expected {min_len} bytes, found {app_data_len}")
.as_bytes(),
2023-02-15 11:19:23 +01:00
),
2023-02-15 11:05:32 +01:00
),
)
.expect("Sending start failure TM failed");
}
// Should never fail after size check
2023-02-15 11:19:23 +01:00
let mode_submode = ModeAndSubmode::from_be_bytes(
app_data[4..4 + ModeAndSubmode::raw_len()]
.try_into()
.unwrap(),
)
.unwrap();
2023-02-15 22:41:11 +01:00
forward_mode_request(
target_id,
ModeRequest::SetMode(ModeCommand::new(target_id, 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(target_id));
}
Subservice::TcAnnounceMode => {
let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
forward_mode_request(target_id, ModeRequest::AnnounceMode(target_id));
}
Subservice::TcAnnounceModeRecursive => {
let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
forward_mode_request(target_id, ModeRequest::AnnounceModeRecursive(target_id));
}
_ => {
2023-02-15 23:32:19 +01:00
warn!("Can not process mode request with subservice {subservice:?}");
invalid_subservice_handler();
valid_subservice = false;
2023-02-15 11:05:32 +01:00
}
}
2023-02-15 23:32:19 +01:00
if valid_subservice {
self.tm_args
.verif_reporter
.start_success(token, Some(self.stamp_helper.stamp()))
.expect("sending start success TM failed");
}
2023-02-15 11:05:32 +01:00
} else {
2023-02-15 23:32:19 +01:00
invalid_subservice_handler();
2023-02-15 11:05:32 +01:00
}
}
2022-09-03 13:47:25 +02:00
}