mirror of
https://github.com/Astatin3/CC2.git
synced 2026-06-09 00:18:00 -06:00
Start decomposing protocol.
This commit is contained in:
+155
-11
@@ -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))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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]);
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user