autd3_firmware_emulator/cpu/
emulator.rs1use autd3_driver::{
2 ethercat::{DcSysTime, EC_OUTPUT_FRAME_SIZE},
3 firmware::cpu::{Header, RxMessage, TxMessage},
4};
5
6use getset::{CopyGetters, Getters, MutGetters};
7
8use crate::fpga::emulator::FPGAEmulator;
9
10use super::params::*;
11
12#[derive(CopyGetters, Getters, MutGetters)]
13pub struct CPUEmulator {
14 #[getset(get_copy = "pub")]
15 pub(crate) idx: usize,
16 pub(crate) ack: u8,
17 pub(crate) last_msg_id: u8,
18 pub(crate) rx_data: u8,
19 #[getset(get_copy = "pub")]
20 pub(crate) reads_fpga_state: bool,
21 pub(crate) reads_fpga_state_store: bool,
22 pub(crate) mod_cycle: u16,
23 pub(crate) stm_cycle: [u16; 2],
24 pub(crate) stm_mode: [u16; 2],
25 pub(crate) stm_rep: [u16; 2],
26 pub(crate) stm_freq_div: [u16; 2],
27 pub(crate) stm_segment: u8,
28 pub(crate) stm_transition_mode: u8,
29 pub(crate) stm_transition_value: u64,
30 pub(crate) num_foci: u8,
31 pub(crate) mod_freq_div: [u16; 2],
32 pub(crate) mod_segment: u8,
33 pub(crate) mod_rep: [u16; 2],
34 pub(crate) mod_transition_mode: u8,
35 pub(crate) mod_transition_value: u64,
36 pub(crate) gain_stm_mode: u8,
37 #[getset(get = "pub", get_mut = "pub")]
38 pub(crate) fpga: FPGAEmulator,
39 #[getset(get_copy = "pub")]
40 pub(crate) synchronized: bool,
41 #[getset(get_copy = "pub")]
42 pub(crate) num_transducers: usize,
43 pub(crate) fpga_flags_internal: u16,
44 #[getset(get_copy = "pub")]
45 pub(crate) silencer_strict_mode: bool,
46 pub(crate) min_freq_div_intensity: u16,
47 pub(crate) min_freq_div_phase: u16,
48 #[cfg(feature = "dynamic_freq")]
49 pub(crate) clk_write: u16,
50 pub(crate) is_rx_data_used: bool,
51 #[getset(get_copy = "pub")]
52 pub(crate) dc_sys_time: DcSysTime,
53 #[getset(get_copy = "pub")]
54 pub(crate) port_a_podr: u8,
55}
56
57impl CPUEmulator {
58 #[must_use]
59 pub fn new(id: usize, num_transducers: usize) -> Self {
60 let mut s = Self {
61 idx: id,
62 ack: 0x00,
63 last_msg_id: 0xFF,
64 rx_data: 0x00,
65 reads_fpga_state: false,
66 reads_fpga_state_store: false,
67 mod_cycle: 0,
68 stm_cycle: [1, 1],
69 stm_mode: [STM_MODE_GAIN, STM_MODE_GAIN],
70 gain_stm_mode: 0,
71 stm_transition_mode: TRANSITION_MODE_SYNC_IDX,
72 stm_transition_value: 0,
73 num_foci: 1,
74 mod_transition_mode: TRANSITION_MODE_SYNC_IDX,
75 mod_transition_value: 0,
76 fpga: FPGAEmulator::new(num_transducers),
77 synchronized: false,
78 num_transducers,
79 fpga_flags_internal: 0x0000,
80 mod_freq_div: [10, 10],
81 mod_segment: 0,
82 stm_freq_div: [0xFFFF, 0xFFFF],
83 stm_segment: 0,
84 silencer_strict_mode: true,
85 min_freq_div_intensity: 10,
86 min_freq_div_phase: 40,
87 #[cfg(feature = "dynamic_freq")]
88 clk_write: 0,
89 is_rx_data_used: false,
90 dc_sys_time: DcSysTime::now(),
91 stm_rep: [0xFFFF, 0xFFFF],
92 mod_rep: [0xFFFF, 0xFFFF],
93 port_a_podr: 0x00,
94 };
95 s.init();
96 s
97 }
98
99 #[must_use]
100 pub const fn rx(&self) -> RxMessage {
101 RxMessage::new(self.rx_data, self.ack)
102 }
103
104 pub fn send(&mut self, tx: &[TxMessage]) {
105 self.ecat_recv(&tx[self.idx]);
106 }
107
108 pub fn init(&mut self) {
109 self.fpga.init();
110 unsafe {
111 _ = self.clear(&[]);
112 }
113 }
114
115 pub fn update(&mut self) {
116 self.update_with_sys_time(DcSysTime::now());
117 }
118
119 pub fn update_with_sys_time(&mut self, sys_time: DcSysTime) {
120 self.fpga.update_with_sys_time(sys_time);
121 self.read_fpga_state();
122 self.dc_sys_time = sys_time;
123 }
124
125 #[must_use]
126 pub const fn should_update(&self) -> bool {
127 self.reads_fpga_state
128 }
129
130 pub fn set_last_msg_id(&mut self, msg_id: u8) {
131 self.last_msg_id = msg_id;
132 self.ack = msg_id;
133 }
134}
135
136impl CPUEmulator {
137 #[must_use]
138 pub(crate) const fn cast<T>(data: &[u8]) -> T {
139 unsafe { (data.as_ptr() as *const T).read_unaligned() }
140 }
141
142 #[must_use]
143 const fn get_addr(select: u8, addr: u16) -> u16 {
144 ((select as u16 & 0x0003) << 14) | (addr & 0x3FFF)
145 }
146
147 #[must_use]
148 pub(crate) fn bram_read(&self, select: u8, addr: u16) -> u16 {
149 let addr = Self::get_addr(select, addr);
150 self.fpga.read(addr)
151 }
152
153 pub(crate) fn bram_write(&mut self, select: u8, addr: u16, data: u16) {
154 let addr = Self::get_addr(select, addr);
155 self.fpga.write(addr, data)
156 }
157
158 pub(crate) fn bram_cpy(&mut self, select: u8, addr_base: u16, data: *const u16, size: usize) {
159 let mut addr = Self::get_addr(select, addr_base);
160 let mut src = data;
161 (0..size).for_each(|_| unsafe {
162 self.fpga.write(addr, src.read());
163 addr += 1;
164 src = src.add(1);
165 })
166 }
167
168 pub(crate) fn bram_set(&mut self, select: u8, addr_base: u16, value: u16, size: usize) {
169 let mut addr = Self::get_addr(select, addr_base);
170 (0..size).for_each(|_| {
171 self.fpga.write(addr, value);
172 addr += 1;
173 })
174 }
175
176 fn read_fpga_state(&mut self) {
177 if self.is_rx_data_used {
178 return;
179 }
180 if self.reads_fpga_state {
181 self.rx_data = FPGA_STATE_READS_FPGA_STATE_ENABLED
182 | self.bram_read(BRAM_SELECT_CONTROLLER, ADDR_FPGA_STATE) as u8;
183 } else {
184 self.rx_data &= !FPGA_STATE_READS_FPGA_STATE_ENABLED;
185 }
186 }
187
188 #[must_use]
189 fn handle_payload(&mut self, data: &[u8]) -> u8 {
190 unsafe {
191 match data[0] {
192 TAG_CLEAR => self.clear(data),
193 TAG_SYNC => self.synchronize(data),
194 TAG_FIRM_INFO => self.firm_info(data),
195 #[cfg(feature = "dynamic_freq")]
196 TAG_CONFIG_FPGA_CLK => self.configure_clk(data),
197 TAG_MODULATION => self.write_mod(data),
198 TAG_MODULATION_CHANGE_SEGMENT => self.change_mod_segment(data),
199 TAG_SILENCER => self.config_silencer(data),
200 TAG_GAIN => self.write_gain(data),
201 TAG_GAIN_CHANGE_SEGMENT => self.change_gain_segment(data),
202 TAG_GAIN_STM_CHANGE_SEGMENT => self.change_gain_stm_segment(data),
203 TAG_FOCI_STM => self.write_foci_stm(data),
204 TAG_FOCI_STM_CHANGE_SEGMENT => self.change_foci_stm_segment(data),
205 TAG_GAIN_STM => self.write_gain_stm(data),
206 TAG_FORCE_FAN => self.configure_force_fan(data),
207 TAG_READS_FPGA_STATE => self.configure_reads_fpga_state(data),
208 TAG_CONFIG_PULSE_WIDTH_ENCODER => self.config_pwe(data),
209 TAG_DEBUG => self.config_debug(data),
210 TAG_EMULATE_GPIO_IN => self.emulate_gpio_in(data),
211 TAG_CPU_GPIO_OUT => self.cpu_gpio_out(data),
212 TAG_PHASE_CORRECTION => self.phase_corr(data),
213 _ => ERR_NOT_SUPPORTED_TAG,
214 }
215 }
216 }
217
218 fn ecat_recv(&mut self, data: *const TxMessage) {
219 let data: &[u8] = unsafe { std::slice::from_raw_parts(data as _, EC_OUTPUT_FRAME_SIZE) };
220
221 let header = unsafe { &*(data.as_ptr() as *const Header) };
222
223 if self.last_msg_id == header.msg_id {
224 return;
225 }
226 self.last_msg_id = header.msg_id;
227
228 self.read_fpga_state();
229
230 if (header.msg_id & 0x80) != 0 {
231 self.ack = ERR_INVALID_MSG_ID;
232 return;
233 }
234
235 self.ack = self.handle_payload(&data[std::mem::size_of::<Header>()..]);
236 if (self.ack & ERR_BIT) != 0 {
237 return;
238 }
239
240 if header.slot_2_offset != 0 {
241 self.ack = self.handle_payload(
242 &data[std::mem::size_of::<Header>() + header.slot_2_offset as usize..],
243 );
244 if (self.ack & ERR_BIT) != 0 {
245 return;
246 }
247 }
248
249 self.bram_write(
250 BRAM_SELECT_CONTROLLER,
251 ADDR_CTL_FLAG,
252 self.fpga_flags_internal,
253 );
254
255 self.ack = header.msg_id;
256 }
257}
258
259#[cfg(test)]
260mod tests {
261 use super::*;
262
263 use rand::Rng;
264
265 #[test]
266 fn cpu_idx() {
267 let mut rng = rand::rng();
268 let idx: u16 = rng.random();
269 let cpu = CPUEmulator::new(idx as _, 249);
270 assert_eq!(idx as usize, cpu.idx());
271 }
272
273 #[test]
274 fn num_transducers() {
275 let cpu = CPUEmulator::new(0, 249);
276 assert_eq!(249, cpu.num_transducers());
277 }
278
279 #[test]
280 fn dc_sys_time() {
281 let mut cpu = CPUEmulator::new(0, 249);
282
283 let sys_time = DcSysTime::now() + std::time::Duration::from_nanos(1111);
284 cpu.update_with_sys_time(sys_time);
285 assert_eq!(sys_time, cpu.dc_sys_time());
286 }
287
288 #[test]
289 fn should_update() {
290 let mut cpu = CPUEmulator::new(0, 249);
291 assert!(!cpu.should_update());
292
293 cpu.reads_fpga_state = true;
294 assert!(cpu.should_update());
295 }
296}