From 2a8d5102c16fdecac4af63ca9d3a3c6c28a73993 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 3 May 2026 12:47:30 -0600 Subject: [PATCH] Decode Klipper message blocks --- serial-test/src/main.rs | 39 ++++++++- serial-test/src/protocol/mod.rs | 141 +++++++++++++++++++++++++++++--- 2 files changed, 163 insertions(+), 17 deletions(-) diff --git a/serial-test/src/main.rs b/serial-test/src/main.rs index a9b8416..8c0f333 100644 --- a/serial-test/src/main.rs +++ b/serial-test/src/main.rs @@ -125,20 +125,51 @@ fn log_packet(packet: &TracePacket) { .unwrap_or_default(); println!( - "{timestamp} {} {} len={} seq=0x{:02x} opcode=0x{:02x} {} trailer={} term=0x{:02x}{}", + "{timestamp} {} {} len={} seq=0x{:02x} message_id={} {} crc={} computed_crc={} crc_ok={} term=0x{:02x} messages={}{}", packet.direction, frame.command, frame.len, frame.seq, - frame.opcode, + message_id(&frame), command_data(&frame), hex_bytes(&frame.trailer), + hex_bytes(&frame.computed_crc), + frame.crc_valid, frame.terminator, + format_messages(&frame), result ); } } +fn message_id(frame: &McuFrame) -> String { + frame + .message_id + .map(|message_id| format!("0x{message_id:02x}")) + .unwrap_or_else(|| "-".to_string()) +} + +fn format_messages(frame: &McuFrame) -> String { + if frame.messages.is_empty() { + return "[]".to_string(); + } + + let messages = frame + .messages + .iter() + .map(|message| { + format!( + "{{id=0x{:02x}, payload={}}}", + message.message_id, + hex_bytes(&message.payload) + ) + }) + .collect::>() + .join(", "); + + format!("[{messages}]") +} + fn command_data(frame: &McuFrame) -> String { match &frame.command { McuCommand::HostPeriodicQuery(data) => format!("data={data}"), @@ -154,8 +185,8 @@ fn command_data(frame: &McuFrame) -> String { McuCommand::DeviceSensorStatus(data) => format!("data={data}"), McuCommand::TransportAck(data) => format!("crc={}", hex_bytes(&data.crc)), McuCommand::Unknown(_) => format!( - "payload={} raw={}", - hex_bytes(&frame.body), + "content={} raw={}", + hex_bytes(&frame.content), hex_bytes(&frame.raw) ), McuCommand::MalformedFrame => format!("raw={}", hex_bytes(&frame.raw)), diff --git a/serial-test/src/protocol/mod.rs b/serial-test/src/protocol/mod.rs index 1b8ecc8..37a0d10 100644 --- a/serial-test/src/protocol/mod.rs +++ b/serial-test/src/protocol/mod.rs @@ -66,8 +66,8 @@ pub struct TransportAckData { } impl McuCommand { - fn from_parts(direction: Direction, opcode: u8, body: &[u8]) -> Self { - match (direction, opcode) { + fn from_parts(direction: Direction, message_id: u8, body: &[u8]) -> Self { + match (direction, message_id) { (Direction::HostWrite, 0x05) => { Self::HostPeriodicQuery(HostPeriodicQueryData::from_body(body)) } @@ -99,7 +99,7 @@ impl McuCommand { (Direction::DeviceRead, 0x48) => { Self::DeviceSensorStatus(DeviceSensorStatusData::from_body(body)) } - _ => Self::Unknown(opcode), + _ => Self::Unknown(message_id), } } } @@ -129,14 +129,24 @@ impl fmt::Display for McuCommand { pub struct McuFrame { pub len: u8, pub seq: u8, - pub opcode: u8, + pub message_id: Option, pub command: McuCommand, + pub messages: Vec, + pub content: Vec, pub body: Vec, pub trailer: Vec, + pub computed_crc: Vec, + pub crc_valid: bool, pub terminator: u8, pub raw: Vec, } +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct KlipperMessage { + pub message_id: u8, + pub payload: Vec, +} + impl McuFrame { pub fn parse(direction: Direction, bytes: &[u8]) -> Self { if bytes.len() < 4 @@ -146,10 +156,14 @@ impl McuFrame { return Self { len: bytes.first().copied().unwrap_or(0), seq: bytes.get(1).copied().unwrap_or(0), - opcode: bytes.get(2).copied().unwrap_or(0), + message_id: bytes.get(2).copied(), command: McuCommand::MalformedFrame, + messages: Vec::new(), + content: Vec::new(), body: Vec::new(), trailer: Vec::new(), + computed_crc: Vec::new(), + crc_valid: false, terminator: bytes.last().copied().unwrap_or(0), raw: bytes.to_vec(), }; @@ -159,28 +173,40 @@ impl McuFrame { let seq = bytes[1]; let trailer_len = 2; let body_end = bytes.len() - trailer_len - 1; - let opcode = if body_end > 2 { bytes[2] } else { 0 }; - let body = if body_end > 3 { - bytes[3..body_end].to_vec() + let content = bytes[2..body_end].to_vec(); + let trailer = bytes[body_end..bytes.len() - 1].to_vec(); + let computed_crc = crc16_ccitt_klipper(&bytes[..body_end]) + .to_be_bytes() + .to_vec(); + let crc_valid = trailer == computed_crc; + let messages = parse_klipper_messages(&content); + let message_id = messages.first().map(|message| message.message_id); + let body = if content.len() > 1 { + content[1..].to_vec() } else { Vec::new() }; - let trailer = bytes[body_end..bytes.len() - 1].to_vec(); let command = if body_end == 2 { McuCommand::TransportAck(TransportAckData { crc: trailer.clone(), }) + } else if let Some(message_id) = message_id { + McuCommand::from_parts(direction, message_id, &body) } else { - McuCommand::from_parts(direction, opcode, &body) + McuCommand::MalformedFrame }; Self { len, seq, - opcode, + message_id, command, + messages, + content, body, trailer, + computed_crc, + crc_valid, terminator: 0x7e, raw: bytes.to_vec(), } @@ -217,6 +243,73 @@ pub fn hex_bytes(bytes: &[u8]) -> String { .join(" ") } +pub fn crc16_ccitt_klipper(bytes: &[u8]) -> u16 { + let mut crc = 0xffffu16; + + for byte in bytes { + crc ^= *byte as u16; + for _ in 0..8 { + if crc & 1 != 0 { + crc = (crc >> 1) ^ 0x8408; + } else { + crc >>= 1; + } + } + } + + crc +} + +pub fn parse_klipper_messages(content: &[u8]) -> Vec { + if content.is_empty() { + return Vec::new(); + } + + let mut messages = Vec::new(); + let mut start = 0; + + for index in 1..content.len() { + if is_observed_message_id(content[index]) { + messages.push(KlipperMessage { + message_id: content[start], + payload: content[start + 1..index].to_vec(), + }); + start = index; + } + } + + messages.push(KlipperMessage { + message_id: content[start], + payload: content[start + 1..].to_vec(), + }); + + messages +} + +fn is_observed_message_id(byte: u8) -> bool { + matches!( + byte, + 0x17 | 0x18 + | 0x19 + | 0x1a + | 0x1d + | 0x1e + | 0x20 + | 0x21 + | 0x22 + | 0x28 + | 0x2a + | 0x3b + | 0x3d + | 0x40 + | 0x41 + | 0x42 + | 0x43 + | 0x46 + | 0x48 + ) +} + #[cfg(test)] mod tests { use super::*; @@ -231,8 +324,9 @@ mod tests { McuCommand::HostPeriodicQuery(HostPeriodicQueryData::Empty) ); assert_eq!(frame.seq, 0x14); - assert_eq!(frame.opcode, 0x05); + assert_eq!(frame.message_id, Some(0x05)); assert_eq!(frame.trailer, [0x4a, 0xb6]); + assert!(frame.crc_valid); } #[test] @@ -253,7 +347,27 @@ mod tests { crc: vec![0xc9, 0x2c] }) ); - assert_eq!(ack.opcode, 0); + assert_eq!(ack.message_id, None); + assert!(ack.crc_valid); + } + + #[test] + fn validates_klipper_crc_vectors() { + assert_eq!(crc16_ccitt_klipper(&[0x05, 0x10]), 0x9e81); + assert_eq!(crc16_ccitt_klipper(&[0x05, 0x1f]), 0x6676); + assert_eq!(crc16_ccitt_klipper(&[0x06, 0x14, 0x05]), 0x4ab6); + } + + #[test] + fn extracts_observed_encoded_messages() { + let content = [ + 0x2a, 0x0c, 0x05, 0xea, 0x03, 0xe8, 0xad, 0xe1, 0x0a, 0x17, 0x10, + ]; + let messages = parse_klipper_messages(&content); + + assert_eq!(messages.len(), 2); + assert_eq!(messages[0].message_id, 0x2a); + assert_eq!(messages[1].message_id, 0x17); } #[test] @@ -270,5 +384,6 @@ mod tests { assert!(matches!(frame.command, McuCommand::HostMotionProgram(_))); assert_eq!(frame.body[0], 0x13); assert_eq!(frame.trailer, [0x53, 0x10]); + assert!(frame.crc_valid); } }