3 files changed, 138 insertions, 31 deletions

diff --git a/embassy-stm32-wpan/src/wb55/mod.rs b/embassy-stm32-wpan/src/wb55/mod.rs
index 95cfe09f1..814303a05 100644
--- a/embassy-stm32-wpan/src/wb55/mod.rs
+++ b/embassy-stm32-wpan/src/wb55/mod.rs
@@ -176,7 +176,7 @@ impl<'d> TlMbox<'d> {
         let mm = sub::mm::MemoryManager::new(ipcc_mm_release_buffer_channel);
         let mut sys = sub::sys::Sys::new(ipcc_system_cmd_rsp_channel, ipcc_system_event_channel);
 
-        debug!("sys event: {}", sys.read().await.payload());
+        debug!("sys event: {}", sys.read_ready().await);
 
         Self {
             sys_subsystem: sys,
diff --git a/embassy-stm32-wpan/src/wb55/shci.rs b/embassy-stm32-wpan/src/wb55/shci.rs
index 3faa79209..3eb9525d3 100644
--- a/embassy-stm32-wpan/src/wb55/shci.rs
+++ b/embassy-stm32-wpan/src/wb55/shci.rs
@@ -12,6 +12,28 @@ const fn opcode(ogf: u16, ocf: u16) -> isize {
     ((ogf << 10) + ocf) as isize
 }
 
+pub(crate) trait SealedSchiFromPacket: Sized {
+    unsafe fn from_packet(cmd_buf: *const CmdPacket) -> Result<Self, ()>;
+}
+
+#[allow(private_bounds)]
+pub trait SchiFromPacket: SealedSchiFromPacket {}
+impl<T: SealedSchiFromPacket> SchiFromPacket for T {}
+
+trait ShciFromEventSerial: TryFrom<u8, Error = ()> {}
+
+impl<T: ShciFromEventSerial> SealedSchiFromPacket for T {
+    unsafe fn from_packet(cmd_buf: *const CmdPacket) -> Result<Self, ()> {
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_evt_payload = p_cmd_serial.add(size_of::<EvtStub>());
+
+        compiler_fence(Ordering::Acquire);
+        let cc_evt = ptr::read_unaligned(p_evt_payload as *const CcEvt);
+
+        cc_evt.payload[0].try_into()
+    }
+}
+
 #[allow(dead_code)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum SchiCommandStatus {
@@ -25,37 +47,20 @@ pub enum SchiCommandStatus {
     ShciFusCmdNotSupported = 0xFF,
 }
 
-impl SchiCommandStatus {
-    pub unsafe fn from_packet(cmd_buf: *const CmdPacket) -> Result<Self, ()> {
-        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
-        let p_evt_payload = p_cmd_serial.add(size_of::<EvtStub>());
-
-        compiler_fence(Ordering::Acquire);
-        let cc_evt = ptr::read_unaligned(p_evt_payload as *const CcEvt);
-
-        cc_evt.payload[0].try_into()
-    }
-}
-
+impl ShciFromEventSerial for SchiCommandStatus {}
 impl TryFrom<u8> for SchiCommandStatus {
     type Error = ();
 
     fn try_from(v: u8) -> Result<Self, Self::Error> {
         match v {
-            x if x == SchiCommandStatus::ShciSuccess as u8 => Ok(SchiCommandStatus::ShciSuccess),
-            x if x == SchiCommandStatus::ShciUnknownCmd as u8 => Ok(SchiCommandStatus::ShciUnknownCmd),
-            x if x == SchiCommandStatus::ShciMemoryCapacityExceededErrCode as u8 => {
-                Ok(SchiCommandStatus::ShciMemoryCapacityExceededErrCode)
-            }
-            x if x == SchiCommandStatus::ShciErrUnsupportedFeature as u8 => {
-                Ok(SchiCommandStatus::ShciErrUnsupportedFeature)
-            }
-            x if x == SchiCommandStatus::ShciErrInvalidHciCmdParams as u8 => {
-                Ok(SchiCommandStatus::ShciErrInvalidHciCmdParams)
-            }
-            x if x == SchiCommandStatus::ShciErrInvalidParams as u8 => Ok(SchiCommandStatus::ShciErrInvalidParams), /* only used for release < v1.13.0 */
-            x if x == SchiCommandStatus::ShciErrInvalidParamsV2 as u8 => Ok(SchiCommandStatus::ShciErrInvalidParamsV2), /* available for release >= v1.13.0 */
-            x if x == SchiCommandStatus::ShciFusCmdNotSupported as u8 => Ok(SchiCommandStatus::ShciFusCmdNotSupported),
+            0x00 => Ok(Self::ShciSuccess),
+            0x01 => Ok(Self::ShciUnknownCmd),
+            0x07 => Ok(Self::ShciMemoryCapacityExceededErrCode),
+            0x11 => Ok(Self::ShciErrUnsupportedFeature),
+            0x12 => Ok(Self::ShciErrInvalidHciCmdParams),
+            0x42 => Ok(Self::ShciErrInvalidParams), /* only used for release < v1.13.0 */
+            0x92 => Ok(Self::ShciErrInvalidParamsV2), /* available for release >= v1.13.0 */
+            0xFF => Ok(Self::ShciFusCmdNotSupported),
             _ => Err(()),
         }
     }
@@ -107,6 +112,71 @@ pub enum ShciOpcode {
     Mac802_15_4DeInit = opcode(SHCI_OGF, 0x78),
 }
 
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum ShciFusGetStateErrorCode {
+    FusStateErrorNoError = 0x00,
+    FusStateErrorImgNotFound = 0x01,
+    FusStateErrorImgCorrupt = 0x02,
+    FusStateErrorImgNotAuthentic = 0x03,
+    FusStateErrorImgNotEnoughSpace = 0x04,
+    FusStateErrorImageUsrAbort = 0x05,
+    FusStateErrorImageErsError = 0x06,
+    FusStateErrorImageWrtError = 0x07,
+    FusStateErrorAuthTagStNotFound = 0x08,
+    FusStateErrorAuthTagCustNotFound = 0x09,
+    FusStateErrorAuthKeyLocked = 0x0A,
+    FusStateErrorFwRollbackError = 0x11,
+    FusStateErrorStateNotRunning = 0xFE,
+    FusStateErrorErrUnknown = 0xFF,
+}
+
+impl ShciFromEventSerial for ShciFusGetStateErrorCode {}
+impl TryFrom<u8> for ShciFusGetStateErrorCode {
+    type Error = ();
+
+    fn try_from(v: u8) -> Result<Self, Self::Error> {
+        match v {
+            0x00 => Ok(Self::FusStateErrorNoError),
+            0x01 => Ok(Self::FusStateErrorImgNotFound),
+            0x02 => Ok(Self::FusStateErrorImgCorrupt),
+            0x03 => Ok(Self::FusStateErrorImgNotAuthentic),
+            0x04 => Ok(Self::FusStateErrorImgNotEnoughSpace),
+            0x05 => Ok(Self::FusStateErrorImageUsrAbort),
+            0x06 => Ok(Self::FusStateErrorImageErsError),
+            0x07 => Ok(Self::FusStateErrorImageWrtError),
+            0x08 => Ok(Self::FusStateErrorAuthTagStNotFound),
+            0x09 => Ok(Self::FusStateErrorAuthTagCustNotFound),
+            0x0A => Ok(Self::FusStateErrorAuthKeyLocked),
+            0x11 => Ok(Self::FusStateErrorFwRollbackError),
+            0xFE => Ok(Self::FusStateErrorStateNotRunning),
+            0xFF => Ok(Self::FusStateErrorErrUnknown),
+            _ => Err(()),
+        }
+    }
+}
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum SchiSysEventReady {
+    WirelessFwRunning = 0x00,
+    FusFwRunning = 0x01,
+    NvmBackupRunning = 0x10,
+    NvmRestoreRunning = 0x11,
+}
+
+impl TryFrom<u8> for SchiSysEventReady {
+    type Error = ();
+
+    fn try_from(v: u8) -> Result<Self, Self::Error> {
+        match v {
+            0x00 => Ok(Self::WirelessFwRunning),
+            0x01 => Ok(Self::FusFwRunning),
+            0x10 => Ok(Self::NvmBackupRunning),
+            0x11 => Ok(Self::NvmRestoreRunning),
+            _ => Err(()),
+        }
+    }
+}
+
 pub const SHCI_C2_CONFIG_EVTMASK1_BIT0_ERROR_NOTIF_ENABLE: u8 = 1 << 0;
 pub const SHCI_C2_CONFIG_EVTMASK1_BIT1_BLE_NVM_RAM_UPDATE_ENABLE: u8 = 1 << 1;
 pub const SHCI_C2_CONFIG_EVTMASK1_BIT2_THREAD_NVM_RAM_UPDATE_ENABLE: u8 = 1 << 2;
diff --git a/embassy-stm32-wpan/src/wb55/sub/sys.rs b/embassy-stm32-wpan/src/wb55/sub/sys.rs
index 2e625a677..3d774eeb7 100644
--- a/embassy-stm32-wpan/src/wb55/sub/sys.rs
+++ b/embassy-stm32-wpan/src/wb55/sub/sys.rs
@@ -1,3 +1,5 @@
+use core::slice;
+
 use embassy_stm32::ipcc::{IpccRxChannel, IpccTxChannel};
 
 use crate::cmd::CmdPacket;
@@ -5,12 +7,17 @@ use crate::consts::TlPacketType;
 use crate::evt::EvtBox;
 #[cfg(feature = "wb55_ble")]
 use crate::shci::ShciBleInitCmdParam;
-use crate::shci::{SchiCommandStatus, ShciOpcode};
+use crate::shci::{SchiCommandStatus, SchiFromPacket, SchiSysEventReady, ShciFusGetStateErrorCode, ShciOpcode};
 use crate::sub::mm;
 use crate::tables::{SysTable, WirelessFwInfoTable};
 use crate::unsafe_linked_list::LinkedListNode;
 use crate::wb55::{SYS_CMD_BUF, SYSTEM_EVT_QUEUE, TL_DEVICE_INFO_TABLE, TL_SYS_TABLE};
 
+const fn slice8_ref(x: &[u32]) -> &[u8] {
+    let len = x.len() * 4;
+    unsafe { slice::from_raw_parts(x.as_ptr() as *const u8, len) }
+}
+
 /// A guard that, once constructed, allows for sys commands to be sent to CPU2.
 pub struct Sys<'a> {
     ipcc_system_cmd_rsp_channel: IpccTxChannel<'a>,
@@ -55,15 +62,15 @@ impl<'a> Sys<'a> {
     }
 
     /// `HW_IPCC_SYS_CmdEvtNot`
-    pub async fn write_and_get_response(
+    pub async fn write_and_get_response<T: SchiFromPacket>(
         &mut self,
         opcode: ShciOpcode,
         payload: &[u8],
-    ) -> Result<SchiCommandStatus, ()> {
+    ) -> Result<T, ()> {
         self.write(opcode, payload).await;
         self.ipcc_system_cmd_rsp_channel.flush().await;
 
-        unsafe { SchiCommandStatus::from_packet(SYS_CMD_BUF.as_ptr()) }
+        unsafe { T::from_packet(SYS_CMD_BUF.as_ptr()) }
     }
 
     #[cfg(feature = "wb55_mac")]
@@ -82,6 +89,36 @@ impl<'a> Sys<'a> {
         self.write_and_get_response(ShciOpcode::BleInit, param.payload()).await
     }
 
+    pub async fn shci_c2_fus_getstate(&mut self) -> Result<ShciFusGetStateErrorCode, ()> {
+        self.write_and_get_response(ShciOpcode::FusStartWirelessStack, &[])
+            .await
+    }
+
+    /// Send a request to CPU2 to start the wireless stack
+    pub async fn shci_c2_fus_startws(&mut self) -> Result<SchiCommandStatus, ()> {
+        self.write_and_get_response(ShciOpcode::FusStartWirelessStack, &[])
+            .await
+    }
+
+    /// Send a request to CPU2 to upgrade the firmware
+    pub async fn shci_c2_fus_fwupgrade(&mut self, fw_src_add: u32, fw_dst_add: u32) -> Result<SchiCommandStatus, ()> {
+        let buf = [fw_src_add, fw_dst_add];
+        let len = if fw_dst_add != 0 {
+            2
+        } else if fw_src_add != 0 {
+            1
+        } else {
+            0
+        };
+
+        self.write_and_get_response(ShciOpcode::FusFirmwareUpgrade, slice8_ref(&buf[..len]))
+            .await
+    }
+
+    pub async fn read_ready(&mut self) -> Result<SchiSysEventReady, ()> {
+        self.read().await.payload()[0].try_into()
+    }
+
     /// `HW_IPCC_SYS_EvtNot`
     ///
     /// This method takes the place of the `HW_IPCC_SYS_EvtNot`/`SysUserEvtRx`/`APPE_SysUserEvtRx`,