1 files changed, 54 insertions, 33 deletions

diff --git a/embassy-stm32-wpan/src/mac/runner.rs b/embassy-stm32-wpan/src/mac/runner.rs
index 92c74c2ee..acca70019 100644
--- a/embassy-stm32-wpan/src/mac/runner.rs
+++ b/embassy-stm32-wpan/src/mac/runner.rs
@@ -6,12 +6,13 @@ use embassy_sync::blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex};
 use embassy_sync::channel::Channel;
 use embassy_sync::mutex::Mutex;
 use embassy_sync::signal::Signal;
+use smoltcp::wire::Ieee802154FrameType;
+use smoltcp::wire::ieee802154::Frame;
 
 use crate::mac::MTU;
 use crate::mac::commands::*;
 use crate::mac::driver::NetworkState;
 use crate::mac::event::MacEvent;
-use crate::mac::typedefs::*;
 use crate::sub::mac::{MacRx, MacTx};
 
 pub type ZeroCopyPubSub<M, T> = blocking_mutex::Mutex<M, RefCell<Option<Signal<NoopRawMutex, T>>>>;
@@ -68,55 +69,75 @@ impl<'a> Runner<'a> {
         }
     }
 
+    async fn send_request<T: MacCommand, U: TryInto<T>>(&self, frame: U) -> Result<(), ()>
+    where
+        (): From<<U as TryInto<T>>::Error>,
+    {
+        let request: T = frame.try_into()?;
+        self.mac_tx.lock().await.send_command(&request).await.map_err(|_| ())?;
+
+        Ok(())
+    }
+
     pub async fn run(&'a self) -> ! {
         join::join(
             async {
                 loop {
                     if let Ok(mac_event) = self.mac_rx.read().await {
                         match mac_event {
+                            MacEvent::MlmeAssociateCnf(_)
+                            | MacEvent::MlmeDisassociateCnf(_)
+                            | MacEvent::MlmeGetCnf(_)
+                            | MacEvent::MlmeGtsCnf(_)
+                            | MacEvent::MlmeResetCnf(_)
+                            | MacEvent::MlmeRxEnableCnf(_)
+                            | MacEvent::MlmeScanCnf(_)
+                            | MacEvent::MlmeSetCnf(_)
+                            | MacEvent::MlmeStartCnf(_)
+                            | MacEvent::MlmePollCnf(_)
+                            | MacEvent::MlmeDpsCnf(_)
+                            | MacEvent::MlmeSoundingCnf(_)
+                            | MacEvent::MlmeCalibrateCnf(_)
+                            | MacEvent::McpsDataCnf(_)
+                            | MacEvent::McpsPurgeCnf(_) => {
+                                self.rx_event_channel.lock(|s| {
+                                    s.borrow().as_ref().map(|signal| signal.signal(mac_event));
+                                });
+                            }
                             MacEvent::McpsDataInd(_) => {
+                                // Pattern should match driver
                                 self.rx_data_channel.send(mac_event).await;
                             }
                             _ => {
-                                self.rx_event_channel.lock(|s| {
-                                    s.borrow().as_ref().map(|signal| signal.signal(mac_event));
-                                });
+                                debug!("unhandled mac event: {:#x}", mac_event);
                             }
                         }
                     }
                 }
             },
             async {
-                let mut msdu_handle = 0x02;
-
                 loop {
-                    let (buf, len) = self.tx_data_channel.receive().await;
-                    let mac_tx = self.mac_tx.lock().await;
-
-                    let pan_id = critical_section::with(|cs| self.network_state.borrow(cs).borrow().pan_id);
-
-                    // TODO: get the destination address from the packet instead of using the broadcast address
-
-                    // The mutex should be dropped on the next loop iteration
-                    mac_tx
-                        .send_command(
-                            DataRequest {
-                                src_addr_mode: AddressMode::Short,
-                                dst_addr_mode: AddressMode::Short,
-                                dst_pan_id: PanId(pan_id),
-                                dst_address: MacAddress::BROADCAST,
-                                msdu_handle: msdu_handle,
-                                ack_tx: 0x00,
-                                gts_tx: false,
-                                security_level: SecurityLevel::Unsecure,
-                                ..Default::default()
-                            }
-                            .set_buffer(&buf[..len]),
-                        )
-                        .await
-                        .unwrap();
-
-                    msdu_handle = msdu_handle.wrapping_add(1);
+                    let (buf, _) = self.tx_data_channel.receive().await;
+
+                    // Smoltcp has created this frame, so there's no need to reparse it.
+                    let frame = Frame::new_unchecked(&buf);
+
+                    let result: Result<(), ()> = match frame.frame_type() {
+                        Ieee802154FrameType::Beacon => Err(()),
+                        Ieee802154FrameType::Data => self.send_request::<DataRequest, _>(frame).await,
+                        Ieee802154FrameType::Acknowledgement => Err(()),
+                        Ieee802154FrameType::MacCommand => Err(()),
+                        Ieee802154FrameType::Multipurpose => Err(()),
+                        Ieee802154FrameType::FragmentOrFrak => Err(()),
+                        Ieee802154FrameType::Extended => Err(()),
+                        _ => Err(()),
+                    };
+
+                    if result.is_err() {
+                        debug!("failed to parse mac frame");
+                    } else {
+                        trace!("data frame sent!");
+                    }
 
                     // The tx channel should always be of equal capacity to the tx_buf channel
                     self.tx_buf_channel.try_send(buf).unwrap();