use bitmatch::bitmatch;
use bitvec::prelude::*;
use std::convert::TryInto;
#[derive(Debug, Clone, PartialEq)]
pub enum TracePacket {
Sync,
Overflow,
LocalTimestamp1 {
ts: u64,
data_relation: TimestampDataRelation,
},
LocalTimestamp2 {
ts: u8,
},
GlobalTimestamp1 {
ts: u64,
wrap: bool,
clkch: bool,
},
GlobalTimestamp2 {
ts: u64,
},
Extension {
page: u8,
},
Instrumentation {
port: u8,
payload: Vec<u8>,
},
EventCounterWrap {
cyc: bool,
fold: bool,
lsu: bool,
sleep: bool,
exc: bool,
cpi: bool,
},
ExceptionTrace {
exception: ExceptionType,
action: ExceptionAction,
},
PCSample {
pc: Option<u32>,
},
DataTracePC {
comparator: u8,
pc: u32,
},
DataTraceAddress {
comparator: u8,
data: Vec<u8>,
},
DataTraceValue {
comparator: u8,
access_type: MemoryAccessType,
value: Vec<u8>,
},
}
#[derive(Debug, Clone, PartialEq)]
pub enum ExceptionAction {
Entered,
Exited,
Returned,
}
#[derive(Debug, Clone, PartialEq)]
pub enum ExceptionType {
Reset,
Nmi,
HardFault,
MemManage,
BusFault,
UsageFault,
SVCall,
DebugMonitor,
PendSV,
SysTick,
ExternalInterrupt(usize),
}
#[derive(Debug, Clone, PartialEq)]
pub enum MemoryAccessType {
Read,
Write,
}
#[derive(Debug, Clone, PartialEq)]
pub enum TimestampDataRelation {
Sync,
UnknownDelay,
AssocEventDelay,
UnknownAssocEventDelay,
}
#[derive(Debug, Clone, PartialEq)]
pub enum DecoderError {
InvalidHeader(u8),
InvalidHardwarePacket {
disc_id: u8,
payload: Vec<u8>,
},
InvalidHardwareDisc {
disc_id: u8,
size: usize,
},
InvalidExceptionTrace {
exception: u16,
function: u8,
},
InvalidPCSampleSize {
payload: Vec<u8>,
},
InvalidGTS2Size {
payload: Vec<u8>,
},
InvalidSyncSize(usize),
InvalidSourcePayload {
header: u8,
size: u8,
},
}
pub struct Decoder {
pub incoming: BitVec,
pub state: DecoderState,
}
#[derive(Debug, Clone, PartialEq)]
pub enum DecoderState {
Header,
Syncing(usize),
Instrumentation {
port: u8,
payload: Vec<u8>,
expected_size: usize,
},
HardwareSource {
disc_id: u8,
payload: Vec<u8>,
expected_size: usize,
},
LocalTimestamp {
data_relation: TimestampDataRelation,
payload: Vec<u8>,
},
GlobalTimestamp1 { payload: Vec<u8> },
GlobalTimestamp2 { payload: Vec<u8> },
}
impl Decoder {
pub fn new() -> Self {
Decoder {
incoming: BitVec::new(),
state: DecoderState::Header,
}
}
pub fn feed(&mut self, data: Vec<u8>) {
self.incoming.extend(BitVec::<LocalBits, _>::from_vec(data));
}
pub fn pull(&mut self) -> Result<Option<TracePacket>, DecoderError> {
loop {
match self.state {
DecoderState::Syncing(_) => return self.handle_sync(),
_ if self.incoming.len() >= 8 => match {
let bv = &self.incoming[0..=7];
let mut b: u8 = 0;
for (i, bit) in bv.iter().enumerate() {
b |= (*bit as u8) << i;
}
self.incoming = self.incoming[8..].into();
self.process_byte(b)
} {
Ok(Some(packet)) => return Ok(Some(packet)),
Ok(None) => continue,
e => return e,
},
_ => return Ok(None),
}
}
}
fn handle_sync(&mut self) -> Result<Option<TracePacket>, DecoderError> {
const MIN_ZEROS: usize = 47;
if let DecoderState::Syncing(mut count) = self.state {
while let Some(bit) = {
self.incoming.rotate_left(1);
self.incoming.pop()
} {
if !bit && count < MIN_ZEROS {
count += 1;
continue;
} else if bit && count >= MIN_ZEROS {
self.state = DecoderState::Header;
return Ok(Some(TracePacket::Sync));
} else {
return Err(DecoderError::InvalidSyncSize(count));
}
}
} else {
unreachable!();
}
Ok(None)
}
fn process_byte(&mut self, b: u8) -> Result<Option<TracePacket>, DecoderError> {
let packet = match &mut self.state {
DecoderState::Header => self.decode_header(b),
DecoderState::Syncing(_count) => unreachable!(),
DecoderState::HardwareSource {
disc_id,
payload,
expected_size,
} => {
payload.push(b);
if payload.len() == *expected_size {
match Decoder::handle_hardware_source(*disc_id, payload.to_vec()) {
Ok(packet) => Ok(Some(packet)),
Err(e) => Err(e),
}
} else {
Ok(None)
}
}
DecoderState::LocalTimestamp {
data_relation,
payload,
} => {
let last_byte = (b >> 7) & 1 == 0;
payload.push(b);
if last_byte {
Ok(Some(TracePacket::LocalTimestamp1 {
data_relation: data_relation.clone(),
ts: Decoder::extract_timestamp(payload.to_vec(), 27),
}))
} else {
Ok(None)
}
}
DecoderState::GlobalTimestamp1 { payload } => {
let last_byte = (b >> 7) & 1 == 0;
payload.push(b);
if last_byte {
Ok(Some(TracePacket::GlobalTimestamp1 {
ts: Decoder::extract_timestamp(payload.to_vec(), 25),
clkch: (payload.last().unwrap() & (1 << 5)) >> 5 == 1,
wrap: (payload.last().unwrap() & (1 << 6)) >> 6 == 1,
}))
} else {
Ok(None)
}
}
DecoderState::GlobalTimestamp2 { payload } => {
let last_byte = (b >> 7) & 1 == 0;
payload.push(b);
if last_byte {
Ok(Some(TracePacket::GlobalTimestamp2 {
ts: Decoder::extract_timestamp(
payload.to_vec(),
match payload.len() {
4 => 47 - 26,
6 => 63 - 26,
_ => {
return Err(DecoderError::InvalidGTS2Size {
payload: payload.to_vec(),
})
}
},
),
}))
} else {
Ok(None)
}
}
DecoderState::Instrumentation {
port,
payload,
expected_size,
} => {
payload.push(b);
if payload.len() == *expected_size {
Ok(Some(TracePacket::Instrumentation {
port: *port,
payload: payload.to_vec(),
}))
} else {
Ok(None)
}
}
};
if let Ok(Some(_)) = packet {
self.state = DecoderState::Header;
}
packet
}
fn extract_timestamp(payload: Vec<u8>, max_len: u64) -> u64 {
let (rtail, head) = payload.split_at(payload.len() - 1);
let mut ts: u64 = 0;
for (i, b) in rtail.iter().enumerate() {
ts |= ((b & !(1 << 7)) as u64)
<< (7 * i);
}
let shift = 7 - (max_len % 7);
let mask: u8 = 0xFFu8.wrapping_shl(shift.try_into().unwrap()) >> shift;
ts | (((head[0] & mask) as u64) << (7 * rtail.len()))
}
#[bitmatch]
fn handle_hardware_source(disc_id: u8, payload: Vec<u8>) -> Result<TracePacket, DecoderError> {
match disc_id {
0 => {
if payload.len() != 1 {
return Err(DecoderError::InvalidHardwarePacket { disc_id, payload });
}
let b = payload[0];
Ok(TracePacket::EventCounterWrap {
cyc: b & (1 << 5) != 0,
fold: b & (1 << 4) != 0,
lsu: b & (1 << 3) != 0,
sleep: b & (1 << 2) != 0,
exc: b & (1 << 1) != 0,
cpi: b & (1 << 0) != 0,
})
}
1 => {
if payload.len() != 2 {
return Err(DecoderError::InvalidHardwarePacket { disc_id, payload });
}
let exception_number = ((payload[1] as u16 & 1) << 8) | payload[0] as u16;
let function = (payload[1] >> 4) & 0b11;
return Ok(TracePacket::ExceptionTrace {
exception: match exception_number {
1 => ExceptionType::Reset,
2 => ExceptionType::Nmi,
3 => ExceptionType::HardFault,
4 => ExceptionType::MemManage,
5 => ExceptionType::BusFault,
6 => ExceptionType::UsageFault,
11 => ExceptionType::SVCall,
12 => ExceptionType::DebugMonitor,
14 => ExceptionType::PendSV,
15 => ExceptionType::SysTick,
n if n >= 16 => ExceptionType::ExternalInterrupt(n as usize - 16),
_ => {
return Err(DecoderError::InvalidExceptionTrace {
exception: exception_number,
function,
})
}
},
action: match function {
0b01 => ExceptionAction::Entered,
0b10 => ExceptionAction::Exited,
0b11 => ExceptionAction::Returned,
_ => {
return Err(DecoderError::InvalidExceptionTrace {
exception: exception_number,
function,
})
}
},
});
}
2 => {
match payload.len() {
1 if payload[0] == 0 => Ok(TracePacket::PCSample { pc: None }),
4 => Ok(TracePacket::PCSample {
pc: Some(u32::from_le_bytes(payload.try_into().unwrap())),
}),
_ => Err(DecoderError::InvalidPCSampleSize { payload }),
}
}
8..=23 => {
#[bitmatch]
let "???t_tccd" = disc_id;
let comparator = c;
match (t, d, payload.len()) {
(0b01, 0, 4) => {
Ok(TracePacket::DataTracePC {
comparator,
pc: u32::from_le_bytes(payload.try_into().unwrap()),
})
}
(0b01, 1, 2) => {
Ok(TracePacket::DataTraceAddress {
comparator,
data: payload,
})
}
(0b10, d, _) => {
Ok(TracePacket::DataTraceValue {
comparator,
access_type: if d == 0 {
MemoryAccessType::Write
} else {
MemoryAccessType::Read
},
value: payload,
})
}
_ => Err(DecoderError::InvalidHardwarePacket { disc_id, payload }),
}
}
_ => unreachable!(),
}
}
#[bitmatch]
fn decode_header(&mut self, header: u8) -> Result<Option<TracePacket>, DecoderError> {
fn translate_ss(ss: u8) -> Option<usize> {
Some(
match ss {
0b01 => 2,
0b10 => 3,
0b11 => 5,
_ => return None,
} - 1,
)
}
#[bitmatch]
match header {
"0000_0000" => {
self.state = DecoderState::Syncing(8);
}
"0111_0000" => {
return Ok(Some(TracePacket::Overflow));
}
"11rr_0000" => {
let tc = r;
self.state = DecoderState::LocalTimestamp {
data_relation: match tc {
0b00 => TimestampDataRelation::Sync,
0b01 => TimestampDataRelation::UnknownDelay,
0b10 => TimestampDataRelation::AssocEventDelay,
0b11 => TimestampDataRelation::UnknownAssocEventDelay,
_ => unreachable!(),
},
payload: vec![],
};
}
"0ttt_0000" => {
return Ok(Some(TracePacket::LocalTimestamp2 { ts: t }));
}
"1001_0100" => {
self.state = DecoderState::GlobalTimestamp1 { payload: vec![] };
}
"1011_0100" => {
self.state = DecoderState::GlobalTimestamp2 { payload: vec![] };
}
"0ppp_1000" => {
return Ok(Some(TracePacket::Extension { page: p }));
}
"aaaa_a0ss" => {
self.state = DecoderState::Instrumentation {
port: a,
payload: vec![],
expected_size: if let Some(s) = translate_ss(s) {
s
} else {
return Err(DecoderError::InvalidSourcePayload { header, size: s });
},
};
}
"aaaa_a1ss" => {
let disc_id = a;
if !(0..=2).contains(&disc_id) && !(8..=23).contains(&disc_id) {
return Err(DecoderError::InvalidHardwareDisc {
disc_id,
size: s.into(),
});
}
self.state = DecoderState::HardwareSource {
disc_id,
payload: vec![],
expected_size: if let Some(s) = translate_ss(s) {
s
} else {
return Err(DecoderError::InvalidSourcePayload { header, size: s });
},
};
}
"hhhh_hhhh" => return Err(DecoderError::InvalidHeader(h)),
}
Ok(None)
}
}
impl Default for Decoder {
fn default() -> Self {
Self::new()
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn decode_sync_packet() {
let mut trace_data: Vec<u8> = [0; 47 / 8].to_vec();
trace_data.push(1 << 7);
let mut decoder = Decoder::new();
decoder.feed(trace_data);
assert_eq!(decoder.pull(), Ok(Some(TracePacket::Sync)));
}
#[test]
fn decode_overflow_packet() {
let mut decoder = Decoder::new();
decoder.feed([0b0111_0000].to_vec());
assert_eq!(decoder.pull(), Ok(Some(TracePacket::Overflow)));
}
#[test]
fn decode_local_timestamp_packets() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b1100_0000,
0b1100_1001,
0b0000_0001,
0b0101_0000,
].to_vec());
for packet in [
TracePacket::LocalTimestamp1 {
ts: 0b11001001,
data_relation: TimestampDataRelation::Sync,
},
TracePacket::LocalTimestamp2 { ts: 0b101 },
]
.iter()
{
assert_eq!(decoder.pull(), Ok(Some(packet.clone())));
}
}
#[test]
fn extract_timestamp() {
#[rustfmt::skip]
let ts: Vec<u8> = [
0b1000_0000,
0b1000_0000,
0b1000_0000,
0b0000_0000,
].to_vec();
assert_eq!(Decoder::extract_timestamp(ts, 25), 0);
#[rustfmt::skip]
let ts: Vec<u8> = [
0b1000_0001,
0b1000_0111,
0b1001_1111,
0b0111_1111
].to_vec();
assert_eq!(
Decoder::extract_timestamp(ts, 27),
0b1111111_0011111_0000111_0000001,
);
#[rustfmt::skip]
let ts: Vec<u8> = [
0b1000_0001,
0b1000_0111,
0b1001_1111,
0b1111_1111
].to_vec();
assert_eq!(
Decoder::extract_timestamp(ts, 25),
0b11111_0011111_0000111_0000001,
);
}
#[test]
fn decode_global_timestamp_packets() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b1001_0100,
0b1000_0000,
0b1010_0000,
0b1000_0100,
0b0110_0000,
0b1011_0100,
0b1011_1101,
0b1111_0100,
0b1001_0001,
0b0000_0001,
0b1011_0100,
0b1011_1101,
0b1111_0100,
0b1001_0001,
0b1000_0001,
0b1111_0100,
0b0000_0111,
].to_vec());
for packet in [
TracePacket::GlobalTimestamp1 {
ts: 0b00000_0000100_0100000_0000000,
wrap: true,
clkch: true,
},
TracePacket::GlobalTimestamp2 {
ts: 0b1_0010001_1110100_0111101,
},
TracePacket::GlobalTimestamp2 {
ts: 0b111_1110100_0000001_0010001_1110100_0111101,
},
]
.iter()
{
assert_eq!(decoder.pull(), Ok(Some(packet.clone())));
}
}
#[test]
fn decode_extention_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0111_1000,
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::Extension { page: 0b111 }))
);
}
#[test]
fn decode_instrumentation_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b1000_1011,
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::Instrumentation {
port: 0b1000_1,
#[rustfmt::skip]
payload: [
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
].to_vec(),
}))
);
}
#[test]
fn decode_eventcounterwrap_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0000_0101,
0b0010_1010,
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::EventCounterWrap {
cyc: true,
fold: false,
lsu: true,
sleep: false,
exc: true,
cpi: false,
}))
);
}
#[test]
fn decode_exceptiontrace_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0000_1110,
0b0010_0000,
0b0011_0000
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::ExceptionTrace {
exception: ExceptionType::ExternalInterrupt(16),
action: ExceptionAction::Returned,
}))
);
}
#[test]
fn decode_pcsample_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0001_0111,
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
0b0001_0101,
0b0000_0000,
].to_vec());
for packet in [
TracePacket::PCSample {
pc: Some(0b11111111_00111111_00001111_00000011),
},
TracePacket::PCSample { pc: None },
]
.iter()
{
assert_eq!(decoder.pull(), Ok(Some(packet.clone())));
}
}
#[test]
fn decode_datatracepc_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0111_0111,
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::DataTracePC {
comparator: 0b11,
pc: 0b11111111_00111111_00001111_00000011,
}))
);
}
#[test]
fn decode_datatraceaddress_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b0110_1110,
0b0000_0011,
0b0000_1111,
].to_vec());
assert_eq!(
decoder.pull(),
Ok(Some(TracePacket::DataTraceAddress {
comparator: 0b10,
#[rustfmt::skip]
data: [
0b0000_0011,
0b0000_1111,
].to_vec(),
}))
);
}
#[test]
fn decode_datatracevalue_packet() {
let mut decoder = Decoder::new();
#[rustfmt::skip]
decoder.feed([
0b1010_1111,
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
0b1010_1110,
0b0000_0011,
0b0000_1111,
0b1010_1101,
0b0000_0011,
].to_vec());
for packet in [
TracePacket::DataTraceValue {
comparator: 0b10,
access_type: MemoryAccessType::Read,
#[rustfmt::skip]
value: [
0b0000_0011,
0b0000_1111,
0b0011_1111,
0b1111_1111,
].to_vec(),
},
TracePacket::DataTraceValue {
comparator: 0b10,
access_type: MemoryAccessType::Read,
#[rustfmt::skip]
value: [
0b0000_0011,
0b0000_1111,
].to_vec(),
},
TracePacket::DataTraceValue {
comparator: 0b10,
access_type: MemoryAccessType::Read,
#[rustfmt::skip]
value: [
0b0000_0011,
].to_vec(),
},
]
.iter()
{
assert_eq!(decoder.pull(), Ok(Some(packet.clone())));
}
}
}