radio: Enable TX of non-QoS frames

src/main.rs

@@ -133,11 +133,14 @@ async fn main(spawner: Spawner) -> ! {
         .inspect_err(log_screen_error);
 
     // Setup WiFi
-    let (_wifi_controller, interfaces) = esp_radio::wifi::new(
+    let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
         peripherals.WIFI,
         esp_radio::wifi::ControllerConfig::default(),
     )
     .expect("Failed to initialize Wi-Fi controller");
+    if let Err(err) = wifi_controller.set_band_mode(esp_radio::wifi::BandMode::_5G) {
+        error!("Failed to set WiFi to 5GHz only: {err}");
+    }
 
     critical_section::with(|cs| {
         WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
@@ -149,6 +152,9 @@ async fn main(spawner: Spawner) -> ! {
 
     info!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running");
 
+    let mac = esp_hal::efuse::base_mac_address();
+    info!("WiFi MAC: {mac}");
+
     // Seeed XIAO L67K:
     // RX:      D7/ GPIO12
     // TX:      D6/ GPIO11

src/radio.rs

@@ -1,14 +1,13 @@
 //! IEEE 802.11p C-ITS Capture with ESP32-C5
+#![allow(unused)]
+
+#[cfg(feature = "esp")]
+use esp_radio::wifi::sniffer;
 
 // -----------------------------
 // missing interface definitions
 // -----------------------------
 
-use alloc::format;
-use alloc::string::String;
-
-use esp_radio::wifi::sniffer;
-
 pub type Channel = core::ffi::c_int;
 
 unsafe extern "C" {
@@ -34,8 +33,9 @@ unsafe extern "C" {
     ///
     /// Options:
     /// - `enable`: Boolean option to enable 802.11p mode
-    /// - `zero`: Unknown, but need to be zero
-    unsafe fn phy_11p_set(enable: u8, zero: u8);
+    /// - `half_rate`: 5 MHz mode
+    unsafe fn phy_11p_set(enable: u8, half_rate: u8);
+
 }
 
 /// Enum values for `wifi_promiscuous_pkt_type_t` used in `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`
@@ -73,15 +73,16 @@ impl PromiscuousPktType {
     }
 }
 
+#[cfg(feature = "esp")]
 /// Configure WiFi Sniffer to a certain 802.11p channel
 pub fn setup_wifi_sniffer(
     channel: Channel,
     sniffer: &mut sniffer::Sniffer,
     callback: fn(sniffer::PromiscuousPkt<'_>),
-) -> Result<(), String> {
+) -> Result<(), alloc::string::String> {
     sniffer
         .set_promiscuous_mode(true)
-        .map_err(|err| format!("Failed to enable promiscuous mode: {err:?}"))?;
+        .map_err(|err| alloc::format!("Failed to enable promiscuous mode: {err:?}"))?;
 
     sniffer.set_receive_cb(callback);
 
@@ -92,7 +93,199 @@ pub fn setup_wifi_sniffer(
         phy_11p_set(1, 0);
         match phy_change_channel(primary, 1, 0, 0) {
             0 => Ok(()),
-            ret => Err(format!("Failed to change channel: {ret}")),
+            ret => Err(alloc::format!("Failed to change channel: {ret}")),
         }
     }
 }
+
+#[cfg(feature = "esp")]
+/// Sends 802.11p broadcast frame
+///
+/// Expects `data` to contain GN header (BTP header and ITS payload) to be a valid C-ITS/ ITS-G5 packet/ frame.
+///
+/// # Errors
+/// Fails when MAC is not the right size or returns frame send error
+pub fn send_80211_bcast_frame(
+    sniffer: &mut sniffer::Sniffer,
+    mac_addr: &[u8],
+    data: &[u8],
+) -> Result<(), alloc::string::String> {
+    const HEADER_SIZE: usize = 24 + 8;
+
+    let mac = mac_addr
+        .try_into()
+        .map_err(|err| alloc::format!("Unexpected MAC address size: {err}"))?;
+
+    // assemble frame
+    let mut frame_buf = alloc::vec::Vec::with_capacity(HEADER_SIZE + data.len());
+    frame_buf.extend_from_slice(&DataFrameHeader::new_80211p(mac).to_binary());
+    frame_buf.extend_from_slice(&LlcHeader::new_80211p().to_binary());
+    frame_buf.extend_from_slice(data);
+
+    // send
+    sniffer
+        .send_raw_frame(true, &frame_buf, true)
+        .map_err(|err| alloc::format!("Failed to send 802.11p frame: {err:?}"))
+}
+
+/// IEEE 802.11p frame header
+#[derive(Debug, Default)]
+struct DataFrameHeader {
+    /// byte 0: subtype in upper nibble, type in lower nibble
+    /// "normal" data frame: 0x08, QoS data frame: 0x88
+    pub type_and_subtype: u8,
+    /// byte 1: frame contol field, 0x00 for 802.11p
+    pub fcf_flags: u8,
+    /// byte 2..3: duration in micros, usually 0x0000 for 802.11p (toDS = 0, fromDS = 0)
+    pub duration: u16,
+    /// byte 4..9: receiver/ destination for 802.11p
+    pub address_1: [u8; 6],
+    /// byte 10..15: transmitter/ source for 802.11p
+    pub address_2: [u8; 6],
+    /// byte 16..21: BSSID for 802.11p
+    pub address_3: [u8; 6],
+    /// byte 22..24: upper 12 bits: seq. no, lower 4 bits: fragment no
+    pub sequence_control: u16,
+    // Note: address 4 is not used in 802.11p
+    // pub qos: Option<[u8; 2]>, // QoS frames are supported by radio library
+    // Note: 802.11p does not use HT mode(s)
+}
+
+impl DataFrameHeader {
+    const BCAST_MAC: [u8; 6] = [0xFF; 6];
+
+    /// Create an 802.11p (non-QoS) data frame header with a source MAC address
+    pub(crate) fn new_80211p(source_mac: &[u8; 6]) -> Self {
+        Self {
+            type_and_subtype: 0x08,
+            address_1: Self::BCAST_MAC,
+            address_2: *source_mac,
+            address_3: Self::BCAST_MAC,
+            ..Default::default()
+        }
+    }
+
+    pub(crate) fn to_binary(&self) -> [u8; 24] {
+        let mut buf = [0u8; 24];
+
+        buf[0] = self.type_and_subtype;
+
+        buf[1] = self.fcf_flags;
+
+        Self::write_u16(&mut buf, 2, self.duration);
+        Self::write_mac_addr(&mut buf, 4, &self.address_1);
+        Self::write_mac_addr(&mut buf, 10, &self.address_2);
+        Self::write_mac_addr(&mut buf, 16, &self.address_3);
+        Self::write_u16(&mut buf, 22, self.sequence_control);
+
+        buf
+    }
+
+    fn write_u16(buf: &mut [u8; 24], offset: usize, data: u16) {
+        for (idx, byte) in data.to_be_bytes().iter().enumerate() {
+            buf[idx + offset] = *byte;
+        }
+    }
+
+    fn write_mac_addr(buf: &mut [u8; 24], offset: usize, data: &[u8; 6]) {
+        for (idx, byte) in data.iter().enumerate() {
+            buf[idx + offset] = *byte;
+        }
+    }
+}
+
+/// IEEE 802.11p LLC header
+#[derive(Debug, Default)]
+struct LlcHeader {
+    /// byte 0: SNAP (0xaa) in 802.11p
+    pub dsap: u8, // SNAP in
+    /// byte 1: SNAP (0xaa) in 802.11p
+    pub lsap: u8, // SNAP in
+    /// byte 2: SNAP (0xaa) in 802.11p
+    pub control_1: u8, // 0x03
+    // Note: optional second control byte not used in 802.11p
+
+    // SNAP fields
+    /// set to 0 since ethertype is used for protocol ID in 802.11p
+    pub oui: [u8; 3],
+    /// ethertype GeoNetworking (0x8947) in 802.11p
+    pub protocol_id: u16,
+}
+
+impl LlcHeader {
+    pub(crate) fn new_80211p() -> Self {
+        Self {
+            dsap: 0xaa,
+            lsap: 0xaa,
+            control_1: 0x03,
+            oui: [0; 3],
+            protocol_id: 0x8947,
+        }
+    }
+
+    pub(crate) fn to_binary(&self) -> [u8; 8] {
+        let mut buf: [u8; 8] = [
+            self.dsap,
+            self.lsap,
+            self.control_1,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+            0x00,
+        ];
+
+        buf[3] = self.oui[0];
+        buf[4] = self.oui[1];
+        buf[5] = self.oui[2];
+
+        buf[6] = self.protocol_id.to_be_bytes()[0];
+        buf[7] = self.protocol_id.to_be_bytes()[1];
+
+        buf
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use super::*;
+
+    #[test]
+    fn data_frame_test() {
+        // build a typical IEEE 802.11p frame
+        const OWN_MAC: [u8; 6] = [0x12, 0x34, 0x56, 0x78, 0xab, 0xcd];
+        const REF_HEADER: [u8; 24] = [
+            0x08, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0x34, 0x56, 0x78,
+            0xab, 0xcd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00,
+        ];
+
+        let frame = DataFrameHeader::new_80211p(&OWN_MAC);
+
+        assert_eq!(0x08, frame.type_and_subtype); // data frame, subtype 0
+        assert_eq!(0x0000, frame.fcf_flags);
+        assert_eq!(0x0000, frame.duration);
+        assert_eq!([0xFF; 6], frame.address_1);
+        assert_eq!(OWN_MAC, frame.address_2);
+        assert_eq!([0xFF; 6], frame.address_3);
+        assert_eq!(0x0000, frame.sequence_control);
+
+        assert_eq!(REF_HEADER, frame.to_binary());
+    }
+
+    #[test]
+    fn llc_test() {
+        // build a typical IEEE 802.11p LLC header
+        const REF_HEADER: [u8; 8] = [0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x89, 0x47];
+
+        let llc = LlcHeader::new_80211p();
+
+        assert_eq!(0xaa, llc.dsap);
+        assert_eq!(0xaa, llc.lsap);
+        assert_eq!(0x03, llc.control_1);
+        assert_eq!([0x00; 3], llc.oui);
+        assert_eq!(0x8947, llc.protocol_id);
+
+        assert_eq!(REF_HEADER, llc.to_binary());
+    }
+}

src/test.rs

@@ -6,6 +6,7 @@ extern crate alloc;
 mod applogic;
 mod cache;
 mod geo_alg;
+mod radio;
 mod testdata;
 
 fn main() {