405 lines
12 KiB
Rust
405 lines
12 KiB
Rust
use crate::serial_core::codec::Codec;
|
|
use crate::serial_core::error::CodecError;
|
|
use crate::serial_core::utils::{calc_crc8_itu, elapsed_millis};
|
|
use std::time::Instant;
|
|
|
|
const FRAME_HEADER: [u8; 2] = [0x55, 0xAA];
|
|
const RESPONSE_DATA_CMD: u8 = 0x81;
|
|
const RESPONSE_DATA_LEGACY_FIXED_LENGTH: usize = 16;
|
|
const RESPONSE_DATA_LEGACY_PREFIX_LENGTH: usize = 19;
|
|
const RESPONSE_DATA_TEMP_FIXED_LENGTH: usize = 20;
|
|
const RESPONSE_DATA_TEMP_PREFIX_LENGTH: usize = 23;
|
|
const MIN_FRAME_LENGTH: usize = 7;
|
|
const DEFAULT_MAX_FRAME_LENGTH: usize = 64 * 1024;
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
struct HandGatewayResponseLayout {
|
|
fixed_length: usize,
|
|
prefix_length: usize,
|
|
timestamp_len: usize,
|
|
config_offset: usize,
|
|
valid_config_offset: usize,
|
|
block_count_offset: usize,
|
|
}
|
|
|
|
const TEMP_RESPONSE_LAYOUT: HandGatewayResponseLayout = HandGatewayResponseLayout {
|
|
fixed_length: RESPONSE_DATA_TEMP_FIXED_LENGTH,
|
|
prefix_length: RESPONSE_DATA_TEMP_PREFIX_LENGTH,
|
|
timestamp_len: 8,
|
|
config_offset: 14,
|
|
valid_config_offset: 18,
|
|
block_count_offset: 22,
|
|
};
|
|
|
|
const LEGACY_RESPONSE_LAYOUT: HandGatewayResponseLayout = HandGatewayResponseLayout {
|
|
fixed_length: RESPONSE_DATA_LEGACY_FIXED_LENGTH,
|
|
prefix_length: RESPONSE_DATA_LEGACY_PREFIX_LENGTH,
|
|
timestamp_len: 4,
|
|
config_offset: 10,
|
|
valid_config_offset: 14,
|
|
block_count_offset: 18,
|
|
};
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub struct HandGatewayNodeConfig {
|
|
pub config_mask: u32,
|
|
pub sample_count: usize,
|
|
}
|
|
|
|
impl HandGatewayNodeConfig {
|
|
pub const fn new(config_mask: u32, sample_count: usize) -> Self {
|
|
Self {
|
|
config_mask,
|
|
sample_count,
|
|
}
|
|
}
|
|
|
|
fn payload_len(&self, bytes_per_sample: usize) -> Option<usize> {
|
|
self.sample_count.checked_mul(bytes_per_sample)
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub struct HandGatewayConfig {
|
|
pub protocol_version: u8,
|
|
pub bytes_per_sample: usize,
|
|
pub max_frame_length: usize,
|
|
pub nodes: Vec<HandGatewayNodeConfig>,
|
|
}
|
|
|
|
impl HandGatewayConfig {
|
|
pub fn new(nodes: Vec<HandGatewayNodeConfig>) -> Self {
|
|
Self {
|
|
protocol_version: 0x01,
|
|
bytes_per_sample: 2,
|
|
max_frame_length: DEFAULT_MAX_FRAME_LENGTH,
|
|
nodes,
|
|
}
|
|
}
|
|
|
|
fn validate(&self) -> Result<(), CodecError> {
|
|
if self.bytes_per_sample == 0 || self.max_frame_length < MIN_FRAME_LENGTH {
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
|
|
let mut used_masks = 0u32;
|
|
for node in &self.nodes {
|
|
if node.config_mask == 0
|
|
|| node.config_mask.count_ones() != 1
|
|
|| used_masks & node.config_mask != 0
|
|
|| node.payload_len(self.bytes_per_sample).is_none()
|
|
{
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
used_masks |= node.config_mask;
|
|
}
|
|
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub struct HandGatewayFrameNode {
|
|
pub config_mask: u32,
|
|
pub valid: bool,
|
|
pub payload: Vec<u8>,
|
|
}
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub struct HandGatewayDataRepFrame {
|
|
pub timestamp: u64,
|
|
pub config: u32,
|
|
pub valid_config: u32,
|
|
pub block_count: u8,
|
|
pub raw: Vec<u8>,
|
|
pub nodes: Vec<HandGatewayFrameNode>,
|
|
pub dts_ms: u64,
|
|
}
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq)]
|
|
pub enum HandGatewayFrame {
|
|
DataRep(HandGatewayDataRepFrame),
|
|
}
|
|
|
|
pub struct HandGatewayCodec {
|
|
buffer: Vec<u8>,
|
|
config: HandGatewayConfig,
|
|
}
|
|
|
|
impl HandGatewayCodec {
|
|
pub fn new(node_sample_counts: &[u16]) -> Self {
|
|
let nodes = node_sample_counts
|
|
.iter()
|
|
.enumerate()
|
|
.map(|(index, &sample_count)| {
|
|
HandGatewayNodeConfig::new(
|
|
1u32.checked_shl(index as u32).unwrap_or(0),
|
|
sample_count as usize,
|
|
)
|
|
})
|
|
.collect();
|
|
|
|
Self {
|
|
buffer: Vec::new(),
|
|
config: HandGatewayConfig::new(nodes),
|
|
}
|
|
}
|
|
|
|
pub fn parse_node_payload(data: &[u8]) -> Result<Vec<u16>, CodecError> {
|
|
if data.len() % 2 != 0 {
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
|
|
Ok(data
|
|
.chunks_exact(2)
|
|
.map(|chunk| u16::from_le_bytes([chunk[0], chunk[1]]))
|
|
.collect())
|
|
}
|
|
|
|
fn retain_possible_header_prefix(&mut self) {
|
|
if self.buffer.last() == Some(&FRAME_HEADER[0]) {
|
|
self.buffer.drain(..self.buffer.len() - 1);
|
|
} else {
|
|
self.buffer.clear();
|
|
}
|
|
}
|
|
|
|
fn parse_data_response_with_layout(
|
|
&self,
|
|
frame: &[u8],
|
|
length: u16,
|
|
session_started_at: Instant,
|
|
layout: HandGatewayResponseLayout,
|
|
) -> Result<HandGatewayFrame, CodecError> {
|
|
let payload_len = usize::from(length)
|
|
.checked_sub(layout.fixed_length)
|
|
.ok_or(CodecError::InvalidLength)?;
|
|
let payload_end = layout
|
|
.prefix_length
|
|
.checked_add(payload_len)
|
|
.ok_or(CodecError::PayloadTooLarge)?;
|
|
|
|
if payload_end + 1 != frame.len() {
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
|
|
let version = frame[5];
|
|
if version != self.config.protocol_version {
|
|
return Err(CodecError::InvalidFrameType);
|
|
}
|
|
|
|
let timestamp = match layout.timestamp_len {
|
|
4 => u32::from_le_bytes(frame[6..10].try_into().unwrap()) as u64,
|
|
8 => u64::from_le_bytes(frame[6..14].try_into().unwrap()),
|
|
_ => return Err(CodecError::InvalidLength),
|
|
};
|
|
let config = u32::from_le_bytes(
|
|
frame[layout.config_offset..layout.config_offset + 4]
|
|
.try_into()
|
|
.unwrap(),
|
|
);
|
|
let valid_config = u32::from_le_bytes(
|
|
frame[layout.valid_config_offset..layout.valid_config_offset + 4]
|
|
.try_into()
|
|
.unwrap(),
|
|
);
|
|
let block_count = frame[layout.block_count_offset];
|
|
|
|
let active_nodes = self
|
|
.config
|
|
.nodes
|
|
.iter()
|
|
.filter(|node| config & node.config_mask != 0)
|
|
.collect::<Vec<_>>();
|
|
|
|
if active_nodes.len() != usize::from(block_count) {
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
|
|
let expected_payload_len = active_nodes.iter().try_fold(0usize, |total, node| {
|
|
let node_len = node
|
|
.payload_len(self.config.bytes_per_sample)
|
|
.ok_or(CodecError::PayloadTooLarge)?;
|
|
total
|
|
.checked_add(node_len)
|
|
.ok_or(CodecError::PayloadTooLarge)
|
|
})?;
|
|
|
|
if expected_payload_len != payload_len {
|
|
return Err(CodecError::InvalidLength);
|
|
}
|
|
|
|
let mut cursor = layout.prefix_length;
|
|
let mut nodes = Vec::with_capacity(active_nodes.len());
|
|
for node in active_nodes {
|
|
let node_len = node
|
|
.payload_len(self.config.bytes_per_sample)
|
|
.ok_or(CodecError::PayloadTooLarge)?;
|
|
let next = cursor + node_len;
|
|
nodes.push(HandGatewayFrameNode {
|
|
config_mask: node.config_mask,
|
|
valid: valid_config & node.config_mask != 0,
|
|
payload: frame[cursor..next].to_vec(),
|
|
});
|
|
cursor = next;
|
|
}
|
|
|
|
Ok(HandGatewayFrame::DataRep(HandGatewayDataRepFrame {
|
|
timestamp,
|
|
config,
|
|
valid_config,
|
|
block_count,
|
|
raw: frame.to_vec(),
|
|
nodes,
|
|
dts_ms: elapsed_millis(session_started_at),
|
|
}))
|
|
}
|
|
|
|
fn parse_data_response(
|
|
&self,
|
|
frame: &[u8],
|
|
length: u16,
|
|
session_started_at: Instant,
|
|
) -> Result<HandGatewayFrame, CodecError> {
|
|
self.parse_data_response_with_layout(
|
|
frame,
|
|
length,
|
|
session_started_at,
|
|
TEMP_RESPONSE_LAYOUT,
|
|
)
|
|
.or_else(|_| {
|
|
self.parse_data_response_with_layout(
|
|
frame,
|
|
length,
|
|
session_started_at,
|
|
LEGACY_RESPONSE_LAYOUT,
|
|
)
|
|
})
|
|
}
|
|
}
|
|
|
|
impl Codec<HandGatewayFrame> for HandGatewayCodec {
|
|
fn decode(
|
|
&mut self,
|
|
input: &[u8],
|
|
session_started_at: Instant,
|
|
) -> Result<Vec<HandGatewayFrame>, CodecError> {
|
|
self.config.validate()?;
|
|
self.buffer.extend_from_slice(input);
|
|
let mut frames = Vec::new();
|
|
|
|
loop {
|
|
let Some(header_pos) = self
|
|
.buffer
|
|
.windows(FRAME_HEADER.len())
|
|
.position(|window| window == FRAME_HEADER)
|
|
else {
|
|
self.retain_possible_header_prefix();
|
|
break;
|
|
};
|
|
|
|
if header_pos > 0 {
|
|
self.buffer.drain(..header_pos);
|
|
}
|
|
|
|
if self.buffer.len() < 4 {
|
|
break;
|
|
}
|
|
|
|
let length = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
|
|
let frame_len = usize::from(length)
|
|
.checked_add(4)
|
|
.ok_or(CodecError::PayloadTooLarge)?;
|
|
|
|
if frame_len < MIN_FRAME_LENGTH || frame_len > self.config.max_frame_length {
|
|
log::debug!("invalid hand gateway frame length: {frame_len}");
|
|
self.buffer.drain(..1);
|
|
continue;
|
|
}
|
|
|
|
if self.buffer.len() < frame_len {
|
|
break;
|
|
}
|
|
|
|
let expected_checksum = calc_crc8_itu(&self.buffer[..frame_len - 1]);
|
|
let received_checksum = self.buffer[frame_len - 1];
|
|
if expected_checksum != received_checksum {
|
|
log::debug!(
|
|
"hand gateway checksum mismatch: expected {expected_checksum:02X}, got {received_checksum:02X}"
|
|
);
|
|
self.buffer.drain(..1);
|
|
continue;
|
|
}
|
|
|
|
if self.buffer[4] == RESPONSE_DATA_CMD {
|
|
match self.parse_data_response(
|
|
&self.buffer[..frame_len],
|
|
length,
|
|
session_started_at,
|
|
) {
|
|
Ok(frame) => frames.push(frame),
|
|
Err(error) => log::debug!("invalid hand gateway data response: {error}"),
|
|
}
|
|
}
|
|
|
|
self.buffer.drain(..frame_len);
|
|
}
|
|
|
|
Ok(frames)
|
|
}
|
|
|
|
fn encode(&self, frame: &HandGatewayFrame) -> Result<Vec<u8>, CodecError> {
|
|
let _ = frame;
|
|
Err(CodecError::InvalidFrameType)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn data_response(config: u32, valid_config: u32, payload: &[u8]) -> Vec<u8> {
|
|
let block_count = config.count_ones() as u8;
|
|
let length = (RESPONSE_DATA_LEGACY_FIXED_LENGTH + payload.len()) as u16;
|
|
let mut frame = Vec::new();
|
|
frame.extend_from_slice(&FRAME_HEADER);
|
|
frame.extend_from_slice(&length.to_le_bytes());
|
|
frame.push(RESPONSE_DATA_CMD);
|
|
frame.push(0x01);
|
|
frame.extend_from_slice(&0x0102_0304u32.to_le_bytes());
|
|
frame.extend_from_slice(&config.to_le_bytes());
|
|
frame.extend_from_slice(&valid_config.to_le_bytes());
|
|
frame.push(block_count);
|
|
frame.extend_from_slice(payload);
|
|
let checksum = calc_crc8_itu(&frame);
|
|
frame.push(checksum);
|
|
frame
|
|
}
|
|
|
|
#[test]
|
|
fn decodes_data_response_frame() {
|
|
let payload = [0x10, 0x00, 0x34, 0x12, 0x08, 0x00];
|
|
let bytes = data_response(0b11, 0b01, &payload);
|
|
let frames = HandGatewayCodec::new(&[2, 1])
|
|
.decode(&bytes, Instant::now())
|
|
.unwrap();
|
|
|
|
assert_eq!(frames.len(), 1);
|
|
let HandGatewayFrame::DataRep(frame) = &frames[0];
|
|
assert_eq!(frame.timestamp, 0x0102_0304);
|
|
assert_eq!(frame.config, 0b11);
|
|
assert_eq!(frame.valid_config, 0b01);
|
|
assert_eq!(frame.block_count, 2);
|
|
assert_eq!(frame.raw, bytes);
|
|
assert_eq!(frame.nodes.len(), 2);
|
|
assert_eq!(
|
|
HandGatewayCodec::parse_node_payload(&frame.nodes[0].payload).unwrap(),
|
|
vec![16, 0x1234]
|
|
);
|
|
assert_eq!(
|
|
HandGatewayCodec::parse_node_payload(&frame.nodes[1].payload).unwrap(),
|
|
vec![8]
|
|
);
|
|
}
|
|
}
|