1//! coordinate rotation digital computer (CORDIC)
2
3use embassy_hal_internal::drop::OnDrop;
4use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
5
6use crate::pac::cordic::vals;
7use crate::{dma, peripherals, rcc};
8
9mod enums;
10pub use enums::*;
11
12mod errors;
13pub use errors::*;
14
15pub mod utils;
16
17/// CORDIC driver
18pub struct Cordic<'d, T: Instance> {
19 peri: PeripheralRef<'d, T>,
20 config: Config,
21}
22
23/// Cordic instance
24trait SealedInstance {
25 /// Get access to CORDIC registers
26 fn regs() -> crate::pac::cordic::Cordic;
27
28 /// Set Function value
29 fn set_func(&self, func: Function) {
30 Self::regs()
31 .csr()
32 .modify(|v| v.set_func(vals::Func::from_bits(func as u8)));
33 }
34
35 /// Set Precision value
36 fn set_precision(&self, precision: Precision) {
37 Self::regs()
38 .csr()
39 .modify(|v| v.set_precision(vals::Precision::from_bits(precision as u8)))
40 }
41
42 /// Set Scale value
43 fn set_scale(&self, scale: Scale) {
44 Self::regs()
45 .csr()
46 .modify(|v| v.set_scale(vals::Scale::from_bits(scale as u8)))
47 }
48
49 /// Enable global interrupt
50 #[allow(unused)]
51 fn enable_irq(&self) {
52 Self::regs().csr().modify(|v| v.set_ien(true))
53 }
54
55 /// Disable global interrupt
56 fn disable_irq(&self) {
57 Self::regs().csr().modify(|v| v.set_ien(false))
58 }
59
60 /// Enable Read DMA
61 fn enable_read_dma(&self) {
62 Self::regs().csr().modify(|v| {
63 v.set_dmaren(true);
64 })
65 }
66
67 /// Disable Read DMA
68 fn disable_read_dma(&self) {
69 Self::regs().csr().modify(|v| {
70 v.set_dmaren(false);
71 })
72 }
73
74 /// Enable Write DMA
75 fn enable_write_dma(&self) {
76 Self::regs().csr().modify(|v| {
77 v.set_dmawen(true);
78 })
79 }
80
81 /// Disable Write DMA
82 fn disable_write_dma(&self) {
83 Self::regs().csr().modify(|v| {
84 v.set_dmawen(false);
85 })
86 }
87
88 /// Set NARGS value
89 fn set_argument_count(&self, n: AccessCount) {
90 Self::regs().csr().modify(|v| {
91 v.set_nargs(match n {
92 AccessCount::One => vals::Num::NUM1,
93 AccessCount::Two => vals::Num::NUM2,
94 })
95 })
96 }
97
98 /// Set NRES value
99 fn set_result_count(&self, n: AccessCount) {
100 Self::regs().csr().modify(|v| {
101 v.set_nres(match n {
102 AccessCount::One => vals::Num::NUM1,
103 AccessCount::Two => vals::Num::NUM2,
104 });
105 })
106 }
107
108 /// Set ARGSIZE and RESSIZE value
109 fn set_data_width(&self, arg: Width, res: Width) {
110 Self::regs().csr().modify(|v| {
111 v.set_argsize(match arg {
112 Width::Bits32 => vals::Size::BITS32,
113 Width::Bits16 => vals::Size::BITS16,
114 });
115 v.set_ressize(match res {
116 Width::Bits32 => vals::Size::BITS32,
117 Width::Bits16 => vals::Size::BITS16,
118 })
119 })
120 }
121
122 /// Read RRDY flag
123 fn ready_to_read(&self) -> bool {
124 Self::regs().csr().read().rrdy()
125 }
126
127 /// Write value to WDATA
128 fn write_argument(&self, arg: u32) {
129 Self::regs().wdata().write_value(arg)
130 }
131
132 /// Read value from RDATA
133 fn read_result(&self) -> u32 {
134 Self::regs().rdata().read()
135 }
136}
137
138/// CORDIC instance trait
139#[allow(private_bounds)]
140pub trait Instance: SealedInstance + Peripheral<P = Self> + crate::rcc::RccPeripheral {}
141
142/// CORDIC configuration
143#[derive(Debug)]
144pub struct Config {
145 function: Function,
146 precision: Precision,
147 scale: Scale,
148}
149
150impl Config {
151 /// Create a config for Cordic driver
152 pub fn new(function: Function, precision: Precision, scale: Scale) -> Result<Self, CordicError> {
153 let config = Self {
154 function,
155 precision,
156 scale,
157 };
158
159 config.check_scale()?;
160
161 Ok(config)
162 }
163
164 fn check_scale(&self) -> Result<(), ConfigError> {
165 use Function::*;
166
167 let scale_raw = self.scale as u8;
168
169 let err_range = match self.function {
170 Cos | Sin | Phase | Modulus if !(0..=0).contains(&scale_raw) => Some([0, 0]),
171
172 Arctan if !(0..=7).contains(&scale_raw) => Some([0, 7]),
173
174 Cosh | Sinh | Arctanh if !(1..=1).contains(&scale_raw) => Some([1, 1]),
175
176 Ln if !(1..=4).contains(&scale_raw) => Some([1, 4]),
177
178 Sqrt if !(0..=2).contains(&scale_raw) => Some([0, 2]),
179
180 Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh | Ln | Sqrt => None,
181 };
182
183 if let Some(range) = err_range {
184 Err(ConfigError {
185 func: self.function,
186 scale_range: range,
187 })
188 } else {
189 Ok(())
190 }
191 }
192}
193
194// common method
195impl<'d, T: Instance> Cordic<'d, T> {
196 /// Create a Cordic driver instance
197 ///
198 /// Note:
199 /// If you need a peripheral -> CORDIC -> peripheral mode,
200 /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguments with [Self::extra_config]
201 pub fn new(peri: impl Peripheral<P = T> + 'd, config: Config) -> Self {
202 rcc::enable_and_reset::<T>();
203
204 into_ref!(peri);
205
206 let mut instance = Self { peri, config };
207
208 instance.reconfigure();
209
210 instance
211 }
212
213 /// Set a new config for Cordic driver
214 pub fn set_config(&mut self, config: Config) {
215 self.config = config;
216 self.reconfigure();
217 }
218
219 /// Set extra config for data count and data width.
220 pub fn extra_config(&mut self, arg_cnt: AccessCount, arg_width: Width, res_width: Width) {
221 self.peri.set_argument_count(arg_cnt);
222 self.peri.set_data_width(arg_width, res_width);
223 }
224
225 fn clean_rrdy_flag(&mut self) {
226 while self.peri.ready_to_read() {
227 self.peri.read_result();
228 }
229 }
230
231 /// Disable IRQ and DMA, clean RRDY, and set ARG2 to +1 (0x7FFFFFFF)
232 pub fn reconfigure(&mut self) {
233 // reset ARG2 to +1
234 {
235 self.peri.disable_irq();
236 self.peri.disable_read_dma();
237 self.peri.disable_write_dma();
238 self.clean_rrdy_flag();
239
240 self.peri.set_func(Function::Cos);
241 self.peri.set_precision(Precision::Iters4);
242 self.peri.set_scale(Scale::Arg1Res1);
243 self.peri.set_argument_count(AccessCount::Two);
244 self.peri.set_data_width(Width::Bits32, Width::Bits32);
245 self.peri.write_argument(0x0u32);
246 self.peri.write_argument(0x7FFFFFFFu32);
247
248 self.clean_rrdy_flag();
249 }
250
251 self.peri.set_func(self.config.function);
252 self.peri.set_precision(self.config.precision);
253 self.peri.set_scale(self.config.scale);
254
255 // we don't set NRES in here, but to make sure NRES is set each time user call "calc"-ish functions,
256 // since each "calc"-ish functions can have different ARGSIZE and RESSIZE, thus NRES should be change accordingly.
257 }
258}
259
260impl<'d, T: Instance> Drop for Cordic<'d, T> {
261 fn drop(&mut self) {
262 rcc::disable::<T>();
263 }
264}
265
266// q1.31 related
267impl<'d, T: Instance> Cordic<'d, T> {
268 /// Run a blocking CORDIC calculation in q1.31 format
269 ///
270 /// Notice:
271 /// If you set `arg1_only` to `true`, please be sure ARG2 value has been set to desired value before.
272 /// This function won't set ARG2 to +1 before or after each round of calculation.
273 /// If you want to make sure ARG2 is set to +1, consider run [.reconfigure()](Self::reconfigure).
274 pub fn blocking_calc_32bit(
275 &mut self,
276 arg: &[u32],
277 res: &mut [u32],
278 arg1_only: bool,
279 res1_only: bool,
280 ) -> Result<usize, CordicError> {
281 if arg.is_empty() {
282 return Ok(0);
283 }
284
285 let res_cnt = Self::check_arg_res_length_32bit(arg.len(), res.len(), arg1_only, res1_only)?;
286
287 self.peri
288 .set_argument_count(if arg1_only { AccessCount::One } else { AccessCount::Two });
289
290 self.peri
291 .set_result_count(if res1_only { AccessCount::One } else { AccessCount::Two });
292
293 self.peri.set_data_width(Width::Bits32, Width::Bits32);
294
295 let mut cnt = 0;
296
297 match arg1_only {
298 true => {
299 // To use cordic preload function, the first value is special.
300 // It is loaded to CORDIC WDATA register out side of loop
301 let first_value = arg[0];
302
303 // preload 1st value to CORDIC, to start the CORDIC calc
304 self.peri.write_argument(first_value);
305
306 for &arg1 in &arg[1..] {
307 // preload arg1 (for next calc)
308 self.peri.write_argument(arg1);
309
310 // then read current result out
311 res[cnt] = self.peri.read_result();
312 cnt += 1;
313 if !res1_only {
314 res[cnt] = self.peri.read_result();
315 cnt += 1;
316 }
317 }
318
319 // read the last result
320 res[cnt] = self.peri.read_result();
321 cnt += 1;
322 if !res1_only {
323 res[cnt] = self.peri.read_result();
324 // cnt += 1;
325 }
326 }
327 false => {
328 // To use cordic preload function, the first and last value is special.
329 // They are load to CORDIC WDATA register out side of loop
330 let first_value = arg[0];
331 let last_value = arg[arg.len() - 1];
332
333 let paired_args = &arg[1..arg.len() - 1];
334
335 // preload 1st value to CORDIC
336 self.peri.write_argument(first_value);
337
338 for args in paired_args.chunks(2) {
339 let arg2 = args[0];
340 let arg1 = args[1];
341
342 // load arg2 (for current calc) first, to start the CORDIC calc
343 self.peri.write_argument(arg2);
344
345 // preload arg1 (for next calc)
346 self.peri.write_argument(arg1);
347
348 // then read current result out
349 res[cnt] = self.peri.read_result();
350 cnt += 1;
351 if !res1_only {
352 res[cnt] = self.peri.read_result();
353 cnt += 1;
354 }
355 }
356
357 // load last value to CORDIC, and finish the calculation
358 self.peri.write_argument(last_value);
359 res[cnt] = self.peri.read_result();
360 cnt += 1;
361 if !res1_only {
362 res[cnt] = self.peri.read_result();
363 // cnt += 1;
364 }
365 }
366 }
367
368 // at this point cnt should be equal to res_cnt
369
370 Ok(res_cnt)
371 }
372
373 /// Run a async CORDIC calculation in q.1.31 format
374 ///
375 /// Notice:
376 /// If you set `arg1_only` to `true`, please be sure ARG2 value has been set to desired value before.
377 /// This function won't set ARG2 to +1 before or after each round of calculation.
378 /// If you want to make sure ARG2 is set to +1, consider run [.reconfigure()](Self::reconfigure).
379 pub async fn async_calc_32bit(
380 &mut self,
381 write_dma: impl Peripheral<P = impl WriteDma<T>>,
382 read_dma: impl Peripheral<P = impl ReadDma<T>>,
383 arg: &[u32],
384 res: &mut [u32],
385 arg1_only: bool,
386 res1_only: bool,
387 ) -> Result<usize, CordicError> {
388 if arg.is_empty() {
389 return Ok(0);
390 }
391
392 let res_cnt = Self::check_arg_res_length_32bit(arg.len(), res.len(), arg1_only, res1_only)?;
393
394 let active_res_buf = &mut res[..res_cnt];
395
396 into_ref!(write_dma, read_dma);
397
398 self.peri
399 .set_argument_count(if arg1_only { AccessCount::One } else { AccessCount::Two });
400
401 self.peri
402 .set_result_count(if res1_only { AccessCount::One } else { AccessCount::Two });
403
404 self.peri.set_data_width(Width::Bits32, Width::Bits32);
405
406 let write_req = write_dma.request();
407 let read_req = read_dma.request();
408
409 self.peri.enable_write_dma();
410 self.peri.enable_read_dma();
411
412 let _on_drop = OnDrop::new(|| {
413 self.peri.disable_write_dma();
414 self.peri.disable_read_dma();
415 });
416
417 unsafe {
418 let write_transfer = dma::Transfer::new_write(
419 &mut write_dma,
420 write_req,
421 arg,
422 T::regs().wdata().as_ptr() as *mut _,
423 Default::default(),
424 );
425
426 let read_transfer = dma::Transfer::new_read(
427 &mut read_dma,
428 read_req,
429 T::regs().rdata().as_ptr() as *mut _,
430 active_res_buf,
431 Default::default(),
432 );
433
434 embassy_futures::join::join(write_transfer, read_transfer).await;
435 }
436
437 Ok(res_cnt)
438 }
439
440 fn check_arg_res_length_32bit(
441 arg_len: usize,
442 res_len: usize,
443 arg1_only: bool,
444 res1_only: bool,
445 ) -> Result<usize, CordicError> {
446 if !arg1_only && arg_len % 2 != 0 {
447 return Err(CordicError::ArgumentLengthIncorrect);
448 }
449
450 let mut minimal_res_length = arg_len;
451
452 if !res1_only {
453 minimal_res_length *= 2;
454 }
455
456 if !arg1_only {
457 minimal_res_length /= 2
458 }
459
460 if minimal_res_length > res_len {
461 return Err(CordicError::ResultLengthNotEnough);
462 }
463
464 Ok(minimal_res_length)
465 }
466}
467
468// q1.15 related
469impl<'d, T: Instance> Cordic<'d, T> {
470 /// Run a blocking CORDIC calculation in q1.15 format
471 ///
472 /// Notice::
473 /// User will take respond to merge two u16 arguments into one u32 data, and/or split one u32 data into two u16 results.
474 pub fn blocking_calc_16bit(&mut self, arg: &[u32], res: &mut [u32]) -> Result<usize, CordicError> {
475 if arg.is_empty() {
476 return Ok(0);
477 }
478
479 if arg.len() > res.len() {
480 return Err(CordicError::ResultLengthNotEnough);
481 }
482
483 let res_cnt = arg.len();
484
485 // In q1.15 mode, 1 write/read to access 2 arguments/results
486 self.peri.set_argument_count(AccessCount::One);
487 self.peri.set_result_count(AccessCount::One);
488
489 self.peri.set_data_width(Width::Bits16, Width::Bits16);
490
491 // To use cordic preload function, the first value is special.
492 // It is loaded to CORDIC WDATA register out side of loop
493 let first_value = arg[0];
494
495 // preload 1st value to CORDIC, to start the CORDIC calc
496 self.peri.write_argument(first_value);
497
498 let mut cnt = 0;
499
500 for &arg_val in &arg[1..] {
501 // preload arg_val (for next calc)
502 self.peri.write_argument(arg_val);
503
504 // then read current result out
505 res[cnt] = self.peri.read_result();
506 cnt += 1;
507 }
508
509 // read last result out
510 res[cnt] = self.peri.read_result();
511 // cnt += 1;
512
513 Ok(res_cnt)
514 }
515
516 /// Run a async CORDIC calculation in q1.15 format
517 ///
518 /// Notice::
519 /// User will take respond to merge two u16 arguments into one u32 data, and/or split one u32 data into two u16 results.
520 pub async fn async_calc_16bit(
521 &mut self,
522 write_dma: impl Peripheral<P = impl WriteDma<T>>,
523 read_dma: impl Peripheral<P = impl ReadDma<T>>,
524 arg: &[u32],
525 res: &mut [u32],
526 ) -> Result<usize, CordicError> {
527 if arg.is_empty() {
528 return Ok(0);
529 }
530
531 if arg.len() > res.len() {
532 return Err(CordicError::ResultLengthNotEnough);
533 }
534
535 let res_cnt = arg.len();
536
537 let active_res_buf = &mut res[..res_cnt];
538
539 into_ref!(write_dma, read_dma);
540
541 // In q1.15 mode, 1 write/read to access 2 arguments/results
542 self.peri.set_argument_count(AccessCount::One);
543 self.peri.set_result_count(AccessCount::One);
544
545 self.peri.set_data_width(Width::Bits16, Width::Bits16);
546
547 let write_req = write_dma.request();
548 let read_req = read_dma.request();
549
550 self.peri.enable_write_dma();
551 self.peri.enable_read_dma();
552
553 let _on_drop = OnDrop::new(|| {
554 self.peri.disable_write_dma();
555 self.peri.disable_read_dma();
556 });
557
558 unsafe {
559 let write_transfer = dma::Transfer::new_write(
560 &mut write_dma,
561 write_req,
562 arg,
563 T::regs().wdata().as_ptr() as *mut _,
564 Default::default(),
565 );
566
567 let read_transfer = dma::Transfer::new_read(
568 &mut read_dma,
569 read_req,
570 T::regs().rdata().as_ptr() as *mut _,
571 active_res_buf,
572 Default::default(),
573 );
574
575 embassy_futures::join::join(write_transfer, read_transfer).await;
576 }
577
578 Ok(res_cnt)
579 }
580}
581
582macro_rules! check_arg_value {
583 ($func_arg1_name:ident, $func_arg2_name:ident, $float_type:ty) => {
584 impl<'d, T: Instance> Cordic<'d, T> {
585 /// check input value ARG1, SCALE and FUNCTION are compatible with each other
586 pub fn $func_arg1_name(&self, arg: $float_type) -> Result<(), ArgError> {
587 let config = &self.config;
588
589 use Function::*;
590
591 struct Arg1ErrInfo {
592 scale: Option<Scale>,
593 range: [f32; 2], // f32 is ok, it only used in error display
594 inclusive_upper_bound: bool,
595 }
596
597 let err_info = match config.function {
598 Cos | Sin | Phase | Modulus | Arctan if !(-1.0..=1.0).contains(arg) => Some(Arg1ErrInfo {
599 scale: None,
600 range: [-1.0, 1.0],
601 inclusive_upper_bound: true,
602 }),
603
604 Cosh | Sinh if !(-0.559..=0.559).contains(arg) => Some(Arg1ErrInfo {
605 scale: None,
606 range: [-0.559, 0.559],
607 inclusive_upper_bound: true,
608 }),
609
610 Arctanh if !(-0.403..=0.403).contains(arg) => Some(Arg1ErrInfo {
611 scale: None,
612 range: [-0.403, 0.403],
613 inclusive_upper_bound: true,
614 }),
615
616 Ln => match config.scale {
617 Scale::Arg1o2Res2 if !(0.0535..0.5).contains(arg) => Some(Arg1ErrInfo {
618 scale: Some(Scale::Arg1o2Res2),
619 range: [0.0535, 0.5],
620 inclusive_upper_bound: false,
621 }),
622 Scale::Arg1o4Res4 if !(0.25..0.75).contains(arg) => Some(Arg1ErrInfo {
623 scale: Some(Scale::Arg1o4Res4),
624 range: [0.25, 0.75],
625 inclusive_upper_bound: false,
626 }),
627 Scale::Arg1o8Res8 if !(0.375..0.875).contains(arg) => Some(Arg1ErrInfo {
628 scale: Some(Scale::Arg1o8Res8),
629 range: [0.375, 0.875],
630 inclusive_upper_bound: false,
631 }),
632 Scale::Arg1o16Res16 if !(0.4375..0.584).contains(arg) => Some(Arg1ErrInfo {
633 scale: Some(Scale::Arg1o16Res16),
634 range: [0.4375, 0.584],
635 inclusive_upper_bound: false,
636 }),
637
638 Scale::Arg1o2Res2 | Scale::Arg1o4Res4 | Scale::Arg1o8Res8 | Scale::Arg1o16Res16 => None,
639
640 _ => unreachable!(),
641 },
642
643 Sqrt => match config.scale {
644 Scale::Arg1Res1 if !(0.027..0.75).contains(arg) => Some(Arg1ErrInfo {
645 scale: Some(Scale::Arg1Res1),
646 range: [0.027, 0.75],
647 inclusive_upper_bound: false,
648 }),
649 Scale::Arg1o2Res2 if !(0.375..0.875).contains(arg) => Some(Arg1ErrInfo {
650 scale: Some(Scale::Arg1o2Res2),
651 range: [0.375, 0.875],
652 inclusive_upper_bound: false,
653 }),
654 Scale::Arg1o4Res4 if !(0.4375..0.584).contains(arg) => Some(Arg1ErrInfo {
655 scale: Some(Scale::Arg1o4Res4),
656 range: [0.4375, 0.584],
657 inclusive_upper_bound: false,
658 }),
659 Scale::Arg1Res1 | Scale::Arg1o2Res2 | Scale::Arg1o4Res4 => None,
660 _ => unreachable!(),
661 },
662
663 Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh => None,
664 };
665
666 if let Some(err) = err_info {
667 return Err(ArgError {
668 func: config.function,
669 scale: err.scale,
670 arg_range: err.range,
671 inclusive_upper_bound: err.inclusive_upper_bound,
672 arg_type: ArgType::Arg1,
673 });
674 }
675
676 Ok(())
677 }
678
679 /// check input value ARG2 and FUNCTION are compatible with each other
680 pub fn $func_arg2_name(&self, arg: $float_type) -> Result<(), ArgError> {
681 let config = &self.config;
682
683 use Function::*;
684
685 struct Arg2ErrInfo {
686 range: [f32; 2], // f32 is ok, it only used in error display
687 }
688
689 let err_info = match config.function {
690 Cos | Sin if !(0.0..=1.0).contains(arg) => Some(Arg2ErrInfo { range: [0.0, 1.0] }),
691
692 Phase | Modulus if !(-1.0..=1.0).contains(arg) => Some(Arg2ErrInfo { range: [-1.0, 1.0] }),
693
694 Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh | Ln | Sqrt => None,
695 };
696
697 if let Some(err) = err_info {
698 return Err(ArgError {
699 func: config.function,
700 scale: None,
701 arg_range: err.range,
702 inclusive_upper_bound: true,
703 arg_type: ArgType::Arg2,
704 });
705 }
706
707 Ok(())
708 }
709 }
710 };
711}
712
713check_arg_value!(check_f64_arg1, check_f64_arg2, &f64);
714check_arg_value!(check_f32_arg1, check_f32_arg2, &f32);
715
716foreach_interrupt!(
717 ($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
718 impl Instance for peripherals::$inst {
719 }
720
721 impl SealedInstance for peripherals::$inst {
722 fn regs() -> crate::pac::cordic::Cordic {
723 crate::pac::$inst
724 }
725 }
726 };
727);
728
729dma_trait!(WriteDma, Instance);
730dma_trait!(ReadDma, Instance);
731