added better generic error handling

Cargo.lock

@@ -201,6 +201,12 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "downcast-rs"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ea835d29036a4087793836fa931b08837ad5e957da9e23886b29586fb9b6650"
+
 [[package]]
 name = "embedded-hal"
 version = "0.2.7"
@@ -216,8 +222,9 @@ name = "fsrc-core"
 version = "0.1.0"
 dependencies = [
  "bus",
+ "downcast-rs",
  "num",
- "postcard 1.0.1",
+ "postcard",
  "serde",
  "spacepackets",
  "thiserror",
@@ -429,17 +436,6 @@ dependencies = [
  "winapi",
 ]
 
-[[package]]
-name = "postcard"
-version = "0.7.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a25c0b0ae06fcffe600ad392aabfa535696c8973f2253d9ac83171924c58a858"
-dependencies = [
- "heapless",
- "postcard-cobs",
- "serde",
-]
-
 [[package]]
 name = "postcard"
 version = "1.0.1"
@@ -451,12 +447,6 @@ dependencies = [
  "serde",
 ]
 
-[[package]]
-name = "postcard-cobs"
-version = "0.1.5-pre"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7c68cb38ed13fd7bc9dd5db8f165b7c8d9c1a315104083a2b10f11354c2af97f"
-
 [[package]]
 name = "proc-macro2"
 version = "1.0.39"
@@ -598,7 +588,7 @@ dependencies = [
  "crc",
  "delegate",
  "num",
- "postcard 0.7.3",
+ "postcard",
  "serde",
  "zerocopy",
 ]

fsrc-core/Cargo.toml

@@ -9,7 +9,14 @@ edition = "2021"
 thiserror = "1.0"
 bus = "2.2.3"
 num = "0.4"
-spacepackets = { path = "../spacepackets"}
+downcast-rs = { version = "1.2.0" }
+
+[dependencies.spacepackets]
+path = "../spacepackets"
+
+#[dependencies.downcast-rs]
+#version = "1.2.0"
+#default-features = false
 
 [dev-dependencies]
 postcard = { version = "1.0.1", features = ["use-std"] }
@@ -18,4 +25,4 @@ zerocopy = "0.6.1"
 
 [features]
 default = ["use_std"]
-use_std = []
+use_std = ["downcast-rs/std"]

fsrc-core/src/any.rs

@@ -1,8 +0,0 @@
-use std::any::Any;
-
-/// This trait encapsulates being able to cast a trait object to its original concrete type
-/// TODO: Add example code and maybe write derive macro because this code is always the same
-pub trait AsAny {
-    fn as_any(&self) -> &dyn Any;
-    fn as_mut_any(&mut self) -> &mut dyn Any;
-}

fsrc-core/src/error.rs

@@ -35,7 +35,7 @@ impl FsrcErrorRaw {
     }
 }
 
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Default)]
 pub struct SimpleStdErrorHandler {}
 
 #[cfg(feature = "use_std")]

fsrc-core/src/hal/host/udp_server.rs

@@ -1,18 +1,24 @@
+use crate::hal::host::udp_server::ReceiveResult::{IoError, ReceiverError};
 use crate::tmtc::ReceivesTc;
 use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
 use std::vec::Vec;
 
