From ede96c8d1704ba6b932538487e7df2712013082c Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 23:50:56 +0200 Subject: [PATCH 01/11] feat: add signing feature with sha2 dependency --- mavlink-core/Cargo.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 246c657537..d131be3c11 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -27,6 +27,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } tokio = { version = "1.0", default-features = false, features = ["io-util"], optional = true } +sha2 = { version = "0.10", optional = true } [features] "std" = ["byteorder/std"] @@ -40,4 +41,5 @@ tokio = { version = "1.0", default-features = false, features = ["io-util"], opt "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] "tokio-1" = ["dep:tokio"] +"signing" = ["dep:sha2", "std"] default = ["std", "tcp", "udp", "direct-serial", "serde"] From f5a0d72318c0f118038a9789aa4e8ad53e02e509 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 23:57:47 +0200 Subject: [PATCH 02/11] feat: Add incompat flag field to MavHeader --- mavlink-core/src/connection/direct_serial.rs | 1 + mavlink-core/src/connection/tcp.rs | 1 + mavlink-core/src/connection/udp.rs | 1 + mavlink-core/src/lib.rs | 5 ++++- mavlink/examples/embedded-async-read/src/main.rs | 1 + mavlink/examples/embedded/src/main.rs | 1 + mavlink/tests/mav_frame_tests.rs | 1 + mavlink/tests/test_shared/mod.rs | 1 + 8 files changed, 11 insertions(+), 1 deletion(-) diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index a3a33a0fa6..2e31bda56d 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -77,6 +77,7 @@ impl MavConnection for SerialConnection { let mut sequence = self.sequence.lock().unwrap(); let header = MavHeader { + incompat_flags: 0, sequence: *sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 35b7f09084..73d456aa3b 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -95,6 +95,7 @@ impl MavConnection for TcpConnection { let mut lock = self.writer.lock().unwrap(); let header = MavHeader { + incompat_flags: 0, sequence: lock.sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 3ec739d3f5..91bbfaef45 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -134,6 +134,7 @@ impl MavConnection for UdpConnection { let state = &mut *guard; let header = MavHeader { + incompat_flags: 0, sequence: state.sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index e6e7aec97a..fa6bea9c48 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -93,6 +93,7 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { +pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -118,6 +119,7 @@ pub const MAV_STX_V2: u8 = 0xFD; impl Default for MavHeader { fn default() -> Self { Self { + incompat_flags: 0, system_id: 255, component_id: 0, sequence: 0, @@ -211,6 +213,7 @@ impl MavFrame { let system_id = buf.get_u8(); let component_id = buf.get_u8(); let header = MavHeader { + incompat_flags: 0, system_id, component_id, sequence, @@ -639,7 +642,7 @@ impl MAVLinkV2MessageRaw { let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ payload_length as u8, - 0, //incompat_flags + header.incompat_flags, 0, //compat_flags header.sequence, header.system_id, diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index 7f53335de3..6b4e5c1dc1 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -39,6 +39,7 @@ async fn main(spawner: Spawner) { // Create our mavlink header and heartbeat message let header = mavlink::MavHeader { + incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs index 4a71ca8fcf..21b70396b1 100644 --- a/mavlink/examples/embedded/src/main.rs +++ b/mavlink/examples/embedded/src/main.rs @@ -82,6 +82,7 @@ fn main() -> ! { fn mavlink_header() -> mavlink::MavHeader { mavlink::MavHeader { + incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs index 1c225efd09..5472dbb8c1 100644 --- a/mavlink/tests/mav_frame_tests.rs +++ b/mavlink/tests/mav_frame_tests.rs @@ -96,6 +96,7 @@ mod mav_frame_tests { fn new(msg: MavMessage) -> MavFrame { MavFrame { header: MavHeader { + incompat_flags: 0, system_id: 1, component_id: 2, sequence: 84, diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 4cba46907a..332e9a1f02 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -1,6 +1,7 @@ #![allow(unused)] pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { + incompat_flags: 0, sequence: 239, system_id: 1, component_id: 2, From 87bb7cb030e586b998b4ef69038d5914be7f2dee Mon Sep 17 00:00:00 2001 From: pv42 Date: Thu, 8 Aug 2024 00:30:36 +0200 Subject: [PATCH 03/11] feat: add MavConnection::setup_signing(), signing functions to MAVLinkV2MessageRaw add calculate_signature, checksum_bytes, signature_link_id[_mut], signature_timestamp[_bytes[_mut]], signature_value[_mut] to MAVLinkV2MessageRaw --- mavlink-core/src/connection/direct_serial.rs | 12 ++ mavlink-core/src/connection/file.rs | 12 ++ mavlink-core/src/connection/mod.rs | 9 ++ mavlink-core/src/connection/signing.rs | 118 +++++++++++++++++++ mavlink-core/src/connection/tcp.rs | 14 +++ mavlink-core/src/connection/udp.rs | 12 ++ mavlink-core/src/lib.rs | 88 +++++++++++++- 7 files changed, 264 insertions(+), 1 deletion(-) create mode 100644 mavlink-core/src/connection/signing.rs diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index 2e31bda56d..daea3a0b77 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -10,6 +10,9 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -45,6 +48,8 @@ pub fn open(settings: &str) -> io::Result { port: Mutex::new(PeekReader::new(port)), sequence: Mutex::new(0), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } @@ -52,6 +57,8 @@ pub struct SerialConnection { port: Mutex>, sequence: Mutex, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for SerialConnection { @@ -95,4 +102,9 @@ impl MavConnection for SerialConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index 85444bb6ae..8209392abc 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -9,18 +9,25 @@ use std::fs::File; use std::io; use std::sync::Mutex; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; Ok(FileConnection { file: Mutex::new(PeekReader::new(file)), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } pub struct FileConnection { file: Mutex>, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for FileConnection { @@ -55,4 +62,9 @@ impl MavConnection for FileConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index 035f63eb28..f4d6e6e332 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -11,6 +11,11 @@ mod udp; #[cfg(feature = "direct-serial")] mod direct_serial; +#[cfg(feature = "signing")] +pub(crate) mod signing; +#[cfg(feature = "signing")] +pub use signing::SigningConfig; + mod file; /// A MAVLink connection @@ -47,6 +52,10 @@ pub trait MavConnection { let header = MavHeader::default(); self.send(&header, data) } + + /// Setup secret key used for message signing, or disable message signing + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option); } /// Connect to a MAVLink node by address string. diff --git a/mavlink-core/src/connection/signing.rs b/mavlink-core/src/connection/signing.rs new file mode 100644 index 0000000000..fe43120e64 --- /dev/null +++ b/mavlink-core/src/connection/signing.rs @@ -0,0 +1,118 @@ +use crate::MAVLinkV2MessageRaw; + +use std::{collections::HashMap, sync::Mutex}; + +// signing configuration +pub struct SigningConfig { + secret_key: [u8; 32], + pub(crate) sign_outgoing: bool, + allow_unsigned: bool, +} + +// mutable state of signing per connection +pub(crate) struct SigningState { + timestamp: u64, + // does never really change but is definitely not part of the setup + link_id: u8, + stream_timestamps: HashMap<(u8, u8, u8), u64>, // TODO unsigned callback +} + +pub struct SigningData { + pub(crate) config: SigningConfig, + pub(crate) state: Mutex, +} + +impl SigningConfig { + pub fn new(secret_key: [u8; 32], sign_outgoing: bool, allow_unsigned: bool) -> Self { + SigningConfig { + secret_key, + sign_outgoing, + allow_unsigned, + } + } +} + +impl SigningData { + pub fn from_config(config: SigningConfig) -> Self { + Self { + config, + state: Mutex::new(SigningState { + timestamp: 0, + link_id: 0, + stream_timestamps: HashMap::new(), + }), + } + } + + pub(crate) fn verify_signature(&self, message: MAVLinkV2MessageRaw) -> bool { + use crate::MAVLINK_IFLAG_SIGNED; + // TODO: fix that unwrap poison + let mut state = self.state.lock().unwrap(); + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let timestamp = message.signature_timestamp(); + let src_system = message.system_id(); + let src_component = message.component_id(); + let stream_key = (message.signature_link_id(), src_system, src_component); + match state.stream_timestamps.get(&stream_key) { + Some(stream_timestamp) => { + if timestamp <= *stream_timestamp { + // reject old timestamp + return false; + } + } + None => { + if timestamp + 60 * 1000 * 100 < state.timestamp { + // bad new stream, more then a minute older the the last one + return false; + } + } + } + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + let result = signature_buffer == message.signature_value(); + if result { + state.stream_timestamps.insert(stream_key, timestamp); + state.timestamp = u64::max(state.timestamp, timestamp) + } + result + } else { + self.config.allow_unsigned + } + } + + pub(crate) fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { + // TODO: fix that unwrap poison + let mut state = self.state.lock().unwrap(); + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let ts_bytes = u64::to_le_bytes(state.timestamp); + message + .signature_timestamp_bytes_mut() + .copy_from_slice(&ts_bytes[0..6]); + // TODO link id set + *message.signature_link_id_mut() = state.link_id; + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + + message + .signature_value_mut() + .copy_from_slice(&signature_buffer); + state.timestamp += 1; + } + + fn get_current_timestamp() -> u64 { + use std::time::SystemTime; + // fallback to 0 if the system time appears to be before epoch + let now = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .map(|n| n.as_micros()) + .unwrap_or(0); + // use 1st January 2015 GMT as offset, fallback to 0 if before that date, this will overflow in April 2104 + ((now + .checked_sub(1420070400u128 * 1000000u128) + .unwrap_or_default()) + / 10u128) as u64 + } +} diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 73d456aa3b..2f92d36bae 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -12,6 +12,9 @@ use std::time::Duration; use super::get_socket_addr; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -42,6 +45,8 @@ pub fn tcpout(address: T) -> io::Result { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } @@ -60,6 +65,8 @@ pub fn tcpin(address: T) -> io::Result { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } Err(e) => { @@ -78,6 +85,8 @@ pub struct TcpConnection { reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } struct TcpWrite { @@ -112,4 +121,9 @@ impl MavConnection for TcpConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 91bbfaef45..80f09adb6b 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -13,6 +13,9 @@ use std::sync::Mutex; use super::get_socket_addr; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -91,6 +94,8 @@ pub struct UdpConnection { writer: Mutex, protocol_version: MavlinkVersion, server: bool, + #[cfg(feature = "signing")] + signing_data: Option, } impl UdpConnection { @@ -108,6 +113,8 @@ impl UdpConnection { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } } @@ -160,6 +167,11 @@ impl MavConnection for UdpConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } #[cfg(test)] diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index fa6bea9c48..3b8402b858 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -54,6 +54,13 @@ pub mod embedded; #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; +#[cfg(feature = "signing")] +use self::connection::signing::SigningData; +#[cfg(feature = "signing")] +pub use self::connection::SigningConfig; +#[cfg(feature = "signing")] +use sha2::{Digest, Sha256}; + pub const MAX_FRAME_SIZE: usize = 280; pub trait Message @@ -93,7 +100,7 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { -pub incompat_flags: u8, + pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -553,6 +560,11 @@ impl MAVLinkV2MessageRaw { self.0[2] } + #[inline] + pub fn incompatibility_flags_mut(&mut self) -> &mut u8 { + &mut self.0[2] + } + #[inline] pub fn compatibility_flags(&self) -> u8 { self.0[3] @@ -593,6 +605,67 @@ impl MAVLinkV2MessageRaw { ]) } + #[cfg(feature = "signing")] + #[inline] + pub fn checksum_bytes(&self) -> &[u8] { + let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize; + &self.0[checksum_offset..(checksum_offset + 2)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id(&self) -> u8 { + let payload_length: usize = self.payload_length().into(); + self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id_mut(&mut self) -> &mut u8 { + let payload_length: usize = self.payload_length().into(); + &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &mut self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp(&self) -> u64 { + let mut timestamp_bytes = [0u8; 8]; + timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes()); + u64::from_le_bytes(timestamp_bytes) + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &self.0[signature_start..(signature_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &mut self.0[signature_start..(signature_start + 6)] + } + fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] { let payload_length: usize = self.payload_length().into(); @@ -617,6 +690,19 @@ impl MAVLinkV2MessageRaw { ) } + #[cfg(feature = "signing")] + pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) { + let mut hasher = Sha256::new(); + hasher.update(secret_key); + hasher.update(&[MAV_STX_V2]); + hasher.update(self.header()); + hasher.update(self.payload()); + hasher.update(self.checksum_bytes()); + hasher.update(&[self.signature_link_id()]); + hasher.update(self.signature_timestamp_bytes()); + target_buffer.copy_from_slice(&hasher.finalize()[0..6]); + } + pub fn raw_bytes(&self) -> &[u8] { let payload_length = self.payload_length() as usize; From b5ea0b09234056b1e2b7135be480250405d1584f Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 11 Aug 2024 19:13:57 +0200 Subject: [PATCH 04/11] fix: move signing mod, remove incompat_flags since it is breaking feat: add distinct pub fn for signing, signing in MavConnection send, add signing feature to mavlink test: add --features signing to all test, add signing test, add Debug, Clone to SigningConfig --- .github/workflows/test.yml | 2 +- mavlink-core/src/connection/direct_serial.rs | 29 +++- mavlink-core/src/connection/file.rs | 16 +- mavlink-core/src/connection/mod.rs | 4 +- mavlink-core/src/connection/tcp.rs | 30 +++- mavlink-core/src/connection/udp.rs | 34 ++++- mavlink-core/src/lib.rs | 143 ++++++++++++++++-- mavlink-core/src/{connection => }/signing.rs | 67 +++++--- mavlink/Cargo.toml | 1 + .../examples/embedded-async-read/src/main.rs | 1 - mavlink/examples/embedded/src/main.rs | 1 - mavlink/tests/mav_frame_tests.rs | 1 - mavlink/tests/signing.rs | 100 ++++++++++++ mavlink/tests/tcp_loopback_tests.rs | 23 ++- mavlink/tests/test_shared/mod.rs | 8 +- 15 files changed, 398 insertions(+), 62 deletions(-) rename mavlink-core/src/{connection => }/signing.rs (55%) create mode 100644 mavlink/tests/signing.rs diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 313eb75862..45d144f3c0 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -34,7 +34,7 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable - name: Run internal tests - run: cargo test --verbose --features ${{ matrix.dialect }} -- --nocapture + run: cargo test --verbose --features ${{ matrix.dialect }} --features signing -- --nocapture mavlink-dump: runs-on: ubuntu-latest diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index daea3a0b77..d2f376aa14 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -2,7 +2,7 @@ use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io; use std::sync::Mutex; @@ -10,8 +10,10 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); @@ -65,7 +67,15 @@ impl MavConnection for SerialConnection { fn recv(&self) -> Result<(MavHeader, M), MessageReadError> { let mut port = self.port.lock().unwrap(); loop { - match read_versioned_msg(port.deref_mut(), self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(port.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + port.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } @@ -84,7 +94,6 @@ impl MavConnection for SerialConnection { let mut sequence = self.sequence.lock().unwrap(); let header = MavHeader { - incompat_flags: 0, sequence: *sequence, system_id: header.system_id, component_id: header.component_id, @@ -92,7 +101,17 @@ impl MavConnection for SerialConnection { *sequence = sequence.wrapping_add(1); - write_versioned_msg(port.reader_mut(), self.protocol_version, header, data) + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(port.reader_mut(), self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + port.reader_mut(), + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index 8209392abc..c635d2be2d 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -3,14 +3,16 @@ use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::fs::File; use std::io; use std::sync::Mutex; +#[cfg(not(feature = "signing"))] +use crate::read_versioned_msg; #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; @@ -37,7 +39,15 @@ impl MavConnection for FileConnection { let mut file = self.file.lock().unwrap(); loop { - match read_versioned_msg(file.deref_mut(), self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(file.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + file.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index f4d6e6e332..e60386a6aa 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -12,9 +12,7 @@ mod udp; mod direct_serial; #[cfg(feature = "signing")] -pub(crate) mod signing; -#[cfg(feature = "signing")] -pub use signing::SigningConfig; +use crate::SigningConfig; mod file; diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 2f92d36bae..04761ff6af 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -2,7 +2,7 @@ use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io; use std::net::ToSocketAddrs; @@ -12,8 +12,11 @@ use std::time::Duration; use super::get_socket_addr; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn select_protocol( address: &str, @@ -97,21 +100,38 @@ struct TcpWrite { impl MavConnection for TcpConnection { fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut reader = self.reader.lock().unwrap(); - read_versioned_msg(reader.deref_mut(), self.protocol_version) + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + result } fn send(&self, header: &MavHeader, data: &M) -> Result { let mut lock = self.writer.lock().unwrap(); let header = MavHeader { - incompat_flags: 0, sequence: lock.sequence, system_id: header.system_id, component_id: header.component_id, }; lock.sequence = lock.sequence.wrapping_add(1); - write_versioned_msg(&mut lock.socket, self.protocol_version, header, data) + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(&mut lock.socket, self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + &mut lock.socket, + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 80f09adb6b..9481684167 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -4,7 +4,7 @@ use std::collections::VecDeque; use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io::{self, Read}; use std::net::ToSocketAddrs; @@ -13,8 +13,11 @@ use std::sync::Mutex; use super::get_socket_addr; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn select_protocol( address: &str, @@ -124,7 +127,14 @@ impl MavConnection for UdpConnection { let mut reader = self.reader.lock().unwrap(); loop { + #[cfg(not(feature = "signing"))] let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); if self.server { if let addr @ Some(_) = reader.reader_ref().last_recv_address { self.writer.lock().unwrap().dest = addr; @@ -141,7 +151,6 @@ impl MavConnection for UdpConnection { let state = &mut *guard; let header = MavHeader { - incompat_flags: 0, sequence: state.sequence, system_id: header.system_id, component_id: header.component_id, @@ -151,7 +160,24 @@ impl MavConnection for UdpConnection { let len = if let Some(addr) = state.dest { let mut buf = Vec::new(); - write_versioned_msg(&mut buf, self.protocol_version, header, data)?; + #[cfg(not(feature = "signing"))] + write_versioned_msg( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; + #[cfg(feature = "signing")] + write_versioned_msg_signed( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; state.socket.send_to(&buf, addr)? } else { 0 diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 3b8402b858..fd9d9476fe 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -54,10 +54,12 @@ pub mod embedded; #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; +#[cfg(not(feature = "signing"))] +type SigningData = (); #[cfg(feature = "signing")] -use self::connection::signing::SigningData; +mod signing; #[cfg(feature = "signing")] -pub use self::connection::SigningConfig; +pub use self::signing::{SigningConfig, SigningData}; #[cfg(feature = "signing")] use sha2::{Digest, Sha256}; @@ -100,7 +102,6 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { - pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -126,7 +127,6 @@ pub const MAV_STX_V2: u8 = 0xFD; impl Default for MavHeader { fn default() -> Self { Self { - incompat_flags: 0, system_id: 255, component_id: 0, sequence: 0, @@ -220,7 +220,6 @@ impl MavFrame { let system_id = buf.get_u8(); let component_id = buf.get_u8(); let header = MavHeader { - incompat_flags: 0, system_id, component_id, sequence, @@ -262,6 +261,18 @@ pub fn read_versioned_msg( } } +#[cfg(feature = "signing")] +pub fn read_versioned_msg_signed( + r: &mut PeekReader, + version: MavlinkVersion, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_inner(r, signing_data), + MavlinkVersion::V1 => read_v1_msg(r), + } +} + #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: `` pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); @@ -721,6 +732,7 @@ impl MAVLinkV2MessageRaw { msgid: u32, payload_length: usize, extra_crc: u8, + incompat_flags: u8, ) { self.0[0] = MAV_STX_V2; let msgid_bytes = msgid.to_le_bytes(); @@ -728,7 +740,7 @@ impl MAVLinkV2MessageRaw { let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ payload_length as u8, - header.incompat_flags, + incompat_flags, 0, //compat_flags header.sequence, header.system_id, @@ -757,6 +769,21 @@ impl MAVLinkV2MessageRaw { message_id, payload_length, M::extra_crc(message_id), + 0, + ); + } + + pub fn serialize_message_for_signing(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + 0x01, ); } @@ -764,14 +791,35 @@ impl MAVLinkV2MessageRaw { let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf); - self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0); } } /// Return a raw buffer with the mavlink message +/// /// V2 maximum size is 280 bytes: `` +#[inline] pub fn read_v2_raw_message( reader: &mut PeekReader, +) -> Result { + read_v2_raw_message_inner::(reader, None) +} + +/// Return a raw buffer with the mavlink message with signing support +/// +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_raw_message_signed( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_inner::(reader, signing_data) +} + +fn read_v2_raw_message_inner( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, ) -> Result { loop { loop { @@ -801,9 +849,20 @@ pub fn read_v2_raw_message( .copy_from_slice(payload_and_checksum_and_sign); if message.has_valid_crc::() { + // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes reader.consume(message.raw_bytes().len() - 1); - return Ok(message); + } else { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } } + + return Ok(message); } } @@ -891,10 +950,28 @@ pub async fn read_v2_raw_message_async( } /// Read a MAVLink v2 message from a Read stream. +#[inline] pub fn read_v2_msg( read: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { - let message = read_v2_raw_message::(read)?; + read_v2_msg_inner(read, None) +} + +/// Read a MAVLink v2 message from a Read stream. +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_msg_signed( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_inner(read, signing_data) +} + +fn read_v2_msg_inner( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_inner::(read, signing_data)?; Ok(( MavHeader { @@ -960,6 +1037,21 @@ pub fn write_versioned_msg( } } +/// Write a message with signing support using the given mavlink version +#[cfg(feature = "signing")] +pub fn write_versioned_msg_signed( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data), + MavlinkVersion::V1 => write_v1_msg(w, header, data), + } +} + /// Async write a message using the given mavlink version #[cfg(feature = "tokio-1")] pub async fn write_versioned_msg_async( @@ -1008,6 +1100,39 @@ pub fn write_v2_msg( Ok(len) } +/// Write a MAVLink v2 message to a Write stream with signing support. +#[cfg(feature = "signing")] +pub fn write_v2_msg_signed( + w: &mut W, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + + let signature_len = if let Some(signing_data) = signing_data { + if signing_data.config.sign_outgoing { + //header.incompat_flags |= MAVLINK_IFLAG_SIGNED; + message_raw.serialize_message_for_signing(header, data); + signing_data.sign_message(&mut message_raw); + MAVLinkV2MessageRaw::SIGNATURE_SIZE + } else { + message_raw.serialize_message(header, data); + 0 + } + } else { + message_raw.serialize_message(header, data); + 0 + }; + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len; + + w.write_all(&message_raw.0[..len])?; + + Ok(len) +} + /// Async write a MAVLink v2 message to a Write stream. #[cfg(feature = "tokio-1")] pub async fn write_v2_msg_async( diff --git a/mavlink-core/src/connection/signing.rs b/mavlink-core/src/signing.rs similarity index 55% rename from mavlink-core/src/connection/signing.rs rename to mavlink-core/src/signing.rs index fe43120e64..6007901806 100644 --- a/mavlink-core/src/connection/signing.rs +++ b/mavlink-core/src/signing.rs @@ -1,8 +1,12 @@ use crate::MAVLinkV2MessageRaw; +use std::time::SystemTime; use std::{collections::HashMap, sync::Mutex}; -// signing configuration +use crate::MAVLINK_IFLAG_SIGNED; + +/// Configuration used for MAVLink 2 messages signing as defined in . +#[derive(Debug, Clone)] pub struct SigningConfig { secret_key: [u8; 32], pub(crate) sign_outgoing: bool, @@ -12,11 +16,12 @@ pub struct SigningConfig { // mutable state of signing per connection pub(crate) struct SigningState { timestamp: u64, - // does never really change but is definitely not part of the setup + // currently link id is constant 0 link_id: u8, - stream_timestamps: HashMap<(u8, u8, u8), u64>, // TODO unsigned callback + stream_timestamps: HashMap<(u8, u8, u8), u64>, } +/// MAVLink 2 message signing data. pub struct SigningData { pub(crate) config: SigningConfig, pub(crate) state: Mutex, @@ -44,10 +49,15 @@ impl SigningData { } } - pub(crate) fn verify_signature(&self, message: MAVLinkV2MessageRaw) -> bool { - use crate::MAVLINK_IFLAG_SIGNED; - // TODO: fix that unwrap poison - let mut state = self.state.lock().unwrap(); + /// Verify the signature of a MAVLink 2 message. + pub fn verify_signature(&self, message: &MAVLinkV2MessageRaw) -> bool { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); let timestamp = message.signature_timestamp(); @@ -73,6 +83,7 @@ impl SigningData { message.calculate_signature(&self.config.secret_key, &mut signature_buffer); let result = signature_buffer == message.signature_value(); if result { + // if signature is valid update timestamps state.stream_timestamps.insert(stream_key, timestamp); state.timestamp = u64::max(state.timestamp, timestamp) } @@ -82,34 +93,40 @@ impl SigningData { } } - pub(crate) fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { - // TODO: fix that unwrap poison - let mut state = self.state.lock().unwrap(); - state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); - let ts_bytes = u64::to_le_bytes(state.timestamp); - message - .signature_timestamp_bytes_mut() - .copy_from_slice(&ts_bytes[0..6]); - // TODO link id set - *message.signature_link_id_mut() = state.link_id; + /// Sign a MAVLink 2 message if its incompatibility flag is set accordingly. + pub fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let ts_bytes = u64::to_le_bytes(state.timestamp); + message + .signature_timestamp_bytes_mut() + .copy_from_slice(&ts_bytes[0..6]); + *message.signature_link_id_mut() = state.link_id; - let mut signature_buffer = [0u8; 6]; - message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); - message - .signature_value_mut() - .copy_from_slice(&signature_buffer); - state.timestamp += 1; + message + .signature_value_mut() + .copy_from_slice(&signature_buffer); + state.timestamp += 1; + } } fn get_current_timestamp() -> u64 { - use std::time::SystemTime; // fallback to 0 if the system time appears to be before epoch let now = SystemTime::now() .duration_since(SystemTime::UNIX_EPOCH) .map(|n| n.as_micros()) .unwrap_or(0); - // use 1st January 2015 GMT as offset, fallback to 0 if before that date, this will overflow in April 2104 + // use 1st January 2015 GMT as offset, fallback to 0 if before that date, the used 48bit of this will overflow in 2104 ((now .checked_sub(1420070400u128 * 1000000u128) .unwrap_or_default()) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 46896b017c..0573047572 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -92,6 +92,7 @@ serde_arrays = { version = "0.1.0", optional = true } "std" = ["mavlink-core/std"] "udp" = ["mavlink-core/udp"] "tcp" = ["mavlink-core/tcp"] +"signing" = ["mavlink-core/signing"] "direct-serial" = ["mavlink-core/direct-serial"] # NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. # Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index 6b4e5c1dc1..7f53335de3 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -39,7 +39,6 @@ async fn main(spawner: Spawner) { // Create our mavlink header and heartbeat message let header = mavlink::MavHeader { - incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs index 21b70396b1..4a71ca8fcf 100644 --- a/mavlink/examples/embedded/src/main.rs +++ b/mavlink/examples/embedded/src/main.rs @@ -82,7 +82,6 @@ fn main() -> ! { fn mavlink_header() -> mavlink::MavHeader { mavlink::MavHeader { - incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs index 5472dbb8c1..1c225efd09 100644 --- a/mavlink/tests/mav_frame_tests.rs +++ b/mavlink/tests/mav_frame_tests.rs @@ -96,7 +96,6 @@ mod mav_frame_tests { fn new(msg: MavMessage) -> MavFrame { MavFrame { header: MavHeader { - incompat_flags: 0, system_id: 1, component_id: 2, sequence: 84, diff --git a/mavlink/tests/signing.rs b/mavlink/tests/signing.rs new file mode 100644 index 0000000000..5ac3837f9d --- /dev/null +++ b/mavlink/tests/signing.rs @@ -0,0 +1,100 @@ +mod test_shared; + +#[cfg(feature = "signing")] +mod signing { + use mavlink::{ + common::HEARTBEAT_DATA, peek_reader::PeekReader, read_v2_raw_message, MAVLinkV2MessageRaw, + MavHeader, SigningConfig, SigningData, MAV_STX_V2, + }; + + use crate::test_shared::SECRET_KEY; + + const HEARTBEAT_SIGNED: &[u8] = &[ + MAV_STX_V2, + 0x09, + 0x01, // MAVLINK_IFLAG_SIGNED + 0x00, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload + 0x00, + 0x00, + 0x00, + 0x02, + 0x03, + 0x59, + 0x03, + 0x03, + 0xc9, // checksum + 0x8b, + 0x00, // link_id + 0xff, // use max timestamp to ensure test will never fail against current time + 0xff, + 0xff, + 0xff, + 0xff, + 0xff, + 0x27, // signature + 0x18, + 0xb1, + 0x68, + 0xcc, + 0xf5, + ]; + + #[test] + pub fn test_verify() { + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let msg = read_v2_raw_message::(&mut r).unwrap(); + assert!( + signing_data.verify_signature(&msg), + "Message verification failed" + ); + } + + #[test] + pub fn test_invalid_ts() { + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let mut msg = read_v2_raw_message::(&mut r).unwrap(); + msg.signature_timestamp_bytes_mut() + .copy_from_slice(&[0, 0, 0, 0, 0, 0]); // set timestamp to min causing the timestamp test to fail + assert!( + !signing_data.verify_signature(&msg), + "Invalid message verified" + ); + } + + #[test] + pub fn test_sign_verify() { + use mavlink::common::MavMessage; + let heartbeat_message = MavMessage::HEARTBEAT(HEARTBEAT_DATA::default()); + let mut message = MAVLinkV2MessageRaw::new(); + let header = MavHeader { + system_id: 4, + component_id: 3, + sequence: 42, + }; + message.serialize_message_for_signing(header, &heartbeat_message); + + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + signing_data.sign_message(&mut message); + assert!( + signing_data.verify_signature(&message), + "Message verification failed" + ); + // the same message must not be allowed to be verified again + assert!( + !signing_data.verify_signature(&message), + "Invalid message verified" + ); + } +} diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index abea99598b..e097febe42 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -4,14 +4,27 @@ mod test_shared; mod test_tcp_connections { use std::thread; - /// Test whether we can send a message via TCP and receive it OK + #[cfg(feature = "signing")] + use mavlink::SigningConfig; + + use crate::test_shared; + + /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] pub fn test_tcp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 5; + #[cfg(feature = "signing")] + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, true, false); + #[cfg(feature = "signing")] + let singing_cfg_client = singing_cfg_server.clone(); + let server_thread = thread::spawn(move || { //TODO consider using get_available_port to use a random port - let server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + let mut server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + + #[cfg(feature = "signing")] + server.setup_signing(Some(singing_cfg_server)); let mut recv_count = 0; for _i in 0..RECEIVE_CHECK_COUNT { @@ -40,8 +53,12 @@ mod test_tcp_connections { thread::spawn(move || { let msg = mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); - let client = + let mut client = mavlink::connect("tcpout:127.0.0.1:14550").expect("Couldn't create client"); + + #[cfg(feature = "signing")] + client.setup_signing(Some(singing_cfg_client)); + for _i in 0..RECEIVE_CHECK_COUNT { client.send_default(&msg).ok(); } diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 332e9a1f02..45a72bf054 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -1,12 +1,18 @@ #![allow(unused)] pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { - incompat_flags: 0, sequence: 239, system_id: 1, component_id: 2, }; +#[cfg(feature = "signing")] +pub const SECRET_KEY: [u8; 32] = [ + 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, + 0x16, 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, + 0x56, 0xb3, +]; + #[cfg(feature = "common")] pub fn get_heartbeat_msg() -> mavlink::common::HEARTBEAT_DATA { mavlink::common::HEARTBEAT_DATA { From 3e5fa52c58b3c6da587f3b51c1b98b1d758bda96 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 11 Aug 2024 19:32:14 +0200 Subject: [PATCH 05/11] fix: remove std requirement for signing, fmt --- mavlink-core/Cargo.toml | 2 +- mavlink/tests/tcp_loopback_tests.rs | 5 +++-- mavlink/tests/test_shared/mod.rs | 5 ++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index d131be3c11..d276b2c44e 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -41,5 +41,5 @@ sha2 = { version = "0.10", optional = true } "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] "tokio-1" = ["dep:tokio"] -"signing" = ["dep:sha2", "std"] +"signing" = ["dep:sha2"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index e097febe42..7977c67256 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -9,7 +9,7 @@ mod test_tcp_connections { use crate::test_shared; - /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. + /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] pub fn test_tcp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 5; @@ -21,7 +21,8 @@ mod test_tcp_connections { let server_thread = thread::spawn(move || { //TODO consider using get_available_port to use a random port - let mut server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + let mut server = + mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); #[cfg(feature = "signing")] server.setup_signing(Some(singing_cfg_server)); diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 45a72bf054..90f019cebd 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -8,9 +8,8 @@ pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { #[cfg(feature = "signing")] pub const SECRET_KEY: [u8; 32] = [ - 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, - 0x16, 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, - 0x56, 0xb3, + 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, 0x16, + 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, 0x56, 0xb3, ]; #[cfg(feature = "common")] From 959264c60fac1fbde71bb6eea1c0c49e09b773c2 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:02:29 +0200 Subject: [PATCH 06/11] test: add features signing, tokio-1 to clippy test: add signing to msrv check --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 45d144f3c0..08992a9375 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -23,7 +23,7 @@ jobs: components: clippy - uses: actions-rs-plus/clippy-check@v2 with: - args: --all --all-targets --features format-generated-code + args: --all --all-targets --features format-generated-code --features signing --features tokio-1 internal-tests: runs-on: ubuntu-latest @@ -59,7 +59,7 @@ jobs: with: use-cross: true command: check - args: --all --all-targets + args: --all --all-targets --features signing build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] From d3f8c2c52d443fdf7f1246efd8735ab70e48d4b2 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:03:49 +0200 Subject: [PATCH 07/11] feat: add link_id to signing config --- mavlink-core/src/signing.rs | 16 ++++++++++------ mavlink/tests/signing.rs | 6 +++--- mavlink/tests/tcp_loopback_tests.rs | 6 +++--- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/mavlink-core/src/signing.rs b/mavlink-core/src/signing.rs index 6007901806..f1b129037c 100644 --- a/mavlink-core/src/signing.rs +++ b/mavlink-core/src/signing.rs @@ -9,6 +9,7 @@ use crate::MAVLINK_IFLAG_SIGNED; #[derive(Debug, Clone)] pub struct SigningConfig { secret_key: [u8; 32], + link_id: u8, pub(crate) sign_outgoing: bool, allow_unsigned: bool, } @@ -16,8 +17,6 @@ pub struct SigningConfig { // mutable state of signing per connection pub(crate) struct SigningState { timestamp: u64, - // currently link id is constant 0 - link_id: u8, stream_timestamps: HashMap<(u8, u8, u8), u64>, } @@ -28,9 +27,15 @@ pub struct SigningData { } impl SigningConfig { - pub fn new(secret_key: [u8; 32], sign_outgoing: bool, allow_unsigned: bool) -> Self { - SigningConfig { + pub fn new( + secret_key: [u8; 32], + link_id: u8, + sign_outgoing: bool, + allow_unsigned: bool, + ) -> Self { + Self { secret_key, + link_id, sign_outgoing, allow_unsigned, } @@ -43,7 +48,6 @@ impl SigningData { config, state: Mutex::new(SigningState { timestamp: 0, - link_id: 0, stream_timestamps: HashMap::new(), }), } @@ -108,7 +112,7 @@ impl SigningData { message .signature_timestamp_bytes_mut() .copy_from_slice(&ts_bytes[0..6]); - *message.signature_link_id_mut() = state.link_id; + *message.signature_link_id_mut() = self.config.link_id; let mut signature_buffer = [0u8; 6]; message.calculate_signature(&self.config.secret_key, &mut signature_buffer); diff --git a/mavlink/tests/signing.rs b/mavlink/tests/signing.rs index 5ac3837f9d..52ca63cd46 100644 --- a/mavlink/tests/signing.rs +++ b/mavlink/tests/signing.rs @@ -48,7 +48,7 @@ mod signing { #[test] pub fn test_verify() { - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); let mut r = PeekReader::new(HEARTBEAT_SIGNED); let msg = read_v2_raw_message::(&mut r).unwrap(); @@ -60,7 +60,7 @@ mod signing { #[test] pub fn test_invalid_ts() { - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); let mut r = PeekReader::new(HEARTBEAT_SIGNED); let mut msg = read_v2_raw_message::(&mut r).unwrap(); @@ -84,7 +84,7 @@ mod signing { }; message.serialize_message_for_signing(header, &heartbeat_message); - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); signing_data.sign_message(&mut message); assert!( diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index 7977c67256..170189cb97 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -5,9 +5,9 @@ mod test_tcp_connections { use std::thread; #[cfg(feature = "signing")] - use mavlink::SigningConfig; - use crate::test_shared; + #[cfg(feature = "signing")] + use mavlink::SigningConfig; /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] @@ -15,7 +15,7 @@ mod test_tcp_connections { const RECEIVE_CHECK_COUNT: i32 = 5; #[cfg(feature = "signing")] - let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, true, false); + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, 0, true, false); #[cfg(feature = "signing")] let singing_cfg_client = singing_cfg_server.clone(); From 51e6de1515bb2ff4e4a01c56c8b6cc1c848c58e0 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:04:47 +0200 Subject: [PATCH 08/11] feat: add signing to async read/write functions --- mavlink-core/src/lib.rs | 32 +++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index fd9d9476fe..1ac75b915a 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -867,10 +867,21 @@ fn read_v2_raw_message_inner( } /// Async read a raw buffer with the mavlink message +/// /// V2 maximum size is 280 bytes: `` #[cfg(feature = "tokio-1")] pub async fn read_v2_raw_message_async( reader: &mut R, +) -> Result { + read_v2_raw_message_async_inner::(reader, None).await +} + +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "tokio-1")] +async fn read_v2_raw_message_async_inner( + reader: &mut R, + signing_data: Option<&SigningData>, ) -> Result { loop { loop { @@ -895,10 +906,29 @@ pub async fn read_v2_raw_message_async() { + if !message.has_valid_crc::() { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } + } + return Ok(message); } } + +/// Async read a raw buffer with the mavlink message with signing support +/// V2 maximum size is 280 bytes: `` +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_v2_raw_message_async_signed( + reader: &mut R, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_async_inner::(reader, signing_data).await } /// Async read a raw buffer with the mavlink message From 552560b775af553e278be0f75337d2819ff579ac Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:05:16 +0200 Subject: [PATCH 09/11] fix: clippy warnings --- mavlink-core/src/connection/direct_serial.rs | 2 +- mavlink-core/src/connection/file.rs | 2 +- mavlink-core/src/connection/tcp.rs | 2 +- mavlink-core/src/connection/udp.rs | 2 +- mavlink-core/src/lib.rs | 8 ++++---- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index d2f376aa14..2954b98107 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -124,6 +124,6 @@ impl MavConnection for SerialConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index c635d2be2d..7258259347 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -75,6 +75,6 @@ impl MavConnection for FileConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 04761ff6af..d362a5664a 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -144,6 +144,6 @@ impl MavConnection for TcpConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 9481684167..d2058fc896 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -196,7 +196,7 @@ impl MavConnection for UdpConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 1ac75b915a..12d470b614 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -705,11 +705,11 @@ impl MAVLinkV2MessageRaw { pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) { let mut hasher = Sha256::new(); hasher.update(secret_key); - hasher.update(&[MAV_STX_V2]); + hasher.update([MAV_STX_V2]); hasher.update(self.header()); hasher.update(self.payload()); hasher.update(self.checksum_bytes()); - hasher.update(&[self.signature_link_id()]); + hasher.update([self.signature_link_id()]); hasher.update(self.signature_timestamp_bytes()); target_buffer.copy_from_slice(&hasher.finalize()[0..6]); } @@ -917,9 +917,9 @@ async fn read_v2_raw_message_async_inner` From 614cd28686438a6c6ace0b6643ec23c65ad1a7c9 Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 00:03:23 +0200 Subject: [PATCH 10/11] fix: remove redundant feature gate test: add signing to internal-tests matrix test: add signing as msrv matrix option --- .github/workflows/test.yml | 8 ++++++-- mavlink-core/src/connection/udp.rs | 3 --- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 08992a9375..071df1ef7b 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -30,11 +30,12 @@ jobs: strategy: matrix: dialect: ["ardupilotmega", "asluav", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] + signing: ["", "--features signing"] steps: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable - name: Run internal tests - run: cargo test --verbose --features ${{ matrix.dialect }} --features signing -- --nocapture + run: cargo test --verbose --features ${{ matrix.dialect }} ${{ matrix.signing }} -- --nocapture mavlink-dump: runs-on: ubuntu-latest @@ -46,6 +47,9 @@ jobs: msrv: runs-on: ubuntu-latest + strategy: + matrix: + signing: ["", "--features signing"] steps: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master @@ -59,7 +63,7 @@ jobs: with: use-cross: true command: check - args: --all --all-targets --features signing + args: --all --all-targets ${{ matrix.signing }} build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index d2058fc896..6a4c75defe 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -166,8 +166,6 @@ impl MavConnection for UdpConnection { self.protocol_version, header, data, - #[cfg(feature = "signing")] - self.signing_data.as_ref(), )?; #[cfg(feature = "signing")] write_versioned_msg_signed( @@ -175,7 +173,6 @@ impl MavConnection for UdpConnection { self.protocol_version, header, data, - #[cfg(feature = "signing")] self.signing_data.as_ref(), )?; state.socket.send_to(&buf, addr)? From 7459923daeaf4bd6fa3a1502a67bc3d9ea053ade Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 00:06:04 +0200 Subject: [PATCH 11/11] fix: fmt --- mavlink-core/src/connection/udp.rs | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 6a4c75defe..75e5be3c48 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -161,12 +161,7 @@ impl MavConnection for UdpConnection { let len = if let Some(addr) = state.dest { let mut buf = Vec::new(); #[cfg(not(feature = "signing"))] - write_versioned_msg( - &mut buf, - self.protocol_version, - header, - data, - )?; + write_versioned_msg(&mut buf, self.protocol_version, header, data)?; #[cfg(feature = "signing")] write_versioned_msg_signed( &mut buf,