Start decomposing protocol.

This commit is contained in:
Michael Mikovsky
2026-05-03 12:13:26 -06:00
parent 2a005416cd
commit 59980950bf
23 changed files with 6919 additions and 11 deletions
+155 -11
View File
@@ -1,17 +1,161 @@
use std::fs::OpenOptions;
use std::io::{Read, Write};
use std::time::Duration;
mod protocol;
mod strace_parser;
use std::io::{BufRead, BufReader};
use std::process::{Command, Stdio};
use protocol::{McuCommand, McuFrame, hex_bytes, split_frames};
use strace_parser::{TracePacket, parse_trace_line};
const DEFAULT_PROCESS: &str = "elegoo_printer";
const DEFAULT_FD: u32 = 26;
fn main() -> anyhow::Result<()> {
let mut dev = OpenOptions::new()
.read(true)
.write(true)
.open("/dev/rpmsg1")?;
let config = Config::from_args()?;
let mut buf = [0u8; 4096];
eprintln!(
"starting passive trace: process={} fd={} strace -f -tt -xx -s 512 -e trace=read,write,ioctl",
config.process, config.fd
);
loop {
let n = dev.read(&mut buf)?;
println!("rpmsg read {n} bytes: {:02x?}", &buf[..n]);
let pid = pidof(&config.process)?;
let mut strace = Command::new("strace")
.args([
"-f",
"-tt",
"-xx",
"-s",
"512",
"-e",
"trace=read,write,ioctl",
"-p",
&pid,
])
.stderr(Stdio::piped())
.spawn()?;
let stderr = strace
.stderr
.take()
.ok_or_else(|| anyhow::anyhow!("failed to capture strace stderr"))?;
for line in BufReader::new(stderr).lines() {
let line = line?;
if let Some(packet) = parse_trace_line(&line, config.fd) {
log_packet(&packet);
}
}
let status = strace.wait()?;
if !status.success() {
anyhow::bail!("strace exited with {status}");
}
Ok(())
}
#[derive(Debug)]
struct Config {
process: String,
fd: u32,
}
impl Config {
fn from_args() -> anyhow::Result<Self> {
let mut process = DEFAULT_PROCESS.to_string();
let mut fd = DEFAULT_FD;
let mut args = std::env::args().skip(1);
while let Some(arg) = args.next() {
match arg.as_str() {
"--process" | "-p" => {
process = args
.next()
.ok_or_else(|| anyhow::anyhow!("missing value for {arg}"))?;
}
"--fd" | "-f" => {
let value = args
.next()
.ok_or_else(|| anyhow::anyhow!("missing value for {arg}"))?;
fd = value.parse()?;
}
"--help" | "-h" => {
print_help();
std::process::exit(0);
}
_ => anyhow::bail!("unknown argument: {arg}"),
}
}
Ok(Self { process, fd })
}
}
fn print_help() {
println!(
"Usage: serial-test [--process elegoo_printer] [--fd 26]\n\n\
Passively wraps:\n\
strace -f -tt -xx -s 512 -e trace=read,write,ioctl -p $(pidof PROCESS)\n\n\
It filters the selected fd in-process and prints named MCU protocol frames."
);
}
fn pidof(process: &str) -> anyhow::Result<String> {
let output = Command::new("pidof").arg(process).output()?;
if !output.status.success() {
anyhow::bail!("pidof {process} failed; is the process running?");
}
let pid = String::from_utf8(output.stdout)?
.split_whitespace()
.next()
.ok_or_else(|| anyhow::anyhow!("pidof {process} returned no pid"))?
.to_string();
Ok(pid)
}
fn log_packet(packet: &TracePacket) {
for frame in split_frames(&packet.bytes) {
let frame = McuFrame::parse(packet.direction, frame);
let timestamp = packet.timestamp.as_deref().unwrap_or("-");
let result = packet
.result_len
.map(|len| format!(" syscall_len={len}"))
.unwrap_or_default();
println!(
"{timestamp} {} {} len={} seq=0x{:02x} opcode=0x{:02x} {} trailer={} term=0x{:02x}{}",
packet.direction,
frame.command,
frame.len,
frame.seq,
frame.opcode,
command_data(&frame),
hex_bytes(&frame.trailer),
frame.terminator,
result
);
}
}
fn command_data(frame: &McuFrame) -> String {
match &frame.command {
McuCommand::HostPeriodicQuery(data) => format!("data={data}"),
McuCommand::HostMotionControl(data) => format!("data={data}"),
McuCommand::HostControlResponse(data) => format!("data={data}"),
McuCommand::HostAction(data) => format!("data={data}"),
McuCommand::DeviceQueryResponse(data) => format!("data={data}"),
McuCommand::DeviceHomingAck(data) => format!("data={data}"),
McuCommand::DeviceHomingStatus(data) => format!("data={data}"),
McuCommand::DevicePeriodicStatus(data) => format!("data={data}"),
McuCommand::DeviceActionResponse(data) => format!("data={data}"),
McuCommand::DeviceSensorStatus(data) => format!("data={data}"),
McuCommand::Unknown(_) => format!(
"payload={} raw={}",
hex_bytes(&frame.body),
hex_bytes(&frame.raw)
),
McuCommand::MalformedFrame => format!("raw={}", hex_bytes(&frame.raw)),
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DeviceActionResponseData {
UnknownPayload(Vec<u8>),
}
impl DeviceActionResponseData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for DeviceActionResponseData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,30 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DeviceHomingAckData {
Empty,
UnknownPayload(Vec<u8>),
}
impl DeviceHomingAckData {
pub fn from_body(body: &[u8]) -> Self {
if body.is_empty() {
Self::Empty
} else {
Self::UnknownPayload(body.to_vec())
}
}
}
impl fmt::Display for DeviceHomingAckData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::Empty => f.write_str("Empty"),
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DeviceHomingStatusData {
UnknownPayload(Vec<u8>),
}
impl DeviceHomingStatusData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for DeviceHomingStatusData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DevicePeriodicStatusData {
UnknownPayload(Vec<u8>),
}
impl DevicePeriodicStatusData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for DevicePeriodicStatusData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DeviceQueryResponseData {
UnknownPayload(Vec<u8>),
}
impl DeviceQueryResponseData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for DeviceQueryResponseData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum DeviceSensorStatusData {
UnknownPayload(Vec<u8>),
}
impl DeviceSensorStatusData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for DeviceSensorStatusData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
+24
View File
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum HostActionData {
UnknownPayload(Vec<u8>),
}
impl HostActionData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for HostActionData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum HostControlResponseData {
UnknownPayload(Vec<u8>),
}
impl HostControlResponseData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for HostControlResponseData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,24 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum HostMotionControlData {
UnknownPayload(Vec<u8>),
}
impl HostMotionControlData {
pub fn from_body(body: &[u8]) -> Self {
Self::UnknownPayload(body.to_vec())
}
}
impl fmt::Display for HostMotionControlData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
@@ -0,0 +1,30 @@
use std::fmt;
use super::hex_bytes;
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum HostPeriodicQueryData {
Empty,
UnknownPayload(Vec<u8>),
}
impl HostPeriodicQueryData {
pub fn from_body(body: &[u8]) -> Self {
if body.is_empty() {
Self::Empty
} else {
Self::UnknownPayload(body.to_vec())
}
}
}
impl fmt::Display for HostPeriodicQueryData {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::Empty => f.write_str("Empty"),
Self::UnknownPayload(payload) => {
write!(f, "UnknownPayload(payload={})", hex_bytes(payload))
}
}
}
}
+241
View File
@@ -0,0 +1,241 @@
mod device_action_response;
mod device_homing_ack;
mod device_homing_status;
mod device_periodic_status;
mod device_query_response;
mod device_sensor_status;
mod host_action;
mod host_control_response;
mod host_motion_control;
mod host_periodic_query;
use std::fmt;
pub use device_action_response::DeviceActionResponseData;
pub use device_homing_ack::DeviceHomingAckData;
pub use device_homing_status::DeviceHomingStatusData;
pub use device_periodic_status::DevicePeriodicStatusData;
pub use device_query_response::DeviceQueryResponseData;
pub use device_sensor_status::DeviceSensorStatusData;
pub use host_action::HostActionData;
pub use host_control_response::HostControlResponseData;
pub use host_motion_control::HostMotionControlData;
pub use host_periodic_query::HostPeriodicQueryData;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Direction {
HostWrite,
DeviceRead,
Ioctl,
}
impl fmt::Display for Direction {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::HostWrite => f.write_str("host->mcu"),
Self::DeviceRead => f.write_str("mcu->host"),
Self::Ioctl => f.write_str("ioctl"),
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum McuCommand {
HostPeriodicQuery(HostPeriodicQueryData),
HostMotionControl(HostMotionControlData),
HostControlResponse(HostControlResponseData),
HostAction(HostActionData),
DeviceQueryResponse(DeviceQueryResponseData),
DeviceHomingAck(DeviceHomingAckData),
DeviceHomingStatus(DeviceHomingStatusData),
DevicePeriodicStatus(DevicePeriodicStatusData),
DeviceActionResponse(DeviceActionResponseData),
DeviceSensorStatus(DeviceSensorStatusData),
Unknown(u8),
MalformedFrame,
}
impl McuCommand {
fn from_parts(direction: Direction, opcode: u8, body: &[u8]) -> Self {
match (direction, opcode) {
(Direction::HostWrite, 0x05) => {
Self::HostPeriodicQuery(HostPeriodicQueryData::from_body(body))
}
(Direction::HostWrite, 0x17) => {
Self::HostMotionControl(HostMotionControlData::from_body(body))
}
(Direction::HostWrite, 0x21) => {
Self::HostControlResponse(HostControlResponseData::from_body(body))
}
(Direction::HostWrite, 0x2a) => Self::HostAction(HostActionData::from_body(body)),
(Direction::DeviceRead, 0x3b) => {
Self::DeviceQueryResponse(DeviceQueryResponseData::from_body(body))
}
(Direction::DeviceRead, 0x31) => {
Self::DeviceHomingAck(DeviceHomingAckData::from_body(body))
}
(Direction::DeviceRead, 0x42) => {
Self::DeviceHomingStatus(DeviceHomingStatusData::from_body(body))
}
(Direction::DeviceRead, 0x43) => {
Self::DevicePeriodicStatus(DevicePeriodicStatusData::from_body(body))
}
(Direction::DeviceRead, 0x46) => {
Self::DeviceActionResponse(DeviceActionResponseData::from_body(body))
}
(Direction::DeviceRead, 0x48) => {
Self::DeviceSensorStatus(DeviceSensorStatusData::from_body(body))
}
_ => Self::Unknown(opcode),
}
}
}
impl fmt::Display for McuCommand {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::HostPeriodicQuery(_) => f.write_str("HostPeriodicQuery"),
Self::HostMotionControl(_) => f.write_str("HostMotionControl"),
Self::HostControlResponse(_) => f.write_str("HostControlResponse"),
Self::HostAction(_) => f.write_str("HostAction"),
Self::DeviceQueryResponse(_) => f.write_str("DeviceQueryResponse"),
Self::DeviceHomingAck(_) => f.write_str("DeviceHomingAck"),
Self::DeviceHomingStatus(_) => f.write_str("DeviceHomingStatus"),
Self::DevicePeriodicStatus(_) => f.write_str("DevicePeriodicStatus"),
Self::DeviceActionResponse(_) => f.write_str("DeviceActionResponse"),
Self::DeviceSensorStatus(_) => f.write_str("DeviceSensorStatus"),
Self::Unknown(opcode) => write!(f, "Unknown(0x{opcode:02x})"),
Self::MalformedFrame => f.write_str("MalformedFrame"),
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct McuFrame {
pub len: u8,
pub seq: u8,
pub opcode: u8,
pub command: McuCommand,
pub body: Vec<u8>,
pub trailer: Vec<u8>,
pub terminator: u8,
pub raw: Vec<u8>,
}
impl McuFrame {
pub fn parse(direction: Direction, bytes: &[u8]) -> Self {
if bytes.len() < 4
|| bytes.first().copied() != Some(bytes.len() as u8)
|| bytes.last().copied() != Some(0x7e)
{
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),
command: McuCommand::MalformedFrame,
body: Vec::new(),
trailer: Vec::new(),
terminator: bytes.last().copied().unwrap_or(0),
raw: bytes.to_vec(),
};
}
let len = bytes[0];
let seq = bytes[1];
let opcode = bytes[2];
let trailer_len = if bytes.len() >= 6 { 2 } else { 1 };
let body_end = bytes.len() - trailer_len - 1;
let body = bytes[3..body_end].to_vec();
let trailer = bytes[body_end..bytes.len() - 1].to_vec();
let command = McuCommand::from_parts(direction, opcode, &body);
Self {
len,
seq,
opcode,
command,
body,
trailer,
terminator: 0x7e,
raw: bytes.to_vec(),
}
}
}
pub fn split_frames(bytes: &[u8]) -> Vec<&[u8]> {
let mut frames = Vec::new();
let mut offset = 0;
while offset < bytes.len() {
let Some(&len) = bytes.get(offset) else {
break;
};
let len = len as usize;
if len == 0 || offset + len > bytes.len() {
frames.push(&bytes[offset..]);
break;
}
frames.push(&bytes[offset..offset + len]);
offset += len;
}
frames
}
pub fn hex_bytes(bytes: &[u8]) -> String {
bytes
.iter()
.map(|byte| format!("{byte:02x}"))
.collect::<Vec<_>>()
.join(" ")
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn parses_periodic_query_frame() {
let bytes = [0x06, 0x14, 0x05, 0x4a, 0xb6, 0x7e];
let frame = McuFrame::parse(Direction::HostWrite, &bytes);
assert_eq!(
frame.command,
McuCommand::HostPeriodicQuery(HostPeriodicQueryData::Empty)
);
assert_eq!(frame.seq, 0x14);
assert_eq!(frame.opcode, 0x05);
assert_eq!(frame.trailer, [0x4a, 0xb6]);
}
#[test]
fn splits_aggregated_read_frames() {
let bytes = [
0x0b, 0x15, 0x3b, 0x82, 0x9f, 0xb7, 0xbb, 0x5d, 0xe5, 0x75, 0x7e, 0x05, 0x15, 0xc9,
0x2c, 0x7e,
];
let frames = split_frames(&bytes);
assert_eq!(frames.len(), 2);
assert_eq!(frames[0].len(), 11);
assert_eq!(frames[1].len(), 5);
}
#[test]
fn unknown_command_keeps_only_unknown_payload() {
let bytes = [
0x3a, 0x10, 0x18, 0x13, 0x00, 0x17, 0x13, 0x80, 0xf0, 0xe0, 0x82, 0x65, 0x01, 0x00,
0x17, 0x13, 0x83, 0xb0, 0x5c, 0x02, 0xff, 0x8f, 0x3a, 0x17, 0x13, 0x82, 0x81, 0x71,
0x03, 0xe6, 0x2e, 0x17, 0x13, 0x81, 0xb7, 0x5d, 0x06, 0xf6, 0x75, 0x17, 0x13, 0x81,
0x83, 0x73, 0x0a, 0xfc, 0x39, 0x17, 0x13, 0x80, 0xe2, 0x1d, 0x10, 0xfe, 0x3c, 0x53,
0x10, 0x7e,
];
let frame = McuFrame::parse(Direction::HostWrite, &bytes);
assert_eq!(frame.command, McuCommand::Unknown(0x18));
assert_eq!(frame.body[0], 0x13);
assert_eq!(frame.trailer, [0x53, 0x10]);
}
}
+159
View File
@@ -0,0 +1,159 @@
use crate::protocol::Direction;
#[derive(Debug)]
pub struct TracePacket {
pub timestamp: Option<String>,
pub direction: Direction,
pub bytes: Vec<u8>,
pub result_len: Option<usize>,
}
pub fn parse_trace_line(line: &str, fd: u32) -> Option<TracePacket> {
let syscall_start = line
.find("read(")
.or_else(|| line.find("write("))
.or_else(|| line.find("ioctl("))?;
let syscall = &line[syscall_start..];
let timestamp = line[..syscall_start]
.split_whitespace()
.last()
.map(str::to_string);
let (direction, rest) = if let Some(rest) = syscall.strip_prefix("read(") {
(Direction::DeviceRead, rest)
} else if let Some(rest) = syscall.strip_prefix("write(") {
(Direction::HostWrite, rest)
} else {
let rest = syscall.strip_prefix("ioctl(")?;
(Direction::Ioctl, rest)
};
let fd_prefix = format!("{fd},");
if !rest.starts_with(&fd_prefix) {
return None;
}
if direction == Direction::Ioctl {
println!(
"{} ioctl fd={fd}: {line}",
timestamp.as_deref().unwrap_or("-")
);
return None;
}
let bytes = parse_bytes_argument(rest)?;
let result_len = parse_result_len(syscall);
Some(TracePacket {
timestamp,
direction,
bytes,
result_len,
})
}
fn parse_bytes_argument(rest: &str) -> Option<Vec<u8>> {
let first_quote = rest.find('"')?;
let encoded = &rest[first_quote + 1..];
let end_quote = find_unescaped_quote(encoded)?;
Some(decode_strace_bytes(&encoded[..end_quote]))
}
fn find_unescaped_quote(s: &str) -> Option<usize> {
let bytes = s.as_bytes();
let mut i = 0;
while i < bytes.len() {
match bytes[i] {
b'"' => return Some(i),
b'\\' => i += 2,
_ => i += 1,
}
}
None
}
fn decode_strace_bytes(s: &str) -> Vec<u8> {
let bytes = s.as_bytes();
let mut out = Vec::new();
let mut i = 0;
while i < bytes.len() {
if bytes[i] == b'\\' && i + 1 < bytes.len() {
match bytes[i + 1] {
b'x' if i + 3 < bytes.len() => {
if let Ok(value) = u8::from_str_radix(&s[i + 2..i + 4], 16) {
out.push(value);
i += 4;
continue;
}
}
b'n' => {
out.push(b'\n');
i += 2;
continue;
}
b'r' => {
out.push(b'\r');
i += 2;
continue;
}
b't' => {
out.push(b'\t');
i += 2;
continue;
}
b'\\' => {
out.push(b'\\');
i += 2;
continue;
}
b'"' => {
out.push(b'"');
i += 2;
continue;
}
b'0'..=b'7' => {
let mut end = i + 2;
while end < bytes.len() && end < i + 4 && bytes[end].is_ascii_digit() {
end += 1;
}
if let Ok(value) = u8::from_str_radix(&s[i + 1..end], 8) {
out.push(value);
i = end;
continue;
}
}
value => {
out.push(value);
i += 2;
continue;
}
}
}
out.push(bytes[i]);
i += 1;
}
out
}
fn parse_result_len(syscall: &str) -> Option<usize> {
let result = syscall.rsplit_once(" = ")?.1;
result.split_whitespace().next()?.parse().ok()
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn parses_write_line() {
let line = r#"12:34:56.789012 write(26, "\x06\x14\x05\x4a\xb6\x7e", 6) = 6"#;
let packet = parse_trace_line(line, 26).unwrap();
assert_eq!(packet.direction, Direction::HostWrite);
assert_eq!(packet.bytes, [0x06, 0x14, 0x05, 0x4a, 0xb6, 0x7e]);
assert_eq!(packet.result_len, Some(6));
}
}