autd3_firmware_emulator/cpu/
emulator.rs

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

autd3_firmware_emulator/cpu/
emulator.rs
