diff --git a/Cargo.lock b/Cargo.lock index 0b7e161..ab58dc3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -131,9 +131,9 @@ checksum = "c4512299f36f043ab09a583e57bceb5a5aab7a73db1805848e8fef3c9e8c78b3" [[package]] name = "bitvec" -version = "1.0.1" +version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1bc2832c24239b0141d5674bb9174f9d68a8b5b3f2753311927c172ca46f7e9c" +checksum = "ddcec3d12c579d40898fe0a9a358a803c23e9c52ca3c425707f81c9436211837" dependencies = [ "funty", "radium", @@ -186,9 +186,9 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" [[package]] name = "c-its-parser" -version = "2.2.5" +version = "2.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "993244dbda1234597ca13fe2e2565fc6f2ece585040404faa011482b55ed544e" +checksum = "f9bd739e1f76c017fc9fc6c21b9ff2aee4536bef357075d454e387e94a4d6287" dependencies = [ "chrono", "etherparse", @@ -1347,9 +1347,9 @@ dependencies = [ [[package]] name = "geonetworking" -version = "0.2.1" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ea4f48e7b1cdf91ad77ec1e4c5617aee3666a929288f4a3f11e473b67d3a6314" +checksum = "6f6d36111f5057d3be7eb4e312a07edac4737fc791e4b0a73d812c6f710161e7" dependencies = [ "bitvec", "bitvec-nom2", diff --git a/Cargo.toml b/Cargo.toml index 3eddcc6..546cb89 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -85,7 +85,7 @@ esp-radio = { version = "0.18.0", optional = true, features = [ embassy_gps = { version = "0.1.0", optional = true, default-features = false, features = ["esp", "log-04"], git = "https://github.com/jbeyerstedt/embassy_gps.git", branch = "feat/rework-gpsfix" } embedded-hal-bus = { version = "0.3.0", optional = true } embedded-graphics = { version = "0.8.2", optional = true } -c-its-parser = { version = "2.2.4", default-features = false, features = [ +c-its-parser = { version = "2.3.0", default-features = false, features = [ "time", "geo", "libm", diff --git a/src/main.rs b/src/main.rs index 35f6602..7f0fb48 100644 --- a/src/main.rs +++ b/src/main.rs @@ -7,9 +7,11 @@ )] // #![deny(clippy::large_stack_frames)] +use alloc::string::ToString as _; use alloc::vec::Vec; use core::cell::RefCell; +use c_its_parser::gn as geonetworking; use critical_section::Mutex; use crossbeam_queue::ArrayQueue; use embassy_executor::Spawner; @@ -21,6 +23,7 @@ use esp_hal::timer::timg::TimerGroup; #[cfg(all(target_arch = "riscv32", feature = "spat"))] use esp_println::println; use esp_radio::wifi; +use geonetworking::Decode as _; use log::{debug, error, info, warn}; extern crate alloc; @@ -204,12 +207,12 @@ async fn main(spawner: Spawner) -> ! { } }); if let Some(data) = new_data { - match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) { + match c_its_parser::de::decode(&data, c_its_parser::Headers::None) { Ok(msg) => { use c_its_parser::ItsMessage; - use c_its_parser::standards::extensions::ItsMessageId; + // use c_its_parser::standards::extensions::ItsMessageId; - debug!("Got new {:?} message", ItsMessageId::from(&msg)); + // debug!("Got new {:?} message", ItsMessageId::from(&msg)); match msg { #[cfg(feature = "spat")] @@ -253,8 +256,9 @@ async fn main(spawner: Spawner) -> ! { } } Err(err) => { - // will give false-positives when a message is received which wasn't enabled - debug!("Failed to parse V2X message: {err}"); + // we already filtered out all unsupported message types in the wifi rx callback + // so these are actual errors + warn!("Failed to parse V2X message: {err}"); } } } @@ -294,16 +298,77 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { // info!("Received frame with {} bytes", frame.len); - // Send data to main thread - critical_section::with(|cs| { - let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow(); - // unwrap is fine b/c we stored something in it before - let queue = queue_rc.as_ref().unwrap(); - - if queue.push(frame.data.to_vec()).is_err() { - error!("V2X RX queue is full"); + // parse GN headers + match c_its_parser::pcap::remove_wlan_headers(frame.data) + .map_err(|err| alloc::format!("Failed to parse WLAN headers: {err}")) + .and_then(|data| { + geonetworking::Packet::decode(data) + .map_err(|err| alloc::format!("Failed to parse GN headers: {err:?}")) + }) { + Err(err) => { + warn!("{err}"); } - }); + Ok(packet) => { + // drop hopped (for now) + if packet.decoded.is_hopped() { + return; + } + + // "parse" ITS PDU Header + if let Ok((payload, message_id, _)) = get_its_header(&packet) { + // drop all unsupported message IDs + if match message_id { + #[cfg(feature = "denm")] + c_its_parser::standards::extensions::ItsMessageId::Denm => false, + #[cfg(feature = "cam")] + c_its_parser::standards::extensions::ItsMessageId::Cam => false, + #[cfg(feature = "spat")] + c_its_parser::standards::extensions::ItsMessageId::Spatem + | c_its_parser::standards::extensions::ItsMessageId::Mapem => false, + _ => true, + } { + return; + } + + // Send data to main thread + critical_section::with(|cs| { + let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow(); + // unwrap is fine b/c we stored something in it before + let queue = queue_rc.as_ref().unwrap(); + + if queue.push(payload.to_vec()).is_err() { + error!("V2X RX queue is full"); + } + }); + } + } + } +} + +fn get_its_header<'p>( + packet: &geonetworking::Decoded>, +) -> Result< + ( + &'p [u8], + c_its_parser::standards::extensions::ItsMessageId, + u32, + ), + alloc::string::String, +> { + let payload = packet.decoded.btp_payload()?; + + if payload.len() < 6 { + return Err("ITS payload too small for ItsPduHeader".to_string()); + } + + if let Ok(message_id) = payload[1].try_into() { + let station_id_buf: [u8; 4] = payload[2..6].try_into().unwrap(); + let station_id = u32::from_be_bytes(station_id_buf); + + Ok((payload, message_id, station_id)) + } else { + Err("Unknown message ID in ItsPduHeader".to_string()) + } } #[cfg(feature = "gnss")]