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
133
134
135
136
137
138
139
140
141
|
use bedrock_core::*;
pub struct MathDevice {
pub op1: u16,
pub op2: u16,
pub sqrt: Option<u16>,
pub atan: Option<u16>,
pub prod: Option<(u16, u16)>, // (low, high)
pub quot: Option<u16>,
pub rem: Option<u16>,
}
impl MathDevice {
pub fn new() -> Self {
Self {
op1: 0,
op2: 0,
sqrt: None,
atan: None,
prod: None,
quot: None,
rem: None,
}
}
pub fn clear(&mut self) {
self.sqrt = None;
self.atan = None;
self.prod = None;
self.quot = None;
self.rem = None;
}
pub fn atan(&mut self) -> u16 {
match self.atan {
Some(atan) => atan,
None => {
let x = self.op1 as i16 as f64;
let y = self.op2 as i16 as f64;
const SCALE: f64 = 10430.378350470453; // PI * 32768
self.atan = Some((f64::atan2(x, y) * SCALE) as i16 as u16);
self.atan.unwrap()
}
}
}
pub fn sqrt(&mut self) -> u16 {
match self.sqrt {
Some(sqrt) => sqrt,
None => {
let input = ((self.op1 as u32) << 16) | (self.op2 as u32);
self.sqrt = Some((input as f64).sqrt() as u16);
self.sqrt.unwrap()
}
}
}
pub fn prod(&mut self) -> (u16, u16) {
match self.prod {
Some(prod) => prod,
None => {
self.prod = Some(self.op1.widening_mul(self.op2));
self.prod.unwrap()
}
}
}
pub fn quot(&mut self) -> u16 {
match self.quot {
Some(quot) => quot,
None => {
self.quot = Some(self.op1.checked_div(self.op2).unwrap_or(0));
self.quot.unwrap()
}
}
}
pub fn rem(&mut self) -> u16 {
match self.rem {
Some(rem) => rem,
None => {
self.rem = Some(self.op1.checked_rem(self.op2).unwrap_or(0));
self.rem.unwrap()
}
}
}
}
impl Device for MathDevice {
fn read(&mut self, port: u8) -> u8 {
match port {
0x0 => read_h!(self.op1),
0x1 => read_l!(self.op1),
0x2 => read_h!(self.op2),
0x3 => read_l!(self.op2),
0x4 => read_h!(self.sqrt()),
0x5 => read_l!(self.sqrt()),
0x6 => read_h!(self.atan()),
0x7 => read_l!(self.atan()),
0x8 => read_h!(self.prod().1),
0x9 => read_l!(self.prod().1),
0xa => read_h!(self.prod().0),
0xb => read_l!(self.prod().0),
0xc => read_h!(self.quot()),
0xd => read_l!(self.quot()),
0xe => read_h!(self.rem()),
0xf => read_l!(self.rem()),
_ => unreachable!(),
}
}
fn write(&mut self, port: u8, value: u8) -> Option<Signal> {
match port {
0x0 => { write_h!(self.op1, value); self.clear(); },
0x1 => { write_l!(self.op1, value); self.clear(); },
0x2 => { write_h!(self.op2, value); self.clear(); },
0x3 => { write_l!(self.op2, value); self.clear(); },
0x4 => (),
0x5 => (),
0x6 => (),
0x7 => (),
0x8 => (),
0x9 => (),
0xa => (),
0xb => (),
0xc => (),
0xd => (),
0xe => (),
0xf => (),
_ => unreachable!(),
};
return None;
}
fn wake(&mut self) -> bool {
false
}
}
|
