diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 8d742e2..01448c6 100644 --- a/docs/ble-protocol.md +++ b/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 diff --git a/src/ble.rs b/src/ble.rs index 78403f7..8fc86e5 100644 --- a/src/ble.rs +++ b/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>> = Mutex::new(RefCell::new(None)); +pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); // TODO: this needs to be an array! pub static RX_DATA: Mutex>> = 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])] - cam_event: [u8; CAM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_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])] - denm_event: [u8; DENM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_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(server: &Server<'_>, conn: &GattConnection<' match new_data { #[cfg(feature = "cam")] - Some(BleSendable::Cam(cam_event)) => { - let mut buf = cam_event.serialize_ble_proto(); - - 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(); - + Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() { + Ok(value) => { 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)) => { - let mut buf = denm_event.serialize_ble_proto(); - - 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(); - + Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() { + Ok(value) => { 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 => {} } } diff --git a/src/io.rs b/src/io.rs index 3ef8c6d..aeeab92 100644 --- a/src/io.rs +++ b/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 { + 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 { + 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 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 { + 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 { diff --git a/src/main.rs b/src/main.rs index 5e4e0c5..451ec21 100644 --- a/src/main.rs +++ b/src/main.rs @@ -506,17 +506,45 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Mapem { geonetworking: _, transport: _, - etsi, - } => state.handle_mapem(&etsi), + ref 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 {