5 files changed, 84 insertions, 69 deletions

diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml
index 082d00f1c..ab58714d6 100644
--- a/embassy-stm32-wpan/Cargo.toml
+++ b/embassy-stm32-wpan/Cargo.toml
@@ -18,6 +18,7 @@ embassy-futures = { version = "0.1.0", path = "../embassy-futures" }
 embassy-hal-common = { version = "0.1.0", path = "../embassy-hal-common" }
 embassy-embedded-hal = { version = "0.1.0", path = "../embassy-embedded-hal" }
 embassy-net-driver-channel = { version = "0.1.0", path = "../embassy-net-driver-channel", optional=true }
+embassy-net-driver = { version = "0.1.0", path = "../embassy-net-driver", optional=true }
 
 defmt = { version = "0.3", optional = true }
 cortex-m = "0.7.6"
@@ -27,13 +28,14 @@ aligned = "0.4.1"
 bit_field = "0.10.2"
 stm32-device-signature = { version = "0.3.3", features = ["stm32wb5x"] }
 stm32wb-hci = { version = "0.1.3", optional = true }
+futures = { version = "0.3.17", default-features = false, features = ["async-await"] }
 bitflags = { version = "2.3.3", optional = true }
 
 [features]
 defmt = ["dep:defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt", "stm32wb-hci?/defmt"]
 
 ble = ["dep:stm32wb-hci"]
-mac = ["dep:bitflags", "dep:embassy-net-driver-channel"]
+mac = ["dep:bitflags", "dep:embassy-net-driver-channel", "dep:embassy-net-driver"]
 
 stm32wb10cc = [ "embassy-stm32/stm32wb10cc" ]
 stm32wb15cc = [ "embassy-stm32/stm32wb15cc" ]