-pub struct UdpTmtcServer {
+pub struct UdpTmtcServer<E> {
     socket: UdpSocket,
     recv_buf: Vec<u8>,
-    tc_receiver: Box<dyn ReceivesTc>,
+    tc_receiver: Box<dyn ReceivesTc<Error = E>>,
 }
 
-impl UdpTmtcServer {
+pub enum ReceiveResult<E> {
-    pub fn new<A: ToSocketAddrs, E>(
+    IoError(std::io::Error),
+    ReceiverError(E),
+}
+
+impl<E> UdpTmtcServer<E> {
+    pub fn new<A: ToSocketAddrs>(
         addr: A,
         max_recv_size: usize,
-        tc_receiver: Box<dyn ReceivesTc>,
+        tc_receiver: Box<dyn ReceivesTc<Error = E>>,
     ) -> Result<Self, std::io::Error> {
         Ok(Self {
             socket: UdpSocket::bind(addr)?,
@@ -21,9 +27,14 @@ impl UdpTmtcServer {
         })
     }
 
-    pub fn recv_tc(&mut self) -> Result<(usize, SocketAddr), std::io::Error> {
+    pub fn recv_tc(&mut self) -> Result<(usize, SocketAddr), ReceiveResult<E>> {
-        let res = self.socket.recv_from(&mut self.recv_buf)?;
+        let res = self
-        self.tc_receiver.pass_tc(&self.recv_buf[0..res.0]);
+            .socket
+            .recv_from(&mut self.recv_buf)
+            .map_err(|e| IoError(e))?;
+        self.tc_receiver
+            .pass_tc(&self.recv_buf[0..res.0])
+            .map_err(|e| ReceiverError(e))?;
         Ok(res)
     }
 }

fsrc-core/src/lib.rs

@@ -1,5 +1,4 @@
 //! # Core components of the Flight Software Rust Crate (FSRC) collection
-pub mod any;
 pub mod error;
 pub mod event_man;
 pub mod events;
@@ -8,3 +7,5 @@ pub mod hal;
 pub mod objects;
 pub mod pool;
 pub mod tmtc;
+
+extern crate downcast_rs;

fsrc-core/src/tmtc/ccsds_distrib.rs

@@ -1,34 +1,138 @@
-use crate::any::AsAny;
+//! CCSDS packet routing components.
+//!
+//! The routing components consist of two core components:
+//!  1. [CcsdsDistributor] component which dispatches received packets to a user-provided handler
+//!  2. [ApidPacketHandler] trait which should be implemented by the user-provided packet handler.
+//!
+//! The [CcsdsDistributor] 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.
+//!
+//! # Example
+//!
+//! ```rust
+//! use fsrc_core::error::SimpleStdErrorHandler;
+//! use fsrc_core::tmtc::ccsds_distrib::{ApidPacketHandler, CcsdsDistributor};
+//! use fsrc_core::tmtc::ReceivesTc;
+//! use spacepackets::{CcsdsPacket, SpHeader};
+//! use spacepackets::tc::PusTc;
+//!
+//! struct ConcreteApidHandler {
+//!     custom_field: u32
+//! }
+//!
+//! impl ApidPacketHandler for ConcreteApidHandler {
+//!     type Error = ();
+//!     fn valid_apids(&self) -> &'static [u16] { &[0x002] }
+//!     fn handle_known_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
+//!         assert_eq!(sp_header.apid(), 0x002);
+//!         assert_eq!(tc_raw.len(), 13);
+//!         Ok(())
+//!     }
+//!     fn handle_unknown_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
+//!         assert_eq!(sp_header.apid(), 0x003);
+//!         assert_eq!(tc_raw.len(), 13);
+//!         Ok(())
+//!     }
+//! }
+//!
+//! let apid_handler = ConcreteApidHandler {
+//!     custom_field: 0x42
+//! };
+//! let err_handler = SimpleStdErrorHandler::default();
+//! let mut ccsds_distributor = CcsdsDistributor::new(Box::new(apid_handler), Box::new(err_handler));
+//!
+//! // Create and pass PUS telecommand with a valid APID
+//! let mut space_packet_header = SpHeader::tc(0x002, 0x34, 0).unwrap();
+//! let mut pus_tc = PusTc::new_simple(&mut space_packet_header, 17, 1, None, true);
+//! let mut test_buf: [u8; 32] = [0; 32];
+//! let mut size = pus_tc
+//!     .write_to(test_buf.as_mut_slice())
+//!     .expect("Error writing TC to buffer");
+//! let tc_slice = &test_buf[0..size];
+//! ccsds_distributor.pass_tc(&tc_slice).expect("Passing TC slice failed");
+//!
+//! // Now pass a packet with an unknown APID to the distributor
+//! pus_tc.set_apid(0x003);
+//! size = pus_tc
+//!     .write_to(test_buf.as_mut_slice())
+//!     .expect("Error writing TC to buffer");
+//! let tc_slice = &test_buf[0..size];
+//! ccsds_distributor.pass_tc(&tc_slice).expect("Passing TC slice failed");
+//!
+//! // User helper function to retrieve concrete class
+//! let concrete_handler_ref: &ConcreteApidHandler = ccsds_distributor
+//!     .apid_handler_ref()
+//!     .expect("Casting back to concrete type failed");
+//! assert_eq!(concrete_handler_ref.custom_field, 0x42);
+//! ```
 use crate::error::FsrcErrorHandler;
-use crate::tmtc::{ReceivesTc, FROM_BYTES_SLICE_TOO_SMALL_ERROR, FROM_BYTES_ZEROCOPY_ERROR};
+use crate::tmtc::{
+    ReceivesCcsdsTc, ReceivesTc, FROM_BYTES_SLICE_TOO_SMALL_ERROR, FROM_BYTES_ZEROCOPY_ERROR,
+};
+use downcast_rs::Downcast;
 use spacepackets::{CcsdsPacket, PacketError, SpHeader};
 
-pub trait ApidPacketHandler: AsAny {
+/// Generic trait for a handler or dispatcher object handling CCSDS packets.
+///
+/// Users should implement this trait on their custom CCSDS packet handler and then pass a boxed
+/// instance of this handler to the [CcsdsDistributor]. The distributor will use the trait
+/// interface to dispatch received packets to the user based on the Application Process Identifier
+/// (APID) field of the CCSDS packet.
+///
+/// This trait automatically implements the [downcast_rs::Downcast] to allow a more convenient API
+/// to cast trait objects back to their concrete type after the handler was passed to the
+/// distributor.
+pub trait ApidPacketHandler: Downcast {
+    type Error;
+
     fn valid_apids(&self) -> &'static [u16];
-    fn handle_known_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]);
+    fn handle_known_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8])
-    fn handle_unknown_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]);
+        -> Result<(), Self::Error>;
+    fn handle_unknown_apid(
+        &mut self,
+        sp_header: &SpHeader,
+        tc_raw: &[u8],
+    ) -> Result<(), Self::Error>;
 }
 
