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

//! ITM stimulus ports functionality.

use core::fmt::{self, Write};
use core::slice;

const ADDRESS_BASE: usize = 0xE000_0000;

/// ITM stimulus port pointer.
pub struct Port(usize);

/// Types that can be transmitted through ITM stimulus port.
pub trait Transmit: Copy {
  /// Writes its value to provided address of a stimulus port register.
  ///
  /// It retries on buffer overflow.
  fn transmit(self, address: usize);
}

impl Port {
  /// Constructs a new `Port`.
  ///
  /// # Panics
  ///
  /// If `port` is greater or equals to `0x20`.
  pub fn new(port: usize) -> Port {
    assert!(port < 0x20);
    Port(ADDRESS_BASE + port)
  }

  /// Writes a buffer in most effective chunks, splitting it to 8- and 32-bit
  /// slices.
  pub fn write_stream(&self, buffer: &[u8]) {
    let mut end = buffer.len();
    if end < 4 {
      return self.write_all(buffer);
    }
    let mut start = buffer.as_ptr() as usize;
    let mut rem = start & 0b11;
    end += start;
    if rem != 0 {
      rem = 0b100 - rem;
      self.write_all(unsafe { slice::from_raw_parts(start as *const u8, rem) });
      start += rem;
    }
    rem = end & 0b11;
    end -= rem;
    self.write_all(unsafe {
      slice::from_raw_parts(start as *const u32, end - start >> 2)
    });
    self.write_all(unsafe { slice::from_raw_parts(end as *const u8, rem) });
  }

  /// Writes an entire buffer in chunks of `size_of::<T>() * 8` bits.
  pub fn write_all<T: Transmit>(&self, buffer: &[T]) {
    for item in buffer {
      self.write(*item);
    }
  }

  /// Writes a value into a port.
  pub fn write<T: Transmit>(&self, value: T) {
    value.transmit(self.0);
  }
}

impl Write for Port {
  fn write_str(&mut self, string: &str) -> fmt::Result {
    self.write_stream(string.as_bytes());
    Ok(())
  }
}

impl Transmit for u8 {
  fn transmit(self, address: usize) {
    unsafe {
      asm!("
        0:
          ldrexb r0, [$1]
          cmp r0, #0
          itt ne
          strexbne r0, $0, [$1]
          cmpne r0, #1
          beq 0b
      " :
        : "r"(self), "r"(address as *mut u8)
        : "r0", "cc"
        : "volatile");
    }
  }
}

impl Transmit for u16 {
  fn transmit(self, address: usize) {
    unsafe {
      asm!("
        0:
          ldrexh r0, [$1]
          cmp r0, #0
          itt ne
          strexhne r0, $0, [$1]
          cmpne r0, #1
          beq 0b
      " :
        : "r"(self), "r"(address as *mut u16)
        : "r0", "cc"
        : "volatile");
    }
  }
}

impl Transmit for u32 {
  fn transmit(self, address: usize) {
    unsafe {
      asm!("
        0:
          ldrex r0, [$1]
          cmp r0, #0
          itt ne
          strexne r0, $0, [$1]
          cmpne r0, #1
          beq 0b
      " :
        : "r"(self), "r"(address as *mut u32)
        : "r0", "cc"
        : "volatile");
    }
  }
}