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
use core::{fmt, ptr, slice};
use peripheral::Stim;
fn round_up_to_multiple_of(x: usize, k: usize) -> usize {
let rem = x % k;
if rem == 0 { x } else { x + k - rem }
}
fn round_down_to_multiple_of(x: usize, k: usize) -> usize {
x - (x % k)
}
unsafe fn split(buffer: &[u8]) -> (&[u8], &[u32], &[u8]) {
let start = buffer.as_ptr();
let end = start.offset(buffer.len() as isize);
let sbody = round_up_to_multiple_of(start as usize, 4);
let ebody = round_down_to_multiple_of(end as usize, 4);
let head = slice::from_raw_parts(start, sbody - start as usize);
let body = slice::from_raw_parts(sbody as *const _, (ebody - sbody) >> 2);
let tail = slice::from_raw_parts(ebody as *const _, end as usize - ebody);
(head, body, tail)
}
fn write_bytes(stim: &Stim, bytes: &[u8]) {
for byte in bytes {
while !stim.is_fifo_ready() {}
stim.write_u8(*byte);
}
}
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(())
}
}
pub fn write_all(port: &Stim, buffer: &[u8]) {
if buffer.len() < 7 {
write_bytes(port, buffer);
} else {
let (head, body, tail) = unsafe { split(buffer) };
write_bytes(port, head);
unsafe { write_words(port, body) }
write_bytes(port, tail);
}
}
pub fn write_fmt(port: &Stim, args: fmt::Arguments) {
use core::fmt::Write;
Port(port).write_fmt(args).ok();
}
pub fn write_str(port: &Stim, string: &str) {
write_all(port, string.as_bytes())
}