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
|
use pio::{Program, SideSet, Wrap};
pub struct CodeIterator<'a, I>
where
I: Iterator<Item = &'a u16>,
{
iter: I,
offset: u8,
}
impl<'a, I: Iterator<Item = &'a u16>> CodeIterator<'a, I> {
pub fn new(iter: I, offset: u8) -> CodeIterator<'a, I> {
CodeIterator { iter, offset }
}
}
impl<'a, I> Iterator for CodeIterator<'a, I>
where
I: Iterator<Item = &'a u16>,
{
type Item = u16;
fn next(&mut self) -> Option<Self::Item> {
self.iter.next().map(|&instr| {
if instr & 0b1110_0000_0000_0000 == 0 {
// this is a JMP instruction -> add offset to address
let address = (instr & 0b1_1111) as u8;
let address = address.wrapping_add(self.offset) % 32;
instr & (!0b11111) | address as u16
} else {
instr
}
})
}
}
pub struct RelocatedProgram<'a, const PROGRAM_SIZE: usize> {
program: &'a Program<PROGRAM_SIZE>,
origin: u8,
}
impl<'a, const PROGRAM_SIZE: usize> RelocatedProgram<'a, PROGRAM_SIZE> {
pub fn new_with_origin(program: &Program<PROGRAM_SIZE>, origin: u8) -> RelocatedProgram<'_, PROGRAM_SIZE> {
RelocatedProgram { program, origin }
}
pub fn code(&'a self) -> CodeIterator<'a, core::slice::Iter<'a, u16>> {
CodeIterator::new(self.program.code.iter(), self.origin)
}
pub fn wrap(&self) -> Wrap {
let wrap = self.program.wrap;
let origin = self.origin;
Wrap {
source: wrap.source.wrapping_add(origin) % 32,
target: wrap.target.wrapping_add(origin) % 32,
}
}
pub fn side_set(&self) -> SideSet {
self.program.side_set
}
pub fn origin(&self) -> u8 {
self.origin
}
}
|
