1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152

//! Creates a named pipe and listens for ITM stream.

use clap::{App, Arg, ArgMatches, SubCommand};
use colored::*;
use libc;
use std::ffi::CString;
use std::fs::{self, File};
use std::io::{self, Read, Write};
use std::os::unix::ffi::OsStringExt;
use std::path::PathBuf;
use std::thread;
use std::time::Duration;

#[doc(hidden)]
pub const DEBUG_HELP: &str = "Debug ITM packets";

enum Error {
  Overflow,
  Io(io::Error),
}

impl From<io::Error> for Error {
  fn from(err: io::Error) -> Error {
    Error::Io(err)
  }
}

#[doc(hidden)]
pub fn command<'a, 'b>() -> App<'a, 'b> {
  SubCommand::with_name("itmdump")
    .arg(
      Arg::with_name("PATH")
        .help("Named pipe to use")
        .required(true),
    )
    .arg(
      Arg::with_name("debug")
        .short("d")
        .long("debug")
        .help(DEBUG_HELP),
    )
    .about("Creates a named pipe and listens for ITM stream")
}

#[doc(hidden)]
pub fn invoke(matches: &ArgMatches) {
  let path = matches.value_of("PATH").unwrap();
  create_pipe(path);
  run_loop(path, matches.is_present("debug"));
}

/// Creates a new named pipe with given `name`.
pub fn create_pipe(name: &str) {
  let pipe = PathBuf::from(name);
  if pipe.exists() {
    fs::remove_file(&pipe).unwrap();
  }
  unsafe {
    let cname = CString::new(pipe.clone().into_os_string().into_vec()).unwrap();
    let result = libc::mkfifo(cname.as_ptr(), 0o644);
    assert_eq!(result, 0);
  }
}

/// Opens a pipe, and runs a read loop.
pub fn run_loop(pipe: &str, debug: bool) {
  let mut stream = io::BufReader::new(File::open(pipe).unwrap());
  let mut buffer = [0; 4];
  loop {
    match read(&mut stream, &mut buffer, debug) {
      Ok(Some(payload)) => if debug {
        for byte in payload {
          print!("{:?} ", *byte as char);
        }
        println!();
      } else {
        io::stdout().write_all(payload).unwrap();
        io::stdout().flush().unwrap();
      },
      Ok(None) => if debug {
        println!("{}", "[IGN]".yellow());
      },
      Err(Error::Overflow) => {
        writeln!(io::stderr(), "{}", "[OFLW]".bold().white().on_red()).unwrap();
      }
      Err(Error::Io(ref err)) if err.kind() == io::ErrorKind::UnexpectedEof => {
        writeln!(io::stderr(), "{}", "[EOF]".red()).unwrap();
        thread::sleep(Duration::from_millis(100));
      }
      Err(Error::Io(ref err)) => {
        let message = format!("[IO: {:?}]", err.kind());
        writeln!(io::stderr(), "{}", message.red()).unwrap();
      }
    }
  }
}

/// Reads a packet from ITM stream.
fn read<'a, 'b>(
  stream: &'a mut io::BufReader<File>,
  buffer: &'b mut [u8],
  debug: bool,
) -> Result<Option<&'b [u8]>, Error> {
  stream.read_exact(&mut buffer[0..1])?;
  let header = buffer[0];
  if debug {
    debug_print_byte(header);
  }
  if header == 0 {
    // Synchronization
    return Ok(None);
  }
  let size = header & 0b11;
  if size == 0 {
    if header == 0b0111_0000 {
      // Protocol overflow
      Err(Error::Overflow)
    } else {
      // Protocol other
      Ok(None)
    }
  } else if header & 0b100 == 0 {
    // Instrumentation source
    let port = header >> 3;
    if port != 0 {
      return Ok(None);
    }
    let payload = match size {
      0b01 => &mut buffer[0..1],
      0b10 => &mut buffer[0..2],
      0b11 => &mut buffer[0..4],
      _ => unreachable!(),
    };
    if debug {
      for byte in payload.chunks_mut(1) {
        stream.read_exact(byte)?;
        debug_print_byte(byte[0]);
      }
    } else {
      stream.read_exact(payload)?;
    }
    Ok(Some(payload))
  } else {
    // Hardware source
    Ok(None)
  }
}

fn debug_print_byte(byte: u8) {
  print!("{:08b} ", byte);
  io::stdout().flush().unwrap();
}