autd3_firmware_emulator/cpu/
emulator.rs

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

autd3_firmware_emulator/cpu/
emulator.rs
