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.peri.set_result_count(if self.config.first_result {
- Count::One
+ self.check_input_f64(arg1s, arg2s)?;
+
+ 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(),
+ };
- self.check_input_f32(arg1s, arg2s);
+ if !output_length_enough {
+ return Err(CordicError::OutputLengthNotEnough);
+ }
+
+ 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"
- ),
+ 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 => assert!(
- arg1s.iter().all(|v| (-0.403..=0.403).contains(v)),
- "ARG1 should be: -0.403 <= ARG1 <= 0.403"
- ),
+ 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::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!(),
- };
- }
+ 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,
- 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"
- ),
_ => unreachable!(),
},
+
+ Sqrt => match config.scale {
+ 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"
- ),
+ struct Arg2ErrInfo {
+ range: [f32; 2],
+ }
- Phase | Modulus => assert!(
- arg2s.iter().all(|v| (-1.0..=1.0).contains(v)),
- "ARG2 should be: -1 <= ARG2 <= 1"
- ),
+ 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,
});
})
}