-pub struct CcsdsDistributor {
+downcast_rs::impl_downcast!(ApidPacketHandler assoc Error);
-    pub apid_handler: Box<dyn ApidPacketHandler>,
+
-    error_handler: Box<dyn FsrcErrorHandler>,
+/// The CCSDS distributor dispatches received CCSDS packets to a user provided packet handler.
+pub struct CcsdsDistributor<E> {
+    /// User provided APID handler stored as a generic trait object.
+    /// It can be cast back to the original concrete type using the [Self::apid_handler_ref] or
+    /// the [Self::apid_handler_mut] method.
+    pub apid_handler: Box<dyn ApidPacketHandler<Error = E>>,
+    pub error_handler: Box<dyn FsrcErrorHandler>,
 }
 
-impl CcsdsDistributor {
+#[derive(Debug)]
-    pub fn new(
+pub enum CcsdsError<E> {
-        apid_handler: Box<dyn ApidPacketHandler>,
+    CustomError(E),
-        error_handler: Box<dyn FsrcErrorHandler>,
+    PacketError(PacketError),
-    ) -> Self {
+}
-        CcsdsDistributor {
+
-            apid_handler,
+impl<E: 'static> ReceivesCcsdsTc for CcsdsDistributor<E> {
-            error_handler,
+    type Error = CcsdsError<E>;
-        }
+
+    fn pass_ccsds(&mut self, header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
+        self.dispatch_ccsds(header, tc_raw)
     }
 }
 
-impl ReceivesTc for CcsdsDistributor {
+impl<E: 'static> ReceivesTc for CcsdsDistributor<E> {
-    fn pass_tc(&mut self, tm_raw: &[u8]) {
+    type Error = CcsdsError<E>;
-        let sp_header = match SpHeader::from_raw_slice(tm_raw) {
+
+    fn pass_tc(&mut self, tc_raw: &[u8]) -> Result<(), Self::Error> {
+        let sp_header = match SpHeader::from_raw_slice(tc_raw) {
             Ok(header) => header,
             Err(e) => {
                 match e {
@@ -46,17 +150,48 @@ impl ReceivesTc for CcsdsDistributor {
                         // TODO: Unexpected error
                     }
                 }
-                return;
+                return Err(CcsdsError::PacketError(e));
             }
         };
+        self.dispatch_ccsds(&sp_header, tc_raw)
+    }
+}
+
+impl<E: 'static> CcsdsDistributor<E> {
+    pub fn new(
+        apid_handler: Box<dyn ApidPacketHandler<Error = E>>,
+        error_handler: Box<dyn FsrcErrorHandler>,
+    ) -> Self {
+        CcsdsDistributor {
+            apid_handler,
+            error_handler,
+        }
+    }
+
+    /// This function can be used to retrieve the concrete instance of the APID handler
+    /// after it was passed to the distributor.
+    pub fn apid_handler_ref<T: ApidPacketHandler<Error = E>>(&self) -> Option<&T> {
+        self.apid_handler.downcast_ref::<T>()
+    }
+
+    pub fn apid_handler_mut<T: ApidPacketHandler<Error = E>>(&mut self) -> Option<&mut T> {
+        self.apid_handler.downcast_mut::<T>()
+    }
+
+    fn dispatch_ccsds(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), CcsdsError<E>> {
         let apid = sp_header.apid();
         let valid_apids = self.apid_handler.valid_apids();
         for &valid_apid in valid_apids {
             if valid_apid == apid {
-                return self.apid_handler.handle_known_apid(&sp_header, tm_raw);
+                return self
+                    .apid_handler
+                    .handle_known_apid(sp_header, tc_raw)
+                    .map_err(|e| CcsdsError::CustomError(e));
             }
         }
-        self.apid_handler.handle_unknown_apid(&sp_header, tm_raw);
+        self.apid_handler
+            .handle_unknown_apid(sp_header, tc_raw)
+            .map_err(|e| CcsdsError::CustomError(e))
     }
 }
 
@@ -65,9 +200,9 @@ pub(crate) mod tests {
     use super::*;
     use crate::error::SimpleStdErrorHandler;
     use crate::tmtc::ccsds_distrib::{ApidPacketHandler, CcsdsDistributor};
+    use crate::tmtc::pus_distrib::PusDistribError;
     use spacepackets::tc::PusTc;
     use spacepackets::CcsdsPacket;
-    use std::any::Any;
     use std::collections::VecDeque;
     use std::sync::{Arc, Mutex};
 
@@ -82,65 +217,66 @@ pub(crate) mod tests {
         pub unknown_packet_queue: VecDeque<(u16, Vec<u8>)>,
     }
 
-    impl AsAny for BasicApidHandlerSharedQueue {
-        fn as_any(&self) -> &dyn Any {
-            self
-        }
-
-        fn as_mut_any(&mut self) -> &mut dyn Any {
-            self
-        }
-    }
-
     impl ApidPacketHandler for BasicApidHandlerSharedQueue {
+        type Error = ();
         fn valid_apids(&self) -> &'static [u16] {
             &[0x000, 0x002]
         }
 
-        fn handle_known_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) {
+        fn handle_known_apid(
+            &mut self,
+            sp_header: &SpHeader,
+            tc_raw: &[u8],
+        ) -> Result<(), Self::Error> {
             let mut vec = Vec::new();
             vec.extend_from_slice(tc_raw);
-            self.known_packet_queue
+            Ok(self
+                .known_packet_queue
                 .lock()
                 .unwrap()
-                .push_back((sp_header.apid(), vec));
+                .push_back((sp_header.apid(), vec)))
         }
 
-        fn handle_unknown_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) {
+        fn handle_unknown_apid(
+            &mut self,
+            sp_header: &SpHeader,
+            tc_raw: &[u8],
+        ) -> Result<(), Self::Error> {
             let mut vec = Vec::new();
             vec.extend_from_slice(tc_raw);
-            self.unknown_packet_queue
+            Ok(self
+                .unknown_packet_queue
                 .lock()
                 .unwrap()
-                .push_back((sp_header.apid(), vec));
+                .push_back((sp_header.apid(), vec)))
-        }
-    }
-
-    impl AsAny for BasicApidHandlerOwnedQueue {
-        fn as_any(&self) -> &dyn Any {
-            self
-        }
-
-        fn as_mut_any(&mut self) -> &mut dyn Any {
-            self
         }
     }
 
     impl ApidPacketHandler for BasicApidHandlerOwnedQueue {
+        type Error = PusDistribError<()>;
+
         fn valid_apids(&self) -> &'static [u16] {
             &[0x000, 0x002]
         }
 
-        fn handle_known_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) {
+        fn handle_known_apid(
+            &mut self,
+            sp_header: &SpHeader,
+            tc_raw: &[u8],
+        ) -> Result<(), Self::Error> {
             let mut vec = Vec::new();
             vec.extend_from_slice(tc_raw);
-            self.known_packet_queue.push_back((sp_header.apid(), vec));
+            Ok(self.known_packet_queue.push_back((sp_header.apid(), vec)))
         }
 
-        fn handle_unknown_apid(&mut self, sp_header: &SpHeader, tc_raw: &[u8]) {
+        fn handle_unknown_apid(
+            &mut self,
+            sp_header: &SpHeader,
+            tc_raw: &[u8],
+        ) -> Result<(), Self::Error> {
             let mut vec = Vec::new();
             vec.extend_from_slice(tc_raw);
-            self.unknown_packet_queue.push_back((sp_header.apid(), vec));
+            Ok(self.unknown_packet_queue.push_back((sp_header.apid(), vec)))
         }
     }
 
@@ -161,7 +297,7 @@ pub(crate) mod tests {
         pus_tc
             .write_to(test_buf.as_mut_slice())
             .expect("Error writing TC to buffer");
-        ccsds_distrib.pass_tc(&test_buf);
+        ccsds_distrib.pass_tc(&test_buf).expect("Passing TC failed");
         let recvd = known_packet_queue.lock().unwrap().pop_front();
         assert!(unknown_packet_queue.lock().unwrap().is_empty());
         assert!(recvd.is_some());
@@ -187,7 +323,7 @@ pub(crate) mod tests {
         pus_tc
             .write_to(test_buf.as_mut_slice())
             .expect("Error writing TC to buffer");
-        ccsds_distrib.pass_tc(&test_buf);
+        ccsds_distrib.pass_tc(&test_buf).expect("Passing TC failed");
         let recvd = unknown_packet_queue.lock().unwrap().pop_front();
         assert!(known_packet_queue.lock().unwrap().is_empty());
         assert!(recvd.is_some());

fsrc-core/src/tmtc/mod.rs

@@ -1,5 +1,7 @@
+//! TMTC module. Contains packet routing components with special support for CCSDS and ECSS packets.
 use crate::error::{FsrcErrorRaw, FsrcGroupIds};
-use spacepackets::{PacketError, SpHeader};
+use spacepackets::tc::PusTc;
+use spacepackets::SpHeader;
 
 pub mod ccsds_distrib;
 pub mod pus_distrib;
@@ -24,9 +26,18 @@ const FROM_BYTES_ZEROCOPY_ERROR: FsrcErrorRaw = FsrcErrorRaw::new(
 );
 
 pub trait ReceivesTc {
-    fn pass_tc(&mut self, tc_raw: &[u8]);
+    type Error;
+    // TODO: Maybe it makes sense to return Result<(), Self::Error> here with Error being an associated
+    //       type..
+    fn pass_tc(&mut self, tc_raw: &[u8]) -> Result<(), Self::Error>;
 }
 
 pub trait ReceivesCcsdsTc {
-    fn pass_ccsds(&mut self, header: &SpHeader, tc_raw: &[u8]) -> Result<(), PacketError>;
+    type Error;
+    fn pass_ccsds(&mut self, header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error>;
+}
+
+pub trait ReceivesEcssPusTc {
+    type Error;
+    fn pass_pus_tc(&mut self, header: &SpHeader, pus_tc: &PusTc) -> Result<(), Self::Error>;
 }

fsrc-core/src/tmtc/pus_distrib.rs

@@ -1,54 +1,101 @@
-use crate::any::AsAny;
+//! 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, ReceivesTc};
+use crate::tmtc::{ReceivesCcsdsTc, ReceivesEcssPusTc, ReceivesTc};
+use downcast_rs::Downcast;
 use spacepackets::ecss::{PusError, PusPacket};
 use spacepackets::tc::PusTc;
-use spacepackets::{CcsdsPacket, PacketError, SpHeader};
+use spacepackets::{PacketError, SpHeader};
 
-pub trait PusServiceProvider: AsAny {
+pub trait PusServiceProvider: Downcast {
-    fn handle_pus_tc_packet(&mut self, service: u8, apid: u16, pus_tc: &PusTc);
+    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 struct PusDistributor {
+pub enum PusDistribError<E> {
-    pub service_provider: Box<dyn PusServiceProvider>,
+    CustomError(E),
-    error_handler: Box<dyn FsrcErrorHandler>,
+    PusError(PusError),
 }
 
-impl ReceivesTc for PusDistributor {
+impl<E> ReceivesTc for PusDistributor<E> {
-    fn pass_tc(&mut self, tm_raw: &[u8]) {
+    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) => {
+            Ok(sp_header) => self.pass_ccsds(&sp_header, tm_raw),
-                self.pass_ccsds(&sp_header, tm_raw).unwrap();
-            }
             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 ReceivesCcsdsTc for PusDistributor {
+impl<E> ReceivesCcsdsTc for PusDistributor<E> {
-    fn pass_ccsds(&mut self, _header: &SpHeader, tm_raw: &[u8]) -> Result<(), PacketError> {
+    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) => {
+            Err(e) => return Err(PusDistribError::PusError(e)),
-                match e {
-                    PusError::VersionNotSupported(_) => {}
-                    PusError::IncorrectCrc(_) => {}
-                    PusError::RawDataTooShort(_) => {}
-                    PusError::NoRawData => {}
-                    PusError::CrcCalculationMissing => {}
-                    PusError::PacketError(_) => {}
-                }
-                return Ok(());
-            }
         };
 
         self.service_provider
-            .handle_pus_tc_packet(tc.service(), tc.apid(), &tc);
+            .handle_pus_tc_packet(tc.service(), header, &tc)
-        Ok(())
+            .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>()
     }
 }
 
@@ -61,7 +108,7 @@ mod tests {
     };
     use crate::tmtc::ccsds_distrib::{ApidPacketHandler, CcsdsDistributor};
     use spacepackets::tc::PusTc;
-    use std::any::Any;
+    use spacepackets::CcsdsPacket;
     use std::collections::VecDeque;
     use std::sync::{Arc, Mutex};
 
@@ -74,104 +121,149 @@ mod tests {
         pub pus_queue: VecDeque<(u8, u16, Vec<u8>)>,
     }
 
-    impl AsAny for PusHandlerSharedQueue {
-        fn as_any(&self) -> &dyn Any {
-            self
-        }
-
-        fn as_mut_any(&mut self) -> &mut dyn Any {
-            self
-        }
-    }
-
     impl PusServiceProvider for PusHandlerSharedQueue {
-        fn handle_pus_tc_packet(&mut self, service: u8, apid: u16, pus_tc: &PusTc) {
+        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
+            pus_tc.append_to_vec(&mut vec)?;
-                .append_to_vec(&mut vec)
+            Ok(self
-                .expect("Appending raw PUS TC to vector failed");
+                .pus_queue
-            self.pus_queue
                 .lock()
                 .unwrap()
-                .push_back((service, apid, vec));
+                .push_back((service, sp_header.apid(), vec)))
-        }
-    }
-
-    impl AsAny for PusHandlerOwnedQueue {
-        fn as_any(&self) -> &dyn Any {
-            self
-        }
-
-        fn as_mut_any(&mut self) -> &mut dyn Any {
-            self
         }
     }
 
     impl PusServiceProvider for PusHandlerOwnedQueue {
-        fn handle_pus_tc_packet(&mut self, service: u8, apid: u16, pus_tc: &PusTc) {
+        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
+            pus_tc.append_to_vec(&mut vec)?;
-                .append_to_vec(&mut vec)
+            Ok(self.pus_queue.push_back((service, sp_header.apid(), vec)))
-                .expect("Appending raw PUS TC to vector failed");
-            self.pus_queue.push_back((service, apid, vec));
         }
     }
 
     struct ApidHandlerShared {
-        pub pus_distrib: PusDistributor,
+        pub pus_distrib: PusDistributor<PusError>,
         handler_base: BasicApidHandlerSharedQueue,
     }
 
-    impl AsAny for ApidHandlerShared {
-        fn as_any(&self) -> &dyn Any {
-            self
-        }
-
-        fn as_mut_any(&mut self) -> &mut dyn Any {
-            self
-        }
-    }
-
     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]) {
+            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]) {
+            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);
             }
         };
     }
 
-    impl ApidPacketHandler for ApidHandlerShared {
-        apid_handler_impl!();
-    }
-
     struct ApidHandlerOwned {
-        pub pus_distrib: PusDistributor,
+        pub pus_distrib: PusDistributor<PusError>,
         handler_base: BasicApidHandlerOwnedQueue,
     }
 
-    impl AsAny for ApidHandlerOwned {
+    impl ApidPacketHandler for ApidHandlerOwned {
-        fn as_any(&self) -> &dyn Any {
+        //apid_handler_impl!();
-            self
+        type Error = PusError;
+
+        fn valid_apids(&self) -> &'static [u16] {
+            &[0x000, 0x002]
         }
 
-        fn as_mut_any(&mut self) -> &mut dyn Any {
+        fn handle_known_apid(
-            self
+            &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 ApidHandlerOwned {
+    impl ApidPacketHandler for ApidHandlerShared {
-        apid_handler_impl!();
+        //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]
@@ -207,7 +299,9 @@ mod tests {
             .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);
+        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());
@@ -246,12 +340,12 @@ mod tests {
             .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);
+        ccsds_distrib
+            .pass_tc(tc_slice)
+            .expect("Passing TC slice failed");
 
         let apid_handler_casted_back: &mut ApidHandlerOwned = ccsds_distrib
-            .apid_handler
+            .apid_handler_mut()
-            .as_mut_any()
-            .downcast_mut::<ApidHandlerOwned>()
             .expect("Cast to concrete type ApidHandler failed");
         assert!(!apid_handler_casted_back
             .handler_base
@@ -259,10 +353,8 @@ mod tests {
             .is_empty());
         let handler_casted_back: &mut PusHandlerOwnedQueue = apid_handler_casted_back
             .pus_distrib
-            .service_provider
+            .service_provider_mut()
-            .as_mut_any()
+            .expect("Cast to concrete type PusHandlerOwnedQueue failed");
-            .downcast_mut::<PusHandlerOwnedQueue>()
-            .expect("Cast to concrete type PusHandler 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);

spacepackets

@@ -1 +1 @@
-Subproject commit fde3fe9cba62b816e004a174821b2d4760003d23
+Subproject commit 1969f1bfa0ebb8327d155e5e44d5a1a2d88b955c