WIP: try to debug V2X rx dropouts

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,8 +7,11 @@
 )]
 // #![deny(clippy::large_stack_frames)]
 
+use alloc::string::ToString as _;
 use alloc::vec::Vec;
 use core::cell::RefCell;
+#[cfg(feature = "spat")]
+use core::cell::UnsafeCell;
 
 use critical_section::Mutex;
 use crossbeam_queue::ArrayQueue;
@@ -23,6 +26,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;
@@ -40,6 +45,8 @@ const WIFI_CHANNEL: radio::Channel = 180;
 pub const DISPLAY_WIDTH: u16 = 320;
 #[cfg(feature = "screen")]
 pub const DISPLAY_HEIGHT: u16 = 172;
+#[cfg(feature = "spat")]
+const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message
 
 // This creates a default app-descriptor required by the esp-idf bootloader.
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
@@ -227,12 +234,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));
+                    // info!("Got new {:?} message", ItsMessageId::from(&msg));
 
                     match msg {
                         #[cfg(feature = "spat")]
@@ -276,8 +283,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}");
                 }
             }
         }
@@ -291,7 +299,30 @@ async fn main(spawner: Spawner) -> ! {
     clippy::needless_pass_by_value,
     reason = "adhering to callback interface"
 )]
+#[allow(clippy::too_many_lines)]
 fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
+    #[cfg(feature = "spat")]
+    const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60);
+    #[cfg(feature = "spat")]
+    static mut RATELIMIT_CACHE_LAST_PRUNE: UnsafeCell<chrono::NaiveDateTime> =
+        UnsafeCell::new(chrono::NaiveDateTime::MIN);
+
+    #[cfg(feature = "spat")]
+    static mut SPAT_RATELIMIT_CACHE: UnsafeCell<cache::Cache<u32, RatelimitData>> = UnsafeCell::new(
+        cache::Cache::<u32, RatelimitData>::new(chrono::Duration::seconds(20)),
+    );
+
+    #[cfg(feature = "spat")]
+    let now = {
+        let time = esp_hal::time::Instant::now().duration_since_epoch();
+        // unwrap is fine since value range is u64 microseconds
+        chrono::DateTime::from_timestamp(time.as_secs().cast_signed(), 0)
+            .unwrap()
+            .naive_utc()
+    };
+
+    esp_println::print!("."); // TODO: debug only!
+
     // Ignore frames with errors and non-data frames
     if frame.rx_cntl.rx_state != 0 {
         warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state);
@@ -319,16 +350,131 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
 
     debug!("Received frame with {} bytes", frame.len);
 
-    // Send data to main thread
+    // parse GN headers
-    critical_section::with(|cs| {
+    match c_its_parser::pcap::remove_wlan_headers(frame.data)
-        let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow();
+        .map_err(|err| alloc::format!("Failed to parse WLAN headers: {err}"))
-        // unwrap is fine b/c we stored something in it before
+        .and_then(|data| {
-        let queue = queue_rc.as_ref().unwrap();
+            geonetworking::Packet::decode(data)
-
+                .map_err(|err| alloc::format!("Failed to parse GN headers: {err:?}"))
-        if queue.push(frame.data.to_vec()).is_err() {
+        }) {
-            error!("V2X RX queue is full");
+        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, station_id)) = get_its_header(&packet) {
+                // drop all unsupported message IDs and rate-limit SPATEMs
+                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 => {
+                        // reduce SPAT rate per station ID
+                        let mut drop_msg = false;
+                        unsafe {
+                            #[allow(
+                                static_mut_refs,
+                                reason = "this function will only run consecutive"
+                            )]
+                            let spat_cache = SPAT_RATELIMIT_CACHE.get();
+
+                            (*spat_cache).update_or_init(now, station_id, |i| {
+                                i.msg_count = i.msg_count.wrapping_add(1);
+
+                                if (i.msg_count % SPAT_RATE_LIMIT) > 0 {
+                                    drop_msg = true;
+                                }
+                            });
+                        }
+
+                        drop_msg
+                    }
+                    #[cfg(feature = "spat")]
+                    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");
+                    }
+                });
+            }
+        }
+    }
+
+    #[cfg(feature = "spat")]
+    unsafe {
+        #[allow(static_mut_refs, reason = "this function will only run consecutive")]
+        let last_prune_time = RATELIMIT_CACHE_LAST_PRUNE.get();
+        if *last_prune_time + PRUNE_INTERVAL < now {
+            #[allow(static_mut_refs, reason = "this function will only run consecutive")]
+            let spat_cache = SPAT_RATELIMIT_CACHE.get();
+            (*spat_cache).prune(now);
+
+            *last_prune_time = now;
+        }
+    }
+}
+
+#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
+struct RatelimitData {
+    station_id: u32,
+    msg_count: u8,
+}
+
+impl cache::Cachable<u32> for RatelimitData {
+    fn key(&self) -> u32 {
+        self.station_id
+    }
+}
+impl cache::Initable<Self, u32> for RatelimitData {
+    fn init(key: u32) -> Self {
+        Self {
+            station_id: key,
+            ..Default::default()
+        }
+    }
+}
+
+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")]