Integrate hand gateway and spatial force rendering

src/serial_core/codecs/hand_gateway.rs

@@ -0,0 +1,404 @@
+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]
+        );
+    }
+}