1pub(crate) mod boxed;
2mod clear;
3mod cpu_gpio_out;
4mod debug;
5mod force_fan;
6mod gain;
7mod gpio_in;
8mod info;
9mod modulation;
10mod phase_corr;
11mod pulse_width_encoder;
12mod reads_fpga_state;
13mod segment;
14mod silencer;
15mod stm;
16mod sync;
17pub(crate) mod v10;
18
19pub(crate) use autd3_core::datagram::NullOp;
20use autd3_core::link::MsgId;
21pub use boxed::BoxedOperation;
22pub(crate) use clear::*;
23pub(crate) use cpu_gpio_out::*;
24pub(crate) use debug::*;
25pub(crate) use force_fan::*;
26pub(crate) use gain::*;
27pub(crate) use gpio_in::*;
28pub use info::FirmwareVersionType;
29pub(crate) use info::*;
30pub(crate) use modulation::*;
31pub(crate) use phase_corr::*;
32pub(crate) use pulse_width_encoder::*;
33pub(crate) use reads_fpga_state::*;
34pub use segment::SwapSegment;
35pub(crate) use segment::*;
36pub(crate) use silencer::*;
37pub(crate) use stm::*;
38pub use stm::{ControlPoint, ControlPoints, FociSTMIterator, GainSTMIterator};
39pub(crate) use sync::*;
40use zerocopy::{Immutable, IntoBytes};
41
42use crate::{
43 error::AUTDDriverError,
44 firmware::cpu::TxMessage,
45 geometry::{Device, Geometry},
46};
47
48use rayon::prelude::*;
49
50#[derive(PartialEq, Debug, IntoBytes, Immutable)]
51#[repr(u8)]
52#[non_exhaustive]
53pub(crate) enum TypeTag {
54 Clear = 0x01,
55 Sync = 0x02,
56 FirmwareVersion = 0x03,
57 Modulation = 0x10,
58 ModulationSwapSegment = 0x11,
59 Silencer = 0x21,
60 Gain = 0x30,
61 GainSwapSegment = 0x31,
62 GainSTM = 0x41,
63 FociSTM = 0x42,
64 GainSTMSwapSegment = 0x43,
65 FociSTMSwapSegment = 0x44,
66 ForceFan = 0x60,
67 ReadsFPGAState = 0x61,
68 ConfigPulseWidthEncoder = 0x72,
69 PhaseCorrection = 0x80,
70 Debug = 0xF0,
71 EmulateGPIOIn = 0xF1,
72 CpuGPIOOut = 0xF2,
73}
74
75pub use autd3_core::datagram::Operation;
76
77#[doc(hidden)]
78pub trait OperationGenerator {
79 type O1: Operation;
80 type O2: Operation;
81 #[must_use]
82 fn generate(&mut self, device: &Device) -> Option<(Self::O1, Self::O2)>;
83}
84
85#[doc(hidden)]
86pub struct OperationHandler {}
87
88impl OperationHandler {
89 #[must_use]
90 pub fn generate<G: OperationGenerator>(
91 mut generator: G,
92 geometry: &Geometry,
93 ) -> Vec<Option<(G::O1, G::O2)>> {
94 geometry
95 .devices()
96 .map(|dev| generator.generate(dev))
97 .collect()
98 }
99
100 #[must_use]
101 pub fn is_done<O1, O2>(operations: &[Option<(O1, O2)>]) -> bool
102 where
103 O1: Operation,
104 O2: Operation,
105 {
106 operations.iter().all(|op| {
107 op.as_ref()
108 .is_none_or(|(op1, op2)| op1.is_done() && op2.is_done())
109 })
110 }
111
112 pub fn pack<O1, O2>(
113 msg_id: MsgId,
114 operations: &mut [Option<(O1, O2)>],
115 geometry: &Geometry,
116 sent_flags: &mut [bool],
117 tx: &mut [TxMessage],
118 parallel: bool,
119 ) -> Result<(), AUTDDriverError>
120 where
121 O1: Operation,
122 O2: Operation,
123 AUTDDriverError: From<O1::Error> + From<O2::Error>,
124 {
125 if parallel {
126 geometry
127 .iter()
128 .zip(sent_flags.iter_mut())
129 .zip(tx.iter_mut())
130 .filter_map(|((dev, sent), tx)| {
131 *sent = dev.enable;
132 dev.enable.then_some((dev, sent, tx))
133 })
134 .zip(operations.iter_mut())
135 .par_bridge()
136 .try_for_each(|((dev, sent, tx), op)| {
137 if let Some((op1, op2)) = op {
138 Self::pack_op2(msg_id, op1, op2, dev, sent, tx)
139 } else {
140 *sent = false;
141 Ok(())
142 }
143 })
144 } else {
145 geometry
146 .iter()
147 .zip(sent_flags.iter_mut())
148 .zip(tx.iter_mut())
149 .filter_map(|((dev, sent), tx)| {
150 *sent = dev.enable;
151 dev.enable.then_some((dev, sent, tx))
152 })
153 .zip(operations.iter_mut())
154 .try_for_each(|((dev, sent, tx), op)| {
155 if let Some((op1, op2)) = op {
156 Self::pack_op2(msg_id, op1, op2, dev, sent, tx)
157 } else {
158 *sent = false;
159 Ok(())
160 }
161 })
162 }
163 }
164
165 fn pack_op2<O1, O2>(
166 msg_id: MsgId,
167 op1: &mut O1,
168 op2: &mut O2,
169 dev: &Device,
170 sent: &mut bool,
171 tx: &mut TxMessage,
172 ) -> Result<(), AUTDDriverError>
173 where
174 O1: Operation,
175 O2: Operation,
176 AUTDDriverError: From<O1::Error> + From<O2::Error>,
177 {
178 *sent = !op1.is_done() || !op2.is_done();
179 match (op1.is_done(), op2.is_done()) {
180 (true, true) => Result::<_, AUTDDriverError>::Ok(()),
181 (true, false) => Self::pack_op(msg_id, op2, dev, tx).map(|_| Ok(()))?,
182 (false, true) => Self::pack_op(msg_id, op1, dev, tx).map(|_| Ok(()))?,
183 (false, false) => {
184 let op1_size = Self::pack_op(msg_id, op1, dev, tx)?;
185 if tx.payload().len() - op1_size >= op2.required_size(dev) {
186 op2.pack(dev, &mut tx.payload_mut()[op1_size..])?;
187 tx.header.slot_2_offset = op1_size as u16;
188 }
189 Ok(())
190 }
191 }
192 }
193
194 fn pack_op<O>(
195 msg_id: MsgId,
196 op: &mut O,
197 dev: &Device,
198 tx: &mut TxMessage,
199 ) -> Result<usize, AUTDDriverError>
200 where
201 O: Operation,
202 AUTDDriverError: From<O::Error>,
203 {
204 tx.header.msg_id = msg_id;
205 tx.header.slot_2_offset = 0;
206 Ok(op.pack(dev, tx.payload_mut())?)
207 }
208}
209
210#[inline(always)]
211pub(crate) fn write_to_tx<T: IntoBytes + Immutable>(tx: &mut [u8], data: T) {
212 tx[..size_of::<T>()].copy_from_slice(data.as_bytes());
213}
214
215#[cfg(test)]
216pub(crate) mod tests {
217
218 use std::mem::size_of;
219
220 use zerocopy::FromZeros;
221
222 use crate::{
223 ethercat::EC_OUTPUT_FRAME_SIZE,
224 firmware::cpu::Header,
225 geometry::{Point3, Transducer, UnitQuaternion},
226 };
227
228 use super::*;
229
230 pub fn create_device(n: u8) -> Device {
231 Device::new(
232 UnitQuaternion::identity(),
233 (0..n).map(|_| Transducer::new(Point3::origin())).collect(),
234 )
235 }
236
237 struct OperationMock {
238 pub pack_size: usize,
239 pub required_size: usize,
240 pub num_frames: usize,
241 pub broken: bool,
242 }
243
244 impl Operation for OperationMock {
245 type Error = AUTDDriverError;
246
247 fn required_size(&self, _: &Device) -> usize {
248 self.required_size
249 }
250
251 fn pack(&mut self, _: &Device, _: &mut [u8]) -> Result<usize, AUTDDriverError> {
252 if self.broken {
253 return Err(AUTDDriverError::NotSupportedTag);
254 }
255 self.num_frames -= 1;
256 Ok(self.pack_size)
257 }
258
259 fn is_done(&self) -> bool {
260 self.num_frames == 0
261 }
262 }
263
264 #[rstest::rstest]
265 #[test]
266 #[case::serial(false)]
267 #[case::parallel(true)]
268 fn operation_handler(#[case] parallel: bool) {
269 use crate::geometry::Point3;
270
271 let geometry = Geometry::new(vec![Device::new(
272 UnitQuaternion::identity(),
273 vec![Transducer::new(Point3::origin())],
274 )]);
275
276 let mut op = vec![Some((
277 OperationMock {
278 pack_size: 1,
279 required_size: 2,
280 num_frames: 3,
281 broken: false,
282 },
283 OperationMock {
284 pack_size: 1,
285 required_size: 2,
286 num_frames: 3,
287 broken: false,
288 },
289 ))];
290
291 assert!(!OperationHandler::is_done(&op));
292
293 let msg_id = MsgId::new(0);
294 let mut sent_flags = vec![false; 1];
295 let mut tx = vec![TxMessage::new_zeroed(); 1];
296
297 assert!(
298 OperationHandler::pack(
299 msg_id,
300 &mut op,
301 &geometry,
302 &mut sent_flags,
303 &mut tx,
304 parallel
305 )
306 .is_ok()
307 );
308 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 2);
309 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 2);
310 assert!(sent_flags[0]);
311 assert!(!OperationHandler::is_done(&op));
312
313 op[0].as_mut().unwrap().0.pack_size =
314 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size;
315 assert!(
316 OperationHandler::pack(
317 msg_id,
318 &mut op,
319 &geometry,
320 &mut sent_flags,
321 &mut tx,
322 parallel
323 )
324 .is_ok()
325 );
326 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 1);
327 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
328 assert!(sent_flags[0]);
329 assert!(!OperationHandler::is_done(&op));
330
331 op[0].as_mut().unwrap().0.pack_size =
332 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size
333 + 1;
334 assert!(
335 OperationHandler::pack(
336 msg_id,
337 &mut op,
338 &geometry,
339 &mut sent_flags,
340 &mut tx,
341 parallel
342 )
343 .is_ok()
344 );
345 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
346 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
347 assert!(sent_flags[0]);
348 assert!(!OperationHandler::is_done(&op));
349
350 assert!(
351 OperationHandler::pack(
352 msg_id,
353 &mut op,
354 &geometry,
355 &mut sent_flags,
356 &mut tx,
357 parallel
358 )
359 .is_ok()
360 );
361 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
362 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 0);
363 assert!(sent_flags[0]);
364 assert!(OperationHandler::is_done(&op));
365 }
366
367 #[rstest::rstest]
368 #[test]
369 #[case::serial(false)]
370 #[case::parallel(true)]
371 fn operation_handler_none(#[case] parallel: bool) {
372 use crate::geometry::Point3;
373
374 let geometry = Geometry::new(vec![Device::new(
375 UnitQuaternion::identity(),
376 vec![Transducer::new(Point3::origin())],
377 )]);
378
379 let mut op: Vec<Option<(OperationMock, OperationMock)>> = vec![None, None];
380
381 assert!(OperationHandler::is_done(&op));
382
383 let msg_id = MsgId::new(0);
384 let mut sent_flags = vec![false; 1];
385 let mut tx = vec![TxMessage::new_zeroed(); 1];
386
387 assert!(
388 OperationHandler::pack(
389 msg_id,
390 &mut op,
391 &geometry,
392 &mut sent_flags,
393 &mut tx,
394 parallel
395 )
396 .is_ok()
397 );
398 assert!(!sent_flags[0]);
399 assert!(OperationHandler::is_done(&op));
400 }
401
402 #[test]
403 fn test_first() {
404 let geometry = Geometry::new(vec![Device::new(
405 UnitQuaternion::identity(),
406 vec![Transducer::new(Point3::origin())],
407 )]);
408
409 let mut op = vec![Some((
410 OperationMock {
411 pack_size: 0,
412 required_size: 0,
413 num_frames: 1,
414 broken: false,
415 },
416 OperationMock {
417 pack_size: 0,
418 required_size: 0,
419 num_frames: 0,
420 broken: false,
421 },
422 ))];
423
424 assert!(!op[0].as_ref().unwrap().0.is_done());
425 assert!(op[0].as_ref().unwrap().1.is_done());
426 assert!(!OperationHandler::is_done(&op));
427
428 let msg_id = MsgId::new(0);
429 let mut sent_flags = vec![false; 1];
430 let mut tx = vec![TxMessage::new_zeroed(); 1];
431
432 assert!(
433 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
434 .is_ok()
435 );
436 assert!(op[0].as_ref().unwrap().0.is_done());
437 assert!(op[0].as_ref().unwrap().1.is_done());
438 assert!(sent_flags[0]);
439 assert!(OperationHandler::is_done(&op));
440 }
441
442 #[test]
443 fn test_second() {
444 let geometry = Geometry::new(vec![Device::new(
445 UnitQuaternion::identity(),
446 vec![Transducer::new(Point3::origin())],
447 )]);
448
449 let mut op = vec![Some((
450 OperationMock {
451 pack_size: 0,
452 required_size: 0,
453 num_frames: 0,
454 broken: false,
455 },
456 OperationMock {
457 pack_size: 0,
458 required_size: 0,
459 num_frames: 1,
460 broken: false,
461 },
462 ))];
463
464 assert!(op[0].as_ref().unwrap().0.is_done());
465 assert!(!op[0].as_ref().unwrap().1.is_done());
466 assert!(!OperationHandler::is_done(&op));
467
468 let msg_id = MsgId::new(0);
469 let mut sent_flags = vec![false; 1];
470 let mut tx = vec![TxMessage::new_zeroed(); 1];
471
472 assert!(
473 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
474 .is_ok()
475 );
476 assert!(op[0].as_ref().unwrap().0.is_done());
477 assert!(op[0].as_ref().unwrap().1.is_done());
478 assert!(sent_flags[0]);
479 assert!(OperationHandler::is_done(&op));
480 }
481
482 #[test]
483 fn test_broken_pack() {
484 let geometry = Geometry::new(vec![Device::new(
485 UnitQuaternion::identity(),
486 vec![Transducer::new(Point3::origin())],
487 )]);
488
489 let mut op = vec![Some((
490 OperationMock {
491 pack_size: 0,
492 required_size: 0,
493 num_frames: 1,
494 broken: true,
495 },
496 OperationMock {
497 pack_size: 0,
498 required_size: 0,
499 num_frames: 1,
500 broken: false,
501 },
502 ))];
503
504 let msg_id = MsgId::new(0);
505 let mut sent_flags = vec![false; 1];
506 let mut tx = vec![TxMessage::new_zeroed(); 1];
507
508 assert_eq!(
509 Err(AUTDDriverError::NotSupportedTag),
510 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
511 );
512
513 op[0].as_mut().unwrap().0.broken = false;
514 op[0].as_mut().unwrap().1.broken = true;
515
516 assert_eq!(
517 Err(AUTDDriverError::NotSupportedTag),
518 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
519 );
520
521 op[0].as_mut().unwrap().0.num_frames = 0;
522
523 assert_eq!(
524 Err(AUTDDriverError::NotSupportedTag),
525 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
526 );
527
528 op[0].as_mut().unwrap().0.broken = true;
529 op[0].as_mut().unwrap().1.broken = false;
530
531 op[0].as_mut().unwrap().0.num_frames = 1;
532 op[0].as_mut().unwrap().1.num_frames = 0;
533
534 assert_eq!(
535 Err(AUTDDriverError::NotSupportedTag),
536 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
537 );
538 }
539
540 #[test]
541 fn test_finished() {
542 let geometry = Geometry::new(vec![Device::new(
543 UnitQuaternion::identity(),
544 vec![Transducer::new(Point3::origin())],
545 )]);
546
547 let mut op = vec![Some((
548 OperationMock {
549 pack_size: 0,
550 required_size: 0,
551 num_frames: 0,
552 broken: false,
553 },
554 OperationMock {
555 pack_size: 0,
556 required_size: 0,
557 num_frames: 0,
558 broken: false,
559 },
560 ))];
561
562 assert!(OperationHandler::is_done(&op));
563
564 let msg_id = MsgId::new(0);
565 let mut sent_flags = vec![false; 1];
566 let mut tx = vec![TxMessage::new_zeroed(); 1];
567
568 assert!(
569 OperationHandler::pack(msg_id, &mut op, &geometry, &mut sent_flags, &mut tx, false)
570 .is_ok()
571 );
572 assert!(!sent_flags[0]);
573 assert!(OperationHandler::is_done(&op));
574 }
575}