diff --git a/decoder/src/lib.rs b/decoder/src/lib.rs index 17c0ac45..d7c5888b 100644 --- a/decoder/src/lib.rs +++ b/decoder/src/lib.rs @@ -17,6 +17,7 @@ mod decoder; mod elf2table; mod frame; pub mod log; +mod stream; use std::{ collections::{BTreeMap, HashMap}, @@ -31,6 +32,7 @@ use elf2table::parse_impl; pub use elf2table::{Location, Locations}; pub use frame::Frame; +pub use stream::StreamDecoder; /// Specifies the origin of a format string #[derive(PartialEq, Eq, Debug)] @@ -232,6 +234,15 @@ impl Table { let consumed = len - decoder.bytes.len(); Ok((frame, consumed)) } + + pub fn new_stream_decoder(&self) -> Box { + Box::new(stream::Rzcobs::new(self)) + } + + // temporary, will remove + pub fn new_stream_decoder_raw(&self) -> Box { + Box::new(stream::Raw::new(self)) + } } // NOTE follows `parser::Type` diff --git a/decoder/src/stream/mod.rs b/decoder/src/stream/mod.rs new file mode 100644 index 00000000..7d692213 --- /dev/null +++ b/decoder/src/stream/mod.rs @@ -0,0 +1,15 @@ +mod raw; +mod rzcobs; + +pub use raw::Raw; +pub use rzcobs::Rzcobs; + +use crate::{DecodeError, Frame}; + +pub trait StreamDecoder { + /// Push received data to the decoder. The decoder stores it + /// internally, and makes decoded frames available through [`decode`]. + fn received(&mut self, data: &[u8]); + + fn decode(&mut self) -> Result, DecodeError>; +} diff --git a/decoder/src/stream/raw.rs b/decoder/src/stream/raw.rs new file mode 100644 index 00000000..db2239eb --- /dev/null +++ b/decoder/src/stream/raw.rs @@ -0,0 +1,32 @@ +use super::StreamDecoder; +use crate::{DecodeError, Frame, Table}; + +pub struct Raw<'a> { + table: &'a Table, + data: Vec, +} + +impl<'a> Raw<'a> { + pub fn new(table: &'a Table) -> Self { + Self { + table, + data: Vec::new(), + } + } +} + +impl<'a> StreamDecoder for Raw<'a> { + fn received(&mut self, data: &[u8]) { + self.data.extend_from_slice(data); + } + + fn decode(&mut self) -> Result, DecodeError> { + match self.table.decode(&self.data) { + Ok((frame, consumed)) => { + self.data.drain(0..consumed); + Ok(frame) + } + Err(e) => Err(e), + } + } +} diff --git a/decoder/src/stream/rzcobs.rs b/decoder/src/stream/rzcobs.rs new file mode 100644 index 00000000..b87204cb --- /dev/null +++ b/decoder/src/stream/rzcobs.rs @@ -0,0 +1,95 @@ +use super::StreamDecoder; +use crate::{DecodeError, Frame, Table}; + +/// Decode a full message. +/// +/// `data` must be a full rzCOBS encoded message. Decoding partial +/// messages is not possible. `data` must NOT include any `0x00` separator byte. +fn rzcobs_decode(data: &[u8]) -> Result, DecodeError> { + let mut res = vec![]; + let mut data = data.iter().rev().cloned(); + while let Some(x) = data.next() { + match x { + 0 => return Err(DecodeError::Malformed), + 0x01..=0x7f => { + for i in 0..7 { + if x & (1 << (6 - i)) == 0 { + res.push(data.next().ok_or(DecodeError::Malformed)?); + } else { + res.push(0); + } + } + } + 0x80..=0xfe => { + let n = (x & 0x7f) + 7; + res.push(0); + for _ in 0..n { + res.push(data.next().ok_or(DecodeError::Malformed)?); + } + } + 0xff => { + for _ in 0..134 { + res.push(data.next().ok_or(DecodeError::Malformed)?); + } + } + } + } + + res.reverse(); + Ok(res) +} + +pub struct Rzcobs<'a> { + table: &'a Table, + raw: Vec, +} + +impl<'a> Rzcobs<'a> { + pub fn new(table: &'a Table) -> Self { + Self { + table, + raw: Vec::new(), + } + } +} + +impl<'a> StreamDecoder for Rzcobs<'a> { + fn received(&mut self, mut data: &[u8]) { + // Trim zeros from the left, start storing at first non-zero byte. + if self.raw.is_empty() { + while data.get(0) == Some(&0) { + data = &data[1..] + } + } + + self.raw.extend_from_slice(data); + } + + fn decode(&mut self) -> Result, DecodeError> { + // Find frame separator. If not found, we don't have enough data yet. + let zero = self + .raw + .iter() + .position(|&x| x == 0) + .ok_or(DecodeError::UnexpectedEof)?; + + let frame = rzcobs_decode(&self.raw[..zero]); + + // Even if it failed, pop the data off so we don't get stuck. + // Pop off the frame + 1 or more separator zero-bytes + if let Some(nonzero) = self.raw[zero..].iter().position(|&x| x != 0) { + self.raw.drain(0..zero + nonzero); + } else { + self.raw.clear(); + } + + assert!(self.raw.is_empty() || self.raw[0] != 0); + + let frame: Vec = frame?; + match self.table.decode(&frame) { + Ok((frame, _consumed)) => Ok(frame), + Err(DecodeError::UnexpectedEof) => Err(DecodeError::Malformed), + Err(DecodeError::Malformed) => Err(DecodeError::Malformed), + } + } +} diff --git a/qemu-run/src/main.rs b/qemu-run/src/main.rs index 14a18ede..e5f95ff7 100644 --- a/qemu-run/src/main.rs +++ b/qemu-run/src/main.rs @@ -10,7 +10,7 @@ use std::{ }; use anyhow::{anyhow, bail}; -use defmt_decoder::{DecodeError, Table}; +use defmt_decoder::{DecodeError, StreamDecoder, Table}; use process::Child; fn main() -> Result<(), anyhow::Error> { @@ -64,29 +64,22 @@ fn notmain() -> Result, anyhow::Error> { .take() .ok_or_else(|| anyhow!("failed to acquire child's stdout handle"))?; - let mut frames = vec![]; + let mut decoder = table.new_stream_decoder(); + let mut readbuf = [0; 256]; let exit_code; loop { let n = stdout.read(&mut readbuf)?; - - if n != 0 { - frames.extend_from_slice(&readbuf[..n]); - - decode(&mut frames, &table)?; - } + decoder.received(&readbuf[..n]); + decode(&mut *decoder)?; if let Some(status) = child.0.try_wait()? { exit_code = status.code(); - stdout.read_to_end(&mut frames)?; - decode(&mut frames, &table)?; - if !frames.is_empty() { - return Err(anyhow!( - "couldn't decode all data (remaining: {:x?})", - frames - )); - } + let mut data = Vec::new(); + stdout.read_to_end(&mut data)?; + decoder.received(&data); + decode(&mut *decoder)?; break; } @@ -95,18 +88,15 @@ fn notmain() -> Result, anyhow::Error> { Ok(exit_code) } -fn decode(frames: &mut Vec, table: &Table) -> Result<(), DecodeError> { +fn decode(decoder: &mut dyn StreamDecoder) -> Result<(), DecodeError> { loop { - match table.decode(&frames) { - Ok((frame, consumed)) => { - println!("{}", frame.display(true)); - let n = frames.len(); - frames.rotate_left(consumed); - frames.truncate(n - consumed); + match decoder.decode() { + Ok(frame) => { + println!("{}", frame.display(true)) } Err(DecodeError::UnexpectedEof) => return Ok(()), Err(DecodeError::Malformed) => { - eprintln!("failed to decode defmt data: {:x?}", frames); + eprintln!("failed to decode defmt data"); return Err(DecodeError::Malformed); } }