motors/print/itm_read.py

#!/usr/bin/python3

# This is a program to parse output from the ITM and DWT.
# The "Debug ITM and DWT Packet Protocol" section of the ARMv7-M Architecture
# Reference Manual is a good reference.
#
# This seems like it might be a poster child for using coroutines, but those
# look scary so we're going to stick with generators.

import io
import os
import sys

def open_file_for_bytes(path):
  '''Returns a file-like object which reads bytes without buffering.'''
  # Not using `open` because it's unclear from the docs how (if it's possible at
  # all) to get something that will only do one read call and return what that
  # gets on a fifo.
  try:
    return io.FileIO(path, 'r')
  except FileNotFoundError:
    # If it wasn't found, try (once) to create it and then open again.
    try:
      os.mkfifo(path)
    except FileExistsError:
      pass
    return io.FileIO(path, 'r')

def read_bytes(path):
  '''Reads bytes from a file. This is appropriate both for regular files and
  fifos.
  Args:
    path: A path-like object to open.
  Yields:
    Individual bytes from the file, until hitting EOF.
  '''
  with open_file_for_bytes(path) as f:
    while True:
      buf = f.read(1024)
      if not buf:
        return
      for byte in buf:
        yield byte

def parse_packets(source):
  '''Parses a stream of bytes into packets.
  Args:
    source: A generator of individual bytes.
  Generates:
    Packets as bytes objects.
  '''
  try:
    while True:
      header = next(source)
      if header == 0:
        # Synchronization packets consist of a bunch of 0 bits (not necessarily
        # a whole number of bytes), followed by a 128 byte. This is for hardware
        # to synchronize on, but we're not in a position to do that, so
        # presumably those should get filtered out before getting here?
        raise 'Not sure how to handle synchronization packets'
      packet = bytearray()
      packet.append(header)
      header_size = header & 3
      if header_size == 0:
        while packet[-1] & 128 and len(packet) < 7:
          packet.append(next(source))
      else:
        if header_size == 3:
          header_size = 4
        for _ in range(header_size):
          packet.append(next(source))
      yield bytes(packet)
  except StopIteration:
    return

class PacketParser(object):
  def __init__(self):
    self.stimulus_handlers = {}

  def register_stimulus_handler(self, port_number, handler):
    '''Registers a function to call on packets to the specified port.'''
    self.stimulus_handlers[port_number] = handler

  def process(self, path):
    for packet in parse_packets(read_bytes(path)):
      header = packet[0]
      header_size = header & 3
      if header_size == 0:
        # TODO(Brian): At least handle overflow packets here.
        pass
      else:
        port_number = header >> 3
        if port_number in self.stimulus_handlers:
          self.stimulus_handlers[port_number](packet[1:])
        else:
          print('Warning: unhandled stimulus port %d' % port_number,
                file=sys.stderr)
          self.stimulus_handlers[port_number] = lambda _: None

if __name__ == '__main__':
  parser = PacketParser()
  def print_byte(payload):
    sys.stdout.write(payload.decode('ascii'))
  parser.register_stimulus_handler(0, print_byte)

  for path in sys.argv[1:]:
    parser.process(path)