Skip to main content

esp_hal/
cordic.rs

1//! # Coordinate Rotation Digital Computer (CORDIC)
2//!
3//! ## Overview
4//! The CORDIC peripheral computes trigonometric and hyperbolic functions,
5//! square roots, and natural logarithms. It also converts Cartesian vectors
6//! to polar coordinates.
7//!
8//! The driver uses signed Q1.15 and Q1.31 values in `[-1, 1)`.
9//! Angle formulas use radians.
10//!
11//! ## Configuration
12//! The default [`Config`] uses the 160 MHz PLL without division.
13//!
14//! ## Implementation State
15//! The driver uses blocking register polling.
16//!
17//! ## Examples
18//!
19//! ```rust, no_run
20#![doc = crate::before_snippet!()]
21//! use esp_hal::{cordic::{ClockSource, Config, Cordic, Precision}, time::Rate};
22//! let config = Config::default()
23//!     .with_clock_source(ClockSource::Xtal)
24//!     .with_frequency(Rate::from_mhz(24))
25//!     .with_precision(Precision::Iterations16);
26//! let mut cordic = Cordic::new(peripherals.CORDIC, config)?;
27//! let (sin, cos) = cordic.sin_cos_q15(0x2000)?; // π/4
28//! # Ok(())
29//! # }
30//! ```
31
32use core::marker::PhantomData;
33
34/// Source of the CORDIC calculation clock.
35pub 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/// Configuration for the CORDIC accelerator.
49#[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    /// Calculation clock source.
54    clock_source: ClockSource,
55    /// Target clock rate. Uses the source rate when unset.
56    ///
57    /// See [`Cordic::frequency`] for the rate after divider rounding.
58    frequency: Option<Rate>,
59    /// Iteration count. Defaults to [`Precision::Maximum`].
60    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/// Number of CORDIC iterations per calculation.
90#[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    /// Highest iteration count supported by the operation.
96    #[default]
97    Maximum,
98    /// 4 iterations.
99    Iterations4  = 4,
100    /// 8 iterations.
101    Iterations8  = 8,
102    /// 12 iterations.
103    Iterations12 = 12,
104    /// 16 iterations.
105    Iterations16 = 16,
106    /// 20 iterations.
107    Iterations20 = 20,
108    /// 24 iterations.
109    Iterations24 = 24,
110}
111
112/// A CORDIC configuration error.
113#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, strum::Display)]
114#[cfg_attr(feature = "defmt", derive(defmt::Format))]
115#[non_exhaustive]
116pub enum ConfigError {
117    /// The requested rate is zero or outside the frequency range for the selected source.
118    UnreachableClockRate,
119}
120
121impl core::error::Error for ConfigError {}
122
123/// An error returned by a CORDIC calculation.
124#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, strum::Display)]
125#[cfg_attr(feature = "defmt", derive(defmt::Format))]
126#[non_exhaustive]
127pub enum Error {
128    /// The input is outside the supported range of the operation.
129    InvalidInput,
130    /// The operation does not support the configured iteration count.
131    UnsupportedPrecision,
132    /// The peripheral did not report completion within the polling timeout.
133    ///
134    /// The driver resets the peripheral before returning this error.
135    Timeout,
136}
137
138impl core::error::Error for Error {}
139
140/// A blocking CORDIC driver.
141#[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
186// These hardware input limits apply with SCALE = 1.
187// See <https://github.com/espressif/esp-idf/blob/0e6b480f858c69862ffb6001d96e9eef74599a6e/docs/en/api-reference/peripherals/cordic.rst#L79-L98>.
188const 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    /// Creates a new CORDIC driver.
257    ///
258    /// # Errors
259    ///
260    /// [`ConfigError::UnreachableClockRate`] when the requested clock rate is unsupported.
261    pub fn new(peripheral: CORDIC<'d>, config: Config) -> Result<Self, ConfigError> {
262        // Keep the calculation clock running during the peripheral reset.
263        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    /// Applies a new clock and precision configuration.
274    ///
275    /// # Errors
276    ///
277    /// [`ConfigError::UnreachableClockRate`] when the requested clock rate is unsupported.
278    /// The current configuration is retained in that case.
279    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    /// Returns the calculation clock rate derived from the selected source and divider.
292    ///
293    /// The actual rate can differ when using the internal RC oscillator.
294    pub fn frequency(&self) -> Rate {
295        Rate::from_hz(CordicInstance::Cordic.function_clock_frequency())
296    }
297
298    /// Returns `(sin(θ), cos(θ))` in Q1.15 for input `θ / π`.
299    ///
300    /// A result of +1 is represented by [`i16::MAX`].
301    ///
302    /// # Errors
303    ///
304    /// [`Error::Timeout`] when the calculation does not complete.
305    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    /// Returns `(sin(θ), cos(θ))` in Q1.31 for input `θ / π`.
310    ///
311    /// A result of +1 is represented by [`i32::MAX`].
312    ///
313    /// # Errors
314    ///
315    /// [`Error::Timeout`] when the calculation does not complete.
316    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    /// Returns `sin(θ)` in Q1.15 for input `θ / π`.
321    ///
322    /// A result of +1 is represented by [`i16::MAX`].
323    /// [`Self::sin_cos_q15`] computes sine and cosine in one operation.
324    ///
325    /// # Errors
326    ///
327    /// [`Error::Timeout`] when the calculation does not complete.
328    pub fn sin_q15(&mut self, angle: i16) -> Result<i16, Error> {
329        self.calculate_q15(Function::Sin, angle, None)
330    }
331
332    /// Returns `sin(θ)` in Q1.31 for input `θ / π`.
333    ///
334    /// A result of +1 is represented by [`i32::MAX`].
335    /// [`Self::sin_cos_q31`] computes sine and cosine in one operation.
336    ///
337    /// # Errors
338    ///
339    /// [`Error::Timeout`] when the calculation does not complete.
340    pub fn sin_q31(&mut self, angle: i32) -> Result<i32, Error> {
341        self.calculate_q31(Function::Sin, angle, None)
342    }
343
344    /// Returns `cos(θ)` in Q1.15 for input `θ / π`.
345    ///
346    /// A result of +1 is represented by [`i16::MAX`].
347    /// [`Self::sin_cos_q15`] computes sine and cosine in one operation.
348    ///
349    /// # Errors
350    ///
351    /// [`Error::Timeout`] when the calculation does not complete.
352    pub fn cos_q15(&mut self, angle: i16) -> Result<i16, Error> {
353        self.calculate_q15(Function::Cos, angle, None)
354    }
355
356    /// Returns `cos(θ)` in Q1.31 for input `θ / π`.
357    ///
358    /// A result of +1 is represented by [`i32::MAX`].
359    /// [`Self::sin_cos_q31`] computes sine and cosine in one operation.
360    ///
361    /// # Errors
362    ///
363    /// [`Error::Timeout`] when the calculation does not complete.
364    pub fn cos_q31(&mut self, angle: i32) -> Result<i32, Error> {
365        self.calculate_q31(Function::Cos, angle, None)
366    }
367
368    /// Returns `(atan2(y, x) / π, hypot(x, y))` in Q1.15.
369    ///
370    /// For `x < 0` and `y = 0`, the phase approximates +1.
371    ///
372    /// # Errors
373    ///
374    /// - [`Error::InvalidInput`] when the vector is zero or has a modulus of at least one.
375    /// - [`Error::Timeout`] when the calculation does not complete.
376    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    /// Returns `(atan2(y, x) / π, hypot(x, y))` in Q1.31.
382    ///
383    /// For `x < 0` and `y = 0`, the phase approximates +1.
384    ///
385    /// # Errors
386    ///
387    /// - [`Error::InvalidInput`] when the vector is zero or has a modulus of at least one.
388    /// - [`Error::Timeout`] when the calculation does not complete.
389    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    /// Returns `atan2(y, x) / π` in Q1.15.
395    ///
396    /// For `x < 0` and `y = 0`, the phase approximates +1.
397    /// [`Self::phase_modulus_q15`] computes the phase and modulus in one operation.
398    ///
399    /// # Errors
400    ///
401    /// - [`Error::InvalidInput`] when the vector is zero or has a modulus of at least one.
402    /// - [`Error::Timeout`] when the calculation does not complete.
403    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    /// Returns `atan2(y, x) / π` in Q1.31.
409    ///
410    /// For `x < 0` and `y = 0`, the phase approximates +1.
411    /// [`Self::phase_modulus_q31`] computes the phase and modulus in one operation.
412    ///
413    /// # Errors
414    ///
415    /// - [`Error::InvalidInput`] when the vector is zero or has a modulus of at least one.
416    /// - [`Error::Timeout`] when the calculation does not complete.
417    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    /// Returns `hypot(x, y)` in Q1.15.
423    ///
424    /// [`Self::phase_modulus_q15`] computes the phase and modulus in one operation.
425    ///
426    /// # Errors
427    ///
428    /// - [`Error::InvalidInput`] when the vector has a modulus of at least one.
429    /// - [`Error::Timeout`] when the calculation does not complete.
430    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    /// Returns `hypot(x, y)` in Q1.31.
436    ///
437    /// [`Self::phase_modulus_q31`] computes the phase and modulus in one operation.
438    ///
439    /// # Errors
440    ///
441    /// - [`Error::InvalidInput`] when the vector has a modulus of at least one.
442    /// - [`Error::Timeout`] when the calculation does not complete.
443    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    /// Returns `atan(x) / π` in Q1.15.
449    ///
450    /// # Errors
451    ///
452    /// [`Error::Timeout`] when the calculation does not complete.
453    pub fn atan_q15(&mut self, x: i16) -> Result<i16, Error> {
454        self.calculate_q15(Function::Atan, x, None)
455    }
456
457    /// Returns `atan(x) / π` in Q1.31.
458    ///
459    /// # Errors
460    ///
461    /// [`Error::Timeout`] when the calculation does not complete.
462    pub fn atan_q31(&mut self, x: i32) -> Result<i32, Error> {
463        self.calculate_q31(Function::Atan, x, None)
464    }
465
466    /// Returns `(sinh(x) / 2, cosh(x) / 2)` in Q1.15 for input `x / 2`.
467    ///
468    /// # Errors
469    ///
470    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
471    /// - [`Error::Timeout`] when the calculation does not complete.
472    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    /// Returns `(sinh(x) / 2, cosh(x) / 2)` in Q1.31 for input `x / 2`.
478    ///
479    /// # Errors
480    ///
481    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
482    /// - [`Error::Timeout`] when the calculation does not complete.
483    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    /// Returns `sinh(x) / 2` in Q1.15 for input `x / 2`.
489    ///
490    /// [`Self::sinh_cosh_q15`] computes sinh and cosh in one operation.
491    ///
492    /// # Errors
493    ///
494    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
495    /// - [`Error::Timeout`] when the calculation does not complete.
496    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    /// Returns `sinh(x) / 2` in Q1.31 for input `x / 2`.
502    ///
503    /// [`Self::sinh_cosh_q31`] computes sinh and cosh in one operation.
504    ///
505    /// # Errors
506    ///
507    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
508    /// - [`Error::Timeout`] when the calculation does not complete.
509    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    /// Returns `cosh(x) / 2` in Q1.15 for input `x / 2`.
515    ///
516    /// [`Self::sinh_cosh_q15`] computes sinh and cosh in one operation.
517    ///
518    /// # Errors
519    ///
520    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
521    /// - [`Error::Timeout`] when the calculation does not complete.
522    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    /// Returns `cosh(x) / 2` in Q1.31 for input `x / 2`.
528    ///
529    /// [`Self::sinh_cosh_q31`] computes sinh and cosh in one operation.
530    ///
531    /// # Errors
532    ///
533    /// - [`Error::InvalidInput`] when `|x / 2| > 0.559`.
534    /// - [`Error::Timeout`] when the calculation does not complete.
535    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    /// Returns `atanh(x) / 2` in Q1.15 for input `x / 2`.
541    ///
542    /// # Errors
543    ///
544    /// - [`Error::InvalidInput`] when `|x / 2| > 0.403`.
545    /// - [`Error::Timeout`] when the calculation does not complete.
546    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    /// Returns `atanh(x) / 2` in Q1.31 for input `x / 2`.
552    ///
553    /// # Errors
554    ///
555    /// - [`Error::InvalidInput`] when `|x / 2| > 0.403`.
556    /// - [`Error::Timeout`] when the calculation does not complete.
557    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    /// Returns `ln(x) / 4` from the ratio `(x - 1) / (2 * (x + 1))`
563    /// for positive `x`, with the input and result in Q1.15.
564    ///
565    /// # Errors
566    ///
567    /// - [`Error::InvalidInput`] when `|ratio| > 0.403`.
568    /// - [`Error::Timeout`] when the calculation does not complete.
569    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    /// Returns `ln(x) / 4` from the ratio `(x - 1) / (2 * (x + 1))`
575    /// for positive `x`, with the input and result in Q1.31.
576    ///
577    /// # Errors
578    ///
579    /// - [`Error::InvalidInput`] when `|ratio| > 0.403`.
580    /// - [`Error::Timeout`] when the calculation does not complete.
581    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    /// Returns `sqrt(x)` in Q1.15.
587    ///
588    /// # Errors
589    ///
590    /// - [`Error::InvalidInput`] when `x` is negative.
591    /// - [`Error::UnsupportedPrecision`] when the iteration count exceeds 12.
592    /// - [`Error::Timeout`] when the calculation does not complete.
593    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    /// Returns `sqrt(x)` in Q1.31.
602    ///
603    /// # Errors
604    ///
605    /// - [`Error::InvalidInput`] when `x` is negative.
606    /// - [`Error::UnsupportedPrecision`] when the iteration count exceeds 12.
607    /// - [`Error::Timeout`] when the calculation does not complete.
608    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            // Normalize to [0.25, 1) for the hardware input range of [0.1069, 1).
624            // See <https://github.com/espressif/esp-idf/blob/0e6b480f858c69862ffb6001d96e9eef74599a6e/docs/en/api-reference/peripherals/cordic.rst#L99-L103>.
625            _ => 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}