NaxdyOrg/embassy
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

stm32 CORDIC: error handle

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

29
embassy-stm32/src/cordic/enums.rs
View file

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

95
embassy-stm32/src/cordic/errors.rs Normal file
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,
}

510
embassy-stm32/src/cordic/mod.rs
View file

@ -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(())
}
}
};

12
embassy-stm32/src/cordic/sealed.rs
View file

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