Initial pipewiremain

1 files changed, 418 insertions, 0 deletions

diff --git a/src/protocol/parser.rs b/src/protocol/parser.rs
new file mode 100644
index 0000000..7db824e
--- /dev/null
+++ b/src/protocol/parser.rs
@@ -0,0 +1,418 @@
+//! UPP protocol parser for decoding USB frames
+
+use super::frame::{UPPFlags, UPPFrame, UPPFrameHeader, UPPUsbFrame};
+use crate::error::{ProtocolError, Result};
+use std::mem;
+use tracing::{debug, trace, warn};
+
+/// UPP protocol parser
+pub struct UPPParser {
+    enable_debug: bool,
+}
+
+impl UPPParser {
+    /// Create a new UPP parser
+    pub fn new() -> Self {
+        Self {
+            enable_debug: false,
+        }
+    }
+
+    /// Create a new UPP parser with debug enabled
+    pub fn new_with_debug(enable_debug: bool) -> Self {
+        Self { enable_debug }
+    }
+
+    /// Parse a raw USB frame into a UPP frame
+    pub fn parse_frame(&self, data: &[u8]) -> Result<UPPFrame> {
+        trace!("Parsing UPP frame: {} bytes", data.len());
+
+        // Parse USB frame header
+        let usb_frame = self.parse_usb_header(data)?;
+
+        // Validate frame length - based on C++ POC analysis
+        let expected_total_size = usb_frame.total_size();
+        let actual_size = data.len();
+        
+        if self.enable_debug {
+            trace!(
+                "Frame size validation: expected={}, actual={}, difference={}",
+                expected_total_size, actual_size, actual_size as i32 - expected_total_size as i32
+            );
+        }
+        
+        // Based on C++ POC: the length field represents total payload size
+        // If we have less data than expected, this is a partial frame (chunked)
+        if actual_size < expected_total_size {
+            return Err(ProtocolError::FrameTooSmall {
+                expected: expected_total_size,
+                actual: actual_size,
+            }
+            .into());
+        }
+
+        // Parse camera frame header
+        let camera_header = self.parse_camera_header(data, &usb_frame)?;
+
+        // Extract frame data - use the expected data size
+        let data_start = mem::size_of::<UPPUsbFrame>() + 7; // Manual header size: 7 bytes
+        let frame_data = if data_start < data.len() {
+            let end_index = std::cmp::min(data_start + usb_frame.expected_data_size(), data.len());
+            data[data_start..end_index].to_vec()
+        } else {
+            Vec::new()
+        };
+
+        if self.enable_debug {
+            trace!(
+                "Parsed UPP frame: ID={}, Camera={}, Data={} bytes, Total={} bytes, Expected={} bytes",
+                camera_header.frame_id,
+                camera_header.camera_number,
+                frame_data.len(),
+                data.len(),
+                expected_total_size
+            );
+        }
+
+        Ok(UPPFrame {
+            header: camera_header,
+            data: frame_data,
+        })
+    }
+
+    /// Parse a raw USB frame with better error handling and diagnostics
+    pub fn parse_frame_robust(&self, data: &[u8]) -> Result<UPPFrame> {
+        trace!("Parsing UPP frame robustly: {} bytes", data.len());
+
+        // First, try to find a valid frame start
+        let frame_start = self.find_frame_start(data)?;
+        let frame_data = &data[frame_start..];
+        
+        if self.enable_debug {
+            debug!("Found frame start at offset {}, processing {} bytes", frame_start, frame_data.len());
+        }
+
+        // Parse the frame from the found start position
+        self.parse_frame(frame_data)
+    }
+
+    /// Find the start of a valid UPP frame in potentially misaligned data
+    fn find_frame_start(&self, data: &[u8]) -> Result<usize> {
+        if data.len() < 12 { // Minimum frame size
+            return Err(ProtocolError::FrameTooSmall {
+                expected: 12,
+                actual: data.len(),
+            }
+            .into());
+        }
+
+        // Look for the magic number (0xBBAA) in the data
+        for i in 0..=data.len().saturating_sub(12) {
+            if data.len() >= i + 2 {
+                let magic_bytes = [data[i], data[i + 1]];
+                let magic = u16::from_le_bytes(magic_bytes);
+                
+                if magic == super::UPP_USB_MAGIC {
+                    if self.enable_debug {
+                        debug!("Found magic number 0x{:04X} at offset {}", magic, i);
+                    }
+                    return Ok(i);
+                }
+            }
+        }
+
+        Err(ProtocolError::InvalidMagic {
+            expected: super::UPP_USB_MAGIC,
+            actual: 0, // Unknown
+        }
+        .into())
+    }
+
+    /// Parse USB frame header
+    pub(crate) fn parse_usb_header(&self, data: &[u8]) -> Result<UPPUsbFrame> {
+        if data.len() < mem::size_of::<UPPUsbFrame>() {
+            return Err(ProtocolError::FrameTooSmall {
+                expected: mem::size_of::<UPPUsbFrame>(),
+                actual: data.len(),
+            }
+            .into());
+        }
+
+        // Safe to transmute since we've checked the size
+        let usb_frame = unsafe { *(data.as_ptr() as *const UPPUsbFrame) };
+
+        // Validate magic number
+        if !usb_frame.is_valid() {
+            return Err(ProtocolError::InvalidMagic {
+                expected: super::UPP_USB_MAGIC,
+                actual: usb_frame.magic,
+            }
+            .into());
+        }
+
+        // Validate camera ID
+        if usb_frame.cid != super::UPP_CAMID_7 {
+            return Err(ProtocolError::UnknownCameraId(usb_frame.cid).into());
+        }
+
+        if self.enable_debug {
+            let magic = usb_frame.magic;
+            let cid = usb_frame.cid;
+            let length = usb_frame.length;
+            trace!(
+                "USB frame: magic=0x{:04X}, cid={}, length={}",
+                magic, cid, length
+            );
+        }
+
+        Ok(usb_frame)
+    }
+
+    /// Parse camera frame header
+    pub(crate) fn parse_camera_header(&self, data: &[u8], _usb_frame: &UPPUsbFrame) -> Result<UPPFrameHeader> {
+        let header_offset = mem::size_of::<UPPUsbFrame>();
+        // Manual header size: frame_id(1) + camera_number(1) + flags(1) + g_sensor(4) = 7 bytes
+        let header_end = header_offset + 7;
+
+        if data.len() < header_end {
+            return Err(ProtocolError::FrameTooSmall {
+                expected: header_end,
+                actual: data.len(),
+            }
+            .into());
+        }
+
+        // Manually parse the header to avoid packed struct issues
+        let frame_id = data[header_offset];
+        let camera_number = data[header_offset + 1];
+        
+        // Read flags byte and extract individual bits
+        let flags_byte = data[header_offset + 2];
+        let has_g = (flags_byte & 0x01) != 0;        // Bit 0
+        let button_press = (flags_byte & 0x02) != 0; // Bit 1
+        let other = (flags_byte >> 2) & 0x3F;        // Bits 2-7 (6 bits)
+
+        if self.enable_debug {
+            trace!(
+                "Raw flags byte: 0x{:02X}, has_g={}, button_press={}, other=0x{:02X}",
+                flags_byte, has_g, button_press, other
+            );
+        }
+
+        // Read G-sensor data (4 bytes, little-endian)
+        let g_sensor_bytes = [
+            data[header_offset + 3],
+            data[header_offset + 4],
+            data[header_offset + 5],
+            data[header_offset + 6],
+        ];
+
+        if self.enable_debug {
+            trace!(
+                "G-sensor bytes: {:02X} {:02X} {:02X} {:02X}",
+                g_sensor_bytes[0], g_sensor_bytes[1], g_sensor_bytes[2], g_sensor_bytes[3]
+            );
+        }
+
+        let g_sensor = u32::from_le_bytes(g_sensor_bytes);
+
+        let camera_header = UPPFrameHeader {
+            frame_id,
+            camera_number,
+            flags: UPPFlags {
+                has_g,
+                button_press,
+                other,
+            },
+            g_sensor,
+        };
+
+        // Validate frame header
+        self.validate_camera_header(&camera_header)?;
+
+        if self.enable_debug {
+            trace!(
+                "Camera header: frame_id={}, camera={}, has_g={}, button={}",
+                camera_header.frame_id,
+                camera_header.camera_number,
+                camera_header.flags.has_g,
+                camera_header.flags.button_press
+            );
+        }
+
+        Ok(camera_header)
+    }
+
+    /// Validate camera frame header
+    fn validate_camera_header(&self, header: &UPPFrameHeader) -> Result<()> {
+        // Validate camera number (should be 0 or 1)
+        if header.camera_number >= 2 {
+            return Err(ProtocolError::InvalidFrameFormat(format!(
+                "Invalid camera number: {}",
+                header.camera_number
+            ))
+            .into());
+        }
+
+        // Validate flags (G-sensor and other flags should be 0 for now)
+        if header.flags.has_g {
+            warn!("G-sensor data not yet supported");
+        }
+
+        if header.flags.other != 0 {
+            warn!("Unknown flags set: 0x{:02X}", header.flags.other);
+        }
+
+        Ok(())
+    }
+
+    /// Get frame statistics
+    pub fn get_stats(&self) -> UPPParserStats {
+        UPPParserStats {
+            enable_debug: self.enable_debug,
+        }
+    }
+}
+
+impl Default for UPPParser {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+/// UPP parser statistics
+#[derive(Debug, Clone)]
+pub struct UPPParserStats {
+    pub enable_debug: bool,
+}
+
+#[cfg(test)]
+mod tests {
+    use super::super::UPP_USB_MAGIC;
+    use super::*;
+    use crate::error::Error;
+
+    // Create test data for a valid UPP frame
+    fn create_test_frame(frame_id: u8, camera_number: u8, data_size: usize) -> Vec<u8> {
+        let mut frame = Vec::new();
+
+        // USB header (5 bytes)
+        frame.extend_from_slice(&UPP_USB_MAGIC.to_le_bytes());
+        frame.push(7); // Camera ID
+        frame.extend_from_slice(&(data_size as u16 + 7).to_le_bytes()); // Length (camera header + data)
+
+        // Camera header (7 bytes)
+        frame.push(frame_id);
+        frame.push(camera_number);
+        // Flags struct (1 byte - packed bit fields)
+        let flags_byte = 0u8; // has_g: false, button_press: false, other: 0
+        frame.push(flags_byte);
+        frame.extend_from_slice(&0u32.to_le_bytes()); // G-sensor data (4 bytes)
+
+        // Frame data
+        frame.extend(std::iter::repeat(0xAA).take(data_size));
+
+        frame
+    }
+
+    #[test]
+    fn test_upp_parser_creation() {
+        let parser = UPPParser::new();
+        assert!(!parser.enable_debug);
+
+        let parser = UPPParser::new_with_debug(true);
+        assert!(parser.enable_debug);
+    }
+
+    #[test]
+    fn test_upp_parser_parse_valid_frame() {
+        let parser = UPPParser::new();
+        let test_frame = create_test_frame(1, 0, 10);
+
+        let result = parser.parse_frame(&test_frame);
+        assert!(result.is_ok());
+
+        let frame = result.unwrap();
+        assert_eq!(frame.frame_id(), 1);
+        assert_eq!(frame.camera_number(), 0);
+        assert_eq!(frame.data.len(), 10);
+        assert!(!frame.button_pressed());
+        assert!(frame.g_sensor_data().is_none());
+    }
+
+    #[test]
+    fn test_upp_parser_frame_too_small() {
+        let parser = UPPParser::new();
+        let short_data = &[0xFF, 0xAA]; // Too short for any header
+
+        let result = parser.parse_frame(short_data);
+        assert!(result.is_err());
+        assert!(matches!(
+            result.unwrap_err(),
+            Error::Protocol(ProtocolError::FrameTooSmall { .. })
+        ));
+    }
+
+    #[test]
+    fn test_upp_parser_invalid_magic() {
+        let parser = UPPParser::new();
+        let mut test_frame = create_test_frame(1, 0, 5);
+        test_frame[0] = 0x12; // Corrupt magic
+
+        let result = parser.parse_frame(&test_frame);
+        assert!(result.is_err());
+        assert!(matches!(
+            result.unwrap_err(),
+            Error::Protocol(ProtocolError::InvalidMagic { .. })
+        ));
+    }
+
+    #[test]
+    fn test_upp_parser_unknown_camera_id() {
+        let parser = UPPParser::new();
+        let mut test_frame = create_test_frame(1, 0, 5);
+        test_frame[2] = 5; // Unknown camera ID
+
+        let result = parser.parse_frame(&test_frame);
+        assert!(result.is_err());
+        assert!(matches!(
+            result.unwrap_err(),
+            Error::Protocol(ProtocolError::UnknownCameraId(5))
+        ));
+    }
+
+    #[test]
+    fn test_upp_parser_button_press() {
+        let parser = UPPParser::new();
+        let mut test_frame = create_test_frame(1, 0, 5);
+        test_frame[7] = 0x02; // Set button press flag (bit 1 of flags byte at index 7)
+
+        let result = parser.parse_frame(&test_frame);
+        assert!(result.is_ok());
+
+        let frame = result.unwrap();
+        assert!(frame.button_pressed());
+    }
+
+    #[test]
+    fn test_upp_parser_g_sensor() {
+        let parser = UPPParser::new();
+        let mut test_frame = create_test_frame(1, 0, 5);
+        test_frame[7] = 0x01; // Set G-sensor flag (bit 0 of flags byte at index 7)
+        test_frame[8..12].copy_from_slice(&0x12345678u32.to_le_bytes()); // G-sensor data (indices 8-11)
+
+        let result = parser.parse_frame(&test_frame);
+        assert!(result.is_ok());
+
+        let frame = result.unwrap();
+        assert!(frame.g_sensor_data().is_some());
+        assert_eq!(frame.g_sensor_data().unwrap(), 0x12345678);
+    }
+
+    #[test]
+    fn test_upp_parser_stats() {
+        let parser = UPPParser::new();
+        let stats = parser.get_stats();
+        assert!(!stats.enable_debug);
+    }
+}