6 files changed, 56 insertions, 38 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
index 57348a925..46a2f41c4 100644
--- a/embassy-stm32-wpan/src/ble.rs
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -48,11 +48,11 @@ impl Ble {
         // TODO: ACL data ack to the user
     }
 
-    pub fn ble_send_cmd(buf: &[u8]) {
+    pub fn send_cmd(buf: &[u8]) {
         debug!("writing ble cmd");
 
         unsafe {
-            let pcmd_buffer: *mut CmdPacket = (*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer;
+            let pcmd_buffer: *mut CmdPacket = BLE_CMD_BUFFER.as_mut_ptr();
             let pcmd_serial: *mut CmdSerial = &mut (*pcmd_buffer).cmdserial;
             let pcmd_serial_buf: *mut u8 = pcmd_serial.cast();
 
diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs
index 1f7dae7f7..a023201ba 100644
--- a/embassy-stm32-wpan/src/cmd.rs
+++ b/embassy-stm32-wpan/src/cmd.rs
@@ -28,6 +28,14 @@ pub struct CmdSerial {
 
 #[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
+pub struct CmdSerialStub {
+    pub ty: u8,
+    pub cmd_code: u16,
+    pub payload_len: u8,
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
 pub struct CmdPacket {
     pub header: PacketHeader,
     pub cmdserial: CmdSerial,
diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs
index aae2265ed..a217aa224 100644
--- a/embassy-stm32-wpan/src/rc.rs
+++ b/embassy-stm32-wpan/src/rc.rs
@@ -20,7 +20,7 @@ impl<'d> RadioCoprocessor<'d> {
         let cmd = TlPacketType::try_from(cmd_code).unwrap();
 
         match &cmd {
-            TlPacketType::BleCmd => Ble::ble_send_cmd(buf),
+            TlPacketType::BleCmd => Ble::send_cmd(buf),
             _ => todo!(),
         }
     }
@@ -33,11 +33,6 @@ impl<'d> RadioCoprocessor<'d> {
                 let event = evt.evt();
 
                 evt.write(&mut self.rx_buf).unwrap();
-
-                if event.kind() == 18 {
-                    shci::shci_ble_init(Default::default());
-                    self.rx_buf[0] = 0x04;
-                }
             }
 
             if self.mbox.pop_last_cc_evt().is_some() {
diff --git a/embassy-stm32-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs
index 8537995ff..6e58a7156 100644
--- a/embassy-stm32-wpan/src/shci.rs
+++ b/embassy-stm32-wpan/src/shci.rs
@@ -1,8 +1,10 @@
+use core::{mem, slice};
+
 use super::cmd::CmdPacket;
 use super::consts::TlPacketType;
 use super::{sys, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE};
 
-const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66;
+pub const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66;
 
 #[derive(Debug, Clone, Copy)]
 #[repr(C, packed)]
@@ -32,6 +34,12 @@ pub struct ShciBleInitCmdParam {
     pub hw_version: u8,
 }
 
+impl ShciBleInitCmdParam {
+    pub fn payload<'a>(&self) -> &'a [u8] {
+        unsafe { slice::from_raw_parts(self as *const _ as *const u8, mem::size_of::<Self>()) }
+    }
+}
+
 impl Default for ShciBleInitCmdParam {
     fn default() -> Self {
         Self {
@@ -66,35 +74,10 @@ pub struct ShciHeader {
 #[derive(Debug, Clone, Copy)]
 #[repr(C, packed)]
 pub struct ShciBleInitCmdPacket {
-    header: ShciHeader,
-    param: ShciBleInitCmdParam,
+    pub header: ShciHeader,
+    pub param: ShciBleInitCmdParam,
 }
 
 pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE;
 #[allow(dead_code)] // Not used currently but reserved
 const TL_BLE_EVT_CS_BUFFER_SIZE: usize = TL_PACKET_HEADER_SIZE + TL_BLE_EVT_CS_PACKET_SIZE;
-
-pub fn shci_ble_init(param: ShciBleInitCmdParam) {
-    debug!("sending SHCI");
-
-    let mut packet = ShciBleInitCmdPacket {
-        header: ShciHeader::default(),
-        param,
-    };
-
-    let packet_ptr: *mut _ = &mut packet;
-
-    unsafe {
-        let cmd_ptr: *mut CmdPacket = packet_ptr.cast();
-
-        (*cmd_ptr).cmdserial.cmd.cmd_code = SCHI_OPCODE_BLE_INIT;
-        (*cmd_ptr).cmdserial.cmd.payload_len = core::mem::size_of::<ShciBleInitCmdParam>() as u8;
-
-        let p_cmd_buffer = &mut *(*TL_SYS_TABLE.as_mut_ptr()).pcmd_buffer;
-        core::ptr::write(p_cmd_buffer, *cmd_ptr);
-
-        p_cmd_buffer.cmdserial.ty = TlPacketType::SysCmd as u8;
-
-        sys::Sys::send_cmd();
-    }
-}
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index 0cff5c746..76f65cbd8 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -1,7 +1,12 @@
+use core::ptr;
+use core::sync::atomic::{compiler_fence, Ordering};
+
 use embassy_stm32::ipcc::Ipcc;
 
-use crate::cmd::{CmdPacket, CmdSerial};
+use crate::cmd::{CmdPacket, CmdSerial, CmdSerialStub};
+use crate::consts::TlPacketType;
 use crate::evt::{CcEvt, EvtBox, EvtSerial};
+use crate::shci::{ShciBleInitCmdParam, SCHI_OPCODE_BLE_INIT};
 use crate::tables::SysTable;
 use crate::unsafe_linked_list::LinkedListNode;
 use crate::{channels, EVT_CHANNEL, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_SYS_TABLE};
@@ -58,7 +63,31 @@ impl Sys {
         Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL);
     }
 
-    pub fn send_cmd() {
+    pub fn shci_ble_init(param: ShciBleInitCmdParam) {
+        debug!("sending SHCI");
+
+        Self::send_cmd(SCHI_OPCODE_BLE_INIT, param.payload());
+    }
+
+    pub fn send_cmd(opcode: u16, payload: &[u8]) {
+        unsafe {
+            let p_cmd_serial = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial as *mut _ as *mut CmdSerialStub;
+            let p_payload = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial.cmd.payload as *mut _;
+
+            ptr::write_volatile(
+                p_cmd_serial,
+                CmdSerialStub {
+                    ty: TlPacketType::SysCmd as u8,
+                    cmd_code: opcode,
+                    payload_len: payload.len() as u8,
+                },
+            );
+
+            ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+        }
+
+        compiler_fence(Ordering::SeqCst);
+
         Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL);
         Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL, true);
     }
diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs
index 4669cbc62..5e7d11ef5 100644
--- a/tests/stm32/src/bin/tl_mbox.rs
+++ b/tests/stm32/src/bin/tl_mbox.rs
@@ -11,6 +11,7 @@ use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::Config;
 use embassy_stm32_wpan::rc::RadioCoprocessor;
+use embassy_stm32_wpan::sys::Sys;
 use embassy_stm32_wpan::TlMbox;
 use embassy_time::{Duration, Timer};
 
@@ -56,6 +57,8 @@ async fn main(_spawner: Spawner) {
     let response = rc.read().await;
     info!("coprocessor ready {}", response);
 
+    Sys::shci_ble_init(Default::default());
+
     rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
     let response = rc.read().await;
     info!("ble reset rsp {}", response);