1#![doc = crate::before_snippet!()]
21use core::marker::PhantomData;
33
34pub use crate::soc::clocks::CordicFunctionClockSource as ClockSource;
36use crate::{
37 Blocking,
38 DriverMode,
39 clock::{
40 dividers::FractionalDivider,
41 ll::{ClockTree, CordicFunctionClockConfig, CordicInstance},
42 },
43 peripherals::CORDIC,
44 system::{GenericPeripheralGuard, Peripheral, PeripheralClockControl},
45 time::{Duration, Instant, Rate},
46};
47
48#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash, procmacros::BuilderLite)]
50#[cfg_attr(feature = "defmt", derive(defmt::Format))]
51#[non_exhaustive]
52pub struct Config {
53 clock_source: ClockSource,
55 frequency: Option<Rate>,
59 precision: Precision,
61}
62
63impl Config {
64 fn clock_config(&self) -> Result<CordicFunctionClockConfig, ConfigError> {
65 let source = self.clock_source;
66 let Some(target_frequency) = self.frequency else {
67 return Ok(CordicFunctionClockConfig::new(source, 1, 0, 1));
68 };
69 let source_hz = CordicInstance::function_clock_source_frequency(source);
70 let target_hz = target_frequency.as_hz();
71 if target_hz == 0 || target_hz > source_hz {
72 return Err(ConfigError::UnreachableClockRate);
73 }
74 let (_, max_divider) = property!("clock_tree.cordic.function_clock.divider");
75 let (_, max_denominator) = property!("clock_tree.cordic.function_clock.denominator");
76 let divider = FractionalDivider::new(source_hz, target_hz, max_denominator);
77 if divider.integer > max_divider {
78 return Err(ConfigError::UnreachableClockRate);
79 }
80 Ok(CordicFunctionClockConfig::new(
81 source,
82 divider.integer,
83 divider.numerator,
84 divider.denominator.max(1),
85 ))
86 }
87}
88
89#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash)]
91#[cfg_attr(feature = "defmt", derive(defmt::Format))]
92#[non_exhaustive]
93#[repr(u8)]
94pub enum Precision {
95 #[default]
97 Maximum,
98 Iterations4 = 4,
100 Iterations8 = 8,
102 Iterations12 = 12,
104 Iterations16 = 16,
106 Iterations20 = 20,
108 Iterations24 = 24,
110}
111
112#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, strum::Display)]
114#[cfg_attr(feature = "defmt", derive(defmt::Format))]
115#[non_exhaustive]
116pub enum ConfigError {
117 UnreachableClockRate,
119}
120
121impl core::error::Error for ConfigError {}
122
123#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, strum::Display)]
125#[cfg_attr(feature = "defmt", derive(defmt::Format))]
126#[non_exhaustive]
127pub enum Error {
128 InvalidInput,
130 UnsupportedPrecision,
132 Timeout,
136}
137
138impl core::error::Error for Error {}
139
140#[derive(Debug)]
142#[cfg_attr(feature = "defmt", derive(defmt::Format))]
143pub struct Cordic<'d, Dm: DriverMode = Blocking> {
144 peripheral: CORDIC<'d>,
145 _clock_guard: CordicClockGuard,
146 _guard: GenericPeripheralGuard<{ Peripheral::Cordic as u8 }>,
147 precision: Precision,
148 _mode: PhantomData<Dm>,
149}
150
151#[derive(Debug)]
152#[cfg_attr(feature = "defmt", derive(defmt::Format))]
153struct CordicClockGuard;
154
155impl CordicClockGuard {
156 fn new(config: CordicFunctionClockConfig) -> Self {
157 ClockTree::with(|clocks| {
158 CordicInstance::Cordic.configure_function_clock(clocks, config);
159 CordicInstance::Cordic.request_function_clock(clocks);
160 });
161 Self
162 }
163}
164
165impl Drop for CordicClockGuard {
166 fn drop(&mut self) {
167 ClockTree::with(|clocks| CordicInstance::Cordic.release_function_clock(clocks));
168 }
169}
170
171#[derive(Clone, Copy)]
172#[repr(u8)]
173enum Function {
174 Cos = 0,
175 Sin = 1,
176 Phase = 2,
177 Modulus = 3,
178 Atan = 4,
179 Cosh = 5,
180 Sinh = 6,
181 Atanh = 7,
182 Ln = 8,
183 Sqrt = 9,
184}
185
186const SINH_COSH_MAX_INPUT_Q31: u32 = (559 * (1_u64 << 31) / 1000) as u32;
189const HYPERBOLIC_VECTOR_MAX_INPUT_Q31: u32 = (403 * (1_u64 << 31) / 1000) as u32;
190
191impl Function {
192 const fn scale_exponent(self) -> u8 {
193 match self {
194 Self::Cosh | Self::Sinh | Self::Atanh | Self::Ln => 1,
195 _ => 0,
196 }
197 }
198}
199
200struct Arguments {
201 arg1_word: u32,
202 arg2_word: u32,
203 two_inputs: bool,
204 q31: bool,
205}
206
207impl Arguments {
208 fn q15(arg1: i16, arg2: Option<i16>) -> Self {
209 Self {
210 arg1_word: u32::from(arg1 as u16) | (u32::from(arg2.unwrap_or(0) as u16) << 16),
211 arg2_word: 0,
212 two_inputs: arg2.is_some(),
213 q31: false,
214 }
215 }
216
217 fn q31(arg1: i32, arg2: Option<i32>) -> Self {
218 Self {
219 arg1_word: arg1 as u32,
220 arg2_word: arg2.unwrap_or(0) as u32,
221 two_inputs: arg2.is_some(),
222 q31: true,
223 }
224 }
225}
226
227enum Outputs {
228 Single,
229 Pair,
230}
231
232fn validate_vector(
233 x: i32,
234 y: i32,
235 fractional_bits: u32,
236 requires_phase: bool,
237) -> Result<(), Error> {
238 let x = i64::from(x);
239 let y = i64::from(y);
240 let squared_modulus = (x * x) as u64 + (y * y) as u64;
241 let one_squared = 1 << (2 * fractional_bits);
242 if squared_modulus >= one_squared || (requires_phase && squared_modulus == 0) {
243 return Err(Error::InvalidInput);
244 }
245 Ok(())
246}
247
248fn validate_magnitude(input: i32, limit: u32) -> Result<(), Error> {
249 if input.unsigned_abs() > limit {
250 return Err(Error::InvalidInput);
251 }
252 Ok(())
253}
254
255impl<'d> Cordic<'d, Blocking> {
256 pub fn new(peripheral: CORDIC<'d>, config: Config) -> Result<Self, ConfigError> {
262 let clock_guard = CordicClockGuard::new(config.clock_config()?);
264 Ok(Self {
265 peripheral,
266 _clock_guard: clock_guard,
267 _guard: GenericPeripheralGuard::new(),
268 precision: config.precision,
269 _mode: PhantomData,
270 })
271 }
272
273 pub fn apply_config(&mut self, config: &Config) -> Result<(), ConfigError> {
280 let clock_config = config.clock_config()?;
281 ClockTree::with(|clocks| {
282 let instance = CordicInstance::Cordic;
283 if instance.function_clock_config(clocks) != Some(clock_config) {
284 instance.configure_function_clock(clocks, clock_config);
285 }
286 });
287 self.precision = config.precision;
288 Ok(())
289 }
290
291 pub fn frequency(&self) -> Rate {
295 Rate::from_hz(CordicInstance::Cordic.function_clock_frequency())
296 }
297
298 pub fn sin_cos_q15(&mut self, angle: i16) -> Result<(i16, i16), Error> {
306 self.calculate_q15_pair(Function::Sin, angle, None)
307 }
308
309 pub fn sin_cos_q31(&mut self, angle: i32) -> Result<(i32, i32), Error> {
317 self.calculate_q31_pair(Function::Sin, angle, None)
318 }
319
320 pub fn sin_q15(&mut self, angle: i16) -> Result<i16, Error> {
329 self.calculate_q15(Function::Sin, angle, None)
330 }
331
332 pub fn sin_q31(&mut self, angle: i32) -> Result<i32, Error> {
341 self.calculate_q31(Function::Sin, angle, None)
342 }
343
344 pub fn cos_q15(&mut self, angle: i16) -> Result<i16, Error> {
353 self.calculate_q15(Function::Cos, angle, None)
354 }
355
356 pub fn cos_q31(&mut self, angle: i32) -> Result<i32, Error> {
365 self.calculate_q31(Function::Cos, angle, None)
366 }
367
368 pub fn phase_modulus_q15(&mut self, x: i16, y: i16) -> Result<(i16, i16), Error> {
377 validate_vector(i32::from(x), i32::from(y), 15, true)?;
378 self.calculate_q15_pair(Function::Phase, x, Some(y))
379 }
380
381 pub fn phase_modulus_q31(&mut self, x: i32, y: i32) -> Result<(i32, i32), Error> {
390 validate_vector(x, y, 31, true)?;
391 self.calculate_q31_pair(Function::Phase, x, Some(y))
392 }
393
394 pub fn phase_q15(&mut self, x: i16, y: i16) -> Result<i16, Error> {
404 validate_vector(i32::from(x), i32::from(y), 15, true)?;
405 self.calculate_q15(Function::Phase, x, Some(y))
406 }
407
408 pub fn phase_q31(&mut self, x: i32, y: i32) -> Result<i32, Error> {
418 validate_vector(x, y, 31, true)?;
419 self.calculate_q31(Function::Phase, x, Some(y))
420 }
421
422 pub fn modulus_q15(&mut self, x: i16, y: i16) -> Result<i16, Error> {
431 validate_vector(i32::from(x), i32::from(y), 15, false)?;
432 self.calculate_q15(Function::Modulus, x, Some(y))
433 }
434
435 pub fn modulus_q31(&mut self, x: i32, y: i32) -> Result<i32, Error> {
444 validate_vector(x, y, 31, false)?;
445 self.calculate_q31(Function::Modulus, x, Some(y))
446 }
447
448 pub fn atan_q15(&mut self, x: i16) -> Result<i16, Error> {
454 self.calculate_q15(Function::Atan, x, None)
455 }
456
457 pub fn atan_q31(&mut self, x: i32) -> Result<i32, Error> {
463 self.calculate_q31(Function::Atan, x, None)
464 }
465
466 pub fn sinh_cosh_q15(&mut self, x_over_two: i16) -> Result<(i16, i16), Error> {
473 validate_magnitude(i32::from(x_over_two), SINH_COSH_MAX_INPUT_Q31 >> 16)?;
474 self.calculate_q15_pair(Function::Sinh, x_over_two, None)
475 }
476
477 pub fn sinh_cosh_q31(&mut self, x_over_two: i32) -> Result<(i32, i32), Error> {
484 validate_magnitude(x_over_two, SINH_COSH_MAX_INPUT_Q31)?;
485 self.calculate_q31_pair(Function::Sinh, x_over_two, None)
486 }
487
488 pub fn sinh_q15(&mut self, x_over_two: i16) -> Result<i16, Error> {
497 validate_magnitude(i32::from(x_over_two), SINH_COSH_MAX_INPUT_Q31 >> 16)?;
498 self.calculate_q15(Function::Sinh, x_over_two, None)
499 }
500
501 pub fn sinh_q31(&mut self, x_over_two: i32) -> Result<i32, Error> {
510 validate_magnitude(x_over_two, SINH_COSH_MAX_INPUT_Q31)?;
511 self.calculate_q31(Function::Sinh, x_over_two, None)
512 }
513
514 pub fn cosh_q15(&mut self, x_over_two: i16) -> Result<i16, Error> {
523 validate_magnitude(i32::from(x_over_two), SINH_COSH_MAX_INPUT_Q31 >> 16)?;
524 self.calculate_q15(Function::Cosh, x_over_two, None)
525 }
526
527 pub fn cosh_q31(&mut self, x_over_two: i32) -> Result<i32, Error> {
536 validate_magnitude(x_over_two, SINH_COSH_MAX_INPUT_Q31)?;
537 self.calculate_q31(Function::Cosh, x_over_two, None)
538 }
539
540 pub fn atanh_q15(&mut self, x_over_two: i16) -> Result<i16, Error> {
547 validate_magnitude(i32::from(x_over_two), HYPERBOLIC_VECTOR_MAX_INPUT_Q31 >> 16)?;
548 self.calculate_q15(Function::Atanh, x_over_two, None)
549 }
550
551 pub fn atanh_q31(&mut self, x_over_two: i32) -> Result<i32, Error> {
558 validate_magnitude(x_over_two, HYPERBOLIC_VECTOR_MAX_INPUT_Q31)?;
559 self.calculate_q31(Function::Atanh, x_over_two, None)
560 }
561
562 pub fn ln_from_ratio_q15(&mut self, ratio: i16) -> Result<i16, Error> {
570 validate_magnitude(i32::from(ratio), HYPERBOLIC_VECTOR_MAX_INPUT_Q31 >> 16)?;
571 self.calculate_q15(Function::Ln, ratio, None)
572 }
573
574 pub fn ln_from_ratio_q31(&mut self, ratio: i32) -> Result<i32, Error> {
582 validate_magnitude(ratio, HYPERBOLIC_VECTOR_MAX_INPUT_Q31)?;
583 self.calculate_q31(Function::Ln, ratio, None)
584 }
585
586 pub fn sqrt_q15(&mut self, x: i16) -> Result<i16, Error> {
594 let Some(root_shift) = self.sqrt_shift(i32::from(x), x.leading_zeros())? else {
595 return Ok(0);
596 };
597 let root = self.calculate_q15(Function::Sqrt, x << (2 * root_shift), None)?;
598 Ok(root >> root_shift)
599 }
600
601 pub fn sqrt_q31(&mut self, x: i32) -> Result<i32, Error> {
609 let Some(root_shift) = self.sqrt_shift(x, x.leading_zeros())? else {
610 return Ok(0);
611 };
612 let root = self.calculate_q31(Function::Sqrt, x << (2 * root_shift), None)?;
613 Ok(root >> root_shift)
614 }
615
616 fn sqrt_shift(&self, x: i32, leading_zeros: u32) -> Result<Option<u32>, Error> {
617 match x {
618 ..0 => Err(Error::InvalidInput),
619 0 => {
620 self.iterations(Function::Sqrt)?;
621 Ok(None)
622 }
623 _ => Ok(Some((leading_zeros - 1) / 2)),
626 }
627 }
628
629 fn iterations(&self, function: Function) -> Result<u8, Error> {
630 let max = match function {
631 Function::Sqrt => Precision::Iterations12,
632 _ => Precision::Iterations24,
633 } as u8;
634 let iterations = match self.precision {
635 Precision::Maximum => max,
636 precision => precision as u8,
637 };
638 if iterations > max {
639 return Err(Error::UnsupportedPrecision);
640 }
641 Ok(iterations)
642 }
643
644 fn calculate_q15(
645 &mut self,
646 function: Function,
647 arg1: i16,
648 arg2: Option<i16>,
649 ) -> Result<i16, Error> {
650 let (first, _) =
651 self.calculate_raw(function, Arguments::q15(arg1, arg2), Outputs::Single)?;
652 Ok(first as i16)
653 }
654
655 fn calculate_q15_pair(
656 &mut self,
657 function: Function,
658 arg1: i16,
659 arg2: Option<i16>,
660 ) -> Result<(i16, i16), Error> {
661 let (packed, _) =
662 self.calculate_raw(function, Arguments::q15(arg1, arg2), Outputs::Pair)?;
663 Ok((packed as i16, (packed >> 16) as i16))
664 }
665
666 fn calculate_q31(
667 &mut self,
668 function: Function,
669 arg1: i32,
670 arg2: Option<i32>,
671 ) -> Result<i32, Error> {
672 let (first, _) =
673 self.calculate_raw(function, Arguments::q31(arg1, arg2), Outputs::Single)?;
674 Ok(first as i32)
675 }
676
677 fn calculate_q31_pair(
678 &mut self,
679 function: Function,
680 arg1: i32,
681 arg2: Option<i32>,
682 ) -> Result<(i32, i32), Error> {
683 let (first, second) =
684 self.calculate_raw(function, Arguments::q31(arg1, arg2), Outputs::Pair)?;
685 Ok((first as i32, second as i32))
686 }
687
688 fn calculate_raw(
689 &mut self,
690 function: Function,
691 arguments: Arguments,
692 outputs: Outputs,
693 ) -> Result<(u32, u32), Error> {
694 const FAST_POLL_ATTEMPTS: usize = 1024;
695 const SLOW_POLL_TIMEOUT: Duration = Duration::from_millis(2);
696
697 let iterations = self.iterations(function)?;
698 let regs = self.peripheral.register_block();
699 let Arguments {
700 arg1_word,
701 arg2_word,
702 two_inputs,
703 q31,
704 } = arguments;
705 let pair = matches!(outputs, Outputs::Pair);
706 regs.csr_cfg().write(|w| unsafe {
707 w.func().bits(function as u8);
708 w.pres().bits(iterations / 4 - 1);
709 w.scale().bits(function.scale_exponent());
710 w.arg_num().bit(two_inputs);
711 w.res_num().bit(pair);
712 w.arg_size().bit(q31);
713 w.res_size().bit(q31)
714 });
715 regs.arg1().write(|w| unsafe { w.data().bits(arg1_word) });
716 if q31 && two_inputs {
717 regs.arg2().write(|w| unsafe { w.data().bits(arg2_word) });
718 }
719 regs.csr_cfg().modify(|_, w| w.update_flag().set_bit());
720
721 let is_ready = || regs.csr_cfg().read().res_rdy_flag().bit_is_set();
722 if !(0..FAST_POLL_ATTEMPTS).any(|_| is_ready()) {
723 let start = Instant::now();
724 while !is_ready() {
725 if start.elapsed() >= SLOW_POLL_TIMEOUT {
726 PeripheralClockControl::reset(Peripheral::Cordic);
727 return Err(Error::Timeout);
728 }
729 }
730 }
731
732 let first = regs.res1().read().data().bits();
733 let second = if q31 && pair {
734 regs.res2().read().data().bits()
735 } else {
736 0
737 };
738 Ok((first, second))
739 }
740}