Merge branch 'main' of https://github.com/embassy-rs/embassy into wpan

4 files changed, 36 insertions, 19 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 1946c55fd..2d94a9cda 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 dffe9942c..718ccae8b 100644
--- a/embassy-stm32-wpan/src/sub/sys.rs
+++ b/embassy-stm32-wpan/src/sub/sys.rs
@@ -4,9 +4,10 @@ use embassy_stm32::ipcc::{IpccRxChannel, IpccTxChannel};
 
 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;
@@ -64,13 +65,7 @@ impl<'a> Sys<'a> {
         self.write(opcode, payload).await;
         self.ipcc_system_cmd_rsp_channel.flush().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")]