commit 72762ca558b51eb80671d9164cf851cce8c42c08 Author: Jannik Beyerstedt Date: Fri Jul 31 15:51:10 2026 +0200 initial commit diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..4df40d3 --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,17 @@ +[target.riscv32imac-unknown-none-elf] +runner = "espflash flash --monitor --chip esp32c5" + +[env] +ESP_LOG="info" + +[build] +rustflags = [ + # Required to obtain backtraces (e.g. when using the "esp-backtrace" crate.) + # NOTE: May negatively impact performance of produced code + "-C", "force-frame-pointers", +] + +target = "riscv32imac-unknown-none-elf" + +[unstable] +build-std = ["alloc", "core"] diff --git a/.clippy.toml b/.clippy.toml new file mode 100644 index 0000000..3e969e0 --- /dev/null +++ b/.clippy.toml @@ -0,0 +1,2 @@ +stack-size-threshold = 1024 +doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", ".."] diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..48dfc48 --- /dev/null +++ b/.gitignore @@ -0,0 +1,29 @@ +# will have compiled files and executables +debug/ +target/ + +# Editor configuration +.vscode/ +.zed/ +.helix/ +.nvim.lua + +# These are backup files generated by rustfmt +**/*.rs.bk + +# MSVC Windows builds of rustc generate these, which store debugging information +*.pdb + +# RustRover +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + + +# Ignore .DS_Store file in mac +**/.DS_Store + +# additional files +/Cargo.lock diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..61c3839 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,39 @@ +[package] +edition = "2024" +name = "esp32-cits-core" +rust-version = "1.88" +version = "0.1.0" + +[features] +default = ["esp32c5"] + +esp32c5 = ["dep:esp-radio", "dep:esp-hal"] +std = ["c-its-parser/std"] +# Enable logging output using version 0.4 of the log crate. +log-04 = ["esp-radio/log-04", "esp-hal/log-04"] +# Enable logging output using defmt and implement `defmt::Format` on certain types. +defmt = ["esp-radio/defmt", "esp-hal/defmt"] + + +[dependencies] +c-its-parser = { version = "2.4.1", default-features = false, features = [ + "time", + "geo", + "libm", + "transport", + "extensions", +] } +chrono = { version = "0.4.44", default-features = false, features = ["alloc"] } +esp-hal = { version = "~1.1.0", optional = true, features = ["esp32c5", "unstable"] } +esp-radio = { version = "0.18.0", optional = true, features = [ + "esp-alloc", + "esp32c5", + "unstable", + "wifi", + "sniffer", +] } +geo-types = { version = "0.7.19", default-features = false } + +[dev-dependencies] +assert_float_eq = "1.2.0" +env_logger = "0.11.10" diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..1d76f0d --- /dev/null +++ b/Readme.md @@ -0,0 +1,79 @@ +# ESP32-C5 C-ITS Core + +Common data structures and helper functions to receive and send C-ITS (V2X) data on an ESP32-C5. + +## Usage + +### Communicate with C-ITS Devices +Setup the connection like this in your `main` function: +```rust +// Setup WiFi +// sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer` +let (mut wifi_controller, interfaces) = esp_radio::wifi::new( + peripherals.WIFI, + esp_radio::wifi::ControllerConfig::default(), +) +.expect("Failed to initialize Wi-Fi controller"); + +if let Err(err) = wifi_controller.set_band_mode(esp_radio::wifi::BandMode::_5G) { + println!("Failed to set WiFi to 5GHz only: {err}"); +} +if let Err(err) = wifi_controller.set_max_tx_power(WIFI_TXPOWER * 4) { + println!("Failed to set WiFi TX power: {err}"); +} + +critical_section::with(|cs| { + WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2))); +}); +let mut wlan_iface = interfaces.sniffer; +radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut wlan_iface, handle_frame) + .expect("Fatal error initializing 802.11p sniffer"); + +println!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running"); + +let mac = esp_hal::efuse::base_mac_address(); +println!("WiFi MAC: {mac}"); +``` + +With a frame handler like this: +```rust +fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { + match esp32_cits_core::rx::is_broadcast_frame(&frame) { + Err(err) => { + warn!("{err}"); + return; + } + Ok(false) => { + // drop frame + return; + } + Ok(true) => { + // continue + } +} + + println!("Received frame with {} bytes", frame.len); + + // parse message + match c_its_parser::de::decode(frame.data, c_its_parser::Headers::IEEE802LlcGnBtp) { + Ok(its_message) => { + println!("New message: {its_message:?}"); + + // do something with the data + }, + Err(err) => println!("Failed to parse ITS message: {err}"), + } +} +``` + +### Sending +Implement `tx::GnItsPayload` and then call `encode_packet()`. +Then send frame with `radio::send_80211_bcast_frame()`. + + +### Run Unit Tests on Host +The unit tests in this project are meant to be run on the host: + +``` +cargo test --target host-tuple --no-default-features -F std +``` diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..4317306 --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,6 @@ +unstable_features = true +format_code_in_doc_comments = true + +group_imports = "StdExternalCrate" +imports_granularity = "Module" +imports_layout = "HorizontalVertical" diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 0000000..7d38e55 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,19 @@ +//! Common ESP32 C-ITS Utilities + +#![cfg_attr(not(test), no_std)] +#![cfg_attr(docsrs, feature(doc_cfg))] + +extern crate alloc; + +/// Position and Velocity Vector +#[allow(unused)] +#[derive(Debug, Default, Clone, PartialEq)] +pub struct PosVel { + pub position: geo_types::Point, + pub heading_deg: Option, + pub speed_mps: Option, +} + +pub mod radio; +pub mod rx; +pub mod tx; diff --git a/src/radio.rs b/src/radio.rs new file mode 100644 index 0000000..ff20d64 --- /dev/null +++ b/src/radio.rs @@ -0,0 +1,294 @@ +//! IEEE 802.11p C-ITS Capture with ESP32-C5 +#![allow(unused)] + +// ----------------------------- +// missing interface definitions +// ----------------------------- + +pub type Channel = core::ffi::c_int; + +unsafe extern "C" { + /// Changes the 802.11 PHY channel + /// + /// C function signature: `void phy_change_channel(int,int,int,int)` + /// + /// Options: + /// - `primary`: Channel frequency in MHz + /// - `ignored1`: Ignored (set to 1) + /// - `ignored2`: Ignored (set to 0) + /// - `ht_mode`: Probably HT mode + unsafe fn phy_change_channel( + primary: Channel, + ignored1: core::ffi::c_int, + ignored2: core::ffi::c_int, + ht_mode: core::ffi::c_int, + ) -> core::ffi::c_int; + + /// Enables 802.11p mode on the ESP32-C5 + /// + /// C function signature: `void phy_11p_set(int,int)` + /// + /// Options: + /// - `enable`: Boolean option to enable 802.11p mode + /// - `half_rate`: 5 MHz mode + unsafe fn phy_11p_set(enable: u8, half_rate: u8); + +} + +/// Enum values for `wifi_promiscuous_pkt_type_t` used in `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type` +/// +/// Original value is actually `core::ffi::c_uint`, but that can't be used in `#[repr()]` attributes, +/// so we're using `u8` as the smallest suitable type here that can be casted to a bigger type. +#[repr(u8)] +#[derive(Debug, Clone, Copy)] +#[allow(dead_code)] +pub enum PromiscuousPktType { + /// Management frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` + Mgmt = 0, + /// Control frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` + Ctrl = 1, + /// Data frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` + Data = 2, + /// Other type, such as MIMO etc. 'buf' argument is `wifi_promiscuous_pkt_t` but the payload is zero length. + Misc = 3, +} + +impl PromiscuousPktType { + #[allow(dead_code)] + #[must_use] + pub fn as_repr(self) -> u8 { + self as u8 + } + + /// Converts to underlying type of `wifi_promiscuous_pkt_type_t` (used by `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`) + #[must_use] + pub fn as_frame_type(self) -> core::ffi::c_uint { + core::ffi::c_uint::from(self.as_repr()) + } + + /// Determines if we are the same frame type as `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type` + #[must_use] + pub fn matches(self, frame_type: core::ffi::c_uint) -> bool { + self.as_frame_type() == frame_type + } +} + +#[cfg(feature = "esp32c5")] +/// Configure WiFi Sniffer to a certain 802.11p channel +/// +/// # Errors +/// Returns a human-readable error description if setup failed +pub fn setup_wifi_sniffer( + channel: Channel, + sniffer: &mut esp_radio::wifi::sniffer::Sniffer, + callback: fn(esp_radio::wifi::sniffer::PromiscuousPkt<'_>), +) -> Result<(), alloc::string::String> { + sniffer + .set_promiscuous_mode(true) + .map_err(|err| alloc::format!("Failed to enable promiscuous mode: {err:?}"))?; + + sniffer.set_receive_cb(callback); + + // convert channel number to frequency in MHz + let primary = 5000 + (channel * 5); + + unsafe { + phy_11p_set(1, 0); + match phy_change_channel(primary, 1, 0, 0) { + 0 => Ok(()), + ret => Err(alloc::format!("Failed to change channel: {ret}")), + } + } +} + +#[cfg(feature = "esp32c5")] +/// Sends 802.11p broadcast frame +/// +/// Expects `data` to contain GN header (BTP header and ITS payload) to be a valid C-ITS/ ITS-G5 packet/ frame. +/// +/// # Errors +/// Fails when MAC is not the right size or returns frame send error +pub fn send_80211_bcast_frame( + sniffer: &mut esp_radio::wifi::sniffer::Sniffer, + mac_addr: &[u8], + data: &[u8], +) -> Result<(), alloc::string::String> { + const HEADER_SIZE: usize = 24 + 8; + + let mac = mac_addr + .try_into() + .map_err(|err| alloc::format!("Unexpected MAC address size: {err}"))?; + + // assemble frame + let mut frame_buf = alloc::vec::Vec::with_capacity(HEADER_SIZE + data.len()); + frame_buf.extend_from_slice(&DataFrameHeader::new_80211p(mac).to_binary()); + frame_buf.extend_from_slice(&LlcHeader::new_80211p().to_binary()); + frame_buf.extend_from_slice(data); + + // send + sniffer + .send_raw_frame(true, &frame_buf, true) + .map_err(|err| alloc::format!("Failed to send 802.11p frame: {err:?}")) +} + +/// IEEE 802.11p frame header +#[derive(Debug, Default)] +struct DataFrameHeader { + /// byte 0: subtype in upper nibble, type in lower nibble + /// "normal" data frame: 0x08, QoS data frame: 0x88 + pub type_and_subtype: u8, + /// byte 1: frame contol field, 0x00 for 802.11p + pub fcf_flags: u8, + /// byte 2..3: duration in micros, usually 0x0000 for 802.11p (toDS = 0, fromDS = 0) + pub duration: u16, + /// byte 4..9: receiver/ destination for 802.11p + pub address_1: [u8; 6], + /// byte 10..15: transmitter/ source for 802.11p + pub address_2: [u8; 6], + /// byte 16..21: BSSID for 802.11p + pub address_3: [u8; 6], + /// byte 22..24: upper 12 bits: seq. no, lower 4 bits: fragment no + pub sequence_control: u16, + // Note: address 4 is not used in 802.11p + // pub qos: Option<[u8; 2]>, // QoS frames are supported by radio library + // Note: 802.11p does not use HT mode(s) +} + +impl DataFrameHeader { + const BCAST_MAC: [u8; 6] = [0xFF; 6]; + + /// Create an 802.11p (non-QoS) data frame header with a source MAC address + pub(crate) fn new_80211p(source_mac: &[u8; 6]) -> Self { + Self { + type_and_subtype: 0x08, + address_1: Self::BCAST_MAC, + address_2: *source_mac, + address_3: Self::BCAST_MAC, + ..Default::default() + } + } + + pub(crate) fn to_binary(&self) -> [u8; 24] { + let mut buf = [0u8; 24]; + + buf[0] = self.type_and_subtype; + + buf[1] = self.fcf_flags; + + Self::write_u16(&mut buf, 2, self.duration); + Self::write_mac_addr(&mut buf, 4, &self.address_1); + Self::write_mac_addr(&mut buf, 10, &self.address_2); + Self::write_mac_addr(&mut buf, 16, &self.address_3); + Self::write_u16(&mut buf, 22, self.sequence_control); + + buf + } + + fn write_u16(buf: &mut [u8; 24], offset: usize, data: u16) { + for (idx, byte) in data.to_be_bytes().iter().enumerate() { + buf[idx + offset] = *byte; + } + } + + fn write_mac_addr(buf: &mut [u8; 24], offset: usize, data: &[u8; 6]) { + for (idx, byte) in data.iter().enumerate() { + buf[idx + offset] = *byte; + } + } +} + +/// IEEE 802.11p LLC header +#[derive(Debug, Default)] +struct LlcHeader { + /// byte 0: SNAP (0xaa) in 802.11p + pub dsap: u8, // SNAP in + /// byte 1: SNAP (0xaa) in 802.11p + pub lsap: u8, // SNAP in + /// byte 2: SNAP (0xaa) in 802.11p + pub control_1: u8, // 0x03 + // Note: optional second control byte not used in 802.11p + + // SNAP fields + /// set to 0 since ethertype is used for protocol ID in 802.11p + pub oui: [u8; 3], + /// ethertype GeoNetworking (0x8947) in 802.11p + pub protocol_id: u16, +} + +impl LlcHeader { + pub(crate) fn new_80211p() -> Self { + Self { + dsap: 0xaa, + lsap: 0xaa, + control_1: 0x03, + oui: [0; 3], + protocol_id: 0x8947, + } + } + + pub(crate) fn to_binary(&self) -> [u8; 8] { + let mut buf: [u8; 8] = [ + self.dsap, + self.lsap, + self.control_1, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + ]; + + buf[3] = self.oui[0]; + buf[4] = self.oui[1]; + buf[5] = self.oui[2]; + + buf[6] = self.protocol_id.to_be_bytes()[0]; + buf[7] = self.protocol_id.to_be_bytes()[1]; + + buf + } +} + +#[cfg(test)] +mod tests { + + use super::*; + + #[test] + fn data_frame_test() { + // build a typical IEEE 802.11p frame + const OWN_MAC: [u8; 6] = [0x12, 0x34, 0x56, 0x78, 0xab, 0xcd]; + const REF_HEADER: [u8; 24] = [ + 0x08, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0x34, 0x56, 0x78, + 0xab, 0xcd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, + ]; + + let frame = DataFrameHeader::new_80211p(&OWN_MAC); + + assert_eq!(0x08, frame.type_and_subtype); // data frame, subtype 0 + assert_eq!(0x0000, frame.fcf_flags); + assert_eq!(0x0000, frame.duration); + assert_eq!([0xFF; 6], frame.address_1); + assert_eq!(OWN_MAC, frame.address_2); + assert_eq!([0xFF; 6], frame.address_3); + assert_eq!(0x0000, frame.sequence_control); + + assert_eq!(REF_HEADER, frame.to_binary()); + } + + #[test] + fn llc_test() { + // build a typical IEEE 802.11p LLC header + const REF_HEADER: [u8; 8] = [0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x89, 0x47]; + + let llc = LlcHeader::new_80211p(); + + assert_eq!(0xaa, llc.dsap); + assert_eq!(0xaa, llc.lsap); + assert_eq!(0x03, llc.control_1); + assert_eq!([0x00; 3], llc.oui); + assert_eq!(0x8947, llc.protocol_id); + + assert_eq!(REF_HEADER, llc.to_binary()); + } +} diff --git a/src/rx/mod.rs b/src/rx/mod.rs new file mode 100644 index 0000000..e957230 --- /dev/null +++ b/src/rx/mod.rs @@ -0,0 +1,36 @@ +//! V2X Reception Utilities + +/// Determines if the WiFi packet is a broadcast data frame +/// +/// # Errors +/// Returns a human-readable string, if data contains unexpected values +pub fn is_broadcast_frame( + frame: &esp_radio::wifi::sniffer::PromiscuousPkt<'_>, +) -> Result { + // Ignore frames with errors and non-data frames + if frame.rx_cntl.rx_state != 0 { + return Err(alloc::format!( + "Received frame has RX error: {}", + frame.rx_cntl.rx_state + )); + } + if !crate::radio::PromiscuousPktType::Data.matches(frame.frame_type) { + return Ok(false); + } + + // Ignore non-broadcast frames + if frame.len < (4 + 6) { + return Err(alloc::format!( + "Received frame too small to be a valid broadcast frame: {} bytes", + frame.len + )); + } + let dest_mac = &frame.data[4..10]; + if dest_mac != [0xff; 6] { + Err(alloc::format!( + "Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}" + )) + } else { + Ok(true) + } +} diff --git a/src/tx/gn.rs b/src/tx/gn.rs new file mode 100644 index 0000000..f7e1d97 --- /dev/null +++ b/src/tx/gn.rs @@ -0,0 +1,111 @@ +//! Geonetworking utilities + +use c_its_parser::gn as geonetworking; + +#[allow(unused)] +/// Creates a Single-Hop Broadcast (SHB) Extended Header from parts +/// +/// # Errors +/// Returns a human-readable error description if values are out of range +pub fn make_shb_eh( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::PosVel, + time: chrono::DateTime, +) -> Result { + let source_position_vector = make_lpv(address, station_type, own_position, time)?; + + Ok(geonetworking::en302636_4_1::ExtendedHeader::SHB( + geonetworking::en302636_4_1::SingleHopBroadcast { + source_position_vector, + media_dependent_data: [0; 4], + }, + )) +} + +#[allow(unused)] +/// Creates a Topo-Scoped Broadcast (TSB) Extended Header from parts +/// +/// # Errors +/// Returns a human-readable error description if values are out of range +pub fn make_tsb_eh( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::PosVel, + time: chrono::DateTime, + sequence_number: u16, +) -> Result { + let source_position_vector = make_lpv(address, station_type, own_position, time)?; + + Ok(geonetworking::en302636_4_1::ExtendedHeader::TSB( + geonetworking::en302636_4_1::TopologicallyScopedBroadcast::new( + sequence_number, + source_position_vector, + ), + )) +} + +// #[allow(unused)] +// /// Creates a Geo-Broadcast Extended Header +// /// +// /// # Panics +// /// Can't panic in practice b/c angle is zero +// #[must_use] +// pub fn make_gbc_eh( +// address: [u8; 6], +// station_type: geonetworking::en302636_4_1::StationType, +// own_position: &crate::PosVel, +// time: chrono::DateTime, +// target_pos: geo_types::Point, +// radius_m: u16, +// sequence_number: u16, +// ) -> geonetworking::en302636_4_1::ExtendedHeader { +// let source_position_vector = make_lpv(address, station_type, own_position, time); +// #[allow(clippy::cast_possible_truncation)] +// let latitude_deg = target_pos.x() as f32; +// #[allow(clippy::cast_possible_truncation)] +// let longitude_deg = target_pos.y() as f32; + +// geonetworking::en302636_4_1::ExtendedHeader::GBC( +// geonetworking::en302636_4_1::GeoBroadcast::try_from_values( +// sequence_number, +// source_position_vector, +// latitude_deg, +// longitude_deg, +// radius_m, +// 0, +// 0, +// ) +// .expect("Failed to create circular GBC"), +// ) +// } + +/// Creates a Long Position Vector from parts +/// +/// # Errors +/// Returns a human-readable error description if values are out of range +pub fn make_lpv( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::PosVel, + time: chrono::DateTime, +) -> Result { + let timestamp_its = c_its_parser::time_utils::TimestampIts::from(time); + #[allow(clippy::cast_possible_truncation)] + let latitude_deg = own_position.position.y() as f32; + #[allow(clippy::cast_possible_truncation)] + let longitude_deg = own_position.position.x() as f32; + let speed_mps = own_position.speed_mps.unwrap_or_default(); + let heading_deg = own_position.heading_deg.unwrap_or_default(); + + geonetworking::en302636_4_1::LongPositionVector::try_from_values( + geonetworking::en302636_4_1::Address::new(false, station_type, address), + timestamp_its.0, + latitude_deg, + longitude_deg, + true, + speed_mps, + heading_deg, + ) + .map_err(|err| alloc::format!("LPV invalid input: {err}")) +} diff --git a/src/tx/mod.rs b/src/tx/mod.rs new file mode 100644 index 0000000..06dde8f --- /dev/null +++ b/src/tx/mod.rs @@ -0,0 +1,164 @@ +//! V2X Transmission Utilities + +use alloc::vec::Vec; + +use c_its_parser::gn as geonetworking; + +pub mod gn; + +/// Interface for ITS Payloads inside a Geonetworking header +pub trait GnItsPayload { + // common GN parameters + const LIFETIME_MILLIS: u32 = 60_000; // itsGnDefaultPacketLifetime is 60 seconds according to ETSI EN 302 636-4-1 V1.4.1 + + // --------------------------------------------- + // methods to build a GN packet with ITS payload + // --------------------------------------------- + + /// Builds the [`geonetworking::en302636_4_1::ExtendedHeader`] and additional information for the ITS message + /// + /// Output: + /// - Extended header + /// - Area type (when using GAC or GBC) + /// - hop limit + /// + /// Note: Make sure to increase the sequence number on multi-hop packets. + /// + /// # Errors + /// Returns a human-readable error description if values are out of range + fn make_eh( + &self, + address: [u8; 6], + own_position: &crate::PosVel, + time: chrono::DateTime, + seq_no: &mut u16, + ) -> Result< + ( + geonetworking::en302636_4_1::ExtendedHeader, + Option, + u8, + ), + alloc::string::String, + >; + + /// Creates the ITS payload (without BTP header) + /// + /// # Errors + /// Returns a human-readable error description if values are out of range + fn make_payload( + &self, + ) -> Result<(Vec, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>; + + // --------------------------------------------- + // common methods (with implementation) + // --------------------------------------------- + + /// Encodes the GN packet + /// + /// # Errors + /// Returns a human-readable error description if values are out of range + fn encode_packet( + &self, + address: [u8; 6], + own_position: &crate::PosVel, + time: chrono::DateTime, + is_mobile: bool, + seq_no: &mut u16, + ) -> Result, alloc::string::String> { + use geonetworking::Encode as _; + + // Build payload (including BTP header) + let (mut its_payload, msg_type) = self.make_payload()?; + let mut payload = make_btp_b(msg_type).encode()?; + + payload.append(&mut its_payload); + + #[allow(clippy::cast_possible_truncation)] + let payload_length = (payload.len()) as u16; + + // get extended header (depending on message type and sometimes content as well) + let (eh, area_type, maximum_hop_limit) = + self.make_eh(address, own_position, time, seq_no)?; + + // Build GN headers + let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new( + false, + false, + make_traffic_class(msg_type), + ) + .map_err(|err| alloc::format!("Failed to create TrafficClass: {err:?}"))?; + let common = geonetworking::en302636_4_1::CommonHeader::from_values( + geonetworking::en302636_4_1::NextAfterCommon::BTPB, + geonetworking::en302636_4_1::HeaderType::new_from(&eh, area_type)?, + traffic_class, + is_mobile, + payload_length, + maximum_hop_limit, + ); + + let remaining_hop_limit = maximum_hop_limit; + let basic = geonetworking::en302636_4_1::BasicHeader::try_new( + 1, + geonetworking::en302636_4_1::NextAfterBasic::CommonHeader, + geonetworking::en302636_4_1::Lifetime::from_milliseconds(Self::LIFETIME_MILLIS), + remaining_hop_limit, + ) + .map_err(|err| alloc::format!("Failed to create BasicHeader: {err}"))?; + + // Assemble and encode packet + let packet = geonetworking::Packet::Unsecured { + basic, + common, + extended: Some(eh), + payload: &payload, + }; + + packet + .encode_to_vec() + .map_err(|err| alloc::format!("Failed to encode GN packet: {err:?}")) + } +} + +/// Creates a BTP-B header based on the ITS message ID (message type) +/// +/// Port numbers and destination port info according to ETSI TS 103 248 +fn make_btp_b( + msg_type: c_its_parser::standards::extensions::ItsMessageId, +) -> c_its_parser::transport::TransportHeader { + let (destination_port, destination_port_info) = match msg_type { + c_its_parser::standards::extensions::ItsMessageId::Denm => (2002, 0), + c_its_parser::standards::extensions::ItsMessageId::Cam => (2001, 0), + c_its_parser::standards::extensions::ItsMessageId::Mapem => (2003, 0), + c_its_parser::standards::extensions::ItsMessageId::Spatem => (2004, 0), + c_its_parser::standards::extensions::ItsMessageId::Saem => (2005, 0), + c_its_parser::standards::extensions::ItsMessageId::Ivim => (2006, 0), + c_its_parser::standards::extensions::ItsMessageId::Srem => (2007, 0), + c_its_parser::standards::extensions::ItsMessageId::Ssem => (2008, 0), + c_its_parser::standards::extensions::ItsMessageId::Cpm => (2009, 0), + c_its_parser::standards::extensions::ItsMessageId::Poi => (2010, 0), + c_its_parser::standards::extensions::ItsMessageId::EvRsr => (2012, 0), + c_its_parser::standards::extensions::ItsMessageId::Rtcmem => (2013, 0), + _ => (0, 0), // no information found for the subsequent types + }; + + c_its_parser::transport::TransportHeader::BtpB(c_its_parser::transport::BasicTransportBHeader { + destination_port, + destination_port_info, + }) +} + +/// Creates a traffic class ID based on the ITS message ID (message type) +fn make_traffic_class(msg_type: c_its_parser::standards::extensions::ItsMessageId) -> u8 { + #[allow(clippy::match_same_arms)] + match msg_type { + c_its_parser::standards::extensions::ItsMessageId::Denm => 1, + c_its_parser::standards::extensions::ItsMessageId::Cam => 2, + c_its_parser::standards::extensions::ItsMessageId::Mapem => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Spatem => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Ivim => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Srem => 2, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Ssem => 2, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Rtcmem => 3, // CSP_MaxPrio=252 -> 3 + _ => 3, // fall-back to lowest priority + } +}