diff options
Diffstat (limited to 'src/protocol/parser.rs')
| -rw-r--r-- | src/protocol/parser.rs | 418 |
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); + } +} |