diff options
Diffstat (limited to 'src/protocol')
| -rw-r--r-- | src/protocol/frame.rs | 259 | ||||
| -rw-r--r-- | src/protocol/jpeg.rs | 351 | ||||
| -rw-r--r-- | src/protocol/mod.rs | 409 | ||||
| -rw-r--r-- | src/protocol/parser.rs | 418 |
4 files changed, 1437 insertions, 0 deletions
diff --git a/src/protocol/frame.rs b/src/protocol/frame.rs new file mode 100644 index 0000000..32475e2 --- /dev/null +++ b/src/protocol/frame.rs @@ -0,0 +1,259 @@ +//! UPP protocol frame structures + +use serde::{Deserialize, Serialize}; +use std::mem; + +/// UPP USB frame header (5 bytes) +#[derive(Debug, Clone, Copy, Serialize, Deserialize)] +#[repr(C, packed)] +pub struct UPPUsbFrame { + pub magic: u16, // 0xBBAA + pub cid: u8, // Camera ID + pub length: u16, // Data length (excluding header) +} + +/// UPP camera frame header (7 bytes) +#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)] +#[repr(C, packed)] +pub struct UPPFrameHeader { + pub frame_id: u8, // Frame ID + pub camera_number: u8, // Camera number + pub flags: UPPFlags, // Various flags + pub g_sensor: u32, // G-sensor data +} + +/// UPP frame flags +#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)] +#[repr(C, packed)] +pub struct UPPFlags { + pub has_g: bool, // Has G-sensor data (bit 0) + pub button_press: bool, // Button press detected (bit 1) + pub other: u8, // Other flags (bits 2-7, 6 bits total) +} + +/// Complete UPP frame +#[derive(Debug, Clone, Serialize, Deserialize, Default)] +pub struct UPPFrame { + pub header: UPPFrameHeader, + pub data: Vec<u8>, +} + +impl UPPUsbFrame { + /// Create a new UPP USB frame + pub fn new(cid: u8, length: u16) -> Self { + Self { + magic: super::UPP_USB_MAGIC, + cid, + length, + } + } + + /// Get the total frame size including header + /// Based on C++ POC: length field includes camera header + data, but not USB header + pub fn total_size(&self) -> usize { + mem::size_of::<Self>() + self.length as usize + } + + /// Validate the frame magic number + pub fn is_valid(&self) -> bool { + self.magic == super::UPP_USB_MAGIC + } + + /// Get the expected payload size (camera header + data) + /// Based on C++ POC: this is what the length field represents + pub fn expected_payload_size(&self) -> usize { + self.length as usize + } + + /// Get the expected data size (excluding camera header) + /// Camera header is 7 bytes, so data size is payload - 7 + pub fn expected_data_size(&self) -> usize { + if self.length >= 7 { + self.length as usize - 7 + } else { + 0 + } + } +} + +impl UPPFrameHeader { + /// Create a new UPP frame header + pub fn new( + frame_id: u8, + camera_number: u8, + has_g: bool, + button_press: bool, + g_sensor: u32, + ) -> Self { + Self { + frame_id, + camera_number, + flags: UPPFlags { + has_g, + button_press, + other: 0, + }, + g_sensor, + } + } + + /// Check if this frame has G-sensor data + pub fn has_g_sensor(&self) -> bool { + self.flags.has_g + } + + /// Check if button press was detected + pub fn button_pressed(&self) -> bool { + self.flags.button_press + } + + /// Get other flags + pub fn other_flags(&self) -> u8 { + self.flags.other + } + + /// Set other flags + pub fn set_other_flags(&mut self, flags: u8) { + self.flags.other = flags & 0x3F; // Only 6 bits + } + + /// Get G-sensor data + pub fn g_sensor_data(&self) -> Option<u32> { + if self.has_g_sensor() { + Some(self.g_sensor) + } else { + None + } + } +} + +impl UPPFrame { + /// Create a new UPP frame + pub fn new(header: UPPFrameHeader, data: Vec<u8>) -> Self { + Self { header, data } + } + + /// Get the total frame size + pub fn total_size(&self) -> usize { + mem::size_of::<UPPFrameHeader>() + self.data.len() + } + + /// Get the frame ID + pub fn frame_id(&self) -> u8 { + self.header.frame_id + } + + /// Get the camera number + pub fn camera_number(&self) -> u8 { + self.header.camera_number + } + + /// Check if button was pressed + pub fn button_pressed(&self) -> bool { + self.header.button_pressed() + } + + /// Get G-sensor data if available + pub fn g_sensor_data(&self) -> Option<u32> { + if self.header.has_g_sensor() { + Some(self.header.g_sensor) + } else { + None + } + } +} + +impl Default for UPPUsbFrame { + fn default() -> Self { + Self { + magic: super::UPP_USB_MAGIC, + cid: super::UPP_CAMID_7, + length: 0, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_upp_usb_frame_creation() { + let frame = UPPUsbFrame::new(7, 1024); + let magic = frame.magic; + let cid = frame.cid; + let length = frame.length; + assert_eq!(magic, super::super::UPP_USB_MAGIC); + assert_eq!(cid, 7); + assert_eq!(length, 1024); + assert!(frame.is_valid()); + } + + #[test] + fn test_upp_usb_frame_validation() { + let mut frame = UPPUsbFrame::new(7, 1024); + assert!(frame.is_valid()); + + frame.magic = 0x1234; + assert!(!frame.is_valid()); + } + + #[test] + fn test_upp_frame_header_creation() { + let header = UPPFrameHeader::new(1, 0, true, false, 12345); + assert_eq!(header.frame_id, 1); + assert_eq!(header.camera_number, 0); + assert!(header.has_g_sensor()); + assert!(!header.button_pressed()); + assert_eq!(header.g_sensor_data(), Some(12345)); + } + + #[test] + fn test_upp_frame_header_flags() { + let mut header = UPPFrameHeader::default(); + assert!(!header.has_g_sensor()); + assert!(!header.button_pressed()); + + header.flags.has_g = true; + header.flags.button_press = true; + assert!(header.has_g_sensor()); + assert!(header.button_pressed()); + } + + #[test] + fn test_upp_frame_creation() { + let header = UPPFrameHeader::new(1, 0, false, false, 0); + let data = vec![1, 2, 3, 4, 5]; + let frame = UPPFrame::new(header, data.clone()); + + assert_eq!(frame.frame_id(), 1); + assert_eq!(frame.camera_number(), 0); + assert_eq!(frame.data, data); + assert_eq!(frame.total_size(), mem::size_of::<UPPFrameHeader>() + 5); + } + + #[test] + fn test_upp_frame_defaults() { + let frame = UPPFrame::default(); + assert_eq!(frame.frame_id(), 0); + assert_eq!(frame.camera_number(), 0); + assert!(frame.data.is_empty()); + assert!(!frame.button_pressed()); + assert!(frame.g_sensor_data().is_none()); + } + + #[test] + fn test_upp_flags_other_bits() { + let mut header = UPPFrameHeader::default(); + header.set_other_flags(0xFF); + assert_eq!(header.other_flags(), 0x3F); // Only 6 bits should be set + } + + #[test] + fn test_memory_layout() { + // Ensure packed structs have correct sizes + assert_eq!(mem::size_of::<UPPUsbFrame>(), 5); + // UPPFrameHeader: frame_id(1) + camera_number(1) + flags(3) + g_sensor(4) = 9 bytes + assert_eq!(mem::size_of::<UPPFrameHeader>(), 9); + } +} diff --git a/src/protocol/jpeg.rs b/src/protocol/jpeg.rs new file mode 100644 index 0000000..8398800 --- /dev/null +++ b/src/protocol/jpeg.rs @@ -0,0 +1,351 @@ +//! JPEG parsing utilities for the UPP protocol + +use crate::error::{JpegError, Result}; +use tracing::{debug, trace, warn}; + +/// JPEG marker constants +const JPEG_SOI: u8 = 0xD8; // Start of Image +const JPEG_EOI: u8 = 0xD9; // End of Image +const JPEG_SOS: u8 = 0xDA; // Start of Scan +const JPEG_SOF0: u8 = 0xC0; // Start of Frame (Baseline DCT) +const JPEG_SOF1: u8 = 0xC1; // Start of Frame (Extended sequential DCT) +const JPEG_SOF2: u8 = 0xC2; // Start of Frame (Progressive DCT) + +/// JPEG image dimensions +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct JpegDimensions { + pub width: u16, + pub height: u16, +} + +impl JpegDimensions { + /// Create new dimensions + pub fn new(width: u16, height: u16) -> Self { + Self { width, height } + } + + /// Check if dimensions are valid + pub fn is_valid(&self) -> bool { + self.width > 0 && self.height > 0 + } + + /// Get aspect ratio + pub fn aspect_ratio(&self) -> f64 { + if self.height > 0 { + self.width as f64 / self.height as f64 + } else { + 0.0 + } + } +} + +/// JPEG parser for extracting metadata +pub struct JpegParser { + enable_debug: bool, +} + +impl JpegParser { + /// Create a new JPEG parser + pub fn new() -> Self { + Self { + enable_debug: false, + } + } + + /// Create a new JPEG parser with debug enabled + pub fn new_with_debug(enable_debug: bool) -> Self { + Self { enable_debug } + } + + /// Parse JPEG dimensions from raw data + pub fn parse_dimensions(&self, data: &[u8]) -> Result<JpegDimensions> { + if data.len() < 4 { + return Err(JpegError::InvalidHeader.into()); + } + + // Check JPEG SOI marker + if data[0] != 0xFF || data[1] != JPEG_SOI { + return Err(JpegError::InvalidHeader.into()); + } + + trace!("Parsing JPEG dimensions from {} bytes", data.len()); + + let mut i = 2; + while i + 3 < data.len() { + // Look for marker + if data[i] != 0xFF { + i += 1; + continue; + } + + let marker = data[i + 1]; + i += 2; + + // Check for end markers + if marker == JPEG_EOI || marker == JPEG_SOS { + break; + } + + // Check if we have enough data for segment length + if i + 1 >= data.len() { + break; + } + + // Read segment length (big-endian) + let segment_length = ((data[i] as u16) << 8) | (data[i + 1] as u16); + if segment_length < 2 || i + segment_length as usize > data.len() { + warn!("Invalid segment length: {}", segment_length); + break; + } + + // Check for SOF markers (Start of Frame) + if self.is_sof_marker(marker) { + if segment_length < 7 { + return Err(JpegError::InvalidHeader.into()); + } + + // Height and width are in big-endian format + let height = ((data[i + 3] as u16) << 8) | (data[i + 4] as u16); + let width = ((data[i + 5] as u16) << 8) | (data[i + 6] as u16); + + if self.enable_debug { + debug!( + "Found SOF marker 0x{:02X}, dimensions: {}x{}", + marker, width, height + ); + } + + if width > 0 && height > 0 { + return Ok(JpegDimensions::new(width, height)); + } else { + return Err(JpegError::DimensionsNotFound.into()); + } + } + + // Move to next segment + i += segment_length as usize; + } + + Err(JpegError::DimensionsNotFound.into()) + } + + /// Check if a marker is a Start of Frame marker + fn is_sof_marker(&self, marker: u8) -> bool { + matches!(marker, JPEG_SOF0 | JPEG_SOF1 | JPEG_SOF2) + } + + /// Extract JPEG metadata + pub fn parse_metadata(&self, data: &[u8]) -> Result<JpegMetadata> { + let dimensions = self.parse_dimensions(data)?; + + Ok(JpegMetadata { + dimensions, + file_size: data.len(), + is_valid: true, + }) + } + + /// Validate JPEG data + pub fn validate_jpeg(&self, data: &[u8]) -> Result<bool> { + if data.len() < 4 { + return Ok(false); + } + + // Check SOI marker + if data[0] != 0xFF || data[1] != JPEG_SOI { + return Ok(false); + } + + // Check EOI marker (should be near the end) + if data.len() >= 2 { + let end = data.len() - 2; + if data[end] == 0xFF && data[end + 1] == JPEG_EOI { + return Ok(true); + } + } + + // If we can't find EOI, check if we can parse dimensions + Ok(self.parse_dimensions(data).is_ok()) + } + + /// Check if data represents a complete JPEG frame + pub fn is_complete_jpeg(&self, data: &[u8]) -> bool { + if data.len() < 4 { + return false; + } + + // Must start with SOI marker + if data[0] != 0xFF || data[1] != JPEG_SOI { + return false; + } + + // Must end with EOI marker + if data.len() >= 2 { + let end = data.len() - 2; + if data[end] == 0xFF && data[end + 1] == JPEG_EOI { + return true; + } + } + + false + } +} + +impl Default for JpegParser { + fn default() -> Self { + Self::new() + } +} + +/// JPEG metadata +#[derive(Debug, Clone)] +pub struct JpegMetadata { + pub dimensions: JpegDimensions, + pub file_size: usize, + pub is_valid: bool, +} + +impl JpegMetadata { + /// Create new metadata + pub fn new(dimensions: JpegDimensions, file_size: usize) -> Self { + Self { + dimensions, + file_size, + is_valid: true, + } + } + + /// Get estimated bit depth (assume 8-bit for most JPEGs) + pub fn estimated_bit_depth(&self) -> u8 { + 8 + } + + /// Get estimated color space (assume YUV for most JPEGs) + pub fn estimated_color_space(&self) -> &'static str { + "YUV" + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::error::Error; + + // Sample JPEG data for testing (minimal valid JPEG) + const MINIMAL_JPEG: &[u8] = &[ + 0xFF, 0xD8, // SOI + 0xFF, 0xE0, // APP0 + 0x00, 0x10, // Length + 0x4A, 0x46, 0x49, 0x46, 0x00, // "JFIF\0" + 0x01, 0x01, // Version + 0x00, // Units + 0x00, 0x01, // Density + 0x00, 0x01, 0x00, 0x00, // No thumbnail + 0xFF, 0xC0, // SOF0 + 0x00, 0x0B, // Length + 0x08, // Precision + 0x00, 0x40, // Height (64) + 0x00, 0x40, // Width (64) + 0x03, // Components + 0x01, 0x11, 0x00, // Y component + 0x02, 0x11, 0x01, // U component + 0x03, 0x11, 0x01, // V component + 0xFF, 0xD9, // EOI + ]; + + #[test] + fn test_jpeg_dimensions_creation() { + let dims = JpegDimensions::new(640, 480); + assert_eq!(dims.width, 640); + assert_eq!(dims.height, 480); + assert!(dims.is_valid()); + assert_eq!(dims.aspect_ratio(), 640.0 / 480.0); + } + + #[test] + fn test_jpeg_dimensions_validation() { + let dims = JpegDimensions::new(0, 480); + assert!(!dims.is_valid()); + + let dims = JpegDimensions::new(640, 0); + assert!(!dims.is_valid()); + + let dims = JpegDimensions::new(0, 0); + assert!(!dims.is_valid()); + } + + #[test] + fn test_jpeg_parser_creation() { + let parser = JpegParser::new(); + assert!(!parser.enable_debug); + + let parser = JpegParser::new_with_debug(true); + assert!(parser.enable_debug); + } + + #[test] + fn test_jpeg_parser_parse_dimensions() { + let parser = JpegParser::new(); + let dimensions = parser.parse_dimensions(MINIMAL_JPEG).unwrap(); + assert_eq!(dimensions.width, 64); + assert_eq!(dimensions.height, 64); + } + + #[test] + fn test_jpeg_parser_invalid_header() { + let parser = JpegParser::new(); + let invalid_data = &[0x00, 0x01, 0x02, 0x03]; + + let result = parser.parse_dimensions(invalid_data); + assert!(result.is_err()); + assert!(matches!( + result.unwrap_err(), + Error::Jpeg(JpegError::InvalidHeader) + )); + } + + #[test] + fn test_jpeg_parser_short_data() { + let parser = JpegParser::new(); + let short_data = &[0xFF, 0xD8]; + + let result = parser.parse_dimensions(short_data); + assert!(result.is_err()); + assert!(matches!( + result.unwrap_err(), + Error::Jpeg(JpegError::InvalidHeader) + )); + } + + #[test] + fn test_jpeg_parser_validate_jpeg() { + let parser = JpegParser::new(); + assert!(parser.validate_jpeg(MINIMAL_JPEG).unwrap()); + + let invalid_data = &[0x00, 0x01, 0x02, 0x03]; + assert!(!parser.validate_jpeg(invalid_data).unwrap()); + } + + #[test] + fn test_jpeg_metadata_creation() { + let dimensions = JpegDimensions::new(640, 480); + let metadata = JpegMetadata::new(dimensions, 1024); + + assert_eq!(metadata.dimensions.width, 640); + assert_eq!(metadata.dimensions.height, 480); + assert_eq!(metadata.file_size, 1024); + assert!(metadata.is_valid); + assert_eq!(metadata.estimated_bit_depth(), 8); + assert_eq!(metadata.estimated_color_space(), "YUV"); + } + + #[test] + fn test_sof_marker_detection() { + let parser = JpegParser::new(); + assert!(parser.is_sof_marker(JPEG_SOF0)); + assert!(parser.is_sof_marker(JPEG_SOF1)); + assert!(parser.is_sof_marker(JPEG_SOF2)); + assert!(!parser.is_sof_marker(0x00)); + assert!(!parser.is_sof_marker(JPEG_SOI)); + } +} diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs new file mode 100644 index 0000000..cf0426e --- /dev/null +++ b/src/protocol/mod.rs @@ -0,0 +1,409 @@ +//! UPP protocol implementation for the Geek szitman supercamera + +mod frame; +mod jpeg; +mod parser; + +pub use frame::{UPPFrame, UPPFrameHeader, UPPUsbFrame}; +pub use jpeg::JpegParser; +pub use parser::UPPParser; + +use crate::error::Result; +use serde::{Deserialize, Serialize}; +use std::sync::Arc; +use std::sync::Mutex; +use tracing::trace; + +/// UPP protocol constants +pub const UPP_USB_MAGIC: u16 = 0xBBAA; +pub const UPP_CAMID_7: u8 = 7; + +/// UPP camera instance +pub struct UPPCamera { + parser: Arc<UPPParser>, + jpeg_parser: Arc<JpegParser>, + frame_buffer: Arc<Mutex<Vec<u8>>>, + current_frame_id: Arc<Mutex<Option<u8>>>, + frame_callbacks: Arc<Mutex<Vec<Box<dyn FrameCallback + Send + Sync>>>>, + button_callbacks: Arc<Mutex<Vec<Box<dyn ButtonCallback + Send + Sync>>>>, + // Buffer for assembling frames across USB reads + input_buffer: Arc<Mutex<Vec<u8>>>, +} + +/// Frame callback trait +pub trait FrameCallback { + /// Called when a complete frame is received + fn on_frame(&self, frame: &UPPFrame); +} + +/// Button callback trait +pub trait ButtonCallback { + /// Called when a button press is detected + fn on_button_press(&self); +} + +impl UPPCamera { + /// Create a new UPP camera instance + pub fn new() -> Self { + Self { + parser: Arc::new(UPPParser::new()), + jpeg_parser: Arc::new(JpegParser::new()), + frame_buffer: Arc::new(Mutex::new(Vec::new())), + current_frame_id: Arc::new(Mutex::new(None)), + frame_callbacks: Arc::new(Mutex::new(Vec::new())), + button_callbacks: Arc::new(Mutex::new(Vec::new())), + input_buffer: Arc::new(Mutex::new(Vec::new())), + } + } + + /// Create a new UPP camera instance with debug enabled + pub fn new_with_debug(enable_debug: bool) -> Self { + Self { + parser: Arc::new(UPPParser::new_with_debug(enable_debug)), + jpeg_parser: Arc::new(JpegParser::new_with_debug(enable_debug)), + frame_buffer: Arc::new(Mutex::new(Vec::new())), + current_frame_id: Arc::new(Mutex::new(None)), + frame_callbacks: Arc::new(Mutex::new(Vec::new())), + button_callbacks: Arc::new(Mutex::new(Vec::new())), + input_buffer: Arc::new(Mutex::new(Vec::new())), + } + } + + /// Enable or disable debug mode + pub fn set_debug_mode(&self, enable: bool) { + // Create a new parser with the desired debug setting + let _new_parser = Arc::new(UPPParser::new_with_debug(enable)); + // Replace the parser (this requires interior mutability or a different approach) + // For now, we'll just log the change + tracing::info!("Debug mode {} for UPP parser", if enable { "enabled" } else { "disabled" }); + } +} + +impl Default for UPPCamera { + fn default() -> Self { + Self::new() + } +} + +impl UPPCamera { + /// Handle incoming UPP frame data + pub fn handle_frame(&self, data: &[u8]) -> Result<()> { + // Reduce logging frequency - only log every 100th frame + static mut FRAME_COUNT: u32 = 0; + unsafe { + FRAME_COUNT += 1; + if FRAME_COUNT % 100 == 0 { + trace!("Handling frame data: {} bytes", data.len()); + } + } + + // Backward-compatible: feed bytes and process all parsed frames + for frame in self.feed_bytes(data)? { + self.process_frame(frame)?; + } + + Ok(()) + } + + /// Handle incoming UPP frame data with better error handling + pub fn handle_frame_robust(&self, data: &[u8]) -> Result<()> { + trace!("Handling frame data robustly: {} bytes", data.len()); + + // Streaming-friendly: feed bytes and process all parsed frames + let frames = self.feed_bytes(data)?; + for frame in frames { + self.process_frame(frame)?; + } + Ok(()) + } + + /// Feed raw bytes from USB and extract as many complete protocol frames as possible. + /// Based on C++ POC: frames are chunked across multiple USB reads and need assembly. + pub fn feed_bytes(&self, chunk: &[u8]) -> Result<Vec<UPPFrame>> { + let mut out_frames = Vec::new(); + let mut buffer = self.input_buffer.lock().unwrap(); + buffer.extend_from_slice(chunk); + + // Parse loop: find and assemble frames from buffer + loop { + // Need at least 5 bytes for USB header + if buffer.len() < 5 { + break; + } + + // Search for magic 0xBBAA (little-endian) + let mut start_index = None; + for i in 0..=buffer.len() - 2 { + let magic = u16::from_le_bytes([buffer[i], buffer[i + 1]]); + if magic == UPP_USB_MAGIC { + start_index = Some(i); + break; + } + } + + let Some(start) = start_index else { + // No magic found; drop buffer to avoid infinite growth + buffer.clear(); + break; + }; + + // Drop any leading garbage before magic + if start > 0 { + buffer.drain(0..start); + } + + // Re-check size for header + if buffer.len() < 5 { + break; + } + + // Parse USB header fields directly from bytes + let _magic = u16::from_le_bytes([buffer[0], buffer[1]]); + let _cid = buffer[2]; + let payload_length = u16::from_le_bytes([buffer[3], buffer[4]]) as usize; + let total_frame_size = 5 + payload_length; // USB header (5) + payload + + if buffer.len() < total_frame_size { + // Wait for more data next call + break; + } + + // We have a complete frame; extract it + let frame_bytes: Vec<u8> = buffer.drain(0..total_frame_size).collect(); + + // Parse the complete frame + match self.parser.parse_frame(&frame_bytes) { + Ok(frame) => { + out_frames.push(frame); + } + Err(e) => { + tracing::warn!("Failed to parse complete frame: {}", e); + // Continue processing other frames + } + } + } + + Ok(out_frames) + } + + /// Process a parsed UPP frame with frame assembly logic + /// Based on C++ POC: accumulate frame data until frame ID changes + fn process_frame(&self, frame: UPPFrame) -> Result<()> { + let mut frame_buffer = self.frame_buffer.lock().unwrap(); + let mut current_frame_id = self.current_frame_id.lock().unwrap(); + + // Check if this is a new frame (frame ID changed) + if let Some(id) = *current_frame_id { + if id != frame.header.frame_id { + // New frame started, process the previous one + if !frame_buffer.is_empty() { + self.notify_frame_complete(&frame_buffer); + frame_buffer.clear(); + } + } + } + + // Update current frame ID + *current_frame_id = Some(frame.header.frame_id); + + // Add frame data to buffer (accumulate chunks) + frame_buffer.extend_from_slice(&frame.data); + + // Check for button press + if frame.header.flags.button_press { + self.notify_button_press(); + } + + Ok(()) + } + + /// Notify frame callbacks of complete frame + fn notify_frame_complete(&self, frame_data: &[u8]) { + let frame_callbacks = self.frame_callbacks.lock().unwrap(); + + for callback in frame_callbacks.iter() { + callback.on_frame(&UPPFrame { + header: UPPFrameHeader::new(0, 0, false, false, 0), + data: frame_data.to_vec(), + }); + } + } + + /// Notify button callbacks of button press + fn notify_button_press(&self) { + let button_callbacks = self.button_callbacks.lock().unwrap(); + + for callback in button_callbacks.iter() { + callback.on_button_press(); + } + } + + /// Add a frame callback + pub fn add_frame_callback<F>(&self, callback: F) + where + F: FrameCallback + Send + Sync + 'static, + { + let mut frame_callbacks = self.frame_callbacks.lock().unwrap(); + frame_callbacks.push(Box::new(callback)); + } + + /// Add a button callback + pub fn add_button_callback<F>(&self, callback: F) + where + F: ButtonCallback + Send + Sync + 'static, + { + let mut button_callbacks = self.button_callbacks.lock().unwrap(); + button_callbacks.push(Box::new(callback)); + } + + /// Get the complete frame if available + pub fn get_complete_frame(&self) -> Option<Vec<u8>> { + let frame_buffer = self.frame_buffer.lock().unwrap(); + if frame_buffer.is_empty() { + None + } else { + // Check if the frame is a complete JPEG before returning it + if self.jpeg_parser.is_complete_jpeg(&frame_buffer) { + Some(frame_buffer.clone()) + } else { + None + } + } + } + + /// Get the current frame buffer size (for debugging frame assembly) + pub fn get_frame_buffer_size(&self) -> usize { + let frame_buffer = self.frame_buffer.lock().unwrap(); + frame_buffer.len() + } + + /// Get the current input buffer size (for debugging chunk accumulation) + pub fn get_input_buffer_size(&self) -> usize { + let input_buffer = self.input_buffer.lock().unwrap(); + input_buffer.len() + } + + /// Clear the frame buffer + pub fn clear_frame_buffer(&self) { + let mut frame_buffer = self.frame_buffer.lock().unwrap(); + frame_buffer.clear(); + } + + /// Get frame statistics + pub fn get_stats(&self) -> UPPStats { + let frame_buffer = self.frame_buffer.lock().unwrap(); + let current_frame_id = self.current_frame_id.lock().unwrap(); + + UPPStats { + buffer_size: frame_buffer.len(), + current_frame_id: *current_frame_id, + total_frames_processed: 0, // TODO: implement counter + } + } +} + +/// UPP protocol statistics +#[derive(Debug, Clone, Serialize, Deserialize, Default)] +pub struct UPPStats { + pub buffer_size: usize, + pub current_frame_id: Option<u8>, + pub total_frames_processed: u64, +} + +/// UPP camera configuration +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct UPPConfig { + pub expected_camera_id: u8, + pub max_frame_size: usize, + pub enable_debug: bool, +} + +impl Default for UPPConfig { + fn default() -> Self { + Self { + expected_camera_id: UPP_CAMID_7, + max_frame_size: 0x10000, // 64KB + enable_debug: false, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::sync::Arc; + + // Mock frame callback for testing + struct MockFrameCallback { + frame_count: Arc<std::sync::Mutex<u32>>, + } + + impl FrameCallback for MockFrameCallback { + fn on_frame(&self, _frame: &UPPFrame) { + let mut count = self.frame_count.lock().unwrap(); + *count += 1; + } + } + + // Mock button callback for testing + struct MockButtonCallback { + press_count: Arc<std::sync::Mutex<u32>>, + } + + impl ButtonCallback for MockButtonCallback { + fn on_button_press(&self) { + let mut count = self.press_count.lock().unwrap(); + *count += 1; + } + } + + #[test] + fn test_upp_constants() { + assert_eq!(UPP_USB_MAGIC, 0xBBAA); + assert_eq!(UPP_CAMID_7, 7); + } + + #[test] + fn test_upp_config_default() { + let config = UPPConfig::default(); + assert_eq!(config.expected_camera_id, UPP_CAMID_7); + assert_eq!(config.max_frame_size, 0x10000); + assert!(!config.enable_debug); + } + + #[test] + fn test_upp_stats_default() { + let stats = UPPStats::default(); + assert_eq!(stats.buffer_size, 0); + assert!(stats.current_frame_id.is_none()); + assert_eq!(stats.total_frames_processed, 0); + } + + #[test] + fn test_upp_camera_creation() { + let camera = UPPCamera::new(); + let stats = camera.get_stats(); + assert_eq!(stats.buffer_size, 0); + assert!(stats.current_frame_id.is_none()); + } + + #[test] + fn test_upp_camera_callbacks() { + let camera = UPPCamera::new(); + + let frame_callback = MockFrameCallback { + frame_count: Arc::new(std::sync::Mutex::new(0u32)), + }; + + let button_callback = MockButtonCallback { + press_count: Arc::new(std::sync::Mutex::new(0u32)), + }; + + camera.add_frame_callback(frame_callback); + camera.add_button_callback(button_callback); + + // Verify callbacks were added + let stats = camera.get_stats(); + assert_eq!(stats.buffer_size, 0); + } +} 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); + } +} |