diff --git a/embassy-stm32-wpan/src/mac/driver.rs b/embassy-stm32-wpan/src/mac/driver.rs
index 8ebfb2b72..a41b75094 100644
--- a/embassy-stm32-wpan/src/mac/driver.rs
+++ b/embassy-stm32-wpan/src/mac/driver.rs
@@ -29,13 +29,14 @@ impl<'d> embassy_net_driver::Driver for Driver<'d> {
     type TxToken<'a> = TxToken<'d> where Self: 'a;
 
     fn receive(&mut self, cx: &mut Context) -> Option<(Self::RxToken<'_>, Self::TxToken<'_>)> {
-        if self.runner.rx_channel.poll_ready_to_receive(cx) && self.runner.tx_channel.poll_ready_to_receive(cx) {
+        if self.runner.rx_channel.poll_ready_to_receive(cx) && self.runner.tx_buf_channel.poll_ready_to_receive(cx) {
             Some((
                 RxToken {
                     rx: &self.runner.rx_channel,
                 },
                 TxToken {
                     tx: &self.runner.tx_channel,
+                    tx_buf: &self.runner.tx_buf_channel,
                 },
             ))
         } else {
@@ -44,9 +45,10 @@ impl<'d> embassy_net_driver::Driver for Driver<'d> {
     }
 
     fn transmit(&mut self, cx: &mut Context) -> Option<Self::TxToken<'_>> {
-        if self.runner.tx_channel.poll_ready_to_receive(cx) {
+        if self.runner.tx_buf_channel.poll_ready_to_receive(cx) {
             Some(TxToken {
                 tx: &self.runner.tx_channel,
+                tx_buf: &self.runner.tx_buf_channel,
             })
         } else {
             None
@@ -106,8 +108,8 @@ impl<'d> embassy_net_driver::RxToken for RxToken<'d> {
 }
 
 pub struct TxToken<'d> {
-    tx: &'d Channel<CriticalSectionRawMutex, &'d [u8], 1>,
-    // tx: &'a mut TDesRing<'d>,
+    tx: &'d Channel<CriticalSectionRawMutex, &'d mut [u8], 5>,
+    tx_buf: &'d Channel<CriticalSectionRawMutex, &'d mut [u8], 5>,
 }
 
 impl<'d> embassy_net_driver::TxToken for TxToken<'d> {
@@ -115,11 +117,13 @@ impl<'d> embassy_net_driver::TxToken for TxToken<'d> {
     where
         F: FnOnce(&mut [u8]) -> R,
     {
-        // NOTE(unwrap): we checked the queue wasn't full when creating the token.
-        // let pkt = unwrap!(self.tx.available());
-        let pkt = &mut [];
-        let r = f(&mut pkt[..len]);
-        // self.tx.transmit(len);
+        // Only valid tx buffers should be put into the queue
+        let buf = self.tx_buf.try_recv().unwrap();
+        let r = f(&mut buf[..len]);
+
+        // The tx channel should always be of equal capacity to the tx_buf channel
+        self.tx.try_send(buf).unwrap();
+
         r
     }
 }
diff --git a/embassy-stm32-wpan/src/mac/runner.rs b/embassy-stm32-wpan/src/mac/runner.rs
index 911ff60b9..9edcff9b1 100644
--- a/embassy-stm32-wpan/src/mac/runner.rs
+++ b/embassy-stm32-wpan/src/mac/runner.rs
@@ -1,83 +1,80 @@
-use embassy_futures::select::{select3, Either3};
+use embassy_futures::join;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::Channel;
-use embassy_sync::waitqueue::AtomicWaker;
 
+use crate::mac::commands::DataRequest;
 use crate::mac::event::{Event, MacEvent};
+use crate::mac::typedefs::{AddressMode, MacAddress, PanId, SecurityLevel};
 use crate::mac::MTU;
 use crate::sub::mac::Mac;
 
-pub(crate) struct TxRing {
-    // stores n packets of up to mtu size
-    ring: [[u8; MTU]; 5],
-    pending: bool,
-    //    start: u8,
-    //    end: u8,
-}
-
-impl TxRing {
-    pub(crate) fn new() -> Self {
-        Self {
-            ring: [[0; MTU]; 5],
-            pending: false,
-        }
-    }
-
-    // wait for a free packet to become available
-    pub fn is_packet_free(&self) -> bool {
-        !self.pending
-    }
-
-    // get the next available free packet
-    pub fn get_free_packet<'a>(&'a mut self) -> &'a mut [u8] {
-        self.pending = true;
-
-        &mut self.ring[0]
-    }
-
-    pub fn get_packet_to_transmit<'a>(&'a mut self) -> Option<&'a [u8]> {
-        if self.pending {
-            self.pending = false;
-
-            Some(&self.ring[0])
-        } else {
-            None
-        }
-    }
-}
-
 pub struct Runner<'a> {
     mac_subsystem: Mac,
     pub(crate) rx_channel: Channel<CriticalSectionRawMutex, Event, 1>,
-    pub(crate) tx_channel: Channel<CriticalSectionRawMutex, &'a [u8], 1>,
+    pub(crate) tx_channel: Channel<CriticalSectionRawMutex, &'a mut [u8], 5>,
+    pub(crate) tx_buf_channel: Channel<CriticalSectionRawMutex, &'a mut [u8], 5>,
 }
 
 impl<'a> Runner<'a> {
-    pub fn new(mac: Mac) -> Self {
-        Self {
+    pub fn new(mac: Mac, tx_buf_queue: [&'a mut [u8; MTU]; 5]) -> Self {
+        let this = Self {
             mac_subsystem: mac,
             rx_channel: Channel::new(),
             tx_channel: Channel::new(),
+            tx_buf_channel: Channel::new(),
+        };
+
+        for buf in tx_buf_queue {
+            this.tx_buf_channel.try_send(buf).unwrap();
         }
-    }
 
-    pub(crate) async fn init(&mut self, firmware: &[u8]) {
-        debug!("wifi init done");
+        this
     }
 
     pub async fn run(&self) -> ! {
-        loop {
-            let event = self.mac_subsystem.read().await;
-            if let Ok(evt) = event.mac_event() {
-                match evt {
-                    MacEvent::McpsDataInd(data_ind) => {
-                        self.rx_channel.try_send(event);
+        join::join(
+            async {
+                loop {
+                    let event = self.mac_subsystem.read().await;
+                    if let Ok(evt) = event.mac_event() {
+                        match evt {
+                            MacEvent::McpsDataInd(_) => {
+                                self.rx_channel.send(event).await;
+                            }
+                            _ => {}
+                        }
                     }
-                    _ => {}
                 }
-            }
+            },
+            async {
+                loop {
+                    let buf = self.tx_channel.recv().await;
 
-            // TODO: select tx event
-        }
+                    self.mac_subsystem
+                        .send_command(
+                            DataRequest {
+                                src_addr_mode: AddressMode::Short,
+                                dst_addr_mode: AddressMode::Short,
+                                dst_pan_id: PanId([0x1A, 0xAA]),
+                                dst_address: MacAddress::BROADCAST,
+                                msdu_handle: 0x02,
+                                ack_tx: 0x00,
+                                gts_tx: false,
+                                security_level: SecurityLevel::Unsecure,
+                                ..Default::default()
+                            }
+                            .set_buffer(&buf),
+                        )
+                        .await
+                        .unwrap();
+
+                    // The tx channel should always be of equal capacity to the tx_buf channel
+                    self.tx_buf_channel.try_send(buf).unwrap();
+                }
+            },
+        )
+        .await;
+
+        loop {}
     }
 }
diff --git a/examples/stm32wb/Cargo.toml b/examples/stm32wb/Cargo.toml
index 8585b99f9..7c0b83e65 100644
--- a/examples/stm32wb/Cargo.toml
+++ b/examples/stm32wb/Cargo.toml
@@ -21,7 +21,7 @@ embedded-hal = "0.2.6"
 panic-probe = { version = "0.3", features = ["print-defmt"] }
 futures = { version = "0.3.17", default-features = false, features = ["async-await"] }
 heapless = { version = "0.7.5", default-features = false }
-
+static_cell = { version = "1.1", features = ["nightly"]}
 
 [features]
 default = ["ble", "mac"]
@@ -41,6 +41,10 @@ name = "mac_ffd"
 required-features = ["mac"]
 
 [[bin]] 
+name = "mac_ffd_net"
+required-features = ["mac"]
+
+[[bin]] 
 name = "eddystone_beacon"
 required-features = ["ble"]
 
diff --git a/examples/stm32wb/src/bin/mac_ffd_net.rs b/examples/stm32wb/src/bin/mac_ffd_net.rs
index 6072f418c..7b8c4b9dc 100644
--- a/examples/stm32wb/src/bin/mac_ffd_net.rs
+++ b/examples/stm32wb/src/bin/mac_ffd_net.rs
@@ -172,7 +172,15 @@ async fn main(spawner: Spawner) {
         defmt::info!("{:#x}", evt.mac_event());
     }
 
-    let runner = make_static!(Runner::new(mbox.mac_subsystem));
+    let tx_queue = [
+        make_static!([0u8; 127]),
+        make_static!([0u8; 127]),
+        make_static!([0u8; 127]),
+        make_static!([0u8; 127]),
+        make_static!([0u8; 127]),
+    ];
+
+    let runner = make_static!(Runner::new(mbox.mac_subsystem, tx_queue));
 
     spawner.spawn(run_mac(runner)).unwrap();