5 files changed, 37 insertions, 22 deletions

diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs
index 5c81a4aa7..787c22c4b 100644
--- a/embassy-stm32-wpan/src/cmd.rs
+++ b/embassy-stm32-wpan/src/cmd.rs
@@ -1,4 +1,5 @@
 use core::ptr;
+use core::sync::atomic::{Ordering, compiler_fence};
 
 use crate::PacketHeader;
 use crate::consts::TlPacketType;
@@ -45,11 +46,11 @@ pub struct CmdPacket {
 
 impl CmdPacket {
     pub unsafe fn write_into(cmd_buf: *mut CmdPacket, packet_type: TlPacketType, cmd_code: u16, payload: &[u8]) {
-        let p_cmd_serial = &mut (*cmd_buf).cmdserial as *mut _ as *mut CmdSerialStub;
-        let p_payload = &mut (*cmd_buf).cmdserial.cmd.payload as *mut _;
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_payload = p_cmd_serial.add(size_of::<CmdSerialStub>());
 
-        ptr::write_volatile(
-            p_cmd_serial,
+        ptr::write_unaligned(
+            p_cmd_serial as *mut _,
             CmdSerialStub {
                 ty: packet_type as u8,
                 cmd_code,
@@ -58,6 +59,8 @@ impl CmdPacket {
         );
 
         ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+
+        compiler_fence(Ordering::Release);
     }
 }
 
@@ -87,11 +90,11 @@ pub struct AclDataPacket {
 
 impl AclDataPacket {
     pub unsafe fn write_into(cmd_buf: *mut AclDataPacket, packet_type: TlPacketType, handle: u16, payload: &[u8]) {
-        let p_cmd_serial = &mut (*cmd_buf).acl_data_serial as *mut _ as *mut AclDataSerialStub;
-        let p_payload = &mut (*cmd_buf).acl_data_serial.acl_data as *mut _;
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_payload = p_cmd_serial.add(size_of::<AclDataSerialStub>());
 
-        ptr::write_volatile(
-            p_cmd_serial,
+        ptr::write_unaligned(
+            p_cmd_serial as *mut _,
             AclDataSerialStub {
                 ty: packet_type as u8,
                 handle: handle,
@@ -100,5 +103,7 @@ impl AclDataPacket {
         );
 
         ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+
+        compiler_fence(Ordering::Release);
     }
 }
diff --git a/embassy-stm32-wpan/src/evt.rs b/embassy-stm32-wpan/src/evt.rs
index c6528413d..f32821269 100644
--- a/embassy-stm32-wpan/src/evt.rs
+++ b/embassy-stm32-wpan/src/evt.rs
@@ -67,6 +67,7 @@ pub struct EvtSerial {
 pub struct EvtStub {
     pub kind: u8,
     pub evt_code: u8,
+    pub payload_len: u8,
 }
 
 /// This format shall be used for all events (asynchronous and command response) reported
diff --git a/embassy-stm32-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs
index 30d689716..e93f55598 100644
--- a/embassy-stm32-wpan/src/shci.rs
+++ b/embassy-stm32-wpan/src/shci.rs
@@ -1,6 +1,10 @@
-use core::{mem, slice};
+use core::sync::atomic::{Ordering, compiler_fence};
+use core::{mem, ptr, slice};
 
+use crate::PacketHeader;
+use crate::cmd::CmdPacket;
 use crate::consts::{TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE};
+use crate::evt::{CcEvt, EvtStub};
 
 const SHCI_OGF: u16 = 0x3F;
 
@@ -21,6 +25,18 @@ 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 TryFrom<u8> for SchiCommandStatus {
     type Error = ();
 
diff --git a/embassy-stm32-wpan/src/sub/sys.rs b/embassy-stm32-wpan/src/sub/sys.rs
index 8a3382f86..549811685 100644
--- a/embassy-stm32-wpan/src/sub/sys.rs
+++ b/embassy-stm32-wpan/src/sub/sys.rs
@@ -1,10 +1,9 @@
-use core::ptr;
-
 use crate::cmd::CmdPacket;
 use crate::consts::TlPacketType;
-use crate::evt::{CcEvt, EvtBox, EvtPacket};
-#[allow(unused_imports)]
-use crate::shci::{SchiCommandStatus, ShciBleInitCmdParam, ShciOpcode};
+use crate::evt::EvtBox;
+#[cfg(feature = "ble")]
+use crate::shci::ShciBleInitCmdParam;
+use crate::shci::{SchiCommandStatus, ShciOpcode};
 use crate::sub::mm;
 use crate::tables::{SysTable, WirelessFwInfoTable};
 use crate::unsafe_linked_list::LinkedListNode;
@@ -50,13 +49,7 @@ impl Sys {
         self.write(opcode, payload).await;
         Ipcc::flush(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL).await;
 
-        unsafe {
-            let p_event_packet = SYS_CMD_BUF.as_ptr() as *const EvtPacket;
-            let p_command_event = &((*p_event_packet).evt_serial.evt.payload) as *const _ as *const CcEvt;
-            let p_payload = &((*p_command_event).payload) as *const u8;
-
-            ptr::read_volatile(p_payload).try_into()
-        }
+        unsafe { SchiCommandStatus::from_packet(SYS_CMD_BUF.as_ptr()) }
     }
 
     #[cfg(feature = "mac")]
diff --git a/embassy-stm32/src/ipcc.rs b/embassy-stm32/src/ipcc.rs
index e1d8b1c2a..b9be1b1d2 100644
--- a/embassy-stm32/src/ipcc.rs
+++ b/embassy-stm32/src/ipcc.rs
@@ -147,7 +147,7 @@ impl Ipcc {
             // If bit is set to 1 then interrupt is disabled; we want to enable the interrupt
             regs.cpu(0).mr().modify(|w| w.set_chfm(channel as usize, false));
 
-            compiler_fence(Ordering::SeqCst);
+            compiler_fence(Ordering::Release);
 
             if !regs.cpu(0).sr().read().chf(channel as usize) {
                 // If bit is set to 1 then interrupt is disabled; we want to disable the interrupt