From 6f5a9a8a0e3896eb12379627bdce879a582d770b Mon Sep 17 00:00:00 2001 From: Thomas Kroll Date: Fri, 23 Oct 2026 23:36:26 +0200 Subject: [PATCH] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20#171:=20Compute=20the=20fr?= =?UTF-8?q?ame=20checksum=20instead=20of=20replaying=20it?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Framed commands on unit 2, selector 2 carry two CRC-16/USB checksums (reflected polynomial 0xA001, init and final XOR 0xFFFF): one over the 12-byte header and one over the data block, each with its own field zeroed. Until now every command replayed a captured checksum together with its captured sequence number, so no new command could be added without a capture. The new frame builder takes receiver, command id and data and computes both checksums. Each camera hands out its own sequence numbers, seeded from the clock, because the camera drops a frame that repeats the previous sequence number. Sleep, tracking speed, exposure mode type and preset recall now use the builder. Their tests build with the captured sequence numbers and check that the captured frames come out byte for byte, which proves the checksums. Thanks to gnacho for working out the checksum in #171. Co-Authored-By: Claude Opus 5.5 --- src/libs/camera/camera.rs | 17 +- src/libs/camera/command02.rs | 113 ---------- .../camera/commands/exposure_mode_type.rs | 87 ++------ .../camera/commands/goto_preset_position.rs | 100 +++------ src/libs/camera/commands/sleep.rs | 70 ++---- src/libs/camera/commands/tracking_speed.rs | 66 ++---- src/libs/camera/frame.rs | 205 ++++++++++++++++++ src/libs/camera/mod.rs | 4 +- 8 files changed, 315 insertions(+), 347 deletions(-) delete mode 100644 src/libs/camera/command02.rs create mode 100644 src/libs/camera/frame.rs diff --git a/src/libs/camera/camera.rs b/src/libs/camera/camera.rs index 3e774b5..a9c93d9 100644 --- a/src/libs/camera/camera.rs +++ b/src/libs/camera/camera.rs @@ -1,6 +1,7 @@ // SPDX-License-Identifier: EUPL-1.2 use crate::libs::camera::enums::{AIMode, ExposureMode, SleepMode, TrackingSpeed}; +use crate::libs::camera::frame::FrameSequence; use crate::libs::camera::status::CameraStatus; use crate::libs::camera::transport::CameraTransport; use crate::libs::errors::T4lError; @@ -13,6 +14,7 @@ use errno::Errno; pub struct Camera { transport: CameraTransport, debugging: bool, + sequence: FrameSequence, } impl Camera { @@ -20,6 +22,7 @@ impl Camera { Ok(Self { transport: CameraTransport::new(hint)?, debugging: false, + sequence: FrameSequence::new(), }) } @@ -31,6 +34,11 @@ impl Camera { self.transport.send_cmd(unit, selector, cmd, self.debugging) } + /// Returns a fresh sequence number for the next framed command (unit 2, selector 2). + pub fn next_sequence_nr(&self) -> u16 { + self.sequence.next() + } + pub fn get_status(&self) -> Result { self.transport.get_status(self.debugging) } @@ -63,7 +71,7 @@ pub trait Tiny2Camera { impl Tiny2Camera for Camera { fn set_sleep_mode(&self, mode: SleepMode) -> Result<(), T4lError> { - let cmd = SleepCommand::build(mode)?; + let cmd = SleepCommand::build(mode, self.next_sequence_nr())?; self.send_cmd(0x2, 0x2, &cmd) } @@ -83,7 +91,7 @@ impl Tiny2Camera for Camera { } fn goto_preset_position(&self, preset_nr: i8) -> Result<(), T4lError> { - let cmd = GotoPresetPositionCommand::build(preset_nr)?; + let cmd = GotoPresetPositionCommand::build(preset_nr, self.next_sequence_nr())?; self.send_cmd(0x2, 0x2, &cmd) } @@ -93,7 +101,7 @@ impl Tiny2Camera for Camera { } fn set_tracking_speed(&self, speed: TrackingSpeed) -> Result<(), T4lError> { - let cmd = TrackingSpeedCommand::build(speed)?; + let cmd = TrackingSpeedCommand::build(speed, self.next_sequence_nr())?; self.get_status()?.speed = speed; @@ -107,7 +115,8 @@ impl Tiny2Camera for Camera { } fn set_exposure_mode(&self, mode: ExposureMode) -> Result<(), T4lError> { - let exposure_mode_type_command = ExposureModeTypeCommand::build(mode); + let exposure_mode_type_command = + ExposureModeTypeCommand::build(mode, self.next_sequence_nr()); self.send_cmd(0x2, 0x2, &exposure_mode_type_command)?; diff --git a/src/libs/camera/command02.rs b/src/libs/camera/command02.rs deleted file mode 100644 index 0b7589e..0000000 --- a/src/libs/camera/command02.rs +++ /dev/null @@ -1,113 +0,0 @@ -// SPDX-License-Identifier: EUPL-1.2 - -use bon::builder; - -#[builder(finish_fn = build)] -pub fn command02( - function_group: [u8; 6], - sequence_nr: [u8; 2], - checksum: [u8; 2], - command: [u8; 6], - appendix: Option<[u8; 16]>, -) -> [u8; 36] { - const FRAME_ID: [u8; 2] = [0xaa, 0x25]; - const SEGMENT_SIZE: [u8; 2] = [0x0c, 0x00]; - - [ - FRAME_ID.as_slice(), - sequence_nr.as_slice(), - SEGMENT_SIZE.as_slice(), - checksum.as_slice(), - function_group.as_slice(), - command.as_slice(), - appendix.unwrap_or([0x00; 16]).as_slice(), - ] - .concat() - .try_into() - .unwrap() -} - -#[cfg(test)] -mod tests { - mod unit { - use crate::libs::camera::command02::command02; - - #[test] - fn full_builder() { - let command = command02() - .function_group([0x01, 0x02, 0x03, 0x04, 0x05, 0x06]) - .sequence_nr([0x10, 0x11]) - .checksum([0x20, 0x21]) - .command([0x30, 0x31, 0x32, 0x33, 0x34, 0x35]) - .appendix([ - 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, - 0x4d, 0x4e, 0x4f, - ]) - .build(); - - assert_eq!( - command[0..2], - [0xaa, 0x25], - "Command should start with frame id" - ); - assert_eq!(command[2..4], [0x10, 0x11], "Sequence number should be set"); - assert_eq!(command[4..6], [0x0c, 0x00], "Segment size should be fix"); - assert_eq!(command[6..8], [0x20, 0x21], "Checksum should be set"); - assert_eq!( - command[8..14], - [0x01, 0x02, 0x03, 0x04, 0x05, 0x06], - "Function group should be set" - ); - assert_eq!( - command[14..20], - [0x30, 0x31, 0x32, 0x33, 0x34, 0x35], - "Command should be set" - ); - assert_eq!( - command[20..36], - [ - 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, - 0x4d, 0x4e, 0x4f - ], - "Appendix should be set" - ); - } - - #[test] - fn minimal_builder() { - let command = command02() - .function_group([0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5]) - .sequence_nr([0xb0, 0xb1]) - .checksum([0xc0, 0xc1]) - .command([0xd0, 0xd1, 0xd2, 0xd3, 0xd4, 0xd5]) - .build(); - - assert_eq!( - command[0..2], - [0xaa, 0x25], - "Command should start with frame id" - ); - assert_eq!(command[2..4], [0xb0, 0xb1], "Sequence number should be set"); - assert_eq!(command[4..6], [0x0c, 0x00], "Segment size should be fix"); - assert_eq!(command[6..8], [0xc0, 0xc1], "Checksum should be set"); - assert_eq!( - command[8..14], - [0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5], - "Function group should be set" - ); - assert_eq!( - command[14..20], - [0xd0, 0xd1, 0xd2, 0xd3, 0xd4, 0xd5], - "Command should be set" - ); - assert_eq!( - command[20..36], - [ - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00 - ], - "Appendix should be empty" - ); - } - } -} diff --git a/src/libs/camera/commands/exposure_mode_type.rs b/src/libs/camera/commands/exposure_mode_type.rs index 4347da0..034775f 100644 --- a/src/libs/camera/commands/exposure_mode_type.rs +++ b/src/libs/camera/commands/exposure_mode_type.rs @@ -1,32 +1,25 @@ // SPDX-License-Identifier: EUPL-1.2 -use crate::{ExposureMode, command02}; +use crate::{ExposureMode, frame}; pub struct ExposureModeTypeCommand; impl ExposureModeTypeCommand { - pub fn build(mode: ExposureMode) -> [u8; 36] { - const FUNCTION_GROUP_EXPOSURE_MODE_TYPE: [u8; 6] = [0x0a, 0x02, 0x82, 0x29, 0x05, 0x00]; + const RECEIVER: u8 = 0x02; + const COMMAND: u16 = 0x2982; - let (sequence_nr, checksum, command) = if mode == ExposureMode::Manual { - ( - [0x16, 0x00], - [0x58, 0x91], - [0xb2, 0xaf, 0x02, 0x04, 0x00, 0x00], - ) + pub fn build(mode: ExposureMode, sequence_nr: u16) -> [u8; 60] { + let data: [u8; 5] = if mode == ExposureMode::Manual { + [0x02, 0x04, 0x00, 0x00, 0x00] } else { - ( - [0x15, 0x00], - [0xa8, 0x9e], - [0xf9, 0x27, 0x01, 0x32, 0x00, 0x00], - ) + [0x01, 0x32, 0x00, 0x00, 0x00] }; - command02() - .function_group(FUNCTION_GROUP_EXPOSURE_MODE_TYPE) + frame() + .receiver(Self::RECEIVER) + .command(Self::COMMAND) + .data(&data) .sequence_nr(sequence_nr) - .checksum(checksum) - .command(command) .build() } } @@ -36,56 +29,16 @@ mod tests { use crate::{ExposureMode, ExposureModeTypeCommand}; use test_case::test_case; - #[test] - fn manual_exposure_mode() { - let exposure_command = ExposureModeTypeCommand::build(ExposureMode::Manual); + // Captured frames, reproduced byte for byte when given the captured sequence number. + #[test_case(ExposureMode::Manual, 0x0016, "aa2516000c0058910a0282290500b2af0204000000"; "manual exposure")] + #[test_case(ExposureMode::Global, 0x0015, "aa2515000c00a89e0a0282290500f9270132000000"; "global automatic exposure")] + #[test_case(ExposureMode::Face, 0x0015, "aa2515000c00a89e0a0282290500f9270132000000"; "face automatic exposure")] + fn exposure_mode_type(mode: ExposureMode, sequence_nr: u16, captured: &str) { + let captured = hex::decode(captured).unwrap(); - assert_eq!( - exposure_command[8..14], - [0x0a, 0x02, 0x82, 0x29, 0x05, 0x00], - "Function group should be set for exposure mode type" - ); - assert_eq!( - exposure_command[2..4], - [0x16, 0x00], - "Sequence number should be set" - ); - assert_eq!( - exposure_command[6..8], - [0x58, 0x91], - "Checksum should be set" - ); - assert_eq!( - exposure_command[14..20], - [0xb2, 0xaf, 0x02, 0x04, 0x00, 0x00], - "Command should be set" - ); - } - - #[test_case(ExposureMode::Global; "global automatic exposure should be set")] - #[test_case(ExposureMode::Face; "face automatic exposure should be set")] - fn automatic_exposure_modes(mode: ExposureMode) { - let exposure_command = ExposureModeTypeCommand::build(mode); + let command = ExposureModeTypeCommand::build(mode, sequence_nr); - assert_eq!( - exposure_command[8..14], - [0x0a, 0x02, 0x82, 0x29, 0x05, 0x00], - "Function group should be set for exposure mode type" - ); - assert_eq!( - exposure_command[2..4], - [0x15, 0x00], - "Sequence number should be set" - ); - assert_eq!( - exposure_command[6..8], - [0xa8, 0x9e], - "Checksum should be set" - ); - assert_eq!( - exposure_command[14..20], - [0xf9, 0x27, 0x01, 0x32, 0x00, 0x00], - "Command should be set" - ); + assert_eq!(command[..captured.len()], captured[..]); + assert!(command[captured.len()..].iter().all(|b| *b == 0)); } } diff --git a/src/libs/camera/commands/goto_preset_position.rs b/src/libs/camera/commands/goto_preset_position.rs index 4293b39..c9fbc24 100644 --- a/src/libs/camera/commands/goto_preset_position.rs +++ b/src/libs/camera/commands/goto_preset_position.rs @@ -1,48 +1,32 @@ // SPDX-License-Identifier: EUPL-1.2 -use crate::command02; +use crate::frame; use crate::libs::errors::T4lError; pub struct GotoPresetPositionCommand; impl GotoPresetPositionCommand { - pub fn build(preset_nr: i8) -> Result<[u8; 36], T4lError> { - const FUNCTION_GROUP_PRESETS: [u8; 6] = [0x0a, 0x04, 0xc4, 0x39, 0x14, 0x00]; - - let (sequence_nr, checksum, command) = match preset_nr { - 0 => ( - [0x20, 0x00], - [0x6b, 0xdc], - [0xd6, 0xfb, 0x00, 0x00, 0x00, 0x00], - ), - 1 => ( - [0x1a, 0x00], - [0x4b, 0x03], - [0xeb, 0x2a, 0x01, 0x00, 0x00, 0x00], - ), - 2 => ( - [0x26, 0x00], - [0x8b, 0xc3], - [0xaf, 0x19, 0x02, 0x00, 0x00, 0x00], - ), - _ => { - println!("Invalid preset nr {}", preset_nr + 1); - return Err(T4lError::InvalidSetting); - } - }; - - Ok(command02() - .function_group(FUNCTION_GROUP_PRESETS) + const RECEIVER: u8 = 0x04; + const COMMAND: u16 = 0x39c4; + + pub fn build(preset_nr: i8, sequence_nr: u16) -> Result<[u8; 60], T4lError> { + if !(0..=2).contains(&preset_nr) { + println!("Invalid preset nr {}", preset_nr as i16 + 1); + return Err(T4lError::InvalidSetting); + } + + // preset index as u32, followed by four f32 1.0 values as sent by the vendor software + let mut data = [0u8; 20]; + data[0..4].copy_from_slice(&(preset_nr as u32).to_le_bytes()); + for i in 1..5 { + data[i * 4..(i + 1) * 4].copy_from_slice(&1.0f32.to_le_bytes()); + } + + Ok(frame() + .receiver(Self::RECEIVER) + .command(Self::COMMAND) + .data(&data) .sequence_nr(sequence_nr) - .checksum(checksum) - .command(command) - .appendix({ - let mut arr = [0u8; 16]; - for i in 0..4 { - arr[i * 4..(i + 1) * 4].copy_from_slice(&[0x00, 0x00, 0x80, 0x3f]); - } - arr - }) .build()) } } @@ -50,38 +34,20 @@ impl GotoPresetPositionCommand { #[cfg(test)] mod tests { use crate::GotoPresetPositionCommand; - use assertables::{assert_err, assert_ok}; + use assertables::assert_err; use test_case::test_case; - #[test_case(0, [0x20, 0x00], [0x6b, 0xdc], [0xd6, 0xfb, 0x00, 0x00, 0x00, 0x00]; "preset position 1 (id 0)")] - #[test_case(1, [0x1a, 0x00], [0x4b, 0x03], [0xeb, 0x2a, 0x01, 0x00, 0x00, 0x00]; "preset position 2 (id 1)")] - #[test_case(2, [0x26, 0x00], [0x8b, 0xc3], [0xaf, 0x19, 0x02, 0x00, 0x00, 0x00]; "preset position 3 (id 2)")] - fn valid_preset_position( - position: i8, - sequence_nr: [u8; 2], - checksum: [u8; 2], - command: [u8; 6], - ) { - let position_command_option = GotoPresetPositionCommand::build(position); + // Captured frames, reproduced byte for byte when given the captured sequence number. + #[test_case(0, 0x0020, "aa2520000c006bdc0a04c4391400d6fb000000000000803f0000803f0000803f0000803f"; "preset position 1 (id 0)")] + #[test_case(1, 0x001a, "aa251a000c004b030a04c4391400eb2a010000000000803f0000803f0000803f0000803f"; "preset position 2 (id 1)")] + #[test_case(2, 0x0026, "aa2526000c008bc30a04c4391400af19020000000000803f0000803f0000803f0000803f"; "preset position 3 (id 2)")] + fn valid_preset_position(position: i8, sequence_nr: u16, captured: &str) { + let captured = hex::decode(captured).unwrap(); - assert_ok!( - &position_command_option, - "preset position command should be built" - ); + let command = GotoPresetPositionCommand::build(position, sequence_nr).unwrap(); - let position_command = position_command_option.unwrap(); - assert_eq!( - position_command[8..14], - [0x0a, 0x04, 0xc4, 0x39, 0x14, 0x00], - "Function group should be set for preset position" - ); - assert_eq!( - position_command[2..4], - sequence_nr, - "Sequence number should be set" - ); - assert_eq!(position_command[6..8], checksum, "Checksum should be set"); - assert_eq!(position_command[14..20], command, "Command should be set"); + assert_eq!(command[..captured.len()], captured[..]); + assert!(command[captured.len()..].iter().all(|b| *b == 0)); } #[test_case(-1; "invalid preset position (lower than 0)")] @@ -89,10 +55,8 @@ mod tests { #[test_case(i8::MIN; "invalid preset position (minimal number)")] #[test_case(i8::MAX - 1; "invalid preset position (maximal number - 1)")] fn invalid_preset_position(invalid_position: i8) { - let position_command_option = GotoPresetPositionCommand::build(invalid_position); - assert_err!( - position_command_option, + GotoPresetPositionCommand::build(invalid_position, 0), "invalid position should return an error" ); } diff --git a/src/libs/camera/commands/sleep.rs b/src/libs/camera/commands/sleep.rs index 28bb873..3789c4f 100644 --- a/src/libs/camera/commands/sleep.rs +++ b/src/libs/camera/commands/sleep.rs @@ -1,37 +1,26 @@ // SPDX-License-Identifier: EUPL-1.2 use crate::libs::errors::T4lError; -use crate::{SleepMode, command02}; +use crate::{SleepMode, frame}; pub struct SleepCommand; impl SleepCommand { - pub fn build(mode: SleepMode) -> Result<[u8; 36], T4lError> { - if mode == SleepMode::Unknown { - return Err(T4lError::InvalidSetting); - } - - const FUNCTION_GROUP_SLEEP: [u8; 6] = [0x0a, 0x02, 0xc2, 0xa0, 0x04, 0x00]; - - let (sequence_nr, checksum, command) = match mode { - SleepMode::Awake => ( - [0xa5, 0x00], - [0x5f, 0xef], - [0xbe, 0x07, 0x00, 0x00, 0x00, 0x00], - ), - SleepMode::Sleep => ( - [0x42, 0x00], - [0xea, 0x63], - [0xbf, 0xfb, 0x01, 0x00, 0x00, 0x00], - ), - SleepMode::Unknown => panic!(), + const RECEIVER: u8 = 0x02; + const COMMAND: u16 = 0xa0c2; + + pub fn build(mode: SleepMode, sequence_nr: u16) -> Result<[u8; 60], T4lError> { + let value: u32 = match mode { + SleepMode::Awake => 0, + SleepMode::Sleep => 1, + SleepMode::Unknown => return Err(T4lError::InvalidSetting), }; - Ok(command02() - .function_group(FUNCTION_GROUP_SLEEP) + Ok(frame() + .receiver(Self::RECEIVER) + .command(Self::COMMAND) + .data(&value.to_le_bytes()) .sequence_nr(sequence_nr) - .checksum(checksum) - .command(command) .build()) } } @@ -39,35 +28,24 @@ impl SleepCommand { #[cfg(test)] mod tests { use crate::{SleepCommand, SleepMode}; - use assertables::assert_ok; + use assertables::assert_err; use test_case::test_case; - #[test_case(SleepMode::Awake, [0xa5, 0x00], [0x5f, 0xef], [0xbe, 0x07, 0x00, 0x00, 0x00, 0x00]; "awake command")] - #[test_case(SleepMode::Sleep, [0x42, 0x00], [0xea, 0x63], [0xbf, 0xfb, 0x01, 0x00, 0x00, 0x00]; "sleep command")] - fn sleep_mode(mode: SleepMode, sequence_nr: [u8; 2], checksum: [u8; 2], command: [u8; 6]) { - let sleep_command_option = SleepCommand::build(mode); + // Frames captured from the vendor software. Building with the captured sequence number + // has to reproduce them byte for byte, which proves both checksums are computed correctly. + #[test_case(SleepMode::Awake, 0x00a5, "aa25a5000c005fef0a02c2a00400be0700000000"; "awake command")] + #[test_case(SleepMode::Sleep, 0x0042, "aa2542000c00ea630a02c2a00400bffb01000000"; "sleep command")] + fn sleep_mode(mode: SleepMode, sequence_nr: u16, captured: &str) { + let captured = hex::decode(captured).unwrap(); - assert_ok!(&sleep_command_option, "sleep mode command should be built"); + let command = SleepCommand::build(mode, sequence_nr).unwrap(); - let sleep_command = sleep_command_option.unwrap(); - assert_eq!( - sleep_command[8..14], - [0x0a, 0x02, 0xc2, 0xa0, 0x04, 0x00], - "Function group should be set for sleep mode" - ); - assert_eq!( - sleep_command[2..4], - sequence_nr, - "Sequence number should be set" - ); - assert_eq!(sleep_command[6..8], checksum, "Checksum should be set"); - assert_eq!(sleep_command[14..20], command, "Command should be set"); + assert_eq!(command[..captured.len()], captured[..]); + assert!(command[captured.len()..].iter().all(|b| *b == 0)); } #[test] - #[should_panic] fn unknown_sleep_mode() { - SleepCommand::build(SleepMode::Unknown) - .expect("Should panic because of unknown sleep mode"); + assert_err!(SleepCommand::build(SleepMode::Unknown, 0)); } } diff --git a/src/libs/camera/commands/tracking_speed.rs b/src/libs/camera/commands/tracking_speed.rs index a794b2c..03ec6f9 100644 --- a/src/libs/camera/commands/tracking_speed.rs +++ b/src/libs/camera/commands/tracking_speed.rs @@ -1,39 +1,25 @@ // SPDX-License-Identifier: EUPL-1.2 use crate::libs::errors::T4lError; -use crate::{TrackingSpeed, command02}; +use crate::{TrackingSpeed, frame}; pub struct TrackingSpeedCommand; impl TrackingSpeedCommand { - pub fn build(speed: TrackingSpeed) -> Result<[u8; 36], T4lError> { - const FUNCTION_GROUP_TRACKING_SPEED: [u8; 6] = [0x0a, 0x04, 0xc4, 0x0c, 0x01, 0x00]; + const RECEIVER: u8 = 0x04; + const COMMAND: u16 = 0x0cc4; - let appendix: [u8; 16] = { - let mut a = [0x00; 16]; - a[..4].fill(0x00); - a + pub fn build(speed: TrackingSpeed, sequence_nr: u16) -> Result<[u8; 60], T4lError> { + let value: u8 = match speed { + TrackingSpeed::Standard => 0, + TrackingSpeed::Sport => 2, }; - let (sequence_nr, checksum, command) = match speed { - TrackingSpeed::Standard => ( - [0x20, 0x00], - [0xab, 0xcb], - [0xe6, 0x3f, 0x00, 0x00, 0x00, 0x00], - ), - TrackingSpeed::Sport => ( - [0x21, 0x00], - [0xfa, 0x0e], - [0x67, 0xfe, 0x02, 0x00, 0x00, 0x00], - ), - }; - - Ok(command02() - .function_group(FUNCTION_GROUP_TRACKING_SPEED) + Ok(frame() + .receiver(Self::RECEIVER) + .command(Self::COMMAND) + .data(&[value]) .sequence_nr(sequence_nr) - .checksum(checksum) - .command(command) - .appendix(appendix) .build()) } } @@ -41,31 +27,17 @@ impl TrackingSpeedCommand { #[cfg(test)] mod tests { use crate::{TrackingSpeed, TrackingSpeedCommand}; - use assertables::assert_ok; use test_case::test_case; - #[test_case(TrackingSpeed::Standard, [0x20, 0x00], [0xab, 0xcb], [0xe6, 0x3f, 0x00, 0x00, 0x00, 0x00]; "standard speed")] - #[test_case(TrackingSpeed::Sport, [0x21, 0x00], [0xfa, 0x0e], [0x67, 0xfe, 0x02, 0x00, 0x00, 0x00]; "sport speed")] - fn speed_mode(mode: TrackingSpeed, sequence_nr: [u8; 2], checksum: [u8; 2], command: [u8; 6]) { - let speed_command_option = TrackingSpeedCommand::build(mode); + // Captured frames, reproduced byte for byte when given the captured sequence number. + #[test_case(TrackingSpeed::Standard, 0x0020, "aa2520000c00abcb0a04c40c0100e63f00"; "standard speed")] + #[test_case(TrackingSpeed::Sport, 0x0021, "aa2521000c00fa0e0a04c40c010067fe02"; "sport speed")] + fn speed_mode(speed: TrackingSpeed, sequence_nr: u16, captured: &str) { + let captured = hex::decode(captured).unwrap(); - assert_ok!( - &speed_command_option, - "tracking speed command should be built" - ); + let command = TrackingSpeedCommand::build(speed, sequence_nr).unwrap(); - let speed_command = speed_command_option.unwrap(); - assert_eq!( - speed_command[8..14], - [0x0a, 0x04, 0xc4, 0x0c, 0x01, 0x00], - "Function group should be set for sleep mode" - ); - assert_eq!( - speed_command[2..4], - sequence_nr, - "Sequence number should be set" - ); - assert_eq!(speed_command[6..8], checksum, "Checksum should be set"); - assert_eq!(speed_command[14..20], command, "Command should be set"); + assert_eq!(command[..captured.len()], captured[..]); + assert!(command[captured.len()..].iter().all(|b| *b == 0)); } } diff --git a/src/libs/camera/frame.rs b/src/libs/camera/frame.rs new file mode 100644 index 0000000..f3aef3a --- /dev/null +++ b/src/libs/camera/frame.rs @@ -0,0 +1,205 @@ +// SPDX-License-Identifier: EUPL-1.2 + +use bon::builder; +use std::sync::atomic::{AtomicU16, Ordering}; +use std::time::{SystemTime, UNIX_EPOCH}; + +/// Size of the buffer the camera expects on unit 2, selector 2. +pub const FRAME_SIZE: usize = 60; +const HEADER_SIZE: usize = 12; +const DATA_HEADER_SIZE: usize = 4; +/// Maximum payload that fits into one frame after both headers. +pub const MAX_FRAME_DATA: usize = FRAME_SIZE - HEADER_SIZE - DATA_HEADER_SIZE; + +/// CRC-16/USB: reflected polynomial `0xA001`, init `0xFFFF`, final XOR `0xFFFF`. +/// +/// The camera checks both the header and the data block of a frame with it. +/// First described in . +pub fn crc16_usb(data: &[u8]) -> u16 { + let mut crc: u16 = 0xffff; + for byte in data { + crc ^= *byte as u16; + for _ in 0..8 { + crc = if crc & 1 != 0 { + (crc >> 1) ^ 0xa001 + } else { + crc >> 1 + }; + } + } + crc ^ 0xffff +} + +/// Builds a framed command for unit 2, selector 2. +/// +/// Layout (all multi-byte fields little endian): +/// `aa 25 | seq:u16 | 0c 00 | header crc:u16 | 0a | receiver:u8 | command:u16 | data len:u16 | data crc:u16 | data` +/// +/// The header CRC covers bytes 0..12 with its own field zeroed, the data CRC covers the +/// data length, the CRC field (zeroed) and the data. The rest of the buffer is zero padded. +/// +/// # Panics +/// If `data` is longer than [`MAX_FRAME_DATA`] bytes. +#[builder(finish_fn = build)] +pub fn frame(receiver: u8, command: u16, data: &[u8], sequence_nr: u16) -> [u8; FRAME_SIZE] { + assert!( + data.len() <= MAX_FRAME_DATA, + "frame data must not exceed {MAX_FRAME_DATA} bytes" + ); + + let mut buffer = [0u8; FRAME_SIZE]; + + buffer[0..2].copy_from_slice(&[0xaa, 0x25]); + buffer[2..4].copy_from_slice(&sequence_nr.to_le_bytes()); + buffer[4..6].copy_from_slice(&(HEADER_SIZE as u16).to_le_bytes()); + buffer[8] = 0x0a; + buffer[9] = receiver; + buffer[10..12].copy_from_slice(&command.to_le_bytes()); + let header_crc = crc16_usb(&buffer[0..HEADER_SIZE]); + buffer[6..8].copy_from_slice(&header_crc.to_le_bytes()); + + let data_end = HEADER_SIZE + DATA_HEADER_SIZE + data.len(); + buffer[12..14].copy_from_slice(&(data.len() as u16).to_le_bytes()); + buffer[16..data_end].copy_from_slice(data); + let data_crc = crc16_usb(&buffer[HEADER_SIZE..data_end]); + buffer[14..16].copy_from_slice(&data_crc.to_le_bytes()); + + buffer +} + +/// Sequence numbers for framed commands. +/// +/// The camera drops a frame that repeats the sequence number of the previous one, +/// so every frame needs a fresh number. Seeding from the clock keeps two short-lived +/// processes (e.g. two CLI calls in a row) from starting with the same number. +pub struct FrameSequence(AtomicU16); + +impl FrameSequence { + pub fn new() -> Self { + let seed = SystemTime::now() + .duration_since(UNIX_EPOCH) + .map(|d| d.as_millis() as u16) + .unwrap_or(0); + Self(AtomicU16::new(seed)) + } + + pub fn next(&self) -> u16 { + self.0.fetch_add(1, Ordering::Relaxed).wrapping_add(1) + } +} + +impl Default for FrameSequence { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + mod unit { + use crate::libs::camera::frame::{FrameSequence, MAX_FRAME_DATA, crc16_usb, frame}; + + #[test] + fn crc16_usb_check_value() { + // Standard check value of CRC-16/USB for the ASCII string "123456789". + assert_eq!(crc16_usb(b"123456789"), 0xb4c8); + } + + #[test] + fn layout() { + let command = frame() + .receiver(0x04) + .command(0x3244) + .data(&[0x11, 0x22, 0x33]) + .sequence_nr(0x1234) + .build(); + + assert_eq!(command[0..2], [0xaa, 0x25], "frame id"); + assert_eq!(command[2..4], [0x34, 0x12], "sequence number little endian"); + assert_eq!(command[4..6], [0x0c, 0x00], "header length"); + assert_eq!(command[8], 0x0a, "fixed header byte"); + assert_eq!(command[9], 0x04, "receiver"); + assert_eq!( + command[10..12], + [0x44, 0x32], + "full 16 bit command id little endian" + ); + assert_eq!(command[12..14], [0x03, 0x00], "data length"); + assert_eq!(command[16..19], [0x11, 0x22, 0x33], "data"); + assert!(command[19..].iter().all(|b| *b == 0), "zero padding"); + } + + #[test] + fn checksums_cover_header_and_data() { + let command = frame() + .receiver(0x04) + .command(0x30c4) + .data(&[0x01]) + .sequence_nr(7) + .build(); + + let mut header = command[0..12].to_vec(); + header[6..8].fill(0); + assert_eq!(command[6..8], crc16_usb(&header).to_le_bytes()); + + let mut data_block = command[12..17].to_vec(); + data_block[2..4].fill(0); + assert_eq!(command[14..16], crc16_usb(&data_block).to_le_bytes()); + } + + #[test] + fn sequence_changes_header_checksum_only() { + let a = frame() + .receiver(0x04) + .command(0x30c4) + .data(&[0x01]) + .sequence_nr(1) + .build(); + let b = frame() + .receiver(0x04) + .command(0x30c4) + .data(&[0x01]) + .sequence_nr(2) + .build(); + + assert_ne!(a[6..8], b[6..8], "header checksum depends on sequence"); + assert_eq!(a[14..16], b[14..16], "data checksum does not"); + } + + #[test] + fn max_data_fits() { + let data = [0xffu8; MAX_FRAME_DATA]; + let command = frame() + .receiver(0x04) + .command(0x0001) + .data(&data) + .sequence_nr(0) + .build(); + assert_eq!(command[16..], data); + } + + #[test] + #[should_panic] + fn too_much_data_panics() { + let data = [0u8; MAX_FRAME_DATA + 1]; + frame() + .receiver(0x04) + .command(0x0001) + .data(&data) + .sequence_nr(0) + .build(); + } + + #[test] + fn sequence_never_repeats_consecutively() { + let sequence = FrameSequence::new(); + let mut previous = sequence.next(); + for _ in 0..70_000 { + let current = sequence.next(); + assert_ne!(current, previous); + assert_eq!(current, previous.wrapping_add(1)); + previous = current; + } + } + } +} diff --git a/src/libs/camera/mod.rs b/src/libs/camera/mod.rs index 0a61e47..9d1c42b 100644 --- a/src/libs/camera/mod.rs +++ b/src/libs/camera/mod.rs @@ -1,15 +1,15 @@ // SPDX-License-Identifier: EUPL-1.2 mod camera; -mod command02; mod commands; mod enums; +mod frame; mod status; mod transport; pub use camera::Camera; pub use camera::Tiny2Camera; -pub use command02::command02; pub use commands::*; pub use enums::*; +pub use frame::{FrameSequence, crc16_usb, frame}; pub use status::CameraStatus;