WIP v2x: drop hopped messages and unsupported message types

TODO: Wait for c-its-parser release with updated GN (for Packet.btp_payload,
		etc.)
TODO: Remove explicit GN dependency when feat/gn-pub-use is merged in
c-its-parser

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,14 +186,13 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33"
 
 [[package]]
 name = "c-its-parser"
-version = "2.2.5"
+version = "2.2.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
+source = "git+https://github.com/consider-it/C-ITS-Parser.git?branch=feat%2Fgn-update#481dc7a92ee43a0aee5389762e15a17168019b3a"
-checksum = "993244dbda1234597ca13fe2e2565fc6f2ece585040404faa011482b55ed544e"
 dependencies = [
  "chrono",
  "etherparse",
  "geo-types",
- "geonetworking",
+ "geonetworking 0.3.0-rc.6 (git+https://github.com/consider-it/geonetworking.git?branch=main)",
  "hex",
  "nom",
  "num-traits",
@@ -1156,6 +1155,7 @@ dependencies = [
  "esp-radio",
  "esp-rtos",
  "geo-types",
+ "geonetworking 0.3.0-rc.6 (registry+https://github.com/rust-lang/crates.io-index)",
  "log",
  "mipidsi",
  "num-traits",
@@ -1347,9 +1347,22 @@ dependencies = [
 
 [[package]]
 name = "geonetworking"
-version = "0.2.1"
+version = "0.3.0-rc.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ea4f48e7b1cdf91ad77ec1e4c5617aee3666a929288f4a3f11e473b67d3a6314"
+checksum = "c60eaa1367ffa087fa9a5c2c9f32578290a808232189e811746d84fcde01e479"
+dependencies = [
+ "bitvec",
+ "bitvec-nom2",
+ "bytes",
+ "nom",
+ "num",
+ "num-traits",
+]
+
+[[package]]
+name = "geonetworking"
+version = "0.3.0-rc.6"
+source = "git+https://github.com/consider-it/geonetworking.git?branch=main#fd97d28f547355e008caced31271361ed2893405"
 dependencies = [
  "bitvec",
  "bitvec-nom2",

Cargo.toml

@@ -85,7 +85,8 @@ 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 = [
+# needs newer GN version
+c-its-parser = { version = "2.2.4", default-features = false, branch = "feat/gn-update", git = "https://github.com/consider-it/C-ITS-Parser.git", features = [
   "time",
   "geo",
   "libm",
@@ -94,6 +95,7 @@ c-its-parser = { version = "2.2.4", default-features = false, features = [
 chrono = { version = "0.4.44", default-features = false, features = ["alloc"] }
 crossbeam-queue = { version = "0.3.12", default-features = false, features = ["alloc"] }
 geo-types = { version = "0.7.19", default-features = false }
+geonetworking = { version = "0.3.0-rc.5", default-features = false }
 mipidsi = { version = "0.10.0", optional = true }
 num-traits = { version = "0.2.19", default-features = false, features = ["libm"] }
 profont = { version = "0.7.0", optional = true }

src/main.rs

@@ -7,6 +7,7 @@
 )]
 // #![deny(clippy::large_stack_frames)]
 
+use alloc::string::ToString as _;
 use alloc::vec::Vec;
 use core::cell::RefCell;
 
@@ -23,6 +24,8 @@ use esp_hal::{delay, gpio, spi, time};
 #[cfg(all(target_arch = "riscv32", feature = "spat"))]
 use esp_println::println;
 use esp_radio::wifi;
+// use c_its_parser::gn as geonetworking;
+use geonetworking::Decode as _;
 use log::{debug, error, info, warn};
 
 extern crate alloc;
@@ -227,12 +230,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")]
@@ -276,8 +279,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}");
                 }
             }
         }
@@ -319,16 +323,77 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
 
     debug!("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")]