diff --git a/Cargo.lock b/Cargo.lock index 312660b..f8769c1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -213,6 +213,11 @@ version = "0.8.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" +[[package]] +name = "corncobs" +version = "0.1.3" +source = "git+https://github.com/de-vri-es/corncobs?rev=68025686e0547fac483baabb63c8066065023d88#68025686e0547fac483baabb63c8066065023d88" + [[package]] name = "defmt" version = "0.3.8" @@ -261,6 +266,7 @@ dependencies = [ "bacnet-macros", "chrono", "clap", + "corncobs", "defmt", "log", "maybe-async", diff --git a/Cargo.toml b/Cargo.toml index 8f3b9f1..d840b79 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,6 +20,9 @@ serde = { version = "1.0", default-features = false, features = [ maybe-async = { version = "0.2.10", default-features = false } bacnet-macros = { path = "./bacnet-macros", version = "0.1.0" } +# https://github.com/cbiffle/corncobs/pull/5 +corncobs = { git = "https://github.com/de-vri-es/corncobs", rev = "68025686e0547fac483baabb63c8066065023d88" } + # TODO: add this when it reaches 1.0 # derive_more = { version = "^1.0.0-beta.6", default-features = false, features = [ "try_from"] } diff --git a/examples/troubleshoot.rs b/examples/troubleshoot.rs index b133bf7..4ea49b5 100644 --- a/examples/troubleshoot.rs +++ b/examples/troubleshoot.rs @@ -1,7 +1,7 @@ // use this example to troubleshoot an invalid BACnet packet // RUST_BACKTRACE=1 cargo run --example troubleshoot -use embedded_bacnet::{common::io::Reader, network_protocol::data_link::DataLink}; +use embedded_bacnet::{common::io::Reader, network_protocol::ip::IpFrame}; fn main() { // a valid BACnet packet @@ -9,11 +9,11 @@ fn main() { 129, 10, 0, 21, 1, 4, 2, 117, 1, 14, 12, 2, 0, 3, 243, 30, 9, 56, 9, 57, 31, ]; - // use the DataLink codec to decode the bytes + // decode the bytes as BACnet/IP frame let mut reader = Reader::default(); // if the packet is invalid this should panid - let message = DataLink::decode(&mut reader, &buf).unwrap(); + let message = IpFrame::decode(&mut reader, &buf).unwrap(); println!("{:?}", message) } diff --git a/examples/who_is_broadcast.rs b/examples/who_is_broadcast.rs index e63fe16..77e54e7 100644 --- a/examples/who_is_broadcast.rs +++ b/examples/who_is_broadcast.rs @@ -10,7 +10,7 @@ use embedded_bacnet::{ }, common::io::{Reader, Writer}, network_protocol::{ - data_link::{DataLink, DataLinkFunction}, + ip::{BvllFunction, IpFrame}, network_pdu::{DestinationAddress, MessagePriority, NetworkMessage, NetworkPdu}, }, }; @@ -58,7 +58,7 @@ fn main() -> Result<(), Error> { let dst = Some(DestinationAddress::new(0xffff, None)); let message = NetworkMessage::Apdu(apdu); let npdu = NetworkPdu::new(None, dst, false, MessagePriority::Normal, message); - let data_link = DataLink::new(DataLinkFunction::OriginalBroadcastNpdu, Some(npdu)); + let data_link = IpFrame::new(BvllFunction::OriginalBroadcastNpdu, Some(npdu)); let mut buffer = vec![0; 1500]; @@ -75,7 +75,7 @@ fn main() -> Result<(), Error> { let payload = &buffer[..n]; println!("Received: {:02x?} from {:?}", payload, peer); let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, payload); + let message = IpFrame::decode(&mut reader, payload); println!("Decoded: {:?}\n", message); } } diff --git a/src/application_protocol/confirmed.rs b/src/application_protocol/confirmed.rs index c93c089..61e9591 100644 --- a/src/application_protocol/confirmed.rs +++ b/src/application_protocol/confirmed.rs @@ -16,7 +16,7 @@ use crate::{ spec::{ErrorClass, ErrorCode}, tag::{ApplicationTagNumber, Tag, TagNumber}, }, - network_protocol::{data_link::DataLink, network_pdu::NetworkMessage}, + network_protocol::{ip::IpFrame, mstp::MstpFrame, network_pdu::NetworkMessage}, }; #[derive(Debug, Clone)] @@ -221,10 +221,26 @@ pub struct SimpleAck { pub service_choice: ConfirmedServiceChoice, } -impl<'a> TryFrom> for SimpleAck { +impl<'a> TryFrom> for SimpleAck { type Error = Error; - fn try_from(value: DataLink<'a>) -> Result { + fn try_from(value: IpFrame<'a>) -> Result { + match value.npdu { + Some(x) => match x.network_message { + NetworkMessage::Apdu(ApplicationPdu::SimpleAck(ack)) => Ok(ack), + _ => Err(Error::ConvertDataLink( + "npdu message is not an apdu simple ack", + )), + }, + _ => Err(Error::ConvertDataLink("no npdu defined in message")), + } + } +} + +impl<'a> TryFrom> for SimpleAck { + type Error = Error; + + fn try_from(value: MstpFrame<'a>) -> Result { match value.npdu { Some(x) => match x.network_message { NetworkMessage::Apdu(ApplicationPdu::SimpleAck(ack)) => Ok(ack), @@ -315,10 +331,26 @@ pub struct ComplexAck<'a> { pub service: ComplexAckService<'a>, } -impl<'a> TryFrom> for ComplexAck<'a> { +impl<'a> TryFrom> for ComplexAck<'a> { + type Error = Error; + + fn try_from(value: IpFrame<'a>) -> Result { + match value.npdu { + Some(x) => match x.network_message { + NetworkMessage::Apdu(ApplicationPdu::ComplexAck(ack)) => Ok(ack), + _ => Err(Error::ConvertDataLink( + "npdu message is not an apdu complex ack", + )), + }, + _ => Err(Error::ConvertDataLink("no npdu defined in message")), + } + } +} + +impl<'a> TryFrom> for ComplexAck<'a> { type Error = Error; - fn try_from(value: DataLink<'a>) -> Result { + fn try_from(value: MstpFrame<'a>) -> Result { match value.npdu { Some(x) => match x.network_message { NetworkMessage::Apdu(ApplicationPdu::ComplexAck(ack)) => Ok(ack), @@ -442,10 +474,26 @@ pub struct SegmentAck { pub proposed_window_size: u8, } -impl<'a> TryFrom> for SegmentAck { +impl<'a> TryFrom> for SegmentAck { + type Error = Error; + + fn try_from(value: IpFrame<'a>) -> Result { + match value.npdu { + Some(x) => match x.network_message { + NetworkMessage::Apdu(ApplicationPdu::SegmentAck(ack)) => Ok(ack), + _ => Err(Error::ConvertDataLink( + "npdu message is not an apdu simple ack", + )), + }, + _ => Err(Error::ConvertDataLink("no npdu defined in message")), + } + } +} + +impl<'a> TryFrom> for SegmentAck { type Error = Error; - fn try_from(value: DataLink<'a>) -> Result { + fn try_from(value: MstpFrame<'a>) -> Result { match value.npdu { Some(x) => match x.network_message { NetworkMessage::Apdu(ApplicationPdu::SegmentAck(ack)) => Ok(ack), diff --git a/src/application_protocol/services/read_property.rs b/src/application_protocol/services/read_property.rs index cbf4e3f..2dfbb6e 100644 --- a/src/application_protocol/services/read_property.rs +++ b/src/application_protocol/services/read_property.rs @@ -16,7 +16,7 @@ use crate::{ spec::BACNET_ARRAY_ALL, tag::{ApplicationTagNumber, Tag, TagNumber}, }, - network_protocol::data_link::DataLink, + network_protocol::{ip::IpFrame, mstp::MstpFrame}, }; #[cfg(feature = "alloc")] @@ -155,10 +155,24 @@ pub struct ReadPropertyAck<'a> { pub property_value: ReadPropertyValue<'a>, } -impl<'a> TryFrom> for ReadPropertyAck<'a> { +impl<'a> TryFrom> for ReadPropertyAck<'a> { type Error = Error; - fn try_from(value: DataLink<'a>) -> Result { + fn try_from(value: IpFrame<'a>) -> Result { + let ack: ComplexAck = value.try_into()?; + match ack.service { + ComplexAckService::ReadProperty(ack) => Ok(ack), + _ => Err(Error::ConvertDataLink( + "apdu message is not a ComplexAckService ReadPropertyAck", + )), + } + } +} + +impl<'a> TryFrom> for ReadPropertyAck<'a> { + type Error = Error; + + fn try_from(value: MstpFrame<'a>) -> Result { let ack: ComplexAck = value.try_into()?; match ack.service { ComplexAckService::ReadProperty(ack) => Ok(ack), diff --git a/src/application_protocol/services/read_property_multiple.rs b/src/application_protocol/services/read_property_multiple.rs index 3af536b..489687d 100644 --- a/src/application_protocol/services/read_property_multiple.rs +++ b/src/application_protocol/services/read_property_multiple.rs @@ -19,7 +19,7 @@ use crate::{ spec::{ErrorClass, ErrorCode, BACNET_ARRAY_ALL}, tag::{ApplicationTagNumber, Tag, TagNumber}, }, - network_protocol::data_link::DataLink, + network_protocol::{ip::IpFrame, mstp::MstpFrame}, }; #[cfg(feature = "alloc")] @@ -57,10 +57,24 @@ impl<'a> IntoIterator for &'_ ReadPropertyMultipleAck<'a> { } } -impl<'a> TryFrom> for ReadPropertyMultipleAck<'a> { +impl<'a> TryFrom> for ReadPropertyMultipleAck<'a> { type Error = Error; - fn try_from(value: DataLink<'a>) -> Result { + fn try_from(value: IpFrame<'a>) -> Result { + let ack: ComplexAck = value.try_into()?; + match ack.service { + ComplexAckService::ReadPropertyMultiple(ack) => Ok(ack), + _ => Err(Error::ConvertDataLink( + "apdu message is not a ComplexAckService ReadPropertyMultipleAck", + )), + } + } +} + +impl<'a> TryFrom> for ReadPropertyMultipleAck<'a> { + type Error = Error; + + fn try_from(value: MstpFrame<'a>) -> Result { let ack: ComplexAck = value.try_into()?; match ack.service { ComplexAckService::ReadPropertyMultiple(ack) => Ok(ack), diff --git a/src/common/error.rs b/src/common/error.rs index ecd77ef..060d357 100644 --- a/src/common/error.rs +++ b/src/common/error.rs @@ -10,6 +10,7 @@ use crate::{ #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub enum Error { + InvalidProtocolVersion(u8), Length((&'static str, u32)), InvalidValue(&'static str), InvalidVariant((&'static str, u32)), diff --git a/src/network_protocol/data_link.rs b/src/network_protocol/ip.rs similarity index 88% rename from src/network_protocol/data_link.rs rename to src/network_protocol/ip.rs index 712612f..3217447 100644 --- a/src/network_protocol/data_link.rs +++ b/src/network_protocol/ip.rs @@ -10,15 +10,15 @@ use crate::{ // Bacnet Virtual Link Control #[derive(Debug, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub struct DataLink<'a> { - pub function: DataLinkFunction, +pub struct IpFrame<'a> { + pub function: BvllFunction, pub npdu: Option>, } #[derive(Debug, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(u8)] -pub enum DataLinkFunction { +pub enum BvllFunction { Result = 0, WriteBroadcastDistributionTable = 1, ReadBroadcastDistTable = 2, @@ -33,7 +33,7 @@ pub enum DataLinkFunction { OriginalBroadcastNpdu = 11, } -impl TryFrom for DataLinkFunction { +impl TryFrom for BvllFunction { type Error = u8; fn try_from(value: u8) -> Result { @@ -57,11 +57,11 @@ impl TryFrom for DataLinkFunction { const BVLL_TYPE_BACNET_IP: u8 = 0x81; -impl<'a> DataLink<'a> { +impl<'a> IpFrame<'a> { // const BVLC_ORIGINAL_UNICAST_NPDU: u8 = 10; // const BVLC_ORIGINAL_BROADCAST_NPDU: u8 = 11; - pub fn new(function: DataLinkFunction, npdu: Option>) -> Self { + pub fn new(function: BvllFunction, npdu: Option>) -> Self { Self { function, npdu } } @@ -69,14 +69,14 @@ impl<'a> DataLink<'a> { let apdu = ApplicationPdu::ConfirmedRequest(req); let message = NetworkMessage::Apdu(apdu); let npdu = NetworkPdu::new(None, None, true, MessagePriority::Normal, message); - DataLink::new(DataLinkFunction::OriginalUnicastNpdu, Some(npdu)) + Self::new(BvllFunction::OriginalUnicastNpdu, Some(npdu)) } pub fn encode(&self, writer: &mut Writer) { writer.push(BVLL_TYPE_BACNET_IP); writer.push(self.function.clone() as u8); match &self.function { - DataLinkFunction::OriginalBroadcastNpdu | DataLinkFunction::OriginalUnicastNpdu => { + BvllFunction::OriginalBroadcastNpdu | BvllFunction::OriginalUnicastNpdu => { writer.extend_from_slice(&[0, 0]); // length placeholder self.npdu.as_ref().unwrap().encode(writer); // should be ok to unwrap here since it has already been checked Self::update_len(writer); @@ -114,7 +114,7 @@ impl<'a> DataLink<'a> { let npdu = match function { // see h_bbmd.c for all the types (only 2 are supported here) - DataLinkFunction::OriginalBroadcastNpdu | DataLinkFunction::OriginalUnicastNpdu => { + BvllFunction::OriginalBroadcastNpdu | BvllFunction::OriginalUnicastNpdu => { Some(NetworkPdu::decode(reader, buf)?) } _ => None, diff --git a/src/network_protocol/mod.rs b/src/network_protocol/mod.rs index a29d139..c0bb150 100644 --- a/src/network_protocol/mod.rs +++ b/src/network_protocol/mod.rs @@ -1,2 +1,11 @@ -pub mod data_link; +pub mod ip; +pub mod mstp; pub mod network_pdu; + +pub mod data_link { + #[deprecated(note = "use IpFrame")] + pub type DataLink<'a> = super::ip::IpFrame<'a>; + + #[deprecated(note = "use IpDataLinkFunction")] + pub type DataLinkFunction = super::ip::BvllFunction; +} diff --git a/src/network_protocol/mstp.rs b/src/network_protocol/mstp.rs new file mode 100644 index 0000000..d9054d4 --- /dev/null +++ b/src/network_protocol/mstp.rs @@ -0,0 +1,700 @@ +use crate::{ + application_protocol::{application_pdu::ApplicationPdu, confirmed::ConfirmedRequest}, + common::{ + error::Error, + io::{Reader, Writer}, + }, + network_protocol::network_pdu::{MessagePriority, NetworkMessage, NetworkPdu}, +}; + +// Bacnet Virtual Link Control +#[derive(Debug, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct MstpFrame<'a> { + pub frame_type: MstpFrameType, + pub destination_address: u8, + pub source_address: u8, + pub npdu: Option>, +} + +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(u8)] +pub enum MstpFrameType { + Token = 0, + PollForManager = 1, + ReplyToPollForManager = 2, + TestRequest = 3, + TestResponse = 4, + BacnetDataExpectingReply = 5, + BacnetDataNotExpectingReply = 6, + ReplyPostponed = 7, + BacnetExtendedDataExpectingReply = 32, + BacnetExtendedDataNotExpectingReply = 33, +} + +impl MstpFrameType { + #[rustfmt::skip] + pub fn has_npdu(self) -> bool { + matches!(self, + | Self::BacnetDataExpectingReply + | Self::BacnetDataNotExpectingReply + | Self::BacnetExtendedDataExpectingReply + | Self::BacnetExtendedDataNotExpectingReply + ) + } + + pub fn is_cobs_encoded(self) -> bool { + matches!(self as u8, 32..=127) + } +} + +impl From for u8 { + fn from(value: MstpFrameType) -> Self { + value as Self + } +} + +impl TryFrom for MstpFrameType { + type Error = u8; + + fn try_from(value: u8) -> Result { + match value { + 0 => Ok(Self::Token), + 1 => Ok(Self::PollForManager), + 2 => Ok(Self::ReplyToPollForManager), + 3 => Ok(Self::TestRequest), + 4 => Ok(Self::TestResponse), + 5 => Ok(Self::BacnetDataExpectingReply), + 6 => Ok(Self::BacnetDataNotExpectingReply), + 7 => Ok(Self::ReplyPostponed), + 32 => Ok(Self::BacnetExtendedDataExpectingReply), + 33 => Ok(Self::BacnetExtendedDataNotExpectingReply), + x => Err(x), + } + } +} + +/// Preamble at the start of a frame. +const PREAMBLE: [u8; 2] = [0x55, 0xFF]; + +/// Length of the total header. +const HEADER_LEN: usize = 8; + +/// Length of the CRC for non COBS-encoded data. +const DATA_CRC_LEN: usize = 2; + +/// Length of the CRC32K after COBS encoding. +const COBS_ENCODED_CRC_LEN: usize = 5; + +/// Offset of the length field in the MS/TP frame header. +const HEADER_LEN_OFFSET: usize = 5; + +/// Offset of the header CRC field in the MS/TP frame header. +const HEADER_CRC_OFFSET: usize = 7; + +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum ScanError { + Garbage(usize), + InvalidHeader, + IncompleteHeader, +} + +impl ScanError { + /// Number of bytes to discard from the front of the input stream. + pub fn discard_len(&self) -> usize { + match self { + Self::Garbage(x) => *x, + Self::InvalidHeader => PREAMBLE.len(), + Self::IncompleteHeader => 0, + } + } +} + +impl<'a> MstpFrame<'a> { + pub fn new( + frame_type: MstpFrameType, + destination_address: u8, + source_address: u8, + npdu: Option>, + ) -> Self { + Self { + frame_type, + destination_address, + source_address, + npdu, + } + } + + pub fn new_confirmed_req( + destination_address: u8, + source_address: u8, + req: ConfirmedRequest<'a>, + ) -> Self { + let apdu = ApplicationPdu::ConfirmedRequest(req); + let message = NetworkMessage::Apdu(apdu); + let npdu = NetworkPdu::new(None, None, true, MessagePriority::Normal, message); + Self::new( + MstpFrameType::BacnetDataExpectingReply, + destination_address, + source_address, + Some(npdu), + ) + } + + pub fn encode(&self, writer: &mut Writer) -> Result<(), Error> { + let header_start = writer.index; + writer.extend_from_slice(&PREAMBLE); + writer.extend_from_slice(&[ + self.frame_type.into(), + self.destination_address, + self.source_address, + 0, // length placeholder (2 bytes) + 0, + 0, // header CRC placeholder + ]); + let data_start = writer.index; + if let Some(npdu) = &self.npdu { + npdu.encode(writer); + } + + match self.frame_type.is_cobs_encoded() { + // For old style frames with data, just add the CRC. + false => { + if writer.index != data_start { + let crc_value = !data_crc(&writer.to_bytes()[data_start..]); // flip the bits of the CRC value + writer.extend_from_slice(&crc_value.to_le_bytes()); // little endian! + } + } + // For COBS-encoded frames, encode the payload. + true => { + // Perform COBS encoding on the message, including the CRC. + let message_buffer = &mut writer.buf[data_start..]; + let message_len = writer.index - data_start; + let encoded_len = encode_cobs(message_buffer, message_len)?; + // Update the position of the writer. + writer.index = data_start + encoded_len; + + // Add the CRC32K checksum (5 bytes...) + let crc_value = encoded_crc32k(&writer.buf[data_start..writer.index]); + writer.extend_from_slice(&crc_value); // little endian! + } + } + + // If there is a payload, update the length field in the header. + if writer.index != data_start { + // Exclude the 2 byte checksum from the length, even for COBS encoded frames (for backwards compatibility). + let len = writer.index - data_start - DATA_CRC_LEN; + let len = u16::try_from(len) + .map_err(|_| Error::Length(("message payload too long", len as u32)))?; + writer.buf[header_start + HEADER_LEN_OFFSET..][..2].copy_from_slice(&len.to_be_bytes()); + } + + let header_crc = !header_crc(&writer.to_bytes()[header_start..data_start]); // NOTE: flip the bits of the CRC + writer.buf[header_start + HEADER_CRC_OFFSET] = header_crc; + Ok(()) + } + + /// Scan the reader for an incoming frame. + /// + /// If a valid frame header is found at the start of the buffer, + /// this function returns the length of the whole frame. + /// Note that this does not guarantee that the complete data payload is also already in the buffer. + /// + /// If no valid frame header is present at the start of the buffer, + /// a [`ScanError`] is returned which will tell you how much data to discard from the buffer. + /// + /// Does not modify the position of the reader. + pub fn scan(reader: &mut Reader, buf: &'a [u8]) -> Result { + let buf = &buf[reader.index..reader.end]; + + // Count bytes before the first valid pre-amble. + let garbage = buf + .array_windows::<2>() + .take_while(|&&data| data != PREAMBLE) + .count(); + if garbage > 0 { + return Err(ScanError::Garbage(garbage)); + } + + // Make sure the buffer contains a complete header. + if buf.len() < HEADER_LEN { + return Err(ScanError::IncompleteHeader); + } + + // Check the header CRC. + if header_crc(&buf[PREAMBLE.len()..HEADER_LEN]) != 0x55 { + return Err(ScanError::InvalidHeader); + } + + // Report the total frame size. + let len_bytes = &buf[HEADER_LEN_OFFSET..][..2]; + let data_len = u16::from_be_bytes([len_bytes[0], len_bytes[1]]) as usize; + if data_len > 0 { + Ok(data_len + HEADER_LEN + DATA_CRC_LEN) + } else { + // No data, no CRC + Ok(HEADER_LEN) + } + } + + /// Decode a BACnet MS/TP frame. + /// + /// NOTE: This will modify the buffer in-place to decode COBS-encoded payloads. + #[cfg_attr(feature = "alloc", bacnet_macros::remove_lifetimes_from_fn_args)] + pub fn decode(reader: &mut Reader, buf: &'a mut [u8]) -> Result { + let preamble: [u8; 2] = reader.read_bytes(buf)?; + if preamble != PREAMBLE { + return Err(Error::InvalidValue("invalid MS/TP frame preamble")); + } + let header_start = reader.index; + + let frame_type = MstpFrameType::try_from(reader.read_byte(buf)?) + .map_err(|_| Error::InvalidValue("invalid or unrecognized MS/TP frame type"))?; + let destination_address = reader.read_byte(buf)?; + let source_address = reader.read_byte(buf)?; + let data_len = u16::from_be_bytes(reader.read_bytes(buf)?) as usize; + let _header_crc = reader.read_byte(buf)?; + let data_start = reader.index; + + // Check the header CRC (before checking length, it's a better error for corrupt transmissions). + if header_crc(&buf[header_start..data_start]) != 0x55 { + return Err(Error::InvalidValue("invalid MS/TP header CRC")); + } + + if data_len == 0 { + return Ok(Self { + frame_type, + destination_address, + source_address, + npdu: None, + }); + } + + // Get a mutable slice for the payload data with (encoded) CRC. + let data_start = reader.index; + reader.read_slice(data_len + DATA_CRC_LEN, buf)?; + let data = &mut buf[data_start..][..data_len + DATA_CRC_LEN]; + + // For "simple" frames, just check the data CRC and then remove it. + let data = match frame_type.is_cobs_encoded() { + false => { + if data_crc(data) != 0xF0B8 { + return Err(Error::InvalidValue("invalid MS/TP data CRC")); + } + &data[..data_len] + } + // For COBS encoded frames, things are a bit more complicated. + true => { + if data.len() < COBS_ENCODED_CRC_LEN { + return Err(Error::Length(( + "COBS encoded payload is too short", + data.len() as u32, + ))); + } + + // First decode the CRC. + // This leaves the decoded CRC in the 4 bytes after the COBS encoded data. + let crc_start = data.len() - COBS_ENCODED_CRC_LEN; + decode_cobs(&mut data[crc_start..]) + .map_err(|()| Error::ConvertDataLink("invalid COBS encoded CRC"))?; + + // Now verify the CRC. + if crc32k(&data[..crc_start + 4]) != 0x0843323B { + return Err(Error::ConvertDataLink( + "Invalid MS/TP COBS-encoded data CRC", + )); + } + + // Now decode the COBS data itself. + let decoded_len = decode_cobs(&mut data[..crc_start]) + .map_err(|()| Error::ConvertDataLink("invalid COBS encoded data payload"))?; + &data[..decoded_len] + } + }; + + let npdu = match frame_type.has_npdu() { + false => None, + true => Some(NetworkPdu::decode( + &mut Reader::new_with_len(data.len()), + data, + )?), + }; + + Ok(Self { + frame_type, + destination_address, + source_address, + npdu, + }) + } +} + +fn header_crc(data: &[u8]) -> u8 { + let mut crc = 0xFF; + for byte in data { + crc = HEADER_CRC_TABLE[usize::from(crc ^ byte)]; + } + + // NOTE: When encoded in the frame, the bits must be flipped, but the CRC value itself is not bit-flipped. + // This matters because the specification also tells you the CRC value to check for, and when they do it is *without* flipped bits. + crc +} + +fn data_crc(data: &[u8]) -> u16 { + let mut crc = 0xFFFF; + + for &byte in data { + // NOTE: This CRC works the opposite way from normal because ASHRAE decided that bit 7 represents x^0 and bit 0 represents x^7. + // For this reason, we XOR each data byte with the least significant byte of the CRC accumulator instead of the most significant byte. + let index = usize::from(crc) & 0xFF ^ usize::from(byte); + crc = crc >> 8 ^ DATA_CRC_TABLE[index] + } + + // NOTE: When encoded in the frame, the bits must be flipped, but the CRC value itself is not bit-flipped. + // This matters because the specification also tells you the CRC value to check for, and when they do it is *without* flipped bits. + crc +} + +fn crc32k(data: &[u8]) -> u32 { + let mut crc = 0xFFFF_FFFF; + + for &byte in data { + // NOTE: This CRC works the opposite way from normal because ASHRAE decided that bit 7 represents x^0 and bit 0 represents x^7. + // For this reason, we XOR each data byte with the least significant byte of the CRC accumulator instead of the most significant byte. + let index = (crc & 0xFF) as usize ^ usize::from(byte); + crc = crc >> 8 ^ CRC32K_TABLE[index] + } + + // NOTE: When encoded in the frame, the bits must be flipped, but the CRC value itself is not bit-flipped. + // This matters because the specification also tells you the CRC value to check for, and when they do it is *without* flipped bits. + crc +} + +fn encoded_crc32k(data: &[u8]) -> [u8; 5] { + let crc = crc32k(data); + let crc = !crc; // flip bits + let crc = crc.to_le_bytes(); // little endian + let mut encoded = [0u8; 6]; + let len = corncobs::encode_buf(&crc, &mut encoded); + debug_assert_eq!(len, 6); + [encoded[0], encoded[1], encoded[2], encoded[3], encoded[4]] +} + +fn decode_cobs(data: &mut [u8]) -> Result { + // XOR all bytes with 0x55 before decoding. + for byte in data.iter_mut() { + *byte ^= 0x55; + } + corncobs::decode_in_place(data).map_err(|_| ()) +} + +fn encode_cobs(buffer: &mut [u8], message_len: usize) -> Result { + if buffer.len() < corncobs::max_encoded_len(message_len) { + return Err(Error::Length(( + "buffer not large enough to encode COBS payload", + buffer.len() as u32, + ))); + } + + let encoded_len = corncobs::encode_in_place(buffer, message_len); + let encoded_len = encoded_len - 1; // COBS adds a trailing 0 byte, BACnet does not + + // XOR all bytes with 0x55 after endecoding. + for byte in &mut buffer[..encoded_len] { + *byte ^= 0x55; + } + + Ok(encoded_len) +} + +/// CRC table for the header checksum. +#[rustfmt::skip] +const HEADER_CRC_TABLE: [u8; 256] = [ + 0x00, 0xFE, 0xFF, 0x01, 0xFD, 0x03, 0x02, 0xFC, + 0xF9, 0x07, 0x06, 0xF8, 0x04, 0xFA, 0xFB, 0x05, + 0xF1, 0x0F, 0x0E, 0xF0, 0x0C, 0xF2, 0xF3, 0x0D, + 0x08, 0xF6, 0xF7, 0x09, 0xF5, 0x0B, 0x0A, 0xF4, + 0xE1, 0x1F, 0x1E, 0xE0, 0x1C, 0xE2, 0xE3, 0x1D, + 0x18, 0xE6, 0xE7, 0x19, 0xE5, 0x1B, 0x1A, 0xE4, + 0x10, 0xEE, 0xEF, 0x11, 0xED, 0x13, 0x12, 0xEC, + 0xE9, 0x17, 0x16, 0xE8, 0x14, 0xEA, 0xEB, 0x15, + 0xC1, 0x3F, 0x3E, 0xC0, 0x3C, 0xC2, 0xC3, 0x3D, + 0x38, 0xC6, 0xC7, 0x39, 0xC5, 0x3B, 0x3A, 0xC4, + 0x30, 0xCE, 0xCF, 0x31, 0xCD, 0x33, 0x32, 0xCC, + 0xC9, 0x37, 0x36, 0xC8, 0x34, 0xCA, 0xCB, 0x35, + 0x20, 0xDE, 0xDF, 0x21, 0xDD, 0x23, 0x22, 0xDC, + 0xD9, 0x27, 0x26, 0xD8, 0x24, 0xDA, 0xDB, 0x25, + 0xD1, 0x2F, 0x2E, 0xD0, 0x2C, 0xD2, 0xD3, 0x2D, + 0x28, 0xD6, 0xD7, 0x29, 0xD5, 0x2B, 0x2A, 0xD4, + 0x81, 0x7F, 0x7E, 0x80, 0x7C, 0x82, 0x83, 0x7D, + 0x78, 0x86, 0x87, 0x79, 0x85, 0x7B, 0x7A, 0x84, + 0x70, 0x8E, 0x8F, 0x71, 0x8D, 0x73, 0x72, 0x8C, + 0x89, 0x77, 0x76, 0x88, 0x74, 0x8A, 0x8B, 0x75, + 0x60, 0x9E, 0x9F, 0x61, 0x9D, 0x63, 0x62, 0x9C, + 0x99, 0x67, 0x66, 0x98, 0x64, 0x9A, 0x9B, 0x65, + 0x91, 0x6F, 0x6E, 0x90, 0x6C, 0x92, 0x93, 0x6D, + 0x68, 0x96, 0x97, 0x69, 0x95, 0x6B, 0x6A, 0x94, + 0x40, 0xBE, 0xBF, 0x41, 0xBD, 0x43, 0x42, 0xBC, + 0xB9, 0x47, 0x46, 0xB8, 0x44, 0xBA, 0xBB, 0x45, + 0xB1, 0x4F, 0x4E, 0xB0, 0x4C, 0xB2, 0xB3, 0x4D, + 0x48, 0xB6, 0xB7, 0x49, 0xB5, 0x4B, 0x4A, 0xB4, + 0xA1, 0x5F, 0x5E, 0xA0, 0x5C, 0xA2, 0xA3, 0x5D, + 0x58, 0xA6, 0xA7, 0x59, 0xA5, 0x5B, 0x5A, 0xA4, + 0x50, 0xAE, 0xAF, 0x51, 0xAD, 0x53, 0x52, 0xAC, + 0xA9, 0x57, 0x56, 0xA8, 0x54, 0xAA, 0xAB, 0x55, +]; + +/// CRC table for the data checksum. +#[rustfmt::skip] +const DATA_CRC_TABLE: [u16; 256] = [ + 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, + 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, + 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E, + 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876, + 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD, + 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5, + 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C, + 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974, + 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB, + 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3, + 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A, + 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72, + 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9, + 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1, + 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738, + 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70, + 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7, + 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF, + 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036, + 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E, + 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5, + 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD, + 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134, + 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C, + 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3, + 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB, + 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232, + 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A, + 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1, + 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9, + 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, + 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78, +]; + +/// CRC table for CRC32K (with most significant bit representing x^0) +#[rustfmt::skip] +const CRC32K_TABLE: [u32; 256] = [ + 0x00000000, 0x9695C4CA, 0xFB4839C9, 0x6DDDFD03, 0x20F3C3CF, 0xB6660705, 0xDBBBFA06, 0x4D2E3ECC, + 0x41E7879E, 0xD7724354, 0xBAAFBE57, 0x2C3A7A9D, 0x61144451, 0xF781809B, 0x9A5C7D98, 0x0CC9B952, + 0x83CF0F3C, 0x155ACBF6, 0x788736F5, 0xEE12F23F, 0xA33CCCF3, 0x35A90839, 0x5874F53A, 0xCEE131F0, + 0xC22888A2, 0x54BD4C68, 0x3960B16B, 0xAFF575A1, 0xE2DB4B6D, 0x744E8FA7, 0x199372A4, 0x8F06B66E, + 0xD1FDAE25, 0x47686AEF, 0x2AB597EC, 0xBC205326, 0xF10E6DEA, 0x679BA920, 0x0A465423, 0x9CD390E9, + 0x901A29BB, 0x068FED71, 0x6B521072, 0xFDC7D4B8, 0xB0E9EA74, 0x267C2EBE, 0x4BA1D3BD, 0xDD341777, + 0x5232A119, 0xC4A765D3, 0xA97A98D0, 0x3FEF5C1A, 0x72C162D6, 0xE454A61C, 0x89895B1F, 0x1F1C9FD5, + 0x13D52687, 0x8540E24D, 0xE89D1F4E, 0x7E08DB84, 0x3326E548, 0xA5B32182, 0xC86EDC81, 0x5EFB184B, + 0x7598EC17, 0xE30D28DD, 0x8ED0D5DE, 0x18451114, 0x556B2FD8, 0xC3FEEB12, 0xAE231611, 0x38B6D2DB, + 0x347F6B89, 0xA2EAAF43, 0xCF375240, 0x59A2968A, 0x148CA846, 0x82196C8C, 0xEFC4918F, 0x79515545, + 0xF657E32B, 0x60C227E1, 0x0D1FDAE2, 0x9B8A1E28, 0xD6A420E4, 0x4031E42E, 0x2DEC192D, 0xBB79DDE7, + 0xB7B064B5, 0x2125A07F, 0x4CF85D7C, 0xDA6D99B6, 0x9743A77A, 0x01D663B0, 0x6C0B9EB3, 0xFA9E5A79, + 0xA4654232, 0x32F086F8, 0x5F2D7BFB, 0xC9B8BF31, 0x849681FD, 0x12034537, 0x7FDEB834, 0xE94B7CFE, + 0xE582C5AC, 0x73170166, 0x1ECAFC65, 0x885F38AF, 0xC5710663, 0x53E4C2A9, 0x3E393FAA, 0xA8ACFB60, + 0x27AA4D0E, 0xB13F89C4, 0xDCE274C7, 0x4A77B00D, 0x07598EC1, 0x91CC4A0B, 0xFC11B708, 0x6A8473C2, + 0x664DCA90, 0xF0D80E5A, 0x9D05F359, 0x0B903793, 0x46BE095F, 0xD02BCD95, 0xBDF63096, 0x2B63F45C, + 0xEB31D82E, 0x7DA41CE4, 0x1079E1E7, 0x86EC252D, 0xCBC21BE1, 0x5D57DF2B, 0x308A2228, 0xA61FE6E2, + 0xAAD65FB0, 0x3C439B7A, 0x519E6679, 0xC70BA2B3, 0x8A259C7F, 0x1CB058B5, 0x716DA5B6, 0xE7F8617C, + 0x68FED712, 0xFE6B13D8, 0x93B6EEDB, 0x05232A11, 0x480D14DD, 0xDE98D017, 0xB3452D14, 0x25D0E9DE, + 0x2919508C, 0xBF8C9446, 0xD2516945, 0x44C4AD8F, 0x09EA9343, 0x9F7F5789, 0xF2A2AA8A, 0x64376E40, + 0x3ACC760B, 0xAC59B2C1, 0xC1844FC2, 0x57118B08, 0x1A3FB5C4, 0x8CAA710E, 0xE1778C0D, 0x77E248C7, + 0x7B2BF195, 0xEDBE355F, 0x8063C85C, 0x16F60C96, 0x5BD8325A, 0xCD4DF690, 0xA0900B93, 0x3605CF59, + 0xB9037937, 0x2F96BDFD, 0x424B40FE, 0xD4DE8434, 0x99F0BAF8, 0x0F657E32, 0x62B88331, 0xF42D47FB, + 0xF8E4FEA9, 0x6E713A63, 0x03ACC760, 0x953903AA, 0xD8173D66, 0x4E82F9AC, 0x235F04AF, 0xB5CAC065, + 0x9EA93439, 0x083CF0F3, 0x65E10DF0, 0xF374C93A, 0xBE5AF7F6, 0x28CF333C, 0x4512CE3F, 0xD3870AF5, + 0xDF4EB3A7, 0x49DB776D, 0x24068A6E, 0xB2934EA4, 0xFFBD7068, 0x6928B4A2, 0x04F549A1, 0x92608D6B, + 0x1D663B05, 0x8BF3FFCF, 0xE62E02CC, 0x70BBC606, 0x3D95F8CA, 0xAB003C00, 0xC6DDC103, 0x504805C9, + 0x5C81BC9B, 0xCA147851, 0xA7C98552, 0x315C4198, 0x7C727F54, 0xEAE7BB9E, 0x873A469D, 0x11AF8257, + 0x4F549A1C, 0xD9C15ED6, 0xB41CA3D5, 0x2289671F, 0x6FA759D3, 0xF9329D19, 0x94EF601A, 0x027AA4D0, + 0x0EB31D82, 0x9826D948, 0xF5FB244B, 0x636EE081, 0x2E40DE4D, 0xB8D51A87, 0xD508E784, 0x439D234E, + 0xCC9B9520, 0x5A0E51EA, 0x37D3ACE9, 0xA1466823, 0xEC6856EF, 0x7AFD9225, 0x17206F26, 0x81B5ABEC, + 0x8D7C12BE, 0x1BE9D674, 0x76342B77, 0xE0A1EFBD, 0xAD8FD171, 0x3B1A15BB, 0x56C7E8B8, 0xC0522C72, +]; + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn header_crc_table() { + fn crc_remainder(data: u8) -> u8 { + // NOTE: This CRC works the opposite way from normal because ASHRAE decided that bit 7 represents x^0 and bit 0 represents x^7. + // Hence, we reverse the bits of the polynomial and we shift right instead of left. + // For this particular polynomial, flipping the bits is a no-op. + // Still, leave it here to understand what is happening better. + const POLYNOMIAL: u8 = 0x81_u8.reverse_bits(); + let mut data = data; + for _ in 0..8 { + if data & 0x01 != 0 { + data = (data >> 1) ^ POLYNOMIAL; + } else { + data >>= 1; + } + } + data + } + extern crate std; + std::eprint!("const HEADER_CRC_TABLE: [u8; 256] = ["); + for i in 0..=255 { + if i % 16 == 0 { + std::eprint!("\n "); + } + std::eprint!(" 0x{:02X},", crc_remainder(i)); + } + std::eprintln!("\n];"); + + for i in 0..=255 { + assert!(HEADER_CRC_TABLE[usize::from(i)] == crc_remainder(i)); + } + } + + #[test] + fn data_crc_table() { + fn crc_remainder(data: u8) -> u16 { + // NOTE: This CRC works the opposite way from normal because ASHRAE decided that bit 7 represents x^0 and bit 0 represents x^7. + // Hence, we reverse the bits of the polynomial and we shift right instead of left. + const POLYNOMIAL: u16 = 0x1021_u16.reverse_bits(); + let mut crc = u16::from(data); + for _ in 0..8 { + if crc & 0x0001 != 0 { + crc = (crc >> 1) ^ POLYNOMIAL; + } else { + crc >>= 1; + } + } + crc + } + + extern crate std; + std::eprint!("const DATA_CRC_TABLE: [u16; 256] = ["); + for i in 0..=255 { + if i % 16 == 0 { + std::eprint!("\n "); + } + std::eprint!(" 0x{:04X},", crc_remainder(i)); + } + std::eprintln!("\n];"); + + for i in 0..=255 { + assert!(DATA_CRC_TABLE[usize::from(i)] == crc_remainder(i)); + } + } + + #[test] + fn crc32k_table() { + const fn crc_remainder(data: u8) -> u32 { + const POLYNOMIAL: u32 = 0xEB31D82E; + let mut data = data as u32; + let mut i = 0; + while i < 8 { + i += 1; + if data & 0x01 != 0 { + data = (data >> 1) ^ POLYNOMIAL; + } else { + data >>= 1; + } + } + data + } + + extern crate std; + std::eprint!("const CRC32K_TABLE: [u32; 256] = ["); + for i in 0..=255 { + if i % 8 == 0 { + std::eprint!("\n "); + } + std::eprint!(" 0x{:08X},", crc_remainder(i)); + } + std::eprintln!("\n];"); + + for i in 0..=255 { + assert!(CRC32K_TABLE[usize::from(i)] == crc_remainder(i)); + } + } + + #[test] + fn test_header_crc() { + // Example from the specification. + assert_eq!(header_crc(&[0x00, 0x10, 0x05, 0x00, 0x00]), 0x73); + assert_eq!(header_crc(&[0x00, 0x10, 0x05, 0x00, 0x00, 0x8C]), 0x55); + } + + #[test] + fn test_data_crc() { + // Example from the specification. + assert_eq!(data_crc(&[0x01, 0x22, 0x30]), 0x42EF); + assert_eq!(data_crc(&[0x01, 0x22, 0x30, 0x10, 0xBD]), 0xF0B8); + } + + #[test] + fn test_crc32k() { + // Example from the specification. + assert_eq!(crc32k(&[0x01, 0x22, 0x30]), 0x83DD5A41); + assert_eq!( + crc32k(&[0x01, 0x22, 0x30, 0xBE, 0xA5, 0x22, 0x7C]), + 0x0843323B + ); + } + + #[test] + fn test_mstp_who_has_decode() { + use crate::application_protocol::unconfirmed::UnconfirmedServiceChoice::WhoHas; + use crate::common::error::Unimplemented::UnconfirmedServiceChoice; + + // Example from the specification. + #[rustfmt::skip] + let mut data = [ + 0x55, 0xFF, 0x21, 0xFF, 0x01, 0x02, 0x00, 0x4E, 0x50, 0x54, 0x75, 0xAA, 0xAA, 0x5D, 0xAA, 0x45, + 0x52, 0x68, 0xAB, 0x54, 0xBA, 0xAA, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, + 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, + 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x16, 0x16, 0x16, 0x16, + 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x11, + 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, + 0x11, 0x11, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, + 0x10, 0x10, 0x10, 0x10, 0x10, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, + 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, + 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x12, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, + 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1D, 0x1C, 0x1C, + 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, + 0x1C, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, + 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, + 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, + 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x1B, 0x1B, 0x1B, + 0x1B, 0x1B, 0x1B, 0x1B, 0xA4, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, 0x1B, + 0x1B, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, 0x1A, + 0x1A, 0x1A, 0x1A, 0x1A, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, + 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, + 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, + 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x06, 0x06, 0x06, + 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, + 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, + 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x0D, 0x0D, 0x0D, 0x0D, + 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0C, + 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, + 0x0C, 0x0C, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, + 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x50, 0xF9, 0xA1, 0xD6, 0xE8, + ]; + + let mut reader = crate::common::io::Reader::new_with_len(data.len()); + + // Sadly,this is a WhoHas service, which is not supported, so for now we test that we get the correct `Unimplemented` error. + let result = MstpFrame::decode(&mut reader, &mut data); + assert!(matches!( + result, + Err(Error::Unimplemented(UnconfirmedServiceChoice(WhoHas))) + )); + } +} diff --git a/src/network_protocol/network_pdu.rs b/src/network_protocol/network_pdu.rs index 140c9fa..bc3272f 100644 --- a/src/network_protocol/network_pdu.rs +++ b/src/network_protocol/network_pdu.rs @@ -43,7 +43,7 @@ impl From for MessagePriority { } } -#[derive(Debug, Clone)] +#[derive(Debug, Copy, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(u8)] enum ControlFlags { @@ -207,7 +207,10 @@ impl<'a> NetworkPdu<'a> { #[cfg_attr(feature = "alloc", bacnet_macros::remove_lifetimes_from_fn_args)] pub fn decode(reader: &mut Reader, buf: &'a [u8]) -> Result { // ignore version - let _version = reader.read_byte(buf)?; + let version = reader.read_byte(buf)?; + if version != Self::VERSION { + return Err(Error::InvalidProtocolVersion(version)); + } // read and decode control byte let control = reader.read_byte(buf)?; @@ -261,25 +264,29 @@ impl<'a> NetworkPdu<'a> { } } -#[derive(Debug, Clone)] +/// Opaque device address. +/// +/// The format of the address depends on the network for which the address is valid. +#[derive(Debug, Copy, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub struct Addr { - pub ipv4: [u8; 4], - pub port: u16, +pub struct DeviceAddress { + len: u8, + data: [u8; Self::MAX_LEN], } -const IPV4_ADDR_LEN: u8 = 6; +#[deprecated(note = "use DeviceAddress")] +pub type Addr = DeviceAddress; pub type SourceAddress = NetworkAddress; -#[derive(Debug, Clone)] +#[derive(Debug, Copy, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct NetworkAddress { pub net: u16, - pub addr: Option, + pub addr: DeviceAddress, } -#[derive(Debug, Clone)] +#[derive(Debug, Copy, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct DestinationAddress { pub network_address: NetworkAddress, @@ -287,7 +294,8 @@ pub struct DestinationAddress { } impl DestinationAddress { - pub fn new(net: u16, addr: Option) -> Self { + pub fn new(net: u16, addr: Option) -> Self { + let addr = addr.unwrap_or(DeviceAddress::BROADCAST); Self { network_address: NetworkAddress { net, addr }, hop_count: 255, @@ -298,34 +306,195 @@ impl DestinationAddress { impl NetworkAddress { pub fn encode(&self, writer: &mut Writer) { writer.extend_from_slice(&self.net.to_be_bytes()); - match self.addr.as_ref() { - Some(addr) => { - writer.push(IPV4_ADDR_LEN); - writer.extend_from_slice(&addr.ipv4); - writer.extend_from_slice(&addr.port.to_be_bytes()); - } - None => writer.push(0), - } + self.addr.encode(writer); } pub fn decode(reader: &mut Reader, buf: &[u8]) -> Result { let net = u16::from_be_bytes(reader.read_bytes(buf)?); + let addr = DeviceAddress::decode(reader, buf)?; + Ok(Self { net, addr }) + } +} + +impl DeviceAddress { + /// The maximum length of an address. + const MAX_LEN: usize = 18; // IPv6 address + port number + + /// The broadcast address. + /// + /// Used to broadcast a message to all devices on a remote network. + pub const BROADCAST: Self = Self { + len: 0, + data: [0; Self::MAX_LEN], + }; + + /// Make a new address from a byte slice. + pub fn new(value: &[u8]) -> Result { + if value.len() > Self::MAX_LEN { + Err(Error::Length(( + "network address too long: maximum length is 18 bytes", + value.len() as u32, + ))) + } else { + let mut data = [0; Self::MAX_LEN]; + data[..value.len()].copy_from_slice(value); + Ok(Self { + len: value.len() as u8, + data, + }) + } + } + + /// Make an address from a byte array. + /// + /// Fails to compile if N > Self::MAX_LEN. + pub const fn from_array(input: [u8; N]) -> Self { + const { + assert!(N <= Self::MAX_LEN); + } + let mut data = [0u8; Self::MAX_LEN]; + let mut i = 0; + while i < N { + data[i] = input[i]; + i += 1; + } + Self { len: N as u8, data } + } + + /// Make a new BACnet/IP IPv4 address (4 byte IP address and 2 byte port number). + pub const fn new_ipv4(addr: core::net::SocketAddrV4) -> Self { + let ip = addr.ip().octets(); + let port = addr.port().to_be_bytes(); + + Self::from_array([ip[0], ip[1], ip[2], ip[3], port[0], port[1]]) + } + + /// Make a new BACnet/IP IPv6 address (16 byte IP address and 2 byte port number). + pub const fn new_ipv6(addr: core::net::SocketAddrV6) -> Self { + let ip = addr.ip().octets(); + let port = addr.port().to_be_bytes(); + + Self::from_array([ + ip[0], ip[1], ip[2], ip[3], ip[4], ip[5], ip[6], ip[7], ip[8], ip[9], ip[10], ip[11], + ip[12], ip[13], ip[14], ip[15], port[0], port[1], + ]) + } + + /// Make a new MS/TP address (a single byte). + pub const fn new_mstp(addr: u8) -> Self { + Self::from_array([addr]) + } + + pub fn len(&self) -> u8 { + self.len + } + + pub fn is_empty(&self) -> bool { + self.len == 0 + } + + pub fn is_broadcast(&self) -> bool { + self.is_empty() + } + + pub fn data(&self) -> &[u8] { + let len = self.len as usize; + &self.data[..len] + } + + pub fn encode(&self, writer: &mut Writer) { + writer.push(self.len); + writer.extend_from_slice(self.data()); + } + + pub fn decode(reader: &mut Reader, buf: &[u8]) -> Result { let len = reader.read_byte(buf)?; - match len { - IPV4_ADDR_LEN => { - let ipv4: [u8; 4] = reader.read_bytes(buf)?; - let port = u16::from_be_bytes(reader.read_bytes(buf)?); - - Ok(Self { - net, - addr: Some(Addr { ipv4, port }), - }) + let data = reader.read_slice(len.into(), buf)?; + Self::new(data) + } +} + +impl TryFrom<&[u8]> for DeviceAddress { + type Error = Error; + + fn try_from(value: &[u8]) -> Result { + Self::new(value) + } +} + +impl From for DeviceAddress { + fn from(value: core::net::SocketAddrV4) -> Self { + Self::new_ipv4(value) + } +} + +impl From for DeviceAddress { + fn from(value: core::net::SocketAddrV6) -> Self { + Self::new_ipv6(value) + } +} + +impl From for DeviceAddress { + fn from(value: core::net::SocketAddr) -> Self { + match value { + core::net::SocketAddr::V4(x) => x.into(), + core::net::SocketAddr::V6(x) => x.into(), + } + } +} + +impl TryFrom for core::net::SocketAddrV4 { + type Error = Error; + + fn try_from(value: DeviceAddress) -> Result { + match value.data() { + &[a, b, c, d, port_high, port_low] => { + let ip = core::net::Ipv4Addr::from_octets([a, b, c, d]); + let port = u16::from_be_bytes([port_high, port_low]); + Ok(Self::new(ip, port)) } - 0 => Ok(Self { net, addr: None }), - x => Err(Error::Length(( - "NetworkAddress decode ip len can only be 6 or 0", - x as u32, + _ => Err(Error::Length(( + "BACnet/IPv4 addresses must be 6 byes long", + value.len().into(), ))), } } } + +impl TryFrom for core::net::SocketAddrV6 { + type Error = Error; + + fn try_from(value: DeviceAddress) -> Result { + match value.data() { + &[b0, b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11, b12, b13, b14, b15, port_high, port_low] => + { + let ip = core::net::Ipv6Addr::from_octets([ + b0, b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11, b12, b13, b14, b15, + ]); + let port = u16::from_be_bytes([port_high, port_low]); + Ok(Self::new(ip, port, 0, 0)) + } + _ => Err(Error::Length(( + "BACnet/IPv6 addresses must be 18 byes long", + value.len().into(), + ))), + } + } +} + +impl TryFrom for core::net::SocketAddr { + type Error = Error; + + fn try_from(value: DeviceAddress) -> Result { + if let Ok(v4) = value.try_into() { + Ok(Self::V4(v4)) + } else if let Ok(v6) = value.try_into() { + Ok(Self::V6(v6)) + } else { + Err(Error::Length(( + "BACnet/IP addresses must be 6 or 18 byes long", + value.len().into(), + ))) + } + } +} diff --git a/src/simple/mod.rs b/src/simple/mod.rs index ce00ecc..f833e7c 100644 --- a/src/simple/mod.rs +++ b/src/simple/mod.rs @@ -37,7 +37,7 @@ use crate::{ io::{Reader, Writer}, }, network_protocol::{ - data_link::{DataLink, DataLinkFunction}, + ip::{BvllFunction, IpFrame}, network_pdu::{DestinationAddress, MessagePriority, NetworkMessage, NetworkPdu}, }, }; @@ -116,7 +116,7 @@ where let dst = Some(DestinationAddress::new(0xffff, None)); let message = NetworkMessage::Apdu(apdu); let npdu = NetworkPdu::new(None, dst, false, MessagePriority::Normal, message); - let data_link = DataLink::new(DataLinkFunction::OriginalBroadcastNpdu, Some(npdu)); + let data_link = IpFrame::new(BvllFunction::OriginalBroadcastNpdu, Some(npdu)); let mut writer = Writer::new(buf); data_link.encode(&mut writer); @@ -130,9 +130,9 @@ where let n = self.io.read(buf).await.map_err(BacnetError::Io)?; let buf = &buf[..n]; - // use the DataLink codec to decode the bytes + // decode the bytes as BACnet/IP frame let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, buf).map_err(BacnetError::Codec)?; + let message = IpFrame::decode(&mut reader, buf).map_err(BacnetError::Codec)?; if let Some(npdu) = message.npdu { if let NetworkMessage::Apdu(ApplicationPdu::UnconfirmedRequest( @@ -199,7 +199,7 @@ where ) -> Result>, BacnetError> { let n = self.io.read(buf).await.map_err(BacnetError::Io)?; let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, &buf[..n])?; + let message = IpFrame::decode(&mut reader, &buf[..n])?; if let Some(npdu) = message.npdu { if let NetworkMessage::Apdu(ApplicationPdu::UnconfirmedRequest( @@ -231,7 +231,7 @@ where } #[maybe_async()] - pub async fn write_property<'a>( + pub async fn write_property( &self, buf: &mut [u8], request: WriteProperty<'_>, @@ -265,9 +265,9 @@ where let n = self.io.read(buf).await.map_err(BacnetError::Io)?; let buf = &buf[..n]; - // use the DataLink codec to decode the bytes + // decode the bytes as BACnet/IP frame let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, buf).map_err(BacnetError::Codec)?; + let message = IpFrame::decode(&mut reader, buf).map_err(BacnetError::Codec)?; match message.npdu { Some(x) => match x.network_message { @@ -304,9 +304,9 @@ where let n = self.io.read(buf).await.map_err(BacnetError::Io)?; let buf = &buf[..n]; - // use the DataLink codec to decode the bytes + // decode the bytes as BACnet/IP frame let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, buf).map_err(BacnetError::Codec)?; + let message = IpFrame::decode(&mut reader, buf).map_err(BacnetError::Codec)?; match message.npdu { Some(x) => match x.network_message { @@ -331,7 +331,7 @@ where */ #[maybe_async()] - async fn send_and_receive_simple_ack<'a>( + async fn send_and_receive_simple_ack( &self, buf: &mut [u8], service: ConfirmedRequestService<'_>, @@ -342,9 +342,9 @@ where let n = self.io.read(buf).await.map_err(BacnetError::Io)?; let buf = &buf[..n]; - // use the DataLink codec to decode the bytes + // decode the bytes as BACnet/IP frame let mut reader = Reader::default(); - let message = DataLink::decode(&mut reader, buf).map_err(BacnetError::Codec)?; + let message = IpFrame::decode(&mut reader, buf).map_err(BacnetError::Codec)?; // TODO: return bacnet error if the server returns one // return message is expected to be a ComplexAck @@ -365,7 +365,7 @@ where let apdu = ApplicationPdu::UnconfirmedRequest(service); let message = NetworkMessage::Apdu(apdu); let npdu = NetworkPdu::new(None, None, true, MessagePriority::Normal, message); - let data_link = DataLink::new(DataLinkFunction::OriginalUnicastNpdu, Some(npdu)); + let data_link = IpFrame::new(BvllFunction::OriginalUnicastNpdu, Some(npdu)); let mut writer = Writer::new(buf); data_link.encode(&mut writer); @@ -386,7 +386,7 @@ where let apdu = ApplicationPdu::ConfirmedRequest(ConfirmedRequest::new(invoke_id, service)); let message = NetworkMessage::Apdu(apdu); let npdu = NetworkPdu::new(None, None, true, MessagePriority::Normal, message); - let data_link = DataLink::new(DataLinkFunction::OriginalUnicastNpdu, Some(npdu)); + let data_link = IpFrame::new(BvllFunction::OriginalUnicastNpdu, Some(npdu)); let mut writer = Writer::new(buf); data_link.encode(&mut writer);