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

autd3_firmware_emulator/cpu/
emulator.rs
