stm32 CORDIC: error handle

This commit is contained in:
eZio Pan 2024-03-19 22:19:06 +08:00
parent 10a9cce855
commit 641da3602e
4 changed files with 385 additions and 261 deletions
embassy-stm32/src/cordic

View file

@ -1,6 +1,7 @@
/// CORDIC function /// CORDIC function
#[allow(missing_docs)] #[allow(missing_docs)]
#[derive(Clone, Copy)] #[derive(Debug, Clone, Copy)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum Function { pub enum Function {
Cos = 0, Cos = 0,
Sin, Sin,
@ -16,7 +17,7 @@ pub enum Function {
/// CORDIC precision /// CORDIC precision
#[allow(missing_docs)] #[allow(missing_docs)]
#[derive(Clone, Copy, Default)] #[derive(Debug, Clone, Copy, Default)]
pub enum Precision { pub enum Precision {
Iters4 = 1, Iters4 = 1,
Iters8, Iters8,
@ -37,25 +38,25 @@ pub enum Precision {
} }
/// CORDIC scale /// CORDIC scale
#[allow(non_camel_case_types)]
#[allow(missing_docs)] #[allow(missing_docs)]
#[derive(Clone, Copy, Default, PartialEq)] #[derive(Debug, Clone, Copy, Default, PartialEq)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum Scale { pub enum Scale {
#[default] #[default]
A1_R1 = 0, Arg1Res1 = 0,
A1o2_R2, Arg1o2Res2,
A1o4_R4, Arg1o4Res4,
A1o8_R8, Arg1o8Res8,
A1o16_R16, Arg1o16Res16,
A1o32_R32, Arg1o32Res32,
A1o64_R64, Arg1o64Res64,
A1o128_R128, Arg1o128Res128,
} }
/// CORDIC argument/result count /// CORDIC argument/result register access count
#[allow(missing_docs)] #[allow(missing_docs)]
#[derive(Clone, Copy, Default)] #[derive(Clone, Copy, Default)]
pub enum Count { pub enum AccessCount {
#[default] #[default]
One, One,
Two, Two,

View file

@ -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,
}

View file

@ -1,4 +1,4 @@
//! CORDIC co-processor //! coordinate rotation digital computer (CORDIC)
use embassy_hal_internal::drop::OnDrop; use embassy_hal_internal::drop::OnDrop;
use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef}; use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
@ -8,6 +8,9 @@ use crate::{dma, peripherals};
mod enums; mod enums;
pub use enums::*; pub use enums::*;
mod errors;
pub use errors::*;
pub mod utils; pub mod utils;
pub(crate) mod sealed; 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 {} pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPeripheral {}
/// CORDIC configuration /// CORDIC configuration
#[derive(Debug)]
pub struct Config { pub struct Config {
function: Function, function: Function,
precision: Precision, precision: Precision,
scale: Scale, scale: Scale,
first_result: bool, res1_only: bool,
} }
impl Config { impl Config {
/// Create a config for Cordic driver /// Create a config for Cordic driver
pub fn new(function: Function, precision: Option<Precision>, scale: Option<Scale>, first_result: bool) -> Self { pub fn new(function: Function, precision: Precision, scale: Scale, res1_only: bool) -> Result<Self, CordicError> {
Self { let config = Self {
function, function,
precision: precision.unwrap_or_default(), precision,
scale: scale.unwrap_or_default(), scale,
first_result, 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; let scale_raw = self.scale as u8;
match self.function { let err_range = match self.function {
Function::Cos | Function::Sin | Function::Phase | Function::Modulus => 0 == scale_raw, Cos | Sin | Phase | Modulus if !(0..=0).contains(&scale_raw) => Some([0, 0]),
Function::Arctan => (0..=7).contains(&scale_raw),
Function::Cosh | Function::Sinh | Function::Arctanh => 1 == scale_raw, Arctan if !(0..=7).contains(&scale_raw) => Some([0, 7]),
Function::Ln => (1..=4).contains(&scale_raw),
Function::Sqrt => (0..=2).contains(&scale_raw), 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); into_ref!(peri);
if !config.check_scale() {
panic!("Scale value is not compatible with Function")
}
let mut instance = Self { peri, config }; let mut instance = Self { peri, config };
instance.reconfigure(); instance.reconfigure();
@ -91,21 +112,12 @@ impl<'d, T: Instance> Cordic<'d, T> {
} }
/// Set extra config for data count and data width. /// 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_argument_count(arg_cnt);
self.peri.set_data_width(arg_width, res_width); self.peri.set_data_width(arg_width, res_width);
} }
fn reconfigure(&mut self) { 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_func(self.config.function);
self.peri.set_precision(self.config.precision); self.peri.set_precision(self.config.precision);
self.peri.set_scale(self.config.scale); 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, // 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. // 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> { 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 // q1.31 related
impl<'d, T: Instance> Cordic<'d, T> { impl<'d, T: Instance> Cordic<'d, T> {
/// Run a blocking CORDIC calculation in q1.31 format /// 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() { if arg1s.is_empty() {
return 0; return Ok(0);
} }
assert!( let output_length_enough = match self.config.res1_only {
match self.config.first_result { true => output.len() >= arg1s.len(),
true => output.len() >= arg1s.len(), false => output.len() >= 2 * arg1s.len(),
false => output.len() >= 2 * arg1s.len(), };
},
"Output buf length is not long enough"
);
self.check_input_f64(arg1s, arg2s); if !output_length_enough {
return Err(CordicError::OutputLengthNotEnough);
}
self.peri.set_result_count(if self.config.first_result { self.check_input_f64(arg1s, arg2s)?;
Count::One
self.peri.set_result_count(if self.config.res1_only {
AccessCount::One
} else { } else {
Count::Two AccessCount::Two
}); });
self.peri.set_data_width(Width::Bits32, Width::Bits32); 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 // handle 2 input args calculation
// //
if arg2s.is_some() && !arg2s.expect("It's infailable").is_empty() { if arg2s.is_some() && !arg2s.unwrap().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);
// Skip 1st value from arg1s, this value will be manually "preload" to cordic, to make use of cordic preload function. // 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. // 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..]; let input_left = &arg1s[consumed_input_len..];
if !input_left.is_empty() { 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) // "preload" value to cordic (at this moment, cordic start to calculating)
self.blocking_write_f64(input_left[0]); 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); 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) { 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, // 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. // 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_buf[*result_index] = utils::q1_31_to_f64(self.peri.read_result());
*result_index += 1; *result_index += 1;
} }
@ -234,27 +293,28 @@ impl<'d, T: Instance> Cordic<'d, T> {
arg1s: &[f64], arg1s: &[f64],
arg2s: Option<&[f64]>, arg2s: Option<&[f64]>,
output: &mut [f64], output: &mut [f64],
) -> usize { ) -> Result<usize, CordicError> {
if arg1s.is_empty() { if arg1s.is_empty() {
return 0; return Ok(0);
} }
assert!( let output_length_enough = match self.config.res1_only {
match self.config.first_result { true => output.len() >= arg1s.len(),
true => output.len() >= arg1s.len(), false => output.len() >= 2 * arg1s.len(),
false => output.len() >= 2 * arg1s.len(), };
},
"Output buf length is not long enough"
);
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); into_ref!(write_dma, read_dma);
self.peri.set_result_count(if self.config.first_result { self.peri.set_result_count(if self.config.res1_only {
Count::One AccessCount::One
} else { } else {
Count::Two AccessCount::Two
}); });
self.peri.set_data_width(Width::Bits32, Width::Bits32); 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() { 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); let double_input = arg1s.iter().zip(arg2s);
@ -320,7 +380,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
if arg1s.len() > consumed_input_len { if arg1s.len() > consumed_input_len {
let input_remain = &arg1s[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 { for &arg in input_remain {
input_buf[input_buf_len] = utils::f64_to_q1_31(arg); 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 // 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: &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 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). // output_buf is the place to store raw value from CORDIC (via DMA).
// For buf size, we assume in this round of calculation: // For buf size, we assume in this round of calculation:
// all input is 1 arg, and all calculation need 2 output, // 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 = [0u32; INPUT_BUF_MAX_LEN * 2];
let mut output_buf_size = input_buf.len(); 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. // if we need 2 result for 1 input, then output_buf length should be 2x long.
output_buf_size *= 2; 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]; let active_output_buf = &mut output_buf[..output_buf_size];
self.peri.enable_write_dma(); self.launch_a_dma_transfer(write_dma, read_dma, input_buf, active_output_buf)
self.peri.enable_read_dma(); .await;
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);
for &mut output_u32 in active_output_buf { for &mut output_u32 in active_output_buf {
output[*output_start_index] = utils::q1_31_to_f64(output_u32); 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 // q1.15 related
impl<'d, T: Instance> Cordic<'d, T> { impl<'d, T: Instance> Cordic<'d, T> {
/// Run a blocking CORDIC calculation in q1.15 format /// 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() { if arg1s.is_empty() {
return 0; return Ok(0);
} }
assert!( let output_length_enough = match self.config.res1_only {
match self.config.first_result { true => output.len() >= arg1s.len(),
true => output.len() >= arg1s.len(), false => output.len() >= 2 * arg1s.len(),
false => output.len() >= 2 * arg1s.len(), };
},
"Output buf length is not long enough"
);
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 // In q1.15 mode, 1 write/read to access 2 arguments/results
self.peri.set_argument_count(Count::One); self.peri.set_argument_count(AccessCount::One);
self.peri.set_result_count(Count::One); self.peri.set_result_count(AccessCount::One);
self.peri.set_data_width(Width::Bits16, Width::Bits16); 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)), .chain(core::iter::repeat(&arg2_default_value)),
); );
let (&arg1, &arg2) = args let (&arg1, &arg2) = args.next().unwrap();
.next()
.expect("This should be infallible, since arg1s is not empty");
// preloading 1 pair of arguments // preloading 1 pair of arguments
self.blocking_write_f32(arg1, arg2); self.blocking_write_f32(arg1, arg2);
@ -487,7 +519,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
// read last pair of value from cordic // read last pair of value from cordic
self.blocking_read_f32_to_buf(output, &mut output_count); 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) { 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, // 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. // 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_buf[*result_index] = res2;
*result_index += 1; *result_index += 1;
} }
@ -519,26 +551,27 @@ impl<'d, T: Instance> Cordic<'d, T> {
arg1s: &[f32], arg1s: &[f32],
arg2s: Option<&[f32]>, arg2s: Option<&[f32]>,
output: &mut [f32], output: &mut [f32],
) -> usize { ) -> Result<usize, CordicError> {
if arg1s.is_empty() { if arg1s.is_empty() {
return 0; return Ok(0);
} }
assert!( let output_length_enough = match self.config.res1_only {
match self.config.first_result { true => output.len() >= arg1s.len(),
true => output.len() >= arg1s.len(), false => output.len() >= 2 * arg1s.len(),
false => output.len() >= 2 * arg1s.len(), };
},
"Output buf length is not long enough"
);
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); into_ref!(write_dma, read_dma);
// In q1.15 mode, 1 write/read to access 2 arguments/results // In q1.15 mode, 1 write/read to access 2 arguments/results
self.peri.set_argument_count(Count::One); self.peri.set_argument_count(AccessCount::One);
self.peri.set_result_count(Count::One); self.peri.set_result_count(AccessCount::One);
self.peri.set_data_width(Width::Bits16, Width::Bits16); self.peri.set_data_width(Width::Bits16, Width::Bits16);
@ -584,7 +617,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
.await; .await;
} }
output_count Ok(output_count)
} }
// this function is highly coupled with async_calc_16bit, and is not intended to use in other place // 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: &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 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). // output_buf is the place to store raw value from CORDIC (via DMA).
let mut output_buf = [0u32; INPUT_BUF_MAX_LEN]; let mut output_buf = [0u32; INPUT_BUF_MAX_LEN];
let active_output_buf = &mut output_buf[..input_buf.len()]; let active_output_buf = &mut output_buf[..input_buf.len()];
self.peri.enable_write_dma(); self.launch_a_dma_transfer(write_dma, read_dma, input_buf, active_output_buf)
self.peri.enable_read_dma(); .await;
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);
for &mut output_u32 in active_output_buf { for &mut output_u32 in active_output_buf {
let (res1, res2) = utils::u32_to_f32_res(output_u32); 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[*output_start_index] = res1;
*output_start_index += 1; *output_start_index += 1;
if !self.config.first_result { if !self.config.res1_only {
output[*output_start_index] = res2; output[*output_start_index] = res2;
*output_start_index += 1; *output_start_index += 1;
} }
@ -654,104 +655,131 @@ impl<'d, T: Instance> Cordic<'d, T> {
macro_rules! check_input_value { macro_rules! check_input_value {
($func_name:ident, $float_type:ty) => { ($func_name:ident, $float_type:ty) => {
impl<'d, T: Instance> Cordic<'d, T> { 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; let config = &self.config;
use Function::*; use Function::*;
// check SCALE value struct Arg1ErrInfo {
match config.function { scale: Option<Scale>,
Cos | Sin | Phase | Modulus => assert!(Scale::A1_R1 == config.scale, "SCALE should be 0"), range: [f32; 2],
Arctan => assert!( inclusive_upper_bound: bool,
(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"
),
} }
// check ARG1 value // check ARG1 value
match config.function { let err_info = match config.function {
Cos | Sin | Phase | Modulus | Arctan => { Cos | Sin | Phase | Modulus | Arctan if arg1s.iter().any(|v| !(-1.0..=1.0).contains(v)) => {
assert!( Some(Arg1ErrInfo {
arg1s.iter().all(|v| (-1.0..=1.0).contains(v)), scale: None,
"ARG1 should be: -1 <= ARG1 <= 1" range: [-1.0, 1.0],
); inclusive_upper_bound: true,
})
} }
Cosh | Sinh => assert!( Cosh | Sinh if arg1s.iter().any(|v| !(-0.559..=0.559).contains(v)) => Some(Arg1ErrInfo {
arg1s.iter().all(|v| (-0.559..=0.559).contains(v)), scale: None,
"ARG1 should be: -0.559 <= ARG1 <= 0.559" range: [-0.559, 0.559],
), inclusive_upper_bound: true,
}),
Arctanh => assert!( Arctanh if arg1s.iter().any(|v| !(-0.403..=0.403).contains(v)) => Some(Arg1ErrInfo {
arg1s.iter().all(|v| (-0.403..=0.403).contains(v)), scale: None,
"ARG1 should be: -0.403 <= ARG1 <= 0.403" range: [-0.403, 0.403],
), inclusive_upper_bound: true,
}),
Ln => { Ln => match config.scale {
match config.scale { Scale::Arg1o2Res2 if arg1s.iter().any(|v| !(0.0535..0.5).contains(v)) => Some(Arg1ErrInfo {
Scale::A1o2_R2 => assert!( scale: Some(Scale::Arg1o2Res2),
arg1s.iter().all(|v| (0.05354..0.5).contains(v)), range: [0.0535, 0.5],
"When SCALE set to 1, ARG1 should be: 0.05354 <= ARG1 < 0.5" inclusive_upper_bound: false,
), }),
Scale::A1o4_R4 => assert!( Scale::Arg1o4Res4 if arg1s.iter().any(|v| !(0.25..0.75).contains(v)) => Some(Arg1ErrInfo {
arg1s.iter().all(|v| (0.25..0.75).contains(v)), scale: Some(Scale::Arg1o4Res4),
"When SCALE set to 2, ARG1 should be: 0.25 <= ARG1 < 0.75" range: [0.25, 0.75],
), inclusive_upper_bound: false,
Scale::A1o8_R8 => assert!( }),
arg1s.iter().all(|v| (0.375..0.875).contains(v)), Scale::Arg1o8Res8 if arg1s.iter().any(|v| !(0.375..0.875).contains(v)) => Some(Arg1ErrInfo {
"When SCALE set to 3, ARG1 should be: 0.375 <= ARG1 < 0.875" scale: Some(Scale::Arg1o8Res8),
), range: [0.375, 0.875],
Scale::A1o16_R16 => assert!( inclusive_upper_bound: false,
arg1s.iter().all(|v| (0.4375..0.584).contains(v)), }),
"When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584" Scale::Arg1o16Res16 if arg1s.iter().any(|v| !(0.4375..0.584).contains(v)) => {
), Some(Arg1ErrInfo {
_ => unreachable!(), 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!(), _ => 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 // check ARG2 value
if let Some(arg2s) = arg2s { if let Some(arg2s) = arg2s {
match config.function { struct Arg2ErrInfo {
Cos | Sin => assert!( range: [f32; 2],
arg2s.iter().all(|v| (0.0..=1.0).contains(v)), }
"ARG2 should be: 0 <= ARG2 <= 1"
),
Phase | Modulus => assert!( let err_info = match config.function {
arg2s.iter().all(|v| (-1.0..=1.0).contains(v)), Cos | Sin if arg2s.iter().any(|v| !(0.0..=1.0).contains(v)) => {
"ARG2 should be: -1 <= ARG2 <= 1" 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(())
} }
} }
}; };

View file

@ -66,21 +66,21 @@ pub trait Instance {
} }
/// Set NARGS value /// Set NARGS value
fn set_argument_count(&self, n: Count) { fn set_argument_count(&self, n: AccessCount) {
Self::regs().csr().modify(|v| { Self::regs().csr().modify(|v| {
v.set_nargs(match n { v.set_nargs(match n {
Count::One => vals::Num::NUM1, AccessCount::One => vals::Num::NUM1,
Count::Two => vals::Num::NUM2, AccessCount::Two => vals::Num::NUM2,
}) })
}) })
} }
/// Set NRES value /// Set NRES value
fn set_result_count(&self, n: Count) { fn set_result_count(&self, n: AccessCount) {
Self::regs().csr().modify(|v| { Self::regs().csr().modify(|v| {
v.set_nres(match n { v.set_nres(match n {
Count::One => vals::Num::NUM1, AccessCount::One => vals::Num::NUM1,
Count::Two => vals::Num::NUM2, AccessCount::Two => vals::Num::NUM2,
}); });
}) })
} }