v2x: drop hopped messages and unsupported message types

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<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")]