v2x: drop hopped messages and unsupported message types

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",

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",

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
+                    // we already filtered out all unsupported message types in the wifi rx callback
-                    debug!("Failed to parse V2X message: {err}");
+                    // so these are actual errors
+                    warn!("Failed to parse V2X message: {err}");
                 }
             }
         }
@@ -294,17 +298,78 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
 
     // info!("Received frame with {} bytes", frame.len);
 
+    // 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(frame.data.to_vec()).is_err() {
+                    if queue.push(payload.to_vec()).is_err() {
                         error!("V2X RX queue is full");
                     }
                 });
             }
+        }
+    }
+}
+
+fn get_its_header<'p>(
+    packet: &geonetworking::Decoded<geonetworking::Packet<'p>>,
+) -> 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")]
 #[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]