From 044a7357785faa7623b961cfebb3368fba0d8fa6 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Fri, 3 Jul 2026 11:29:46 +0200 Subject: [PATCH] radio: Enable TX of non-QoS frames --- .clippy.toml | 2 +- src/main.rs | 8 +- src/radio.rs | 213 ++++++++++++++++++++++++++++++++++++++++++++++++--- src/test.rs | 1 + 4 files changed, 212 insertions(+), 12 deletions(-) diff --git a/.clippy.toml b/.clippy.toml index 98e9866..3e969e0 100644 --- a/.clippy.toml +++ b/.clippy.toml @@ -1,2 +1,2 @@ stack-size-threshold = 1024 -doc-valid-idents = ["WiFi", ".."] +doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", ".."] diff --git a/src/main.rs b/src/main.rs index db182b4..1a5d2ef 100644 --- a/src/main.rs +++ b/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 diff --git a/src/radio.rs b/src/radio.rs index 652dbdf..325cbf0 100644 --- a/src/radio.rs +++ b/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()); + } +} diff --git a/src/test.rs b/src/test.rs index 3eb42dc..5a04150 100644 --- a/src/test.rs +++ b/src/test.rs @@ -6,6 +6,7 @@ extern crate alloc; mod applogic; mod cache; mod geo_alg; +mod radio; mod testdata; fn main() {