autd3_firmware_emulator/cpu/operation/
modulation.rs

1use crate::{CPUEmulator, cpu::params::*};
2
3pub const MOD_BUF_PAGE_SIZE_WIDTH: u16 = 15;
4pub const MOD_BUF_PAGE_SIZE: u16 = 1 << MOD_BUF_PAGE_SIZE_WIDTH;
5pub const MOD_BUF_PAGE_SIZE_MASK: u16 = MOD_BUF_PAGE_SIZE - 1;
6
7#[repr(C, align(2))]
8#[derive(Clone, Copy)]
9struct ModulationHead {
10    tag: u8,
11    flag: u8,
12    size: u8,
13    transition_mode: u8,
14    freq_div: u16,
15    rep: u16,
16    transition_value: u64,
17}
18
19#[repr(C, align(2))]
20#[derive(Clone, Copy)]
21struct ModulationSubseq {
22    tag: u8,
23    flag: u8,
24    size: u16,
25}
26
27#[repr(C, align(2))]
28union Modulation {
29    head: ModulationHead,
30    subseq: ModulationSubseq,
31}
32
33#[repr(C, align(2))]
34#[derive(Clone, Copy)]
35struct ModulationUpdate {
36    tag: u8,
37    segment: u8,
38    transition_mode: u8,
39    __: [u8; 5],
40    transition_value: u64,
41}
42
43impl CPUEmulator {
44    #[must_use]
45    pub(crate) unsafe fn mod_segment_update(&mut self, segment: u8, mode: u8, value: u64) -> u8 {
46        self.bram_write(
47            BRAM_SELECT_CONTROLLER,
48            ADDR_MOD_REQ_RD_SEGMENT,
49            segment as _,
50        );
51        if (mode == TRANSITION_MODE_SYS_TIME)
52            && (value < self.dc_sys_time.sys_time() + SYS_TIME_TRANSITION_MARGIN)
53        {
54            return ERR_MISS_TRANSITION_TIME;
55        }
56        self.bram_write(BRAM_SELECT_CONTROLLER, ADDR_MOD_TRANSITION_MODE, mode as _);
57        self.bram_cpy(
58            BRAM_SELECT_CONTROLLER,
59            ADDR_MOD_TRANSITION_VALUE_0,
60            &raw const value as _,
61            std::mem::size_of::<u64>() >> 1,
62        );
63        self.set_and_wait_update(CTL_FLAG_MOD_SET);
64        NO_ERR
65    }
66
67    pub(crate) unsafe fn change_mod_wr_segment(&mut self, segment: u16) {
68        self.bram_write(BRAM_SELECT_CONTROLLER, ADDR_MOD_MEM_WR_SEGMENT, segment);
69    }
70
71    pub(crate) unsafe fn change_mod_wr_page(&mut self, page: u16) {
72        self.bram_write(BRAM_SELECT_CONTROLLER, ADDR_MOD_MEM_WR_PAGE, page as _);
73    }
74
75    #[must_use]
76    pub(crate) unsafe fn write_mod(&mut self, data: &[u8]) -> u8 {
77        unsafe {
78            let d = Self::cast::<Modulation>(data);
79
80            let segment = if (d.head.flag & MODULATION_FLAG_SEGMENT) != 0 {
81                1
82            } else {
83                0
84            };
85
86            let write;
87            let data = if (d.subseq.flag & MODULATION_FLAG_BEGIN) == MODULATION_FLAG_BEGIN {
88                if Self::validate_transition_mode(
89                    self.mod_segment,
90                    segment,
91                    d.head.rep,
92                    d.head.transition_mode,
93                ) {
94                    return ERR_INVALID_TRANSITION_MODE;
95                }
96
97                if self.validate_silencer_settings(
98                    self.stm_freq_div[self.stm_segment as usize],
99                    d.head.freq_div,
100                ) {
101                    return ERR_INVALID_SILENCER_SETTING;
102                }
103
104                if d.head.transition_mode != TRANSITION_MODE_NONE {
105                    self.mod_segment = segment;
106                }
107                self.mod_cycle = 0;
108                self.mod_transition_mode = d.head.transition_mode;
109                self.mod_transition_value = d.head.transition_value;
110                self.mod_freq_div[segment as usize] = d.head.freq_div;
111                self.mod_rep[segment as usize] = d.head.rep;
112
113                self.bram_write(
114                    BRAM_SELECT_CONTROLLER,
115                    ADDR_MOD_FREQ_DIV0 + segment as u16,
116                    d.head.freq_div,
117                );
118                self.bram_write(
119                    BRAM_SELECT_CONTROLLER,
120                    ADDR_MOD_REP0 + segment as u16,
121                    d.head.rep,
122                );
123
124                self.change_mod_wr_segment(segment as _);
125                self.change_mod_wr_page(0);
126
127                write = d.head.size as u16;
128                data[std::mem::size_of::<ModulationHead>()..].as_ptr() as *const u16
129            } else {
130                write = d.subseq.size;
131                data[std::mem::size_of::<ModulationSubseq>()..].as_ptr() as *const u16
132            };
133
134            let page_capacity =
135                MOD_BUF_PAGE_SIZE - ((self.mod_cycle as u16) & MOD_BUF_PAGE_SIZE_MASK);
136            if write < page_capacity {
137                self.bram_cpy(
138                    BRAM_SELECT_MOD,
139                    (self.mod_cycle as u16 & MOD_BUF_PAGE_SIZE_MASK) >> 1,
140                    data,
141                    ((write + 1) >> 1) as usize,
142                );
143                self.mod_cycle += write as u32;
144            } else {
145                self.bram_cpy(
146                    BRAM_SELECT_MOD,
147                    (self.mod_cycle as u16 & MOD_BUF_PAGE_SIZE_MASK) >> 1,
148                    data,
149                    (page_capacity >> 1) as usize,
150                );
151                self.mod_cycle += page_capacity as u32;
152
153                self.change_mod_wr_page(
154                    (((self.mod_cycle as u16) & !MOD_BUF_PAGE_SIZE_MASK) >> MOD_BUF_PAGE_SIZE_WIDTH)
155                        as _,
156                );
157
158                self.bram_cpy(
159                    BRAM_SELECT_MOD,
160                    0,
161                    data.add((page_capacity >> 1) as _),
162                    ((write - page_capacity + 1) >> 1) as _,
163                );
164                self.mod_cycle += (write - page_capacity) as u32;
165            }
166
167            if (d.subseq.flag & MODULATION_FLAG_END) == MODULATION_FLAG_END {
168                self.bram_write(
169                    BRAM_SELECT_CONTROLLER,
170                    ADDR_MOD_CYCLE0 + segment as u16,
171                    (self.mod_cycle.max(1) - 1) as _,
172                );
173
174                if (d.subseq.flag & MODULATION_FLAG_UPDATE) == MODULATION_FLAG_UPDATE {
175                    return self.mod_segment_update(
176                        segment,
177                        self.mod_transition_mode,
178                        self.mod_transition_value,
179                    );
180                }
181            }
182
183            NO_ERR
184        }
185    }
186
187    #[must_use]
188    pub(crate) unsafe fn change_mod_segment(&mut self, data: &[u8]) -> u8 {
189        unsafe {
190            let d = Self::cast::<ModulationUpdate>(data);
191
192            if Self::validate_transition_mode(
193                self.mod_segment,
194                d.segment,
195                self.mod_rep[d.segment as usize],
196                d.transition_mode,
197            ) {
198                return ERR_INVALID_TRANSITION_MODE;
199            }
200
201            if self.validate_silencer_settings(
202                self.stm_freq_div[self.stm_segment as usize],
203                self.mod_freq_div[d.segment as usize],
204            ) {
205                return ERR_INVALID_SILENCER_SETTING;
206            }
207
208            self.mod_segment = d.segment;
209            self.mod_segment_update(d.segment, d.transition_mode, d.transition_value)
210        }
211    }
212}
213
214#[cfg(test)]
215mod tests {
216    use super::*;
217
218    #[test]
219    fn mem_layout() {
220        assert_eq!(16, std::mem::size_of::<ModulationHead>());
221        assert_eq!(0, std::mem::offset_of!(ModulationHead, tag));
222        assert_eq!(1, std::mem::offset_of!(ModulationHead, flag));
223        assert_eq!(2, std::mem::offset_of!(ModulationHead, size));
224        assert_eq!(3, std::mem::offset_of!(ModulationHead, transition_mode));
225        assert_eq!(4, std::mem::offset_of!(ModulationHead, freq_div));
226        assert_eq!(6, std::mem::offset_of!(ModulationHead, rep));
227        assert_eq!(8, std::mem::offset_of!(ModulationHead, transition_value));
228
229        assert_eq!(4, std::mem::size_of::<ModulationSubseq>());
230        assert_eq!(0, std::mem::offset_of!(ModulationSubseq, tag));
231        assert_eq!(1, std::mem::offset_of!(ModulationSubseq, flag));
232        assert_eq!(2, std::mem::offset_of!(ModulationSubseq, size));
233
234        assert_eq!(16, std::mem::size_of::<Modulation>());
235        assert_eq!(0, std::mem::offset_of!(Modulation, head));
236        assert_eq!(0, std::mem::offset_of!(Modulation, subseq));
237
238        assert_eq!(16, std::mem::size_of::<ModulationUpdate>());
239        assert_eq!(0, std::mem::offset_of!(ModulationUpdate, tag));
240        assert_eq!(1, std::mem::offset_of!(ModulationUpdate, segment));
241        assert_eq!(2, std::mem::offset_of!(ModulationUpdate, transition_mode));
242        assert_eq!(8, std::mem::offset_of!(ModulationUpdate, transition_value));
243    }
244}
autd3_firmware_emulator/cpu/operation/modulation.rs

autd3_firmware_emulator/cpu/operation/
modulation.rs
