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::now(),
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(
144            self.rx_data,
145            Ack::new()
146                .with_err(self.err)
147                .with_msg_id(self.last_msg_id.get() & 0x0F),
148        )
149    }
150
151    pub fn send(&mut self, tx: &[TxMessage]) {
152        if self.broken {
153            return;
154        }
155        self.ecat_recv(&tx[self.idx]);
156    }
157
158    pub fn init(&mut self) {
159        self.fpga.init();
160        unsafe {
161            _ = self.clear(&[]);
162        }
163    }
164
165    pub fn update(&mut self) {
166        self.update_with_sys_time(DcSysTime::now());
167    }
168
169    pub fn update_with_sys_time(&mut self, sys_time: DcSysTime) {
170        self.fpga.update_with_sys_time(sys_time);
171        self.read_fpga_state();
172        self.dc_sys_time = sys_time;
173    }
174
175    #[must_use]
176    pub const fn should_update(&self) -> bool {
177        self.reads_fpga_state
178    }
179
180    pub fn set_last_msg_id(&mut self, msg_id: MsgId) {
181        self.last_msg_id = msg_id;
182    }
183}
184
185impl CPUEmulator {
186    #[must_use]
187    pub(crate) const fn cast<T>(data: &[u8]) -> T {
188        unsafe { (data.as_ptr() as *const T).read_unaligned() }
189    }
190
191    #[must_use]
192    const fn get_addr(select: u8, addr: u16) -> u16 {
193        ((select as u16 & 0x0003) << 14) | (addr & 0x3FFF)
194    }
195
196    #[must_use]
197    pub(crate) fn bram_read(&self, select: u8, addr: u16) -> u16 {
198        let addr = Self::get_addr(select, addr);
199        self.fpga.read(addr)
200    }
201
202    pub(crate) fn bram_write(&mut self, select: u8, addr: u16, data: u16) {
203        let addr = Self::get_addr(select, addr);
204        self.fpga.write(addr, data)
205    }
206
207    pub(crate) fn bram_cpy(
208        &mut self,
209        select: u8,
210        addr_base: u16,
211        src: *const u16,
212        size: usize,
213    ) -> *const u16 {
214        let mut addr = Self::get_addr(select, addr_base);
215        let mut src = src;
216        (0..size).for_each(|_| unsafe {
217            self.fpga.write(addr, src.read());
218            addr = addr.wrapping_add(1);
219            src = src.add(1);
220        });
221        src
222    }
223
224    pub(crate) fn bram_set(&mut self, select: u8, addr_base: u16, value: u16, size: usize) {
225        let mut addr = Self::get_addr(select, addr_base);
226        (0..size).for_each(|_| {
227            self.fpga.write(addr, value);
228            addr += 1;
229        })
230    }
231
232    fn read_fpga_state(&mut self) {
233        if self.is_rx_data_used {
234            return;
235        }
236        if self.reads_fpga_state {
237            self.rx_data = FPGA_STATE_READS_FPGA_STATE_ENABLED
238                | self.bram_read(BRAM_SELECT_CONTROLLER, ADDR_FPGA_STATE) as u8;
239        } else {
240            self.rx_data &= !FPGA_STATE_READS_FPGA_STATE_ENABLED;
241        }
242    }
243
244    #[must_use]
245    fn handle_payload(&mut self, data: &[u8]) -> u8 {
246        unsafe {
247            match data[0] {
248                TAG_NOP => NO_ERR,
249                TAG_CLEAR => self.clear(data),
250                TAG_SYNC => self.synchronize(data),
251                TAG_FIRM_INFO => self.firm_info(data),
252                TAG_MODULATION => self.write_mod(data),
253                TAG_MODULATION_CHANGE_SEGMENT => self.change_mod_segment(data),
254                TAG_SILENCER => self.config_silencer(data),
255                TAG_GAIN => self.write_gain(data),
256                TAG_GAIN_CHANGE_SEGMENT => self.change_gain_segment(data),
257                TAG_GAIN_STM_CHANGE_SEGMENT => self.change_gain_stm_segment(data),
258                TAG_FOCI_STM => self.write_foci_stm(data),
259                TAG_FOCI_STM_CHANGE_SEGMENT => self.change_foci_stm_segment(data),
260                TAG_GAIN_STM => self.write_gain_stm(data),
261                TAG_FORCE_FAN => self.configure_force_fan(data),
262                TAG_READS_FPGA_STATE => self.configure_reads_fpga_state(data),
263                TAG_CONFIG_PULSE_WIDTH_ENCODER => self.config_pwe(data),
264                TAG_DEBUG => self.config_gpio_output(data),
265                TAG_EMULATE_GPIO_IN => self.emulate_gpio_in(data),
266                TAG_CPU_GPIO_OUT => self.cpu_gpio_out(data),
267                TAG_PHASE_CORRECTION => self.phase_corr(data),
268                TAG_OUTPUT_MASK => self.output_mask(data),
269                _ => ERR_NOT_SUPPORTED_TAG,
270            }
271        }
272    }
273
274    fn ecat_recv(&mut self, data: *const TxMessage) {
275        let data: &[u8] = unsafe { std::slice::from_raw_parts(data as _, EC_OUTPUT_FRAME_SIZE) };
276
277        let header = unsafe { &*(data.as_ptr() as *const Header) };
278
279        if self.last_msg_id == header.msg_id {
280            return;
281        }
282        self.last_msg_id = header.msg_id;
283
284        self.read_fpga_state();
285
286        if header.msg_id > MsgId::MAX {
287            self.err = ERR_INVALID_MSG_ID;
288            return;
289        }
290
291        self.err = self.handle_payload(&data[std::mem::size_of::<Header>()..]);
292        if self.err != NO_ERR {
293            return;
294        }
295
296        if header.slot_2_offset != 0 {
297            self.err = self.handle_payload(
298                &data[std::mem::size_of::<Header>() + header.slot_2_offset as usize..],
299            );
300            if self.err != NO_ERR {
301                return;
302            }
303        }
304
305        self.bram_write(
306            BRAM_SELECT_CONTROLLER,
307            ADDR_CTL_FLAG,
308            self.fpga_flags_internal,
309        );
310    }
311}
312
313#[cfg(test)]
314mod tests {
315    use super::*;
316
317    use rand::Rng;
318
319    #[test]
320    fn cpu_idx() {
321        let mut rng = rand::rng();
322        let idx: u16 = rng.random();
323        let cpu = CPUEmulator::new(idx as _, 249);
324        assert_eq!(idx as usize, cpu.idx());
325    }
326
327    #[test]
328    fn num_transducers() {
329        let cpu = CPUEmulator::new(0, 249);
330        assert_eq!(249, cpu.num_transducers());
331    }
332
333    #[test]
334    fn dc_sys_time() {
335        let mut cpu = CPUEmulator::new(0, 249);
336
337        let sys_time = DcSysTime::now() + std::time::Duration::from_nanos(1111);
338        cpu.update_with_sys_time(sys_time);
339        assert_eq!(sys_time, cpu.dc_sys_time());
340    }
341
342    #[test]
343    fn should_update() {
344        let mut cpu = CPUEmulator::new(0, 249);
345        assert!(!cpu.should_update());
346
347        cpu.reads_fpga_state = true;
348        assert!(cpu.should_update());
349    }
350}
autd3_firmware_emulator/cpu/emulator.rs

autd3_firmware_emulator/cpu/
emulator.rs
