2023-07-04 22:26:41 +02:00
|
|
|
use crate::pus::{
|
|
|
|
AcceptedTc, PartialPusHandlingError, PusPacketHandlerResult, PusPacketHandlingError,
|
|
|
|
PusServiceBase,
|
|
|
|
};
|
2023-07-04 18:51:54 +02:00
|
|
|
use delegate::delegate;
|
2023-07-04 21:13:26 +02:00
|
|
|
use log::{error, info, warn};
|
2023-07-04 18:51:54 +02:00
|
|
|
use satrs_core::events::EventU32;
|
|
|
|
use satrs_core::params::Params;
|
2023-07-04 21:13:26 +02:00
|
|
|
use satrs_core::pool::{SharedPool, StoreAddr, StoreError};
|
2023-07-04 15:17:43 +02:00
|
|
|
use satrs_core::pus::verification::{
|
2023-07-04 21:13:26 +02:00
|
|
|
FailParams, StdVerifReporterWithSender, TcStateAccepted, TcStateStarted,
|
|
|
|
VerificationOrSendErrorWithToken, VerificationToken,
|
2023-07-04 15:17:43 +02:00
|
|
|
};
|
|
|
|
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
|
2023-07-04 21:13:26 +02:00
|
|
|
use satrs_core::spacepackets::ecss::{PusError, PusPacket};
|
2023-07-04 15:17:43 +02:00
|
|
|
use satrs_core::spacepackets::tc::PusTc;
|
|
|
|
use satrs_core::spacepackets::time::cds::TimeProvider;
|
2023-07-04 21:13:26 +02:00
|
|
|
use satrs_core::spacepackets::time::{StdTimestampError, TimeWriter};
|
2023-07-04 22:26:41 +02:00
|
|
|
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
|
|
|
|
use satrs_core::spacepackets::SpHeader;
|
2023-07-04 15:17:43 +02:00
|
|
|
use satrs_core::tmtc::tm_helper::{PusTmWithCdsShortHelper, SharedTmStore};
|
2023-07-04 21:13:26 +02:00
|
|
|
use satrs_example::{tmtc_err, TEST_EVENT};
|
2023-07-04 15:17:43 +02:00
|
|
|
use std::sync::mpsc::{Receiver, Sender, TryRecvError};
|
2023-07-04 21:13:26 +02:00
|
|
|
use std::thread;
|
|
|
|
use std::time::Duration;
|
2023-07-03 18:22:08 +02:00
|
|
|
|
2023-07-04 21:13:26 +02:00
|
|
|
pub struct Service17CustomWrapper {
|
|
|
|
pub pus17_handler: PusService17TestHandler,
|
|
|
|
pub test_srv_event_sender: Sender<(EventU32, Option<Params>)>,
|
2023-07-03 18:22:08 +02:00
|
|
|
}
|
|
|
|
|
2023-07-04 21:13:26 +02:00
|
|
|
impl Service17CustomWrapper {
|
|
|
|
pub fn perform_operation(&mut self) -> bool {
|
|
|
|
let mut handled_pings = 0;
|
|
|
|
let res = self.pus17_handler.handle_next_packet();
|
|
|
|
if res.is_err() {
|
|
|
|
warn!("PUS17 handler failed with error {:?}", res.unwrap_err());
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
match res.unwrap() {
|
|
|
|
PusPacketHandlerResult::RequestHandled => {
|
|
|
|
info!("Received PUS ping command TC[17,1]");
|
|
|
|
info!("Sent ping reply PUS TM[17,2]");
|
|
|
|
handled_pings += 1;
|
|
|
|
}
|
2023-07-04 22:26:41 +02:00
|
|
|
PusPacketHandlerResult::RequestHandledPartialSuccess(partial_err) => {
|
|
|
|
warn!(
|
|
|
|
"Handled PUS ping command with partial success: {:?}",
|
|
|
|
partial_err
|
|
|
|
);
|
|
|
|
handled_pings += 1;
|
|
|
|
}
|
2023-07-04 21:13:26 +02:00
|
|
|
PusPacketHandlerResult::CustomSubservice(token) => {
|
|
|
|
let (buf, _) = self.pus17_handler.pus_tc_buf();
|
|
|
|
let (tc, size) = PusTc::from_bytes(buf).unwrap();
|
|
|
|
let time_stamper = TimeProvider::from_now_with_u16_days().unwrap();
|
|
|
|
let mut stamp_buf: [u8; 7] = [0; 7];
|
|
|
|
time_stamper.write_to_bytes(&mut stamp_buf).unwrap();
|
|
|
|
if tc.subservice() == 128 {
|
|
|
|
info!("Generating test event");
|
|
|
|
self.test_srv_event_sender
|
|
|
|
.send((TEST_EVENT.into(), None))
|
|
|
|
.expect("Sending test event failed");
|
|
|
|
let start_token = self
|
|
|
|
.pus17_handler
|
|
|
|
.verification_handler()
|
|
|
|
.start_success(token, Some(&stamp_buf))
|
|
|
|
.expect("Error sending start success");
|
|
|
|
self.pus17_handler
|
|
|
|
.verification_handler()
|
|
|
|
.completion_success(start_token, Some(&stamp_buf))
|
|
|
|
.expect("Error sending completion success");
|
|
|
|
} else {
|
|
|
|
let fail_data = [tc.subservice()];
|
|
|
|
self.pus17_handler
|
|
|
|
.verification_handler()
|
|
|
|
.start_failure(
|
|
|
|
token,
|
|
|
|
FailParams::new(
|
|
|
|
Some(&stamp_buf),
|
|
|
|
&tmtc_err::INVALID_PUS_SUBSERVICE,
|
|
|
|
Some(&fail_data),
|
|
|
|
),
|
|
|
|
)
|
|
|
|
.expect("Sending start failure verification failed");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
PusPacketHandlerResult::Empty => {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
true
|
|
|
|
}
|
2023-07-04 18:51:54 +02:00
|
|
|
}
|
|
|
|
|
2023-07-05 09:37:34 +02:00
|
|
|
pub trait PusServiceHandler {
|
|
|
|
fn psb(&mut self) -> &mut PusServiceBase;
|
|
|
|
fn handle_one_tc(
|
|
|
|
&mut self,
|
|
|
|
addr: StoreAddr,
|
|
|
|
token: VerificationToken<TcStateAccepted>,
|
|
|
|
) -> Result<PusPacketHandlerResult, PusPacketHandlingError>;
|
|
|
|
fn handle_next_packet(&mut self) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
|
|
|
return match self.psb().tc_rx.try_recv() {
|
|
|
|
Ok((addr, token)) => self.handle_one_tc(addr, token),
|
|
|
|
Err(e) => match e {
|
|
|
|
TryRecvError::Empty => Ok(PusPacketHandlerResult::Empty),
|
|
|
|
TryRecvError::Disconnected => Err(PusPacketHandlingError::QueueDisconnected),
|
|
|
|
},
|
|
|
|
};
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
2023-07-04 21:13:26 +02:00
|
|
|
pub struct PusService17TestHandler {
|
|
|
|
psb: PusServiceBase,
|
2023-07-04 20:01:06 +02:00
|
|
|
}
|
|
|
|
|
2023-07-04 18:51:54 +02:00
|
|
|
impl PusService17TestHandler {
|
|
|
|
pub fn new(
|
|
|
|
receiver: Receiver<AcceptedTc>,
|
|
|
|
tc_pool: SharedPool,
|
|
|
|
tm_tx: Sender<StoreAddr>,
|
|
|
|
tm_store: SharedTmStore,
|
2023-07-04 22:26:41 +02:00
|
|
|
tm_apid: u16,
|
2023-07-04 18:51:54 +02:00
|
|
|
verification_handler: StdVerifReporterWithSender,
|
|
|
|
) -> Self {
|
2023-07-04 15:17:43 +02:00
|
|
|
Self {
|
2023-07-04 18:51:54 +02:00
|
|
|
psb: PusServiceBase::new(
|
|
|
|
receiver,
|
|
|
|
tc_pool,
|
|
|
|
tm_tx,
|
|
|
|
tm_store,
|
2023-07-04 22:26:41 +02:00
|
|
|
tm_apid,
|
2023-07-04 18:51:54 +02:00
|
|
|
verification_handler,
|
|
|
|
),
|
2023-07-04 15:17:43 +02:00
|
|
|
}
|
|
|
|
}
|
2023-07-04 18:51:54 +02:00
|
|
|
|
2023-07-04 20:01:06 +02:00
|
|
|
pub fn verification_handler(&mut self) -> &mut StdVerifReporterWithSender {
|
|
|
|
&mut self.psb.verification_handler
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn pus_tc_buf(&self) -> (&[u8], usize) {
|
|
|
|
(&self.psb.pus_buf, self.psb.pus_size)
|
|
|
|
}
|
2023-07-05 09:37:34 +02:00
|
|
|
}
|
2023-07-04 20:01:06 +02:00
|
|
|
|
2023-07-05 09:37:34 +02:00
|
|
|
impl PusServiceHandler for PusService17TestHandler {
|
|
|
|
fn psb(&mut self) -> &mut PusServiceBase {
|
|
|
|
&mut self.psb
|
2023-07-04 20:01:06 +02:00
|
|
|
}
|
|
|
|
|
2023-07-05 09:37:34 +02:00
|
|
|
fn handle_one_tc(
|
2023-07-04 20:01:06 +02:00
|
|
|
&mut self,
|
|
|
|
addr: StoreAddr,
|
|
|
|
token: VerificationToken<TcStateAccepted>,
|
2023-07-04 22:26:41 +02:00
|
|
|
) -> Result<PusPacketHandlerResult, PusPacketHandlingError> {
|
|
|
|
let mut partial_result = None;
|
2023-07-04 15:17:43 +02:00
|
|
|
{
|
|
|
|
// Keep locked section as short as possible.
|
2023-07-04 21:13:26 +02:00
|
|
|
let mut tc_pool = self
|
|
|
|
.psb
|
|
|
|
.tc_store
|
|
|
|
.write()
|
|
|
|
.map_err(|e| PusPacketHandlingError::RwGuardError(format!("{e}")))?;
|
2023-07-04 15:17:43 +02:00
|
|
|
let tc_guard = tc_pool.read_with_guard(addr);
|
2023-07-04 22:26:41 +02:00
|
|
|
let tc_raw = tc_guard.read()?;
|
2023-07-04 18:51:54 +02:00
|
|
|
self.psb.pus_buf[0..tc_raw.len()].copy_from_slice(tc_raw);
|
2023-07-04 15:17:43 +02:00
|
|
|
}
|
2023-07-04 22:26:41 +02:00
|
|
|
let mut partial_error = None;
|
2023-07-04 21:13:26 +02:00
|
|
|
let (tc, tc_size) = PusTc::from_bytes(&self.psb.pus_buf)?;
|
|
|
|
if tc.service() != 17 {
|
|
|
|
return Err(PusPacketHandlingError::WrongService(tc.service()));
|
|
|
|
}
|
2023-07-04 18:51:54 +02:00
|
|
|
if tc.subservice() == 1 {
|
2023-07-04 22:26:41 +02:00
|
|
|
partial_result = self.psb.update_stamp().err();
|
2023-07-04 21:13:26 +02:00
|
|
|
let result = self
|
2023-07-04 18:51:54 +02:00
|
|
|
.psb
|
2023-07-04 15:17:43 +02:00
|
|
|
.verification_handler
|
2023-07-04 22:26:41 +02:00
|
|
|
.start_success(token, Some(&self.psb.stamp_buf))
|
|
|
|
.map_err(|e| PartialPusHandlingError::VerificationError);
|
2023-07-04 21:13:26 +02:00
|
|
|
let start_token = if result.is_err() {
|
2023-07-04 22:26:41 +02:00
|
|
|
partial_error = Some(result.unwrap_err());
|
2023-07-04 21:13:26 +02:00
|
|
|
None
|
|
|
|
} else {
|
|
|
|
Some(result.unwrap())
|
|
|
|
};
|
2023-07-04 15:17:43 +02:00
|
|
|
// Sequence count will be handled centrally in TM funnel.
|
2023-07-04 22:26:41 +02:00
|
|
|
let mut reply_header = SpHeader::tm_unseg(self.psb.tm_apid, 0, 0).unwrap();
|
|
|
|
let tc_header = PusTmSecondaryHeader::new_simple(17, 2, &self.psb.stamp_buf);
|
|
|
|
let ping_reply = PusTm::new(&mut reply_header, tc_header, None, true);
|
2023-07-04 18:51:54 +02:00
|
|
|
let addr = self.psb.tm_store.add_pus_tm(&ping_reply);
|
2023-07-04 22:26:41 +02:00
|
|
|
if let Err(e) = self
|
|
|
|
.psb
|
2023-07-04 18:51:54 +02:00
|
|
|
.tm_tx
|
2023-07-04 15:17:43 +02:00
|
|
|
.send(addr)
|
2023-07-04 22:26:41 +02:00
|
|
|
.map_err(|e| PartialPusHandlingError::TmSendError(format!("{e}")))
|
|
|
|
{
|
|
|
|
partial_error = Some(e);
|
|
|
|
}
|
2023-07-04 21:13:26 +02:00
|
|
|
if let Some(start_token) = start_token {
|
|
|
|
if self
|
|
|
|
.psb
|
|
|
|
.verification_handler
|
|
|
|
.completion_success(start_token, Some(&self.psb.stamp_buf))
|
|
|
|
.is_err()
|
|
|
|
{
|
2023-07-04 22:26:41 +02:00
|
|
|
partial_error = Some(PartialPusHandlingError::VerificationError)
|
2023-07-04 21:13:26 +02:00
|
|
|
}
|
|
|
|
}
|
2023-07-04 22:26:41 +02:00
|
|
|
if partial_error.is_some() {
|
|
|
|
return Ok(PusPacketHandlerResult::RequestHandledPartialSuccess(
|
|
|
|
partial_error.unwrap(),
|
|
|
|
));
|
|
|
|
}
|
|
|
|
return Ok(PusPacketHandlerResult::RequestHandled);
|
2023-07-04 15:17:43 +02:00
|
|
|
}
|
2023-07-04 22:26:41 +02:00
|
|
|
Ok(PusPacketHandlerResult::CustomSubservice(token))
|
2023-07-04 15:17:43 +02:00
|
|
|
}
|
2023-07-05 09:37:34 +02:00
|
|
|
}
|