Major refactoring and update of PUS module

satrs/src/mode.rs

@@ -1,67 +1,95 @@
 use core::mem::size_of;
+use satrs_shared::res_code::ResultU16;
 #[cfg(feature = "serde")]
 use serde::{Deserialize, Serialize};
 use spacepackets::ByteConversionError;
 
-use crate::TargetId;
+#[cfg(feature = "alloc")]
+pub use alloc_mod::*;
+
+#[cfg(feature = "std")]
+pub use std_mod::*;
+
+use crate::{
+    queue::GenericTargetedMessagingError,
+    request::{GenericMessage, MessageMetadata, MessageReceiver, MessageReceiverWithId, RequestId},
+    ComponentId,
+};
+
+pub type Mode = u32;
+pub type Submode = u16;
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
 pub struct ModeAndSubmode {
-    mode: u32,
-    submode: u16,
+    mode: Mode,
+    submode: Submode,
 }
 
 impl ModeAndSubmode {
-    pub const fn new_mode_only(mode: u32) -> Self {
+    pub const RAW_LEN: usize = size_of::<Mode>() + size_of::<Submode>();
+
+    pub const fn new_mode_only(mode: Mode) -> Self {
         Self { mode, submode: 0 }
     }
 
-    pub const fn new(mode: u32, submode: u16) -> Self {
+    pub const fn new(mode: Mode, submode: Submode) -> Self {
         Self { mode, submode }
     }
 
-    pub fn raw_len() -> usize {
-        size_of::<u32>() + size_of::<u16>()
-    }
-
     pub fn from_be_bytes(buf: &[u8]) -> Result<Self, ByteConversionError> {
         if buf.len() < 6 {
             return Err(ByteConversionError::FromSliceTooSmall {
-                expected: 6,
+                expected: Self::RAW_LEN,
                 found: buf.len(),
             });
         }
         Ok(Self {
-            mode: u32::from_be_bytes(buf[0..4].try_into().unwrap()),
-            submode: u16::from_be_bytes(buf[4..6].try_into().unwrap()),
+            mode: Mode::from_be_bytes(buf[0..size_of::<Mode>()].try_into().unwrap()),
+            submode: Submode::from_be_bytes(
+                buf[size_of::<Mode>()..size_of::<Mode>() + size_of::<Submode>()]
+                    .try_into()
+                    .unwrap(),
+            ),
         })
     }
 
-    pub fn mode(&self) -> u32 {
+    pub fn write_to_be_bytes(&self, buf: &mut [u8]) -> Result<usize, ByteConversionError> {
+        if buf.len() < Self::RAW_LEN {
+            return Err(ByteConversionError::ToSliceTooSmall {
+                expected: Self::RAW_LEN,
+                found: buf.len(),
+            });
+        }
+        buf[0..size_of::<Mode>()].copy_from_slice(&self.mode.to_be_bytes());
+        buf[size_of::<Mode>()..Self::RAW_LEN].copy_from_slice(&self.submode.to_be_bytes());
+        Ok(Self::RAW_LEN)
+    }
+
+    pub fn mode(&self) -> Mode {
         self.mode
     }
 
-    pub fn submode(&self) -> u16 {
+    pub fn submode(&self) -> Submode {
         self.submode
     }
 }
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
 pub struct TargetedModeCommand {
-    pub address: TargetId,
+    pub address: ComponentId,
     pub mode_submode: ModeAndSubmode,
 }
 
 impl TargetedModeCommand {
-    pub const fn new(address: TargetId, mode_submode: ModeAndSubmode) -> Self {
+    pub const fn new(address: ComponentId, mode_submode: ModeAndSubmode) -> Self {
         Self {
             address,
             mode_submode,
         }
     }
 
-    pub fn address(&self) -> TargetId {
+    pub fn address(&self) -> ComponentId {
         self.address
     }
 
@@ -81,6 +109,8 @@ impl TargetedModeCommand {
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
 pub enum ModeRequest {
+    /// Mode information. Can be used to notify other components of changed modes.
+    ModeInfo(ModeAndSubmode),
     SetMode(ModeAndSubmode),
     ReadMode,
     AnnounceMode,
@@ -90,6 +120,479 @@ pub enum ModeRequest {
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
 pub struct TargetedModeRequest {
-    target_id: TargetId,
+    target_id: ComponentId,
     mode_request: ModeRequest,
 }
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
+pub enum ModeReply {
+    /// Reply to a mode request to confirm the commanded mode was reached.
+    ModeReply(ModeAndSubmode),
+    // Can not reach the commanded mode. Contains a reason as a [ResultU16].
+    CantReachMode(ResultU16),
+    /// We are in the wrong mode for unknown reasons. Contains the expected and reached mode.
+    WrongMode {
+        expected: ModeAndSubmode,
+        reached: ModeAndSubmode,
+    },
+}
+
+pub type GenericModeReply = GenericMessage<ModeReply>;
+
+pub trait ModeRequestSender {
+    fn local_channel_id(&self) -> ComponentId;
+    fn send_mode_request(
+        &self,
+        request_id: RequestId,
+        target_id: ComponentId,
+        request: ModeRequest,
+    ) -> Result<(), GenericTargetedMessagingError>;
+}
+
+pub trait ModeRequestReceiver {
+    fn try_recv_mode_request(
+        &self,
+    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError>;
+}
+
+impl<R: MessageReceiver<ModeRequest>> ModeRequestReceiver
+    for MessageReceiverWithId<ModeRequest, R>
+{
+    fn try_recv_mode_request(
+        &self,
+    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+        self.try_recv_message()
+    }
+}
+
+#[derive(Debug, Clone)]
+pub enum ModeError {
+    Messaging(GenericTargetedMessagingError),
+}
+
+impl From<GenericTargetedMessagingError> for ModeError {
+    fn from(value: GenericTargetedMessagingError) -> Self {
+        Self::Messaging(value)
+    }
+}
+
+pub trait ModeProvider {
+    fn mode_and_submode(&self) -> ModeAndSubmode;
+
+    fn mode(&self) -> Mode {
+        self.mode_and_submode().mode()
+    }
+
+    fn submode(&self) -> Submode {
+        self.mode_and_submode().submode()
+    }
+}
+
+pub trait ModeRequestHandler: ModeProvider {
+    type Error;
+
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+    ) -> Result<(), Self::Error>;
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool);
+
+    fn handle_mode_reached(
+        &mut self,
+        requestor_info: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error>;
+
+    fn handle_mode_info(
+        &mut self,
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), Self::Error>;
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error>;
+
+    fn handle_mode_request(
+        &mut self,
+        request: GenericMessage<ModeRequest>,
+    ) -> Result<(), Self::Error> {
+        match request.message {
+            ModeRequest::SetMode(mode_and_submode) => {
+                self.start_transition(request.requestor_info, mode_and_submode)
+            }
+            ModeRequest::ReadMode => self.send_mode_reply(
+                request.requestor_info,
+                ModeReply::ModeReply(self.mode_and_submode()),
+            ),
+            ModeRequest::AnnounceMode => {
+                self.announce_mode(Some(request.requestor_info), false);
+                Ok(())
+            }
+            ModeRequest::AnnounceModeRecursive => {
+                self.announce_mode(Some(request.requestor_info), true);
+                Ok(())
+            }
+            ModeRequest::ModeInfo(info) => self.handle_mode_info(request.requestor_info, info),
+        }
+    }
+}
+
+pub trait ModeReplyReceiver {
+    fn try_recv_mode_reply(
+        &self,
+    ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError>;
+}
+
+impl<R: MessageReceiver<ModeReply>> ModeReplyReceiver for MessageReceiverWithId<ModeReply, R> {
+    fn try_recv_mode_reply(
+        &self,
+    ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+        self.try_recv_message()
+    }
+}
+
+pub trait ModeReplySender {
+    fn local_channel_id(&self) -> ComponentId;
+
+    /// The requestor is assumed to be the target of the reply.
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), GenericTargetedMessagingError>;
+}
+
+#[cfg(feature = "alloc")]
+pub mod alloc_mod {
+    use crate::{
+        mode::ModeRequest,
+        queue::GenericTargetedMessagingError,
+        request::{
+            MessageMetadata, MessageSender, MessageSenderAndReceiver, MessageSenderMap,
+            RequestAndReplySenderAndReceiver, RequestId,
+        },
+        ComponentId,
+    };
+
+    use super::*;
+
+    impl<S: MessageSender<ModeReply>> MessageSenderMap<ModeReply, S> {
+        pub fn send_mode_reply(
+            &self,
+            requestor_info: MessageMetadata,
+            target_id: ComponentId,
+            request: ModeReply,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.send_message(requestor_info, target_id, request)
+        }
+
+        pub fn add_reply_target(&mut self, target_id: ComponentId, request_sender: S) {
+            self.add_message_target(target_id, request_sender)
+        }
+    }
+
+    impl<FROM, S: MessageSender<ModeReply>, R: MessageReceiver<FROM>> ModeReplySender
+        for MessageSenderAndReceiver<ModeReply, FROM, S, R>
+    {
+        fn local_channel_id(&self) -> ComponentId {
+            self.local_channel_id_generic()
+        }
+
+        fn send_mode_reply(
+            &self,
+            requestor_info: MessageMetadata,
+            request: ModeReply,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.message_sender_map.send_mode_reply(
+                MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()),
+                requestor_info.sender_id(),
+                request,
+            )
+        }
+    }
+
+    impl<TO, S: MessageSender<TO>, R: MessageReceiver<ModeReply>> ModeReplyReceiver
+        for MessageSenderAndReceiver<TO, ModeReply, S, R>
+    {
+        fn try_recv_mode_reply(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+            self.message_receiver.try_recv_message()
+        }
+    }
+
+    impl<
+            REQUEST,
+            S0: MessageSender<REQUEST>,
+            R0: MessageReceiver<ModeReply>,
+            S1: MessageSender<ModeReply>,
+            R1: MessageReceiver<REQUEST>,
+        > RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+    {
+        pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: S1) {
+            self.reply_sender_map
+                .add_message_target(target_id, reply_sender)
+        }
+    }
+
+    impl<
+            REQUEST,
+            S0: MessageSender<REQUEST>,
+            R0: MessageReceiver<ModeReply>,
+            S1: MessageSender<ModeReply>,
+            R1: MessageReceiver<REQUEST>,
+        > ModeReplySender for RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+    {
+        fn local_channel_id(&self) -> ComponentId {
+            self.local_channel_id_generic()
+        }
+
+        fn send_mode_reply(
+            &self,
+            requestor_info: MessageMetadata,
+            request: ModeReply,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.reply_sender_map.send_mode_reply(
+                MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()),
+                requestor_info.sender_id(),
+                request,
+            )
+        }
+    }
+
+    impl<
+            REQUEST,
+            S0: MessageSender<REQUEST>,
+            R0: MessageReceiver<ModeReply>,
+            S1: MessageSender<ModeReply>,
+            R1: MessageReceiver<REQUEST>,
+        > ModeReplyReceiver
+        for RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+    {
+        fn try_recv_mode_reply(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+            self.reply_receiver.try_recv_message()
+        }
+    }
+
+    /// Helper type definition for a mode handler which can handle mode requests.
+    pub type ModeRequestHandlerInterface<S, R> =
+        MessageSenderAndReceiver<ModeReply, ModeRequest, S, R>;
+
+    impl<S: MessageSender<ModeReply>, R: MessageReceiver<ModeRequest>>
+        ModeRequestHandlerInterface<S, R>
+    {
+        pub fn try_recv_mode_request(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+            self.try_recv_message()
+        }
+
+        pub fn send_mode_reply(
+            &self,
+            requestor_info: MessageMetadata,
+            reply: ModeReply,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.send_message(
+                requestor_info.request_id(),
+                requestor_info.sender_id(),
+                reply,
+            )
+        }
+    }
+
+    /// Helper type defintion for a mode handler object which can send mode requests and receive
+    /// mode replies.
+    pub type ModeRequestorInterface<S, R> = MessageSenderAndReceiver<ModeRequest, ModeReply, S, R>;
+
+    impl<S: MessageSender<ModeRequest>, R: MessageReceiver<ModeReply>> ModeRequestorInterface<S, R> {
+        pub fn try_recv_mode_reply(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+            self.try_recv_message()
+        }
+
+        pub fn send_mode_request(
+            &self,
+            request_id: RequestId,
+            target_id: ComponentId,
+            reply: ModeRequest,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.send_message(request_id, target_id, reply)
+        }
+    }
+
+    /// Helper type defintion for a mode handler object which can both send mode requests and
+    /// process mode requests.
+    pub type ModeInterface<S0, R0, S1, R1> =
+        RequestAndReplySenderAndReceiver<ModeRequest, ModeReply, S0, R0, S1, R1>;
+
+    impl<S: MessageSender<ModeRequest>> MessageSenderMap<ModeRequest, S> {
+        pub fn send_mode_request(
+            &self,
+            requestor_info: MessageMetadata,
+            target_id: ComponentId,
+            request: ModeRequest,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.send_message(requestor_info, target_id, request)
+        }
+
+        pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S) {
+            self.add_message_target(target_id, request_sender)
+        }
+    }
+
+    /*
+    impl<S: MessageSender<ModeRequest>> ModeRequestSender for MessageSenderMapWithId<ModeRequest, S> {
+        fn local_channel_id(&self) -> ComponentId {
+            self.local_channel_id
+        }
+
+        fn send_mode_request(
+            &self,
+            request_id: RequestId,
+            target_id: ComponentId,
+            request: ModeRequest,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.send_message(request_id, target_id, request)
+        }
+    }
+    */
+
+    impl<TO, S: MessageSender<TO>, R: MessageReceiver<ModeRequest>> ModeRequestReceiver
+        for MessageSenderAndReceiver<TO, ModeRequest, S, R>
+    {
+        fn try_recv_mode_request(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+            self.message_receiver.try_recv_message()
+        }
+    }
+
+    impl<FROM, S: MessageSender<ModeRequest>, R: MessageReceiver<FROM>> ModeRequestSender
+        for MessageSenderAndReceiver<ModeRequest, FROM, S, R>
+    {
+        fn local_channel_id(&self) -> ComponentId {
+            self.local_channel_id_generic()
+        }
+
+        fn send_mode_request(
+            &self,
+            request_id: RequestId,
+            target_id: ComponentId,
+            request: ModeRequest,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.message_sender_map.send_mode_request(
+                MessageMetadata::new(request_id, self.local_channel_id()),
+                target_id,
+                request,
+            )
+        }
+    }
+
+    impl<
+            REPLY,
+            S0: MessageSender<ModeRequest>,
+            R0: MessageReceiver<REPLY>,
+            S1: MessageSender<REPLY>,
+            R1: MessageReceiver<ModeRequest>,
+        > RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+    {
+        pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S0) {
+            self.request_sender_map
+                .add_message_target(target_id, request_sender)
+        }
+    }
+
+    impl<
+            REPLY,
+            S0: MessageSender<ModeRequest>,
+            R0: MessageReceiver<REPLY>,
+            S1: MessageSender<REPLY>,
+            R1: MessageReceiver<ModeRequest>,
+        > ModeRequestSender
+        for RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+    {
+        fn local_channel_id(&self) -> ComponentId {
+            self.local_channel_id_generic()
+        }
+
+        fn send_mode_request(
+            &self,
+            request_id: RequestId,
+            target_id: ComponentId,
+            request: ModeRequest,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.request_sender_map.send_mode_request(
+                MessageMetadata::new(request_id, self.local_channel_id()),
+                target_id,
+                request,
+            )
+        }
+    }
+
+    impl<
+            REPLY,
+            S0: MessageSender<ModeRequest>,
+            R0: MessageReceiver<REPLY>,
+            S1: MessageSender<REPLY>,
+            R1: MessageReceiver<ModeRequest>,
+        > ModeRequestReceiver
+        for RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+    {
+        fn try_recv_mode_request(
+            &self,
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+            self.request_receiver.try_recv_message()
+        }
+    }
+}
+
+#[cfg(feature = "std")]
+pub mod std_mod {
+    use std::sync::mpsc;
+
+    use crate::request::GenericMessage;
+
+    use super::*;
+
+    pub type ModeRequestHandlerMpsc = ModeRequestHandlerInterface<
+        mpsc::Sender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeRequest>>,
+    >;
+    pub type ModeRequestHandlerMpscBounded = ModeRequestHandlerInterface<
+        mpsc::SyncSender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeRequest>>,
+    >;
+
+    pub type ModeRequestorMpsc = ModeRequestorInterface<
+        mpsc::Sender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+    >;
+    pub type ModeRequestorBoundedMpsc = ModeRequestorInterface<
+        mpsc::SyncSender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+    >;
+
+    pub type ModeRequestorAndHandlerMpsc = ModeInterface<
+        mpsc::Sender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        mpsc::Sender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeRequest>>,
+    >;
+    pub type ModeRequestorAndHandlerMpscBounded = ModeInterface<
+        mpsc::SyncSender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        mpsc::SyncSender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeRequest>>,
+    >;
+}
+
+#[cfg(test)]
+mod tests {}