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
|
//! Instrumentation Trace Macrocell
use core::{fmt, mem, ptr, slice};
use aligned::Aligned;
use peripheral::Stim;
// NOTE assumes that `bytes` is 32-bit aligned
unsafe fn write_words(stim: &Stim, bytes: &[u32]) {
let mut p = bytes.as_ptr();
for _ in 0..bytes.len() {
while !stim.is_fifo_ready() {}
stim.write_u32(ptr::read(p));
p = p.offset(1);
}
}
struct Port<'p>(&'p Stim);
impl<'p> fmt::Write for Port<'p> {
fn write_str(&mut self, s: &str) -> fmt::Result {
write_all(self.0, s.as_bytes());
Ok(())
}
}
/// Writes a `buffer` to the ITM `port`
pub fn write_all(port: &Stim, buffer: &[u8]) {
unsafe {
let mut len = buffer.len();
let mut ptr = buffer.as_ptr();
if len == 0 {
return;
}
// 0x01 OR 0x03
if ptr as usize % 2 == 1 {
while !port.is_fifo_ready() {}
port.write_u8(*ptr);
// 0x02 OR 0x04
ptr = ptr.offset(1);
len -= 1;
}
// 0x02
if ptr as usize % 4 == 2 {
if len > 1 {
// at least 2 bytes
while !port.is_fifo_ready() {}
port.write_u16(ptr::read(ptr as *const u16));
// 0x04
ptr = ptr.offset(2);
len -= 2;
} else {
if len == 1 {
// last byte
while !port.is_fifo_ready() {}
port.write_u8(*ptr);
}
return;
}
}
write_aligned(port, mem::transmute(slice::from_raw_parts(ptr, len)));
}
}
/// Writes a 4-byte aligned `buffer` to the ITM `port`
///
/// # Examples
///
/// ``` ignore
/// let mut buffer: Aligned<u32, _> = Aligned([0; 14]);
///
/// buffer.copy_from_slice(b"Hello, world!\n");
///
/// itm::write_aligned(&itm.stim[0], &buffer);
///
/// // Or equivalently
/// itm::write_aligned(&itm.stim[0], &Aligned(*b"Hello, world!\n"));
/// ```
pub fn write_aligned(port: &Stim, buffer: &Aligned<u32, [u8]>) {
unsafe {
let len = buffer.len();
if len == 0 {
return;
}
let split = len & !0b11;
write_words(
port,
slice::from_raw_parts(buffer.as_ptr() as *const u32, split >> 2),
);
// 3 bytes or less left
let mut left = len & 0b11;
let mut ptr = buffer.as_ptr().offset(split as isize);
// at least 2 bytes left
if left > 1 {
while !port.is_fifo_ready() {}
port.write_u16(ptr::read(ptr as *const u16));
ptr = ptr.offset(2);
left -= 2;
}
// final byte
if left == 1 {
while !port.is_fifo_ready() {}
port.write_u8(*ptr);
}
}
}
/// Writes `fmt::Arguments` to the ITM `port`
pub fn write_fmt(port: &Stim, args: fmt::Arguments) {
use core::fmt::Write;
Port(port).write_fmt(args).ok();
}
/// Writes a string to the ITM `port`
pub fn write_str(port: &Stim, string: &str) {
write_all(port, string.as_bytes())
}
|
