365 lines
12 KiB
Rust
365 lines
12 KiB
Rust
//! ECSS PUS packet routing components.
|
|
//!
|
|
//! The routing components consist of two core components:
|
|
//! 1. [PusDistributor] component which dispatches received packets to a user-provided handler.
|
|
//! 2. [PusServiceProvider] trait which should be implemented by the user-provided PUS packet
|
|
//! handler.
|
|
//!
|
|
//! The [PusDistributor] implements the [ReceivesCcsdsTc] and [ReceivesTc] trait which allows to
|
|
//! pass raw or CCSDS packets to it. Upon receiving a packet, it performs the following steps:
|
|
//!
|
|
//! 1. It tries to identify the target Application Process Identifier (APID) based on the
|
|
//! respective CCSDS space packet header field. If that process fails, the error
|
|
//! will be reported to the provided [FsrcErrorHandler] instance.
|
|
//! 2. If a valid APID is found and matches one of the APIDs provided by
|
|
//! [ApidPacketHandler::valid_apids], it will pass the packet to the user provided
|
|
//! [ApidPacketHandler::handle_known_apid] function. If no valid APID is found, the packet
|
|
//! will be passed to the [ApidPacketHandler::handle_unknown_apid] function.
|
|
use crate::error::FsrcErrorHandler;
|
|
use crate::tmtc::{ReceivesCcsdsTc, ReceivesEcssPusTc, ReceivesTc};
|
|
use downcast_rs::Downcast;
|
|
use spacepackets::ecss::{PusError, PusPacket};
|
|
use spacepackets::tc::PusTc;
|
|
use spacepackets::{PacketError, SpHeader};
|
|
|
|
pub trait PusServiceProvider: Downcast {
|
|
type Error;
|
|
fn handle_pus_tc_packet(
|
|
&mut self,
|
|
service: u8,
|
|
header: &SpHeader,
|
|
pus_tc: &PusTc,
|
|
) -> Result<(), Self::Error>;
|
|
}
|
|
downcast_rs::impl_downcast!(PusServiceProvider assoc Error);
|
|
|
|
pub struct PusDistributor<E> {
|
|
pub service_provider: Box<dyn PusServiceProvider<Error = E>>,
|
|
pub error_handler: Box<dyn FsrcErrorHandler>,
|
|
}
|
|
|
|
pub enum PusDistribError<E> {
|
|
CustomError(E),
|
|
PusError(PusError),
|
|
}
|
|
|
|
impl<E> ReceivesTc for PusDistributor<E> {
|
|
type Error = PusDistribError<E>;
|
|
fn pass_tc(&mut self, tm_raw: &[u8]) -> Result<(), Self::Error> {
|
|
// Convert to ccsds and call pass_ccsds
|
|
match SpHeader::from_raw_slice(tm_raw) {
|
|
Ok(sp_header) => self.pass_ccsds(&sp_header, tm_raw),
|
|
Err(error) => {
|
|
// TODO: Error handling
|
|
match error {
|
|
PacketError::ToBytesSliceTooSmall(_) => {
|
|
//self.error_handler.error()
|
|
}
|
|
PacketError::FromBytesSliceTooSmall(_) => {}
|
|
PacketError::ToBytesZeroCopyError => {}
|
|
PacketError::FromBytesZeroCopyError => {}
|
|
}
|
|
Err(PusDistribError::PusError(PusError::PacketError(error)))
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<E> ReceivesCcsdsTc for PusDistributor<E> {
|
|
type Error = PusDistribError<E>;
|
|
fn pass_ccsds(&mut self, header: &SpHeader, tm_raw: &[u8]) -> Result<(), Self::Error> {
|
|
// TODO: Better error handling
|
|
let (tc, _) = match PusTc::new_from_raw_slice(tm_raw) {
|
|
Ok(tuple) => tuple,
|
|
Err(e) => return Err(PusDistribError::PusError(e)),
|
|
};
|
|
|
|
self.service_provider
|
|
.handle_pus_tc_packet(tc.service(), header, &tc)
|
|
.map_err(|e| PusDistribError::CustomError(e))
|
|
}
|
|
}
|
|
|
|
impl<E> ReceivesEcssPusTc for PusDistributor<E> {
|
|
type Error = PusDistribError<E>;
|
|
fn pass_pus_tc(&mut self, header: &SpHeader, pus_tc: &PusTc) -> Result<(), Self::Error> {
|
|
self.service_provider
|
|
.handle_pus_tc_packet(pus_tc.service(), header, pus_tc)
|
|
.map_err(|e| PusDistribError::CustomError(e))
|
|
}
|
|
}
|
|
|
|
impl<E: 'static> PusDistributor<E> {
|
|
pub fn service_provider_ref<T: PusServiceProvider<Error = E>>(&self) -> Option<&T> {
|
|
self.service_provider.downcast_ref::<T>()
|
|
}
|
|
|
|
pub fn service_provider_mut<T: PusServiceProvider<Error = E>>(&mut self) -> Option<&mut T> {
|
|
self.service_provider.downcast_mut::<T>()
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::error::SimpleStdErrorHandler;
|
|
use crate::tmtc::ccsds_distrib::tests::{
|
|
BasicApidHandlerOwnedQueue, BasicApidHandlerSharedQueue,
|
|
};
|
|
use crate::tmtc::ccsds_distrib::{ApidPacketHandler, CcsdsDistributor};
|
|
use spacepackets::tc::PusTc;
|
|
use spacepackets::CcsdsPacket;
|
|
use std::collections::VecDeque;
|
|
use std::sync::{Arc, Mutex};
|
|
|
|
struct PusHandlerSharedQueue {
|
|
pub pus_queue: Arc<Mutex<VecDeque<(u8, u16, Vec<u8>)>>>,
|
|
}
|
|
|
|
#[derive(Default)]
|
|
struct PusHandlerOwnedQueue {
|
|
pub pus_queue: VecDeque<(u8, u16, Vec<u8>)>,
|
|
}
|
|
|
|
impl PusServiceProvider for PusHandlerSharedQueue {
|
|
type Error = PusError;
|
|
fn handle_pus_tc_packet(
|
|
&mut self,
|
|
service: u8,
|
|
sp_header: &SpHeader,
|
|
pus_tc: &PusTc,
|
|
) -> Result<(), Self::Error> {
|
|
let mut vec: Vec<u8> = Vec::new();
|
|
pus_tc.append_to_vec(&mut vec)?;
|
|
Ok(self
|
|
.pus_queue
|
|
.lock()
|
|
.unwrap()
|
|
.push_back((service, sp_header.apid(), vec)))
|
|
}
|
|
}
|
|
|
|
impl PusServiceProvider for PusHandlerOwnedQueue {
|
|
type Error = PusError;
|
|
fn handle_pus_tc_packet(
|
|
&mut self,
|
|
service: u8,
|
|
sp_header: &SpHeader,
|
|
pus_tc: &PusTc,
|
|
) -> Result<(), Self::Error> {
|
|
let mut vec: Vec<u8> = Vec::new();
|
|
pus_tc.append_to_vec(&mut vec)?;
|
|
Ok(self.pus_queue.push_back((service, sp_header.apid(), vec)))
|
|
}
|
|
}
|
|
|
|
struct ApidHandlerShared {
|
|
pub pus_distrib: PusDistributor<PusError>,
|
|
handler_base: BasicApidHandlerSharedQueue,
|
|
}
|
|
|
|
macro_rules! apid_handler_impl {
|
|
() => {
|
|
type Error = PusError;
|
|
|
|
fn valid_apids(&self) -> &'static [u16] {
|
|
&[0x000, 0x002]
|
|
}
|
|
|
|
fn handle_known_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
self.handler_base.handle_known_apid(&sp_header, tc_raw);
|
|
self.pus_distrib
|
|
.pass_ccsds(&sp_header, tc_raw)
|
|
.expect("Passing PUS packet failed");
|
|
}
|
|
|
|
fn handle_unknown_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
self.handler_base.handle_unknown_apid(&sp_header, tc_raw);
|
|
}
|
|
};
|
|
}
|
|
|
|
struct ApidHandlerOwned {
|
|
pub pus_distrib: PusDistributor<PusError>,
|
|
handler_base: BasicApidHandlerOwnedQueue,
|
|
}
|
|
|
|
impl ApidPacketHandler for ApidHandlerOwned {
|
|
//apid_handler_impl!();
|
|
type Error = PusError;
|
|
|
|
fn valid_apids(&self) -> &'static [u16] {
|
|
&[0x000, 0x002]
|
|
}
|
|
|
|
fn handle_known_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
self.handler_base.handle_known_apid(&sp_header, tc_raw).ok();
|
|
match self.pus_distrib.pass_ccsds(&sp_header, tc_raw) {
|
|
Ok(_) => Ok(()),
|
|
Err(e) => match e {
|
|
PusDistribError::CustomError(_) => Ok(()),
|
|
PusDistribError::PusError(e) => Err(e),
|
|
},
|
|
}
|
|
}
|
|
|
|
fn handle_unknown_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
match self.handler_base.handle_unknown_apid(&sp_header, tc_raw) {
|
|
Ok(_) => Ok(()),
|
|
Err(e) => match e {
|
|
PusDistribError::CustomError(_) => Ok(()),
|
|
PusDistribError::PusError(e) => Err(e),
|
|
},
|
|
}
|
|
}
|
|
}
|
|
|
|
impl ApidPacketHandler for ApidHandlerShared {
|
|
//apid_handler_impl!();
|
|
type Error = PusError;
|
|
|
|
fn valid_apids(&self) -> &'static [u16] {
|
|
&[0x000, 0x002]
|
|
}
|
|
|
|
fn handle_known_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
self.handler_base.handle_known_apid(&sp_header, tc_raw).ok();
|
|
match self.pus_distrib.pass_ccsds(&sp_header, tc_raw) {
|
|
Ok(_) => Ok(()),
|
|
Err(e) => match e {
|
|
PusDistribError::CustomError(_) => Ok(()),
|
|
PusDistribError::PusError(e) => Err(e),
|
|
},
|
|
}
|
|
}
|
|
|
|
fn handle_unknown_apid(
|
|
&mut self,
|
|
sp_header: &SpHeader,
|
|
tc_raw: &[u8],
|
|
) -> Result<(), Self::Error> {
|
|
Ok(self
|
|
.handler_base
|
|
.handle_unknown_apid(&sp_header, tc_raw)
|
|
.ok()
|
|
.unwrap())
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_pus_distribution() {
|
|
let known_packet_queue = Arc::new(Mutex::default());
|
|
let unknown_packet_queue = Arc::new(Mutex::default());
|
|
let pus_queue = Arc::new(Mutex::default());
|
|
let pus_handler = PusHandlerSharedQueue {
|
|
pus_queue: pus_queue.clone(),
|
|
};
|
|
let handler_base = BasicApidHandlerSharedQueue {
|
|
known_packet_queue: known_packet_queue.clone(),
|
|
unknown_packet_queue: unknown_packet_queue.clone(),
|
|
};
|
|
|
|
let error_handler = SimpleStdErrorHandler {};
|
|
let pus_distrib = PusDistributor {
|
|
service_provider: Box::new(pus_handler),
|
|
error_handler: Box::new(error_handler),
|
|
};
|
|
|
|
let apid_handler = ApidHandlerShared {
|
|
pus_distrib,
|
|
handler_base,
|
|
};
|
|
|
|
let mut ccsds_distrib =
|
|
CcsdsDistributor::new(Box::new(apid_handler), Box::new(error_handler));
|
|
let mut sph = SpHeader::tc(0x002, 0x34, 0).unwrap();
|
|
let pus_tc = PusTc::new_simple(&mut sph, 17, 1, None, true);
|
|
let mut test_buf: [u8; 32] = [0; 32];
|
|
let size = pus_tc
|
|
.write_to(test_buf.as_mut_slice())
|
|
.expect("Error writing TC to buffer");
|
|
let tc_slice = &test_buf[0..size];
|
|
ccsds_distrib
|
|
.pass_tc(tc_slice)
|
|
.expect("Passing TC slice failed");
|
|
let recvd_ccsds = known_packet_queue.lock().unwrap().pop_front();
|
|
assert!(unknown_packet_queue.lock().unwrap().is_empty());
|
|
assert!(recvd_ccsds.is_some());
|
|
let (apid, packet) = recvd_ccsds.unwrap();
|
|
assert_eq!(apid, 0x002);
|
|
assert_eq!(packet.as_slice(), tc_slice);
|
|
let recvd_pus = pus_queue.lock().unwrap().pop_front();
|
|
assert!(recvd_pus.is_some());
|
|
let (service, apid, tc_raw) = recvd_pus.unwrap();
|
|
assert_eq!(service, 17);
|
|
assert_eq!(apid, 0x002);
|
|
assert_eq!(tc_raw, tc_slice);
|
|
}
|
|
|
|
#[test]
|
|
fn test_as_any_cast() {
|
|
let pus_handler = PusHandlerOwnedQueue::default();
|
|
let handler_base = BasicApidHandlerOwnedQueue::default();
|
|
let error_handler = SimpleStdErrorHandler {};
|
|
let pus_distrib = PusDistributor {
|
|
service_provider: Box::new(pus_handler),
|
|
error_handler: Box::new(error_handler),
|
|
};
|
|
|
|
let apid_handler = ApidHandlerOwned {
|
|
pus_distrib,
|
|
handler_base,
|
|
};
|
|
let mut ccsds_distrib =
|
|
CcsdsDistributor::new(Box::new(apid_handler), Box::new(error_handler));
|
|
|
|
let mut sph = SpHeader::tc(0x002, 0x34, 0).unwrap();
|
|
let pus_tc = PusTc::new_simple(&mut sph, 17, 1, None, true);
|
|
let mut test_buf: [u8; 32] = [0; 32];
|
|
let size = pus_tc
|
|
.write_to(test_buf.as_mut_slice())
|
|
.expect("Error writing TC to buffer");
|
|
let tc_slice = &test_buf[0..size];
|
|
ccsds_distrib
|
|
.pass_tc(tc_slice)
|
|
.expect("Passing TC slice failed");
|
|
|
|
let apid_handler_casted_back: &mut ApidHandlerOwned = ccsds_distrib
|
|
.apid_handler_mut()
|
|
.expect("Cast to concrete type ApidHandler failed");
|
|
assert!(!apid_handler_casted_back
|
|
.handler_base
|
|
.known_packet_queue
|
|
.is_empty());
|
|
let handler_casted_back: &mut PusHandlerOwnedQueue = apid_handler_casted_back
|
|
.pus_distrib
|
|
.service_provider_mut()
|
|
.expect("Cast to concrete type PusHandlerOwnedQueue failed");
|
|
assert!(!handler_casted_back.pus_queue.is_empty());
|
|
let (service, apid, packet_raw) = handler_casted_back.pus_queue.pop_front().unwrap();
|
|
assert_eq!(service, 17);
|
|
assert_eq!(apid, 0x002);
|
|
assert_eq!(packet_raw.as_slice(), tc_slice);
|
|
}
|
|
}
|