WIP BLE: Add raw messages

TODO: make BLE TX queue an array

docs/ble-protocol.md

@@ -101,3 +101,11 @@ Repetitions of the same message should be dropped by the BLE server.
 Consecutive messages with no relevant changes may be dropped by the BLE server while keeping some minimal publishing rate, e.g.:
 - only publish MAPEMs every 10 seconds (since content doesn't change, but client may be restarted after first MAPEM from an intersection was received)
 - drop high-frequency CAMs (only publish "full" CAMs which include the low-frequency container)
+
+Maximum message size (assuming realistic value ranges):
+
+field name            | tag size | (max.) data size
+--------------------- | -------- | ---------
+msg_type              |        1 | 1 (VARINT(21))
+payload               |        1 | 2+1394 (LEN(1394) + payload)
+**SUM**               |      2 + | 1397 = 1399 byte

src/ble.rs

@@ -16,14 +16,24 @@ pub const CONTROLLER_SLOTS: usize = 20;
 const CONNECTIONS_MAX: usize = 1;
 const L2CAP_CHANNELS_MAX: usize = 1;
 
-pub static TX_DATA: Mutex<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None));
+pub static TX_DATA: Mutex<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None)); // TODO: this needs to be an array!
 pub static RX_DATA: Mutex<RefCell<Option<io::msg::PositionState>>> = Mutex::new(RefCell::new(None));
 
 pub enum BleSendable {
+    #[cfg(feature = "cam")]
+    CamRaw(io::msg::RawMsgRx),
     #[cfg(feature = "cam")]
     Cam(io::msg::CamEvent),
+
+    #[cfg(feature = "denm")]
+    DenmRaw(io::msg::RawMsgRx),
     #[cfg(feature = "denm")]
     Denm(io::msg::DenmEvent),
+
+    #[cfg(feature = "spat")]
+    MapemRaw(io::msg::RawMsgRx),
+    #[cfg(feature = "spat")]
+    SpatemRaw(io::msg::RawMsgRx),
 }
 
 #[gatt_server]
@@ -31,26 +41,38 @@ struct Server {
     c_its_events: CITSEvents,
 }
 
-#[cfg(feature = "cam")]
-const CAM_EVENT_SIZE: usize = 38;
-#[cfg(feature = "denm")]
-const DENM_EVENT_SIZE: usize = 51;
-
 #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")]
 struct CITSEvents {
     #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)]
     #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)]
     position: [u8; 27],
 
+    #[cfg(feature = "cam")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
+    cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
     #[cfg(feature = "cam")]
     #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)]
-    #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])]
+    #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])]
-    cam_event: [u8; CAM_EVENT_SIZE],
+    cam_event: [u8; io::msg::CamEvent::PROTO_SIZE],
 
+    #[cfg(feature = "denm")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
+    denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
     #[cfg(feature = "denm")]
     #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)]
-    #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])]
+    #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_SIZE])]
-    denm_event: [u8; DENM_EVENT_SIZE],
+    denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE],
+
+    #[cfg(feature = "spat")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4006-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
+    mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
+    #[cfg(feature = "spat")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
+    spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
 }
 
 /// Creates the BLE controller
@@ -220,43 +242,61 @@ async fn custom_task<P: PacketPool>(server: &Server<'_>, conn: &GattConnection<'
 
         match new_data {
             #[cfg(feature = "cam")]
-            Some(BleSendable::Cam(cam_event)) => {
+            Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() {
-                let mut buf = cam_event.serialize_ble_proto();
+                Ok(value) => {
-
-                if buf.len() <= CAM_EVENT_SIZE {
-                    // pad with zeros
-                    let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()];
-                    buf.extend_from_slice(&pad);
-
-                    // unwrap should be fine since we ensured the vector size before
-                    let value = buf.try_into().unwrap();
-
                     if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await {
                         warn!("BLE: failed notifying connection of new CAM event: {err:?}");
                     }
-                } else {
-                    warn!("BLE: CAM event too big with {} bytes", buf.len());
                 }
-            }
+                Err(err) => warn!("BLE: CAM event {err}"),
+            },
+
             #[cfg(feature = "denm")]
-            Some(BleSendable::Denm(denm_event)) => {
+            Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() {
-                let mut buf = denm_event.serialize_ble_proto();
+                Ok(value) => {
-
-                if buf.len() <= DENM_EVENT_SIZE {
-                    // pad with zeros
-                    let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()];
-                    buf.extend_from_slice(&pad);
-
-                    // unwrap should be fine since we ensured the vector size before
-                    let value = buf.try_into().unwrap();
-
                     if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await {
                         warn!("BLE: failed notifying connection of new DENM event: {err:?}");
                     }
-                } else {
-                    warn!("BLE: DENM event too big with {} bytes", buf.len());
                 }
-            }
+                Err(err) => warn!("BLE: DENM event {err}"),
+            },
+
+            #[cfg(feature = "cam")]
+            Some(BleSendable::CamRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
+                Ok(value) => {
+                    if let Err(err) = server.c_its_events.cam_raw.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new raw CAM: {err:?}");
+                    }
+                }
+                Err(err) => warn!("BLE: Raw CAM too big: {err}"),
+            },
+            #[cfg(feature = "denm")]
+            Some(BleSendable::DenmRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
+                Ok(value) => {
+                    if let Err(err) = server.c_its_events.denm_raw.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new raw DENM: {err:?}");
+                    }
+                }
+                Err(err) => warn!("BLE: Raw DENM {err}"),
+            },
+            #[cfg(feature = "spat")]
+            Some(BleSendable::MapemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
+                Ok(value) => {
+                    if let Err(err) = server.c_its_events.mapem_raw.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new raw MAPEM: {err:?}");
+                    }
+                }
+                Err(err) => warn!("BLE: Raw MAPEM {err}"),
+            },
+            #[cfg(feature = "spat")]
+            Some(BleSendable::SpatemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
+                Ok(value) => {
+                    if let Err(err) = server.c_its_events.spatem_raw.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new raw SPATEM: {err:?}");
+                    }
+                }
+                Err(err) => warn!("BLE: Raw SPATEM {err}"),
+            },
             None => {}
         }
     }

src/io.rs

@@ -40,20 +40,60 @@ impl msg::RawMsgRx {
 #[cfg(feature = "denm")]
 impl msg::DenmEvent {
     #[cfg(feature = "ble")]
-    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
+    pub const PROTO_SIZE: usize = 51;
+
+    #[cfg(feature = "ble")]
+    pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> {
         use prost::Message as _;
 
-        self.encode_to_vec()
+        let mut data = [0u8; Self::PROTO_SIZE];
+
+        let mut mut_buf = &mut data[..];
+        if let Err(err) = self.encode_length_delimited(&mut mut_buf) {
+            Err(alloc::format!("Buffer too small: {err:?}"))
+        } else {
+            Ok(data)
+        }
     }
 }
 
 #[cfg(feature = "cam")]
 impl msg::CamEvent {
     #[cfg(feature = "ble")]
-    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
+    pub const PROTO_SIZE: usize = 38;
+
+    #[cfg(feature = "ble")]
+    pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> {
         use prost::Message as _;
 
-        self.encode_to_vec()
+        let mut data = [0u8; Self::PROTO_SIZE];
+
+        let mut mut_buf = &mut data[..];
+        if let Err(err) = self.encode_length_delimited(&mut mut_buf) {
+            Err(alloc::format!("Buffer too small: {err:?}"))
+        } else {
+            Ok(data)
+        }
+    }
+}
+
+#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))]
+impl msg::RawMsgRx {
+    #[cfg(feature = "ble")]
+    pub const PROTO_SIZE: usize = 1400;
+
+    #[cfg(feature = "ble")]
+    pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> {
+        use prost::Message as _;
+
+        let mut data = [0u8; Self::PROTO_SIZE];
+
+        let mut mut_buf = &mut data[..];
+        if let Err(err) = self.encode_length_delimited(&mut mut_buf) {
+            Err(alloc::format!("Buffer too small: {err:?}"))
+        } else {
+            Ok(data)
+        }
     }
 }
 
@@ -592,6 +632,53 @@ impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection> fo
     }
 }
 
+#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))]
+impl TryFrom<&c_its_parser::ItsMessage<'_>> for msg::RawMsgRx {
+    type Error = alloc::string::String;
+
+    fn try_from(value: &c_its_parser::ItsMessage<'_>) -> Result<Self, Self::Error> {
+        let msg_id = c_its_parser::standards::extensions::ItsMessageId::from(value);
+
+        let payload = match value {
+            #[cfg(feature = "denm")]
+            c_its_parser::ItsMessage::DenmV1 {
+                geonetworking: _,
+                transport: _,
+                etsi,
+            } => etsi.encode_to_uper(),
+            #[cfg(feature = "denm")]
+            c_its_parser::ItsMessage::DenmV2 {
+                geonetworking: _,
+                transport: _,
+                etsi,
+            } => etsi.encode_to_uper(),
+            #[cfg(feature = "cam")]
+            c_its_parser::ItsMessage::Cam {
+                geonetworking: _,
+                transport: _,
+                etsi,
+            } => etsi.encode_to_uper(),
+            #[cfg(feature = "spat")]
+            c_its_parser::ItsMessage::Spatem {
+                geonetworking: _,
+                transport: _,
+                etsi,
+            } => etsi.encode_to_uper(),
+            #[cfg(feature = "spat")]
+            c_its_parser::ItsMessage::Mapem {
+                geonetworking: _,
+                transport: _,
+                etsi,
+            } => etsi.encode_to_uper(),
+        }?;
+
+        Ok(Self {
+            msg_type: msg_id.as_u8().into(),
+            payload,
+        })
+    }
+}
+
 #[cfg(test)]
 mod tests {
 

src/main.rs

@@ -506,17 +506,45 @@ async fn main(spawner: Spawner) -> ! {
                         ItsMessage::Mapem {
                             geonetworking: _,
                             transport: _,
-                            etsi,
+                            ref etsi,
-                        } => state.handle_mapem(&etsi),
+                        } => {
+                            state.handle_mapem(&etsi);
+
+                            // Send data to BLE thread
+                            #[cfg(feature = "ble")]
+                            match io::msg::RawMsgRx::try_from(&msg) {
+                                Ok(data) => {
+                                    critical_section::with(|cs| {
+                                        let ble_update_ref = ble::TX_DATA.borrow(cs);
+                                        ble_update_ref
+                                            .replace(Some(ble::BleSendable::MapemRaw(data)));
+                                    });
+                                }
+                                Err(err) => warn!("Failed to create RawMsgRx: {err}"),
+                            }
+                        }
                         #[cfg(feature = "spat")]
                         ItsMessage::Spatem {
                             geonetworking: _,
                             transport: _,
-                            etsi,
+                            ref etsi,
                         } => {
                             if state.initialized() {
                                 state.handle_spatem(&etsi);
                             }
+
+                            // Send data to BLE thread
+                            #[cfg(feature = "ble")]
+                            match io::msg::RawMsgRx::try_from(&msg) {
+                                Ok(data) => {
+                                    critical_section::with(|cs| {
+                                        let ble_update_ref = ble::TX_DATA.borrow(cs);
+                                        ble_update_ref
+                                            .replace(Some(ble::BleSendable::SpatemRaw(data)));
+                                    });
+                                }
+                                Err(err) => warn!("Failed to create RawMsgRx: {err}"),
+                            }
                         }
 
                         #[cfg(any(feature = "cam", feature = "cam_tx"))]
@@ -534,6 +562,8 @@ async fn main(spawner: Spawner) -> ! {
                                 let ble_update_ref = ble::TX_DATA.borrow(cs);
                                 ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into())));
                             });
+
+                            // TODO: Send Raw message, but only when it has high-freq container
                         }
 
                         #[cfg(feature = "denm")]
@@ -550,6 +580,8 @@ async fn main(spawner: Spawner) -> ! {
                                 let ble_update_ref = ble::TX_DATA.borrow(cs);
                                 ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into())));
                             });
+
+                            // TODO: Send Raw message
                         }
                         #[cfg(feature = "denm")]
                         ItsMessage::DenmV1 {