stm32 CORDIC: error handle

4 files changed, 388 insertions, 264 deletions

diff --git a/embassy-stm32/src/cordic/enums.rs b/embassy-stm32/src/cordic/enums.rs
index 37c73f549..4b92a6cf8 100644
--- a/embassy-stm32/src/cordic/enums.rs
+++ b/embassy-stm32/src/cordic/enums.rs
@@ -1,6 +1,7 @@
 /// CORDIC function
 #[allow(missing_docs)]
-#[derive(Clone, Copy)]
+#[derive(Debug, Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum Function {
     Cos = 0,
     Sin,
@@ -16,7 +17,7 @@ pub enum Function {
 
 /// CORDIC precision
 #[allow(missing_docs)]
-#[derive(Clone, Copy, Default)]
+#[derive(Debug, Clone, Copy, Default)]
 pub enum Precision {
     Iters4 = 1,
     Iters8,
@@ -37,25 +38,25 @@ pub enum Precision {
 }
 
 /// CORDIC scale
-#[allow(non_camel_case_types)]
 #[allow(missing_docs)]
-#[derive(Clone, Copy, Default, PartialEq)]
+#[derive(Debug, Clone, Copy, Default, PartialEq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum Scale {
     #[default]
-    A1_R1 = 0,
-    A1o2_R2,
-    A1o4_R4,
-    A1o8_R8,
-    A1o16_R16,
-    A1o32_R32,
-    A1o64_R64,
-    A1o128_R128,
+    Arg1Res1 = 0,
+    Arg1o2Res2,
+    Arg1o4Res4,
+    Arg1o8Res8,
+    Arg1o16Res16,
+    Arg1o32Res32,
+    Arg1o64Res64,
+    Arg1o128Res128,
 }
 
-/// CORDIC argument/result count
+/// CORDIC argument/result register access count
 #[allow(missing_docs)]
 #[derive(Clone, Copy, Default)]
-pub enum Count {
+pub enum AccessCount {
     #[default]
     One,
     Two,
diff --git a/embassy-stm32/src/cordic/errors.rs b/embassy-stm32/src/cordic/errors.rs
new file mode 100644
index 000000000..d0b2dc618
--- /dev/null
+++ b/embassy-stm32/src/cordic/errors.rs
@@ -0,0 +1,95 @@
+use super::{Function, Scale};
+
+/// Error for [Cordic](super::Cordic)
+#[derive(Debug)]
+pub enum CordicError {
+    /// Config error
+    ConfigError(ConfigError),
+    /// Argument error
+    ArgError(ArgError),
+    /// Output buffer length error
+    OutputLengthNotEnough,
+}
+
+#[cfg(feature = "defmt")]
+impl defmt::Format for CordicError {
+    fn format(&self, fmt: defmt::Formatter) {
+        use CordicError::*;
+
+        match self {
+            ConfigError(e) => defmt::write!(fmt, "{}", e),
+            ArgError(e) => defmt::write!(fmt, "{}", e),
+            OutputLengthNotEnough => defmt::write!(fmt, "Output buffer length is not long enough"),
+        }
+    }
+}
+
+/// Error dring parsing [Cordic::Config](super::Config)
+#[derive(Debug)]
+pub struct ConfigError {
+    pub(super) func: Function,
+    pub(super) scale_range: [u8; 2],
+}
+
+#[cfg(feature = "defmt")]
+impl defmt::Format for ConfigError {
+    fn format(&self, fmt: defmt::Formatter) {
+        defmt::write!(fmt, "For FUNCTION: {},", self.func);
+
+        if self.scale_range[0] == self.scale_range[1] {
+            defmt::write!(fmt, " SCALE value should be {}", self.scale_range[0])
+        } else {
+            defmt::write!(
+                fmt,
+                " SCALE value should be {} <= SCALE <= {}",
+                self.scale_range[0],
+                self.scale_range[1]
+            )
+        }
+    }
+}
+
+/// Error on checking input arguments
+#[derive(Debug)]
+pub struct ArgError {
+    pub(super) func: Function,
+    pub(super) scale: Option<Scale>,
+    pub(super) arg_range: [f32; 2], // only for debug display, f32 is ok
+    pub(super) inclusive_upper_bound: bool,
+    pub(super) arg_type: ArgType,
+}
+
+#[cfg(feature = "defmt")]
+impl defmt::Format for ArgError {
+    fn format(&self, fmt: defmt::Formatter) {
+        defmt::write!(fmt, "For FUNCTION: {},", self.func);
+
+        if let Some(scale) = self.scale {
+            defmt::write!(fmt, " when SCALE is {},", scale);
+        }
+
+        let arg_string = match self.arg_type {
+            ArgType::Arg1 => "ARG1",
+            ArgType::Arg2 => "ARG2",
+        };
+
+        defmt::write!(fmt, " {} should be", arg_string);
+
+        let inclusive_string = if self.inclusive_upper_bound { "=" } else { "" };
+
+        defmt::write!(
+            fmt,
+            " {} <= {} <{} {}",
+            self.arg_range[0],
+            arg_string,
+            inclusive_string,
+            self.arg_range[1]
+        )
+    }
+}
+
+#[derive(Debug)]
+pub(super) enum ArgType {
+    Arg1,
+    Arg2,
+}
diff --git a/embassy-stm32/src/cordic/mod.rs b/embassy-stm32/src/cordic/mod.rs
index a4b98a770..5ac9addd8 100644
--- a/embassy-stm32/src/cordic/mod.rs
+++ b/embassy-stm32/src/cordic/mod.rs
@@ -1,4 +1,4 @@
-//! CORDIC co-processor
+//! coordinate rotation digital computer (CORDIC)
 
 use embassy_hal_internal::drop::OnDrop;
 use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
@@ -8,6 +8,9 @@ use crate::{dma, peripherals};
 mod enums;
 pub use enums::*;
 
+mod errors;
+pub use errors::*;
+
 pub mod utils;
 
 pub(crate) mod sealed;
@@ -30,33 +33,55 @@ pub struct Cordic<'d, T: Instance> {
 pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPeripheral {}
 
 /// CORDIC configuration
+#[derive(Debug)]
 pub struct Config {
     function: Function,
     precision: Precision,
     scale: Scale,
-    first_result: bool,
+    res1_only: bool,
 }
 
 impl Config {
     /// Create a config for Cordic driver
-    pub fn new(function: Function, precision: Option<Precision>, scale: Option<Scale>, first_result: bool) -> Self {
-        Self {
+    pub fn new(function: Function, precision: Precision, scale: Scale, res1_only: bool) -> Result<Self, CordicError> {
+        let config = Self {
             function,
-            precision: precision.unwrap_or_default(),
-            scale: scale.unwrap_or_default(),
-            first_result,
-        }
+            precision,
+            scale,
+            res1_only,
+        };
+
+        config.check_scale()?;
+
+        Ok(config)
     }
 
-    fn check_scale(&self) -> bool {
+    fn check_scale(&self) -> Result<(), CordicError> {
+        use Function::*;
+
         let scale_raw = self.scale as u8;
 
-        match self.function {
-            Function::Cos | Function::Sin | Function::Phase | Function::Modulus => 0 == scale_raw,
-            Function::Arctan => (0..=7).contains(&scale_raw),
-            Function::Cosh | Function::Sinh | Function::Arctanh => 1 == scale_raw,
-            Function::Ln => (1..=4).contains(&scale_raw),
-            Function::Sqrt => (0..=2).contains(&scale_raw),
+        let err_range = match self.function {
+            Cos | Sin | Phase | Modulus if !(0..=0).contains(&scale_raw) => Some([0, 0]),
+
+            Arctan if !(0..=7).contains(&scale_raw) => Some([0, 7]),
+
+            Cosh | Sinh | Arctanh if !(1..=1).contains(&scale_raw) => Some([1, 1]),
+
+            Ln if !(1..=4).contains(&scale_raw) => Some([1, 4]),
+
+            Sqrt if !(0..=2).contains(&scale_raw) => Some([0, 2]),
+
+            Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh | Ln | Sqrt => None,
+        };
+
+        if let Some(range) = err_range {
+            Err(CordicError::ConfigError(ConfigError {
+                func: self.function,
+                scale_range: range,
+            }))
+        } else {
+            Ok(())
         }
     }
 }
@@ -73,10 +98,6 @@ impl<'d, T: Instance> Cordic<'d, T> {
 
         into_ref!(peri);
 
-        if !config.check_scale() {
-            panic!("Scale value is not compatible with Function")
-        }
-
         let mut instance = Self { peri, config };
 
         instance.reconfigure();
@@ -91,21 +112,12 @@ impl<'d, T: Instance> Cordic<'d, T> {
     }
 
     /// Set extra config for data count and data width.
-    pub fn extra_config(&mut self, arg_cnt: Count, arg_width: Width, res_width: Width) {
+    pub fn extra_config(&mut self, arg_cnt: AccessCount, arg_width: Width, res_width: Width) {
         self.peri.set_argument_count(arg_cnt);
         self.peri.set_data_width(arg_width, res_width);
     }
 
     fn reconfigure(&mut self) {
-        if self.peri.ready_to_read() {
-            warn!("At least 1 result hasn't been read, reconfigure will cause DATA LOST");
-        };
-
-        // clean RRDY flag
-        while self.peri.ready_to_read() {
-            self.peri.read_result();
-        }
-
         self.peri.set_func(self.config.function);
         self.peri.set_precision(self.config.precision);
         self.peri.set_scale(self.config.scale);
@@ -113,6 +125,47 @@ impl<'d, T: Instance> Cordic<'d, T> {
         // we don't set NRES in here, but to make sure NRES is set each time user call "calc"-ish functions,
         // since each "calc"-ish functions can have different ARGSIZE and RESSIZE, thus NRES should be change accrodingly.
     }
+
+    async fn launch_a_dma_transfer(
+        &mut self,
+        write_dma: impl Peripheral<P = impl WriteDma<T>>,
+        read_dma: impl Peripheral<P = impl ReadDma<T>>,
+        input: &[u32],
+        output: &mut [u32],
+    ) {
+        into_ref!(write_dma, read_dma);
+
+        let write_req = write_dma.request();
+        let read_req = read_dma.request();
+
+        self.peri.enable_write_dma();
+        self.peri.enable_read_dma();
+
+        let _on_drop = OnDrop::new(|| {
+            self.peri.disable_write_dma();
+            self.peri.disable_read_dma();
+        });
+
+        unsafe {
+            let write_transfer = dma::Transfer::new_write(
+                &mut write_dma,
+                write_req,
+                input,
+                T::regs().wdata().as_ptr() as *mut _,
+                Default::default(),
+            );
+
+            let read_transfer = dma::Transfer::new_read(
+                &mut read_dma,
+                read_req,
+                T::regs().rdata().as_ptr() as *mut _,
+                output,
+                Default::default(),
+            );
+
+            embassy_futures::join::join(write_transfer, read_transfer).await;
+        }
+    }
 }
 
 impl<'d, T: Instance> Drop for Cordic<'d, T> {
@@ -124,25 +177,31 @@ impl<'d, T: Instance> Drop for Cordic<'d, T> {
 // q1.31 related
 impl<'d, T: Instance> Cordic<'d, T> {
     /// Run a blocking CORDIC calculation in q1.31 format
-    pub fn blocking_calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
+    pub fn blocking_calc_32bit(
+        &mut self,
+        arg1s: &[f64],
+        arg2s: Option<&[f64]>,
+        output: &mut [f64],
+    ) -> Result<usize, CordicError> {
         if arg1s.is_empty() {
-            return 0;
+            return Ok(0);
         }
 
-        assert!(
-            match self.config.first_result {
-                true => output.len() >= arg1s.len(),
-                false => output.len() >= 2 * arg1s.len(),
-            },
-            "Output buf length is not long enough"
-        );
+        let output_length_enough = match self.config.res1_only {
+            true => output.len() >= arg1s.len(),
+            false => output.len() >= 2 * arg1s.len(),
+        };
 
-        self.check_input_f64(arg1s, arg2s);
+        if !output_length_enough {
+            return Err(CordicError::OutputLengthNotEnough);
+        }
+
+        self.check_input_f64(arg1s, arg2s)?;
 
-        self.peri.set_result_count(if self.config.first_result {
-            Count::One
+        self.peri.set_result_count(if self.config.res1_only {
+            AccessCount::One
         } else {
-            Count::Two
+            AccessCount::Two
         });
 
         self.peri.set_data_width(Width::Bits32, Width::Bits32);
@@ -155,10 +214,10 @@ impl<'d, T: Instance> Cordic<'d, T> {
         // handle 2 input args calculation
         //
 
-        if arg2s.is_some() && !arg2s.expect("It's infailable").is_empty() {
-            let arg2s = arg2s.expect("It's infailable");
+        if arg2s.is_some() && !arg2s.unwrap().is_empty() {
+            let arg2s = arg2s.unwrap();
 
-            self.peri.set_argument_count(Count::Two);
+            self.peri.set_argument_count(AccessCount::Two);
 
             // Skip 1st value from arg1s, this value will be manually "preload" to cordic, to make use of cordic preload function.
             // And we preserve last value from arg2s, since it need to manually write to cordic, and read the result out.
@@ -191,7 +250,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
         let input_left = &arg1s[consumed_input_len..];
 
         if !input_left.is_empty() {
-            self.peri.set_argument_count(Count::One);
+            self.peri.set_argument_count(AccessCount::One);
 
             // "preload" value to cordic (at this moment, cordic start to calculating)
             self.blocking_write_f64(input_left[0]);
@@ -207,7 +266,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
             self.blocking_read_f64_to_buf(output, &mut output_count);
         }
 
-        output_count
+        Ok(output_count)
     }
 
     fn blocking_read_f64_to_buf(&mut self, result_buf: &mut [f64], result_index: &mut usize) {
@@ -216,7 +275,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
 
         // We don't care about whether the function return 1 or 2 results,
         // the only thing matter is whether user want 1 or 2 results.
-        if !self.config.first_result {
+        if !self.config.res1_only {
             result_buf[*result_index] = utils::q1_31_to_f64(self.peri.read_result());
             *result_index += 1;
         }
@@ -234,27 +293,28 @@ impl<'d, T: Instance> Cordic<'d, T> {
         arg1s: &[f64],
         arg2s: Option<&[f64]>,
         output: &mut [f64],
-    ) -> usize {
+    ) -> Result<usize, CordicError> {
         if arg1s.is_empty() {
-            return 0;
+            return Ok(0);
         }
 
-        assert!(
-            match self.config.first_result {
-                true => output.len() >= arg1s.len(),
-                false => output.len() >= 2 * arg1s.len(),
-            },
-            "Output buf length is not long enough"
-        );
+        let output_length_enough = match self.config.res1_only {
+            true => output.len() >= arg1s.len(),
+            false => output.len() >= 2 * arg1s.len(),
+        };
 
-        self.check_input_f64(arg1s, arg2s);
+        if !output_length_enough {
+            return Err(CordicError::OutputLengthNotEnough);
+        }
+
+        self.check_input_f64(arg1s, arg2s)?;
 
         into_ref!(write_dma, read_dma);
 
-        self.peri.set_result_count(if self.config.first_result {
-            Count::One
+        self.peri.set_result_count(if self.config.res1_only {
+            AccessCount::One
         } else {
-            Count::Two
+            AccessCount::Two
         });
 
         self.peri.set_data_width(Width::Bits32, Width::Bits32);
@@ -269,9 +329,9 @@ impl<'d, T: Instance> Cordic<'d, T> {
         //
 
         if !arg2s.unwrap_or_default().is_empty() {
-            let arg2s = arg2s.expect("It's infailable");
+            let arg2s = arg2s.unwrap();
 
-            self.peri.set_argument_count(Count::Two);
+            self.peri.set_argument_count(AccessCount::Two);
 
             let double_input = arg1s.iter().zip(arg2s);
 
@@ -320,7 +380,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
         if arg1s.len() > consumed_input_len {
             let input_remain = &arg1s[consumed_input_len..];
 
-            self.peri.set_argument_count(Count::One);
+            self.peri.set_argument_count(AccessCount::One);
 
             for &arg in input_remain {
                 input_buf[input_buf_len] = utils::f64_to_q1_31(arg);
@@ -356,7 +416,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
             }
         }
 
-        output_count
+        Ok(output_count)
     }
 
     // this function is highly coupled with async_calc_32bit, and is not intended to use in other place
@@ -369,11 +429,6 @@ impl<'d, T: Instance> Cordic<'d, T> {
         output: &mut [f64],             // caller uses should this as a final output array
         output_start_index: &mut usize, // the index of start point of the output for this round of calculation
     ) {
-        into_ref!(write_dma, read_dma);
-
-        let write_req = write_dma.request();
-        let read_req = read_dma.request();
-
         // output_buf is the place to store raw value from CORDIC (via DMA).
         // For buf size, we assume in this round of calculation:
         // all input is 1 arg, and all calculation need 2 output,
@@ -381,7 +436,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
         let mut output_buf = [0u32; INPUT_BUF_MAX_LEN * 2];
 
         let mut output_buf_size = input_buf.len();
-        if !self.config.first_result {
+        if !self.config.res1_only {
             // if we need 2 result for 1 input, then output_buf length should be 2x long.
             output_buf_size *= 2;
         };
@@ -392,35 +447,8 @@ impl<'d, T: Instance> Cordic<'d, T> {
 
         let active_output_buf = &mut output_buf[..output_buf_size];
 
-        self.peri.enable_write_dma();
-        self.peri.enable_read_dma();
-
-        let on_drop = OnDrop::new(|| {
-            self.peri.disable_write_dma();
-            self.peri.disable_read_dma();
-        });
-
-        unsafe {
-            let write_transfer = dma::Transfer::new_write(
-                &mut write_dma,
-                write_req,
-                input_buf,
-                T::regs().wdata().as_ptr() as *mut _,
-                Default::default(),
-            );
-
-            let read_transfer = dma::Transfer::new_read(
-                &mut read_dma,
-                read_req,
-                T::regs().rdata().as_ptr() as *mut _,
-                active_output_buf,
-                Default::default(),
-            );
-
-            embassy_futures::join::join(write_transfer, read_transfer).await;
-        }
-
-        drop(on_drop);
+        self.launch_a_dma_transfer(write_dma, read_dma, input_buf, active_output_buf)
+            .await;
 
         for &mut output_u32 in active_output_buf {
             output[*output_start_index] = utils::q1_31_to_f64(output_u32);
@@ -432,24 +460,30 @@ impl<'d, T: Instance> Cordic<'d, T> {
 // q1.15 related
 impl<'d, T: Instance> Cordic<'d, T> {
     /// Run a blocking CORDIC calculation in q1.15 format
-    pub fn blocking_calc_16bit(&mut self, arg1s: &[f32], arg2s: Option<&[f32]>, output: &mut [f32]) -> usize {
+    pub fn blocking_calc_16bit(
+        &mut self,
+        arg1s: &[f32],
+        arg2s: Option<&[f32]>,
+        output: &mut [f32],
+    ) -> Result<usize, CordicError> {
         if arg1s.is_empty() {
-            return 0;
+            return Ok(0);
         }
 
-        assert!(
-            match self.config.first_result {
-                true => output.len() >= arg1s.len(),
-                false => output.len() >= 2 * arg1s.len(),
-            },
-            "Output buf length is not long enough"
-        );
+        let output_length_enough = match self.config.res1_only {
+            true => output.len() >= arg1s.len(),
+            false => output.len() >= 2 * arg1s.len(),
+        };
+
+        if !output_length_enough {
+            return Err(CordicError::OutputLengthNotEnough);
+        }
 
-        self.check_input_f32(arg1s, arg2s);
+        self.check_input_f32(arg1s, arg2s)?;
 
         // In q1.15 mode, 1 write/read to access 2 arguments/results
-        self.peri.set_argument_count(Count::One);
-        self.peri.set_result_count(Count::One);
+        self.peri.set_argument_count(AccessCount::One);
+        self.peri.set_result_count(AccessCount::One);
 
         self.peri.set_data_width(Width::Bits16, Width::Bits16);
 
@@ -472,9 +506,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
                 .chain(core::iter::repeat(&arg2_default_value)),
         );
 
-        let (&arg1, &arg2) = args
-            .next()
-            .expect("This should be infallible, since arg1s is not empty");
+        let (&arg1, &arg2) = args.next().unwrap();
 
         // preloading 1 pair of arguments
         self.blocking_write_f32(arg1, arg2);
@@ -487,7 +519,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
         // read last pair of value from cordic
         self.blocking_read_f32_to_buf(output, &mut output_count);
 
-        output_count
+        Ok(output_count)
     }
 
     fn blocking_write_f32(&mut self, arg1: f32, arg2: f32) {
@@ -505,7 +537,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
 
         // We don't care about whether the function return 1 or 2 results,
         // the only thing matter is whether user want 1 or 2 results.
-        if !self.config.first_result {
+        if !self.config.res1_only {
             result_buf[*result_index] = res2;
             *result_index += 1;
         }
@@ -519,26 +551,27 @@ impl<'d, T: Instance> Cordic<'d, T> {
         arg1s: &[f32],
         arg2s: Option<&[f32]>,
         output: &mut [f32],
-    ) -> usize {
+    ) -> Result<usize, CordicError> {
         if arg1s.is_empty() {
-            return 0;
+            return Ok(0);
         }
 
-        assert!(
-            match self.config.first_result {
-                true => output.len() >= arg1s.len(),
-                false => output.len() >= 2 * arg1s.len(),
-            },
-            "Output buf length is not long enough"
-        );
+        let output_length_enough = match self.config.res1_only {
+            true => output.len() >= arg1s.len(),
+            false => output.len() >= 2 * arg1s.len(),
+        };
 
-        self.check_input_f32(arg1s, arg2s);
+        if !output_length_enough {
+            return Err(CordicError::OutputLengthNotEnough);
+        }
+
+        self.check_input_f32(arg1s, arg2s)?;
 
         into_ref!(write_dma, read_dma);
 
         // In q1.15 mode, 1 write/read to access 2 arguments/results
-        self.peri.set_argument_count(Count::One);
-        self.peri.set_result_count(Count::One);
+        self.peri.set_argument_count(AccessCount::One);
+        self.peri.set_result_count(AccessCount::One);
 
         self.peri.set_data_width(Width::Bits16, Width::Bits16);
 
@@ -584,7 +617,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
             .await;
         }
 
-        output_count
+        Ok(output_count)
     }
 
     // this function is highly coupled with async_calc_16bit, and is not intended to use in other place
@@ -596,45 +629,13 @@ impl<'d, T: Instance> Cordic<'d, T> {
         output: &mut [f32], // caller uses should this as a final output array
         output_start_index: &mut usize, // the index of start point of the output for this round of calculation
     ) {
-        into_ref!(write_dma, read_dma);
-
-        let write_req = write_dma.request();
-        let read_req = read_dma.request();
-
         // output_buf is the place to store raw value from CORDIC (via DMA).
         let mut output_buf = [0u32; INPUT_BUF_MAX_LEN];
 
         let active_output_buf = &mut output_buf[..input_buf.len()];
 
-        self.peri.enable_write_dma();
-        self.peri.enable_read_dma();
-
-        let on_drop = OnDrop::new(|| {
-            self.peri.disable_write_dma();
-            self.peri.disable_read_dma();
-        });
-
-        unsafe {
-            let write_transfer = dma::Transfer::new_write(
-                &mut write_dma,
-                write_req,
-                input_buf,
-                T::regs().wdata().as_ptr() as *mut _,
-                Default::default(),
-            );
-
-            let read_transfer = dma::Transfer::new_read(
-                &mut read_dma,
-                read_req,
-                T::regs().rdata().as_ptr() as *mut _,
-                active_output_buf,
-                Default::default(),
-            );
-
-            embassy_futures::join::join(write_transfer, read_transfer).await;
-        }
-
-        drop(on_drop);
+        self.launch_a_dma_transfer(write_dma, read_dma, input_buf, active_output_buf)
+            .await;
 
         for &mut output_u32 in active_output_buf {
             let (res1, res2) = utils::u32_to_f32_res(output_u32);
@@ -642,7 +643,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
             output[*output_start_index] = res1;
             *output_start_index += 1;
 
-            if !self.config.first_result {
+            if !self.config.res1_only {
                 output[*output_start_index] = res2;
                 *output_start_index += 1;
             }
@@ -654,104 +655,131 @@ impl<'d, T: Instance> Cordic<'d, T> {
 macro_rules! check_input_value {
     ($func_name:ident, $float_type:ty) => {
         impl<'d, T: Instance> Cordic<'d, T> {
-            fn $func_name(&self, arg1s: &[$float_type], arg2s: Option<&[$float_type]>) {
+            fn $func_name(&self, arg1s: &[$float_type], arg2s: Option<&[$float_type]>) -> Result<(), CordicError> {
                 let config = &self.config;
 
                 use Function::*;
 
-                // check SCALE value
-                match config.function {
-                    Cos | Sin | Phase | Modulus => assert!(Scale::A1_R1 == config.scale, "SCALE should be 0"),
-                    Arctan => assert!(
-                        (0..=7).contains(&(config.scale as u8)),
-                        "SCALE should be: 0 <= SCALE <= 7"
-                    ),
-                    Cosh | Sinh | Arctanh => assert!(Scale::A1o2_R2 == config.scale, "SCALE should be 1"),
-
-                    Ln => assert!(
-                        (1..=4).contains(&(config.scale as u8)),
-                        "SCALE should be: 1 <= SCALE <= 4"
-                    ),
-                    Sqrt => assert!(
-                        (0..=2).contains(&(config.scale as u8)),
-                        "SCALE should be: 0 <= SCALE <= 2"
-                    ),
+                struct Arg1ErrInfo {
+                    scale: Option<Scale>,
+                    range: [f32; 2],
+                    inclusive_upper_bound: bool,
                 }
 
                 // check ARG1 value
-                match config.function {
-                    Cos | Sin | Phase | Modulus | Arctan => {
-                        assert!(
-                            arg1s.iter().all(|v| (-1.0..=1.0).contains(v)),
-                            "ARG1 should be: -1 <= ARG1 <= 1"
-                        );
+                let err_info = match config.function {
+                    Cos | Sin | Phase | Modulus | Arctan if arg1s.iter().any(|v| !(-1.0..=1.0).contains(v)) => {
+                        Some(Arg1ErrInfo {
+                            scale: None,
+                            range: [-1.0, 1.0],
+                            inclusive_upper_bound: true,
+                        })
                     }
 
-                    Cosh | Sinh => assert!(
-                        arg1s.iter().all(|v| (-0.559..=0.559).contains(v)),
-                        "ARG1 should be: -0.559 <= ARG1 <= 0.559"
-                    ),
-
-                    Arctanh => assert!(
-                        arg1s.iter().all(|v| (-0.403..=0.403).contains(v)),
-                        "ARG1 should be: -0.403 <= ARG1 <= 0.403"
-                    ),
-
-                    Ln => {
-                        match config.scale {
-                            Scale::A1o2_R2 => assert!(
-                                arg1s.iter().all(|v| (0.05354..0.5).contains(v)),
-                                "When SCALE set to 1, ARG1 should be: 0.05354 <= ARG1 < 0.5"
-                            ),
-                            Scale::A1o4_R4 => assert!(
-                                arg1s.iter().all(|v| (0.25..0.75).contains(v)),
-                                "When SCALE set to 2, ARG1 should be: 0.25 <= ARG1 < 0.75"
-                            ),
-                            Scale::A1o8_R8 => assert!(
-                                arg1s.iter().all(|v| (0.375..0.875).contains(v)),
-                                "When SCALE set to 3, ARG1 should be: 0.375 <= ARG1 < 0.875"
-                            ),
-                            Scale::A1o16_R16 => assert!(
-                                arg1s.iter().all(|v| (0.4375..0.584).contains(v)),
-                                "When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584"
-                            ),
-                            _ => unreachable!(),
-                        };
-                    }
+                    Cosh | Sinh if arg1s.iter().any(|v| !(-0.559..=0.559).contains(v)) => Some(Arg1ErrInfo {
+                        scale: None,
+                        range: [-0.559, 0.559],
+                        inclusive_upper_bound: true,
+                    }),
+
+                    Arctanh if arg1s.iter().any(|v| !(-0.403..=0.403).contains(v)) => Some(Arg1ErrInfo {
+                        scale: None,
+                        range: [-0.403, 0.403],
+                        inclusive_upper_bound: true,
+                    }),
+
+                    Ln => match config.scale {
+                        Scale::Arg1o2Res2 if arg1s.iter().any(|v| !(0.0535..0.5).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1o2Res2),
+                            range: [0.0535, 0.5],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1o4Res4 if arg1s.iter().any(|v| !(0.25..0.75).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1o4Res4),
+                            range: [0.25, 0.75],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1o8Res8 if arg1s.iter().any(|v| !(0.375..0.875).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1o8Res8),
+                            range: [0.375, 0.875],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1o16Res16 if arg1s.iter().any(|v| !(0.4375..0.584).contains(v)) => {
+                            Some(Arg1ErrInfo {
+                                scale: Some(Scale::Arg1o16Res16),
+                                range: [0.4375, 0.584],
+                                inclusive_upper_bound: false,
+                            })
+                        }
+
+                        Scale::Arg1o2Res2 | Scale::Arg1o4Res4 | Scale::Arg1o8Res8 | Scale::Arg1o16Res16 => None,
+
+                        _ => unreachable!(),
+                    },
 
                     Sqrt => match config.scale {
-                        Scale::A1_R1 => assert!(
-                            arg1s.iter().all(|v| (0.027..0.75).contains(v)),
-                            "When SCALE set to 0, ARG1 should be: 0.027 <= ARG1 < 0.75"
-                        ),
-                        Scale::A1o2_R2 => assert!(
-                            arg1s.iter().all(|v| (0.375..0.875).contains(v)),
-                            "When SCALE set to 1, ARG1 should be: 0.375 <= ARG1 < 0.875"
-                        ),
-                        Scale::A1o4_R4 => assert!(
-                            arg1s.iter().all(|v| (0.4375..0.585).contains(v)),
-                            "When SCALE set to 2, ARG1 should be: 0.4375  <= ARG1 < 0.585"
-                        ),
+                        Scale::Arg1Res1 if arg1s.iter().any(|v| !(0.027..0.75).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1Res1),
+                            range: [0.027, 0.75],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1o2Res2 if arg1s.iter().any(|v| !(0.375..0.875).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1o2Res2),
+                            range: [0.375, 0.875],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1o4Res4 if arg1s.iter().any(|v| !(0.4375..0.584).contains(v)) => Some(Arg1ErrInfo {
+                            scale: Some(Scale::Arg1o4Res4),
+                            range: [0.4375, 0.584],
+                            inclusive_upper_bound: false,
+                        }),
+                        Scale::Arg1Res1 | Scale::Arg1o2Res2 | Scale::Arg1o4Res4 => None,
                         _ => unreachable!(),
                     },
+
+                    Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh => None,
+                };
+
+                if let Some(err) = err_info {
+                    return Err(CordicError::ArgError(ArgError {
+                        func: config.function,
+                        scale: err.scale,
+                        arg_range: err.range,
+                        inclusive_upper_bound: err.inclusive_upper_bound,
+                        arg_type: ArgType::Arg1,
+                    }));
                 }
 
                 // check ARG2 value
                 if let Some(arg2s) = arg2s {
-                    match config.function {
-                        Cos | Sin => assert!(
-                            arg2s.iter().all(|v| (0.0..=1.0).contains(v)),
-                            "ARG2 should be: 0 <= ARG2 <= 1"
-                        ),
-
-                        Phase | Modulus => assert!(
-                            arg2s.iter().all(|v| (-1.0..=1.0).contains(v)),
-                            "ARG2 should be: -1 <= ARG2 <= 1"
-                        ),
-
-                        _ => (),
+                    struct Arg2ErrInfo {
+                        range: [f32; 2],
+                    }
+
+                    let err_info = match config.function {
+                        Cos | Sin if arg2s.iter().any(|v| !(0.0..=1.0).contains(v)) => {
+                            Some(Arg2ErrInfo { range: [0.0, 1.0] })
+                        }
+
+                        Phase | Modulus if arg2s.iter().any(|v| !(-1.0..=1.0).contains(v)) => {
+                            Some(Arg2ErrInfo { range: [-1.0, 1.0] })
+                        }
+
+                        Cos | Sin | Phase | Modulus | Arctan | Cosh | Sinh | Arctanh | Ln | Sqrt => None,
+                    };
+
+                    if let Some(err) = err_info {
+                        return Err(CordicError::ArgError(ArgError {
+                            func: config.function,
+                            scale: None,
+                            arg_range: err.range,
+                            inclusive_upper_bound: true,
+                            arg_type: ArgType::Arg2,
+                        }));
                     }
                 }
+
+                Ok(())
             }
         }
     };
diff --git a/embassy-stm32/src/cordic/sealed.rs b/embassy-stm32/src/cordic/sealed.rs
index 0f00e380c..f9521ff7a 100644
--- a/embassy-stm32/src/cordic/sealed.rs
+++ b/embassy-stm32/src/cordic/sealed.rs
@@ -66,21 +66,21 @@ pub trait Instance {
     }
 
     /// Set NARGS value
-    fn set_argument_count(&self, n: Count) {
+    fn set_argument_count(&self, n: AccessCount) {
         Self::regs().csr().modify(|v| {
             v.set_nargs(match n {
-                Count::One => vals::Num::NUM1,
-                Count::Two => vals::Num::NUM2,
+                AccessCount::One => vals::Num::NUM1,
+                AccessCount::Two => vals::Num::NUM2,
             })
         })
     }
 
     /// Set NRES value
-    fn set_result_count(&self, n: Count) {
+    fn set_result_count(&self, n: AccessCount) {
         Self::regs().csr().modify(|v| {
             v.set_nres(match n {
-                Count::One => vals::Num::NUM1,
-                Count::Two => vals::Num::NUM2,
+                AccessCount::One => vals::Num::NUM1,
+                AccessCount::Two => vals::Num::NUM2,
             });
         })
     }