autd3_firmware_emulator/cpu/
emulator.rs

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

autd3_firmware_emulator/cpu/
emulator.rs
