autd3_firmware_emulator/cpu/operation/
gain.rs

1use crate::{CPUEmulator, cpu::params::*};
2
3#[repr(C, align(2))]
4struct Gain {
5    tag: u8,
6    segment: u8,
7    flag: u8,
8    __: u8,
9}
10
11#[repr(C, align(2))]
12struct GainUpdate {
13    tag: u8,
14    segment: u8,
15}
16
17impl CPUEmulator {
18    #[must_use]
19    pub(crate) unsafe fn write_gain(&mut self, data: &[u8]) -> u8 {
20        unsafe {
21            let d = Self::cast::<Gain>(data);
22
23            let segment = d.segment;
24            self.stm_segment = segment;
25
26            let data = std::slice::from_raw_parts(
27                data[std::mem::size_of::<Gain>()..].as_ptr() as *const u16,
28                self.num_transducers,
29            );
30
31            self.bram_write(
32                BRAM_SELECT_CONTROLLER,
33                ADDR_STM_FREQ_DIV0 + segment as u16,
34                0xFFFF,
35            );
36            self.bram_write(
37                BRAM_SELECT_CONTROLLER,
38                ADDR_STM_REP0 + segment as u16,
39                0xFFFF,
40            );
41            self.bram_write(BRAM_SELECT_CONTROLLER, ADDR_STM_CYCLE0 + segment as u16, 0);
42            self.bram_write(
43                BRAM_SELECT_CONTROLLER,
44                ADDR_STM_MODE0 + segment as u16,
45                STM_MODE_GAIN,
46            );
47
48            self.stm_cycle[segment as usize] = 1;
49            self.stm_rep[segment as usize] = 0xFFFF;
50            self.stm_freq_div[segment as usize] = 0xFFFF;
51
52            self.change_stm_wr_segment(segment as _);
53            self.change_stm_wr_page(0);
54            (0..self.num_transducers)
55                .for_each(|i| self.bram_write(BRAM_SELECT_STM, i as _, data[i]));
56
57            if (d.flag & GAIN_FLAG_UPDATE) == GAIN_FLAG_UPDATE {
58                self.bram_write(
59                    BRAM_SELECT_CONTROLLER,
60                    ADDR_STM_REQ_RD_SEGMENT,
61                    segment as _,
62                );
63                self.bram_write(
64                    BRAM_SELECT_CONTROLLER,
65                    ADDR_STM_TRANSITION_MODE,
66                    TRANSITION_MODE_SYNC_IDX as _,
67                );
68                self.set_and_wait_update(CTL_FLAG_STM_SET);
69            }
70
71            NO_ERR
72        }
73    }
74
75    #[must_use]
76    pub(crate) unsafe fn change_gain_segment(&mut self, data: &[u8]) -> u8 {
77        let d = Self::cast::<GainUpdate>(data);
78
79        if self.stm_mode[d.segment as usize] != STM_MODE_GAIN
80            || self.stm_cycle[d.segment as usize] != 1
81        {
82            return ERR_INVALID_SEGMENT_TRANSITION;
83        }
84
85        self.stm_segment = d.segment;
86
87        self.bram_write(
88            BRAM_SELECT_CONTROLLER,
89            ADDR_STM_REQ_RD_SEGMENT,
90            d.segment as _,
91        );
92        self.bram_write(
93            BRAM_SELECT_CONTROLLER,
94            ADDR_STM_TRANSITION_MODE,
95            TRANSITION_MODE_SYNC_IDX as _,
96        );
97        self.set_and_wait_update(CTL_FLAG_STM_SET);
98
99        NO_ERR
100    }
101}
102
103#[cfg(test)]
104mod tests {
105    use super::*;
106
107    #[test]
108    fn mem_layout() {
109        assert_eq!(4, std::mem::size_of::<Gain>());
110        assert_eq!(0, std::mem::offset_of!(Gain, tag));
111        assert_eq!(1, std::mem::offset_of!(Gain, segment));
112        assert_eq!(2, std::mem::offset_of!(Gain, flag));
113
114        assert_eq!(2, std::mem::size_of::<GainUpdate>());
115        assert_eq!(0, std::mem::offset_of!(GainUpdate, tag));
116        assert_eq!(1, std::mem::offset_of!(GainUpdate, segment));
117    }
118}
autd3_firmware_emulator/cpu/operation/gain.rs

autd3_firmware_emulator/cpu/operation/
gain.rs
