feat: Add some message parsing

src/main.rs

@@ -7,19 +7,22 @@
 )]
 // #![deny(clippy::large_stack_frames)]
 
+use alloc::vec::Vec;
 use core::cell::RefCell;
 
 use critical_section::Mutex;
+use crossbeam_queue::ArrayQueue;
 use embassy_executor::Spawner;
 use embassy_gps::gps::l76k;
 use esp_backtrace as _;
 use esp_hal::clock::CpuClock;
 use esp_hal::timer::timg::TimerGroup;
 use esp_radio::wifi;
-use log::{debug, info, warn};
+use log::{debug, error, info, warn};
 
 extern crate alloc;
 
+mod applogic;
 mod radio;
 
 const WIFI_CHANNEL: radio::Channel = 180;
@@ -85,6 +88,7 @@ esp_bootloader_esp_idf::esp_app_desc!();
 
 static GNSS_UPDATE: Mutex<RefCell<Option<embassy_gps::types::GpsFix>>> =
     Mutex::new(RefCell::new(None));
+static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
 
 #[allow(
     clippy::large_stack_frames,
@@ -111,6 +115,7 @@ async fn main(spawner: Spawner) -> ! {
     // - GPIO28
 
     esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536);
+    esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
 
     let timg0 = TimerGroup::new(peripherals.TIMG0);
     let sw_interrupt =
@@ -125,6 +130,10 @@ async fn main(spawner: Spawner) -> ! {
     )
     .expect("Failed to initialize Wi-Fi controller");
 
+    critical_section::with(|cs| {
+        WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
+    });
+
     let mut sniffer = interfaces.sniffer;
     radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut sniffer, handle_frame)
         .expect("Fatal error initializing 802.11p sniffer");
@@ -150,7 +159,7 @@ async fn main(spawner: Spawner) -> ! {
 
     let mut state = State::default();
     loop {
-        embassy_time::Timer::after(embassy_time::Duration::from_millis(100)).await;
+        embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await;
 
         critical_section::with(|cs| {
             let gnss_update_ref = GNSS_UPDATE.borrow(cs);
@@ -164,6 +173,68 @@ async fn main(spawner: Spawner) -> ! {
                 }
             }
         });
+
+        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 let Some(data) = queue.pop() {
+                match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) {
+                    Ok(msg) => {
+                        use c_its_parser::ItsMessage;
+                        use c_its_parser::standards::extensions::ItsMessageId;
+
+                        debug!("Got new {:?} message", ItsMessageId::from(&msg));
+
+                        match msg {
+                            #[cfg(feature = "spat")]
+                            ItsMessage::Mapem {
+                                geonetworking: _,
+                                transport: _,
+                                etsi,
+                            } => applogic::tlc::handle_mapem(&etsi),
+                            #[cfg(feature = "spat")]
+                            ItsMessage::Spatem {
+                                geonetworking: _,
+                                transport: _,
+                                etsi,
+                            } => {
+                                if state.initialized {
+                                    applogic::tlc::handle_spatem(&etsi, &state.time);
+                                }
+                            }
+
+                            #[cfg(feature = "cam")]
+                            ItsMessage::Cam {
+                                geonetworking: _,
+                                transport: _,
+                                etsi,
+                            } => applogic::cam::handle_cam(&etsi),
+
+                            #[cfg(feature = "denm")]
+                            ItsMessage::DenmV2 {
+                                geonetworking: _,
+                                transport: _,
+                                etsi,
+                            } => applogic::denm::handle_denm(&etsi),
+                            #[cfg(feature = "denm")]
+                            ItsMessage::DenmV1 {
+                                geonetworking: _,
+                                transport: _,
+                                etsi: _,
+                            } => {
+                                // irrelevant since all received DENMs should be parsed as v2
+                            }
+                        }
+                    }
+                    Err(err) => {
+                        // will give false-positives when a message is received which wasn't enabled
+                        debug!("Failed to parse V2X message: {err}");
+                    }
+                }
+            }
+        });
     }
 }
 
@@ -183,8 +254,32 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
         return;
     }
 
-    // TODO: handle 802.11 frame
-    info!("Received frame with {} bytes", frame.len);
+    // Ignore non-broadcast frames
+    if frame.len < (4 + 6) {
+        warn!(
+            "Received frame too small to be a valid broadcast frame: {} bytes",
+            frame.len
+        );
+        return;
+    }
+    let dest_mac = &frame.data[4..10];
+    if dest_mac != [0xff; 6] {
+        warn!("Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}");
+        return;
+    }
+
+    debug!("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");
+        }
+    });
 }
 
 #[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]