diff --git a/embassy-stm32/build.rs b/embassy-stm32/build.rs index 15bb8ea62..057c4cee2 100644 --- a/embassy-stm32/build.rs +++ b/embassy-stm32/build.rs @@ -484,7 +484,7 @@ fn main() { let expr = if let Some(mux) = self.chained_muxes.get(&v.name) { self.gen_mux(mux) } else { - self.gen_clock(&v.name) + self.gen_clock(v.name) }; match_arms.extend(quote! { crate::pac::rcc::vals::#enum_name::#variant_name => #expr, @@ -1139,11 +1139,18 @@ fn main() { (("timer", "CH2"), quote!(crate::timer::Ch2Dma)), (("timer", "CH3"), quote!(crate::timer::Ch3Dma)), (("timer", "CH4"), quote!(crate::timer::Ch4Dma)), + (("cordic", "WRITE"), quote!(crate::cordic::WriteDma)), // FIXME: stm32u5a crash on Cordic driver + (("cordic", "READ"), quote!(crate::cordic::ReadDma)), // FIXME: stm32u5a crash on Cordic driver ] .into(); for p in METADATA.peripherals { if let Some(regs) = &p.registers { + // FIXME: stm32u5a crash on Cordic driver + if chip_name.starts_with("stm32u5a") && regs.kind == "cordic" { + continue; + } + let mut dupe = HashSet::new(); for ch in p.dma_channels { // Some chips have multiple request numbers for the same (peri, signal, channel) combos. diff --git a/embassy-stm32/src/cordic/enums.rs b/embassy-stm32/src/cordic/enums.rs new file mode 100644 index 000000000..e8695fac7 --- /dev/null +++ b/embassy-stm32/src/cordic/enums.rs @@ -0,0 +1,71 @@ +/// CORDIC function +#[allow(missing_docs)] +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Function { + Cos = 0, + Sin, + Phase, + Modulus, + Arctan, + Cosh, + Sinh, + Arctanh, + Ln, + Sqrt, +} + +/// CORDIC precision +#[allow(missing_docs)] +#[derive(Debug, Clone, Copy, Default)] +pub enum Precision { + Iters4 = 1, + Iters8, + Iters12, + Iters16, + Iters20, + #[default] + Iters24, // this value is recommended by Reference Manual + Iters28, + Iters32, + Iters36, + Iters40, + Iters44, + Iters48, + Iters52, + Iters56, + Iters60, +} + +/// CORDIC scale +#[allow(missing_docs)] +#[derive(Debug, Clone, Copy, Default, PartialEq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Scale { + #[default] + Arg1Res1 = 0, + Arg1o2Res2, + Arg1o4Res4, + Arg1o8Res8, + Arg1o16Res16, + Arg1o32Res32, + Arg1o64Res64, + Arg1o128Res128, +} + +/// CORDIC argument/result register access count +#[allow(missing_docs)] +#[derive(Clone, Copy, Default)] +pub enum AccessCount { + #[default] + One, + Two, +} + +/// CORDIC argument/result data width +#[allow(missing_docs)] +#[derive(Clone, Copy)] +pub enum Width { + Bits32, + Bits16, +} diff --git a/embassy-stm32/src/cordic/errors.rs b/embassy-stm32/src/cordic/errors.rs new file mode 100644 index 000000000..3c70fc9e7 --- /dev/null +++ b/embassy-stm32/src/cordic/errors.rs @@ -0,0 +1,144 @@ +use super::{Function, Scale}; + +/// Error for [Cordic](super::Cordic) +#[derive(Debug)] +pub enum CordicError { + /// Config error + ConfigError(ConfigError), + /// Argument length is incorrect + ArgumentLengthIncorrect, + /// Result buffer length error + ResultLengthNotEnough, + /// Input value is out of range for Q1.x format + NumberOutOfRange(NumberOutOfRange), + /// Argument error + ArgError(ArgError), +} + +impl From for CordicError { + fn from(value: ConfigError) -> Self { + Self::ConfigError(value) + } +} + +impl From for CordicError { + fn from(value: NumberOutOfRange) -> Self { + Self::NumberOutOfRange(value) + } +} + +impl From for CordicError { + fn from(value: ArgError) -> Self { + Self::ArgError(value) + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for CordicError { + fn format(&self, fmt: defmt::Formatter) { + use CordicError::*; + + match self { + ConfigError(e) => defmt::write!(fmt, "{}", e), + ResultLengthNotEnough => defmt::write!(fmt, "Output buffer length is not long enough"), + ArgumentLengthIncorrect => defmt::write!(fmt, "Argument length incorrect"), + NumberOutOfRange(e) => defmt::write!(fmt, "{}", e), + ArgError(e) => defmt::write!(fmt, "{}", e), + } + } +} + +/// Error during parsing [Cordic::Config](super::Config) +#[allow(dead_code)] +#[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] + ) + } + } +} + +/// Input value is out of range for Q1.x format +#[allow(missing_docs)] +#[derive(Debug)] +pub enum NumberOutOfRange { + BelowLowerBound, + AboveUpperBound, +} + +#[cfg(feature = "defmt")] +impl defmt::Format for NumberOutOfRange { + fn format(&self, fmt: defmt::Formatter) { + use NumberOutOfRange::*; + + match self { + BelowLowerBound => defmt::write!(fmt, "input value should be equal or greater than -1"), + AboveUpperBound => defmt::write!(fmt, "input value should be equal or less than 1"), + } + } +} + +/// Error on checking input arguments +#[allow(dead_code)] +#[derive(Debug)] +pub struct ArgError { + pub(super) func: Function, + pub(super) scale: Option, + 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); + } + + defmt::write!(fmt, " {} should be", self.arg_type); + + if self.inclusive_upper_bound { + defmt::write!( + fmt, + " {} <= {} <= {}", + self.arg_range[0], + self.arg_type, + self.arg_range[1] + ) + } else { + defmt::write!( + fmt, + " {} <= {} < {}", + self.arg_range[0], + self.arg_type, + self.arg_range[1] + ) + }; + } +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub(super) enum ArgType { + Arg1, + Arg2, +} diff --git a/embassy-stm32/src/cordic/mod.rs b/embassy-stm32/src/cordic/mod.rs new file mode 100644 index 000000000..6bbc48f2b --- /dev/null +++ b/embassy-stm32/src/cordic/mod.rs @@ -0,0 +1,617 @@ +//! coordinate rotation digital computer (CORDIC) + +use embassy_hal_internal::drop::OnDrop; +use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef}; + +use crate::{dma, peripherals}; + +mod enums; +pub use enums::*; + +mod errors; +pub use errors::*; + +mod sealed; +use self::sealed::SealedInstance; + +pub mod utils; + +/// CORDIC driver +pub struct Cordic<'d, T: Instance> { + peri: PeripheralRef<'d, T>, + config: Config, +} + +/// CORDIC instance trait +#[allow(private_bounds)] +pub trait Instance: SealedInstance + Peripheral

+ crate::rcc::RccPeripheral {} + +/// CORDIC configuration +#[derive(Debug)] +pub struct Config { + function: Function, + precision: Precision, + scale: Scale, +} + +impl Config { + /// Create a config for Cordic driver + pub fn new(function: Function, precision: Precision, scale: Scale) -> Result { + let config = Self { + function, + precision, + scale, + }; + + config.check_scale()?; + + Ok(config) + } + + fn check_scale(&self) -> Result<(), ConfigError> { + use Function::*; + + let scale_raw = self.scale as u8; + + 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(ConfigError { + func: self.function, + scale_range: range, + }) + } else { + Ok(()) + } + } +} + +// common method +impl<'d, T: Instance> Cordic<'d, T> { + /// Create a Cordic driver instance + /// + /// Note: + /// If you need a peripheral -> CORDIC -> peripheral mode, + /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguments with [Self::extra_config] + pub fn new(peri: impl Peripheral

+ 'd, config: Config) -> Self { + T::enable_and_reset(); + + into_ref!(peri); + + let mut instance = Self { peri, config }; + + instance.reconfigure(); + + instance + } + + /// Set a new config for Cordic driver + pub fn set_config(&mut self, config: Config) { + self.config = config; + self.reconfigure(); + } + + /// Set extra config for data count and data 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 clean_rrdy_flag(&mut self) { + while self.peri.ready_to_read() { + self.peri.read_result(); + } + } + + /// Disable IRQ and DMA, clean RRDY, and set ARG2 to +1 (0x7FFFFFFF) + pub fn reconfigure(&mut self) { + // reset ARG2 to +1 + { + self.peri.disable_irq(); + self.peri.disable_read_dma(); + self.peri.disable_write_dma(); + self.clean_rrdy_flag(); + + self.peri.set_func(Function::Cos); + self.peri.set_precision(Precision::Iters4); + self.peri.set_scale(Scale::Arg1Res1); + self.peri.set_argument_count(AccessCount::Two); + self.peri.set_data_width(Width::Bits32, Width::Bits32); + self.peri.write_argument(0x0u32); + self.peri.write_argument(0x7FFFFFFFu32); + + self.clean_rrdy_flag(); + } + + self.peri.set_func(self.config.function); + self.peri.set_precision(self.config.precision); + self.peri.set_scale(self.config.scale); + + // 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 accordingly. + } +} + +impl<'d, T: Instance> Drop for Cordic<'d, T> { + fn drop(&mut self) { + T::disable(); + } +} + +// q1.31 related +impl<'d, T: Instance> Cordic<'d, T> { + /// Run a blocking CORDIC calculation in q1.31 format + /// + /// Notice: + /// If you set `arg1_only` to `true`, please be sure ARG2 value has been set to desired value before. + /// This function won't set ARG2 to +1 before or after each round of calculation. + /// If you want to make sure ARG2 is set to +1, consider run [.reconfigure()](Self::reconfigure). + pub fn blocking_calc_32bit( + &mut self, + arg: &[u32], + res: &mut [u32], + arg1_only: bool, + res1_only: bool, + ) -> Result { + if arg.is_empty() { + return Ok(0); + } + + let res_cnt = Self::check_arg_res_length_32bit(arg.len(), res.len(), arg1_only, res1_only)?; + + self.peri + .set_argument_count(if arg1_only { AccessCount::One } else { AccessCount::Two }); + + self.peri + .set_result_count(if res1_only { AccessCount::One } else { AccessCount::Two }); + + self.peri.set_data_width(Width::Bits32, Width::Bits32); + + let mut cnt = 0; + + match arg1_only { + true => { + // To use cordic preload function, the first value is special. + // It is loaded to CORDIC WDATA register out side of loop + let first_value = arg[0]; + + // preload 1st value to CORDIC, to start the CORDIC calc + self.peri.write_argument(first_value); + + for &arg1 in &arg[1..] { + // preload arg1 (for next calc) + self.peri.write_argument(arg1); + + // then read current result out + res[cnt] = self.peri.read_result(); + cnt += 1; + if !res1_only { + res[cnt] = self.peri.read_result(); + cnt += 1; + } + } + + // read the last result + res[cnt] = self.peri.read_result(); + cnt += 1; + if !res1_only { + res[cnt] = self.peri.read_result(); + // cnt += 1; + } + } + false => { + // To use cordic preload function, the first and last value is special. + // They are load to CORDIC WDATA register out side of loop + let first_value = arg[0]; + let last_value = arg[arg.len() - 1]; + + let paired_args = &arg[1..arg.len() - 1]; + + // preload 1st value to CORDIC + self.peri.write_argument(first_value); + + for args in paired_args.chunks(2) { + let arg2 = args[0]; + let arg1 = args[1]; + + // load arg2 (for current calc) first, to start the CORDIC calc + self.peri.write_argument(arg2); + + // preload arg1 (for next calc) + self.peri.write_argument(arg1); + + // then read current result out + res[cnt] = self.peri.read_result(); + cnt += 1; + if !res1_only { + res[cnt] = self.peri.read_result(); + cnt += 1; + } + } + + // load last value to CORDIC, and finish the calculation + self.peri.write_argument(last_value); + res[cnt] = self.peri.read_result(); + cnt += 1; + if !res1_only { + res[cnt] = self.peri.read_result(); + // cnt += 1; + } + } + } + + // at this point cnt should be equal to res_cnt + + Ok(res_cnt) + } + + /// Run a async CORDIC calculation in q.1.31 format + /// + /// Notice: + /// If you set `arg1_only` to `true`, please be sure ARG2 value has been set to desired value before. + /// This function won't set ARG2 to +1 before or after each round of calculation. + /// If you want to make sure ARG2 is set to +1, consider run [.reconfigure()](Self::reconfigure). + pub async fn async_calc_32bit( + &mut self, + write_dma: impl Peripheral

>, + read_dma: impl Peripheral

>, + arg: &[u32], + res: &mut [u32], + arg1_only: bool, + res1_only: bool, + ) -> Result { + if arg.is_empty() { + return Ok(0); + } + + let res_cnt = Self::check_arg_res_length_32bit(arg.len(), res.len(), arg1_only, res1_only)?; + + let active_res_buf = &mut res[..res_cnt]; + + into_ref!(write_dma, read_dma); + + self.peri + .set_argument_count(if arg1_only { AccessCount::One } else { AccessCount::Two }); + + self.peri + .set_result_count(if res1_only { AccessCount::One } else { AccessCount::Two }); + + self.peri.set_data_width(Width::Bits32, Width::Bits32); + + 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, + arg, + 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_res_buf, + Default::default(), + ); + + embassy_futures::join::join(write_transfer, read_transfer).await; + } + + Ok(res_cnt) + } + + fn check_arg_res_length_32bit( + arg_len: usize, + res_len: usize, + arg1_only: bool, + res1_only: bool, + ) -> Result { + if !arg1_only && arg_len % 2 != 0 { + return Err(CordicError::ArgumentLengthIncorrect); + } + + let mut minimal_res_length = arg_len; + + if !res1_only { + minimal_res_length *= 2; + } + + if !arg1_only { + minimal_res_length /= 2 + } + + if minimal_res_length > res_len { + return Err(CordicError::ResultLengthNotEnough); + } + + Ok(minimal_res_length) + } +} + +// q1.15 related +impl<'d, T: Instance> Cordic<'d, T> { + /// Run a blocking CORDIC calculation in q1.15 format + /// + /// Notice:: + /// User will take respond to merge two u16 arguments into one u32 data, and/or split one u32 data into two u16 results. + pub fn blocking_calc_16bit(&mut self, arg: &[u32], res: &mut [u32]) -> Result { + if arg.is_empty() { + return Ok(0); + } + + if arg.len() > res.len() { + return Err(CordicError::ResultLengthNotEnough); + } + + let res_cnt = arg.len(); + + // In q1.15 mode, 1 write/read to access 2 arguments/results + self.peri.set_argument_count(AccessCount::One); + self.peri.set_result_count(AccessCount::One); + + self.peri.set_data_width(Width::Bits16, Width::Bits16); + + // To use cordic preload function, the first value is special. + // It is loaded to CORDIC WDATA register out side of loop + let first_value = arg[0]; + + // preload 1st value to CORDIC, to start the CORDIC calc + self.peri.write_argument(first_value); + + let mut cnt = 0; + + for &arg_val in &arg[1..] { + // preload arg_val (for next calc) + self.peri.write_argument(arg_val); + + // then read current result out + res[cnt] = self.peri.read_result(); + cnt += 1; + } + + // read last result out + res[cnt] = self.peri.read_result(); + // cnt += 1; + + Ok(res_cnt) + } + + /// Run a async CORDIC calculation in q1.15 format + /// + /// Notice:: + /// User will take respond to merge two u16 arguments into one u32 data, and/or split one u32 data into two u16 results. + pub async fn async_calc_16bit( + &mut self, + write_dma: impl Peripheral

>, + read_dma: impl Peripheral

>, + arg: &[u32], + res: &mut [u32], + ) -> Result { + if arg.is_empty() { + return Ok(0); + } + + if arg.len() > res.len() { + return Err(CordicError::ResultLengthNotEnough); + } + + let res_cnt = arg.len(); + + let active_res_buf = &mut res[..res_cnt]; + + into_ref!(write_dma, read_dma); + + // In q1.15 mode, 1 write/read to access 2 arguments/results + self.peri.set_argument_count(AccessCount::One); + self.peri.set_result_count(AccessCount::One); + + self.peri.set_data_width(Width::Bits16, Width::Bits16); + + 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, + arg, + 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_res_buf, + Default::default(), + ); + + embassy_futures::join::join(write_transfer, read_transfer).await; + } + + Ok(res_cnt) + } +} + +macro_rules! check_arg_value { + ($func_arg1_name:ident, $func_arg2_name:ident, $float_type:ty) => { + impl<'d, T: Instance> Cordic<'d, T> { + /// check input value ARG1, SCALE and FUNCTION are compatible with each other + pub fn $func_arg1_name(&self, arg: $float_type) -> Result<(), ArgError> { + let config = &self.config; + + use Function::*; + + struct Arg1ErrInfo { + scale: Option, + range: [f32; 2], // f32 is ok, it only used in error display + inclusive_upper_bound: bool, + } + + let err_info = match config.function { + Cos | Sin | Phase | Modulus | Arctan if !(-1.0..=1.0).contains(arg) => Some(Arg1ErrInfo { + scale: None, + range: [-1.0, 1.0], + inclusive_upper_bound: true, + }), + + Cosh | Sinh if !(-0.559..=0.559).contains(arg) => Some(Arg1ErrInfo { + scale: None, + range: [-0.559, 0.559], + inclusive_upper_bound: true, + }), + + Arctanh if !(-0.403..=0.403).contains(arg) => Some(Arg1ErrInfo { + scale: None, + range: [-0.403, 0.403], + inclusive_upper_bound: true, + }), + + Ln => match config.scale { + Scale::Arg1o2Res2 if !(0.0535..0.5).contains(arg) => Some(Arg1ErrInfo { + scale: Some(Scale::Arg1o2Res2), + range: [0.0535, 0.5], + inclusive_upper_bound: false, + }), + Scale::Arg1o4Res4 if !(0.25..0.75).contains(arg) => Some(Arg1ErrInfo { + scale: Some(Scale::Arg1o4Res4), + range: [0.25, 0.75], + inclusive_upper_bound: false, + }), + Scale::Arg1o8Res8 if !(0.375..0.875).contains(arg) => Some(Arg1ErrInfo { + scale: Some(Scale::Arg1o8Res8), + range: [0.375, 0.875], + inclusive_upper_bound: false, + }), + Scale::Arg1o16Res16 if !(0.4375..0.584).contains(arg) => 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::Arg1Res1 if !(0.027..0.75).contains(arg) => Some(Arg1ErrInfo { + scale: Some(Scale::Arg1Res1), + range: [0.027, 0.75], + inclusive_upper_bound: false, + }), + Scale::Arg1o2Res2 if !(0.375..0.875).contains(arg) => Some(Arg1ErrInfo { + scale: Some(Scale::Arg1o2Res2), + range: [0.375, 0.875], + inclusive_upper_bound: false, + }), + Scale::Arg1o4Res4 if !(0.4375..0.584).contains(arg) => 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(ArgError { + func: config.function, + scale: err.scale, + arg_range: err.range, + inclusive_upper_bound: err.inclusive_upper_bound, + arg_type: ArgType::Arg1, + }); + } + + Ok(()) + } + + /// check input value ARG2 and FUNCTION are compatible with each other + pub fn $func_arg2_name(&self, arg: $float_type) -> Result<(), ArgError> { + let config = &self.config; + + use Function::*; + + struct Arg2ErrInfo { + range: [f32; 2], // f32 is ok, it only used in error display + } + + let err_info = match config.function { + Cos | Sin if !(0.0..=1.0).contains(arg) => Some(Arg2ErrInfo { range: [0.0, 1.0] }), + + Phase | Modulus if !(-1.0..=1.0).contains(arg) => 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(ArgError { + func: config.function, + scale: None, + arg_range: err.range, + inclusive_upper_bound: true, + arg_type: ArgType::Arg2, + }); + } + + Ok(()) + } + } + }; +} + +check_arg_value!(check_f64_arg1, check_f64_arg2, &f64); +check_arg_value!(check_f32_arg1, check_f32_arg2, &f32); + +foreach_interrupt!( + ($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => { + impl Instance for peripherals::$inst { + } + + impl SealedInstance for peripherals::$inst { + fn regs() -> crate::pac::cordic::Cordic { + crate::pac::$inst + } + } + }; +); + +dma_trait!(WriteDma, Instance); +dma_trait!(ReadDma, Instance); diff --git a/embassy-stm32/src/cordic/sealed.rs b/embassy-stm32/src/cordic/sealed.rs new file mode 100644 index 000000000..8f0bd1830 --- /dev/null +++ b/embassy-stm32/src/cordic/sealed.rs @@ -0,0 +1,116 @@ +use super::*; +use crate::pac::cordic::vals; + +/// Cordic instance +pub(super) trait SealedInstance { + /// Get access to CORDIC registers + fn regs() -> crate::pac::cordic::Cordic; + + /// Set Function value + fn set_func(&self, func: Function) { + Self::regs() + .csr() + .modify(|v| v.set_func(vals::Func::from_bits(func as u8))); + } + + /// Set Precision value + fn set_precision(&self, precision: Precision) { + Self::regs() + .csr() + .modify(|v| v.set_precision(vals::Precision::from_bits(precision as u8))) + } + + /// Set Scale value + fn set_scale(&self, scale: Scale) { + Self::regs() + .csr() + .modify(|v| v.set_scale(vals::Scale::from_bits(scale as u8))) + } + + /// Enable global interrupt + fn enable_irq(&self) { + Self::regs().csr().modify(|v| v.set_ien(true)) + } + + /// Disable global interrupt + fn disable_irq(&self) { + Self::regs().csr().modify(|v| v.set_ien(false)) + } + + /// Enable Read DMA + fn enable_read_dma(&self) { + Self::regs().csr().modify(|v| { + v.set_dmaren(true); + }) + } + + /// Disable Read DMA + fn disable_read_dma(&self) { + Self::regs().csr().modify(|v| { + v.set_dmaren(false); + }) + } + + /// Enable Write DMA + fn enable_write_dma(&self) { + Self::regs().csr().modify(|v| { + v.set_dmawen(true); + }) + } + + /// Disable Write DMA + fn disable_write_dma(&self) { + Self::regs().csr().modify(|v| { + v.set_dmawen(false); + }) + } + + /// Set NARGS value + fn set_argument_count(&self, n: AccessCount) { + Self::regs().csr().modify(|v| { + v.set_nargs(match n { + AccessCount::One => vals::Num::NUM1, + AccessCount::Two => vals::Num::NUM2, + }) + }) + } + + /// Set NRES value + fn set_result_count(&self, n: AccessCount) { + Self::regs().csr().modify(|v| { + v.set_nres(match n { + AccessCount::One => vals::Num::NUM1, + AccessCount::Two => vals::Num::NUM2, + }); + }) + } + + /// Set ARGSIZE and RESSIZE value + fn set_data_width(&self, arg: Width, res: Width) { + Self::regs().csr().modify(|v| { + v.set_argsize(match arg { + Width::Bits32 => vals::Size::BITS32, + Width::Bits16 => vals::Size::BITS16, + }); + v.set_ressize(match res { + Width::Bits32 => vals::Size::BITS32, + Width::Bits16 => vals::Size::BITS16, + }) + }) + } + + /// Read RRDY flag + fn ready_to_read(&self) -> bool { + Self::regs().csr().read().rrdy() + } + + /// Write value to WDATA + fn write_argument(&self, arg: u32) { + Self::regs().wdata().write_value(arg) + } + + /// Read value from RDATA + fn read_result(&self) -> u32 { + Self::regs().rdata().read() + } +} diff --git a/embassy-stm32/src/cordic/utils.rs b/embassy-stm32/src/cordic/utils.rs new file mode 100644 index 000000000..008f50270 --- /dev/null +++ b/embassy-stm32/src/cordic/utils.rs @@ -0,0 +1,62 @@ +//! Common math utils +use super::errors::NumberOutOfRange; + +macro_rules! floating_fixed_convert { + ($f_to_q:ident, $q_to_f:ident, $unsigned_bin_typ:ty, $signed_bin_typ:ty, $float_ty:ty, $offset:literal, $min_positive:literal) => { + /// convert float point to fixed point format + pub fn $f_to_q(value: $float_ty) -> Result<$unsigned_bin_typ, NumberOutOfRange> { + const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) }; + + if value < -1.0 { + return Err(NumberOutOfRange::BelowLowerBound) + } + + if value > 1.0 { + return Err(NumberOutOfRange::AboveUpperBound) + } + + + let value = if 1.0 - MIN_POSITIVE < value && value <= 1.0 { + // make a exception for value between (1.0^{-x} , 1.0] float point, + // convert it to max representable value of q1.x format + (1.0 as $float_ty) - MIN_POSITIVE + } else { + value + }; + + // It's necessary to cast the float value to signed integer, before convert it to a unsigned value. + // Since value from register is actually a "signed value", a "as" cast will keep original binary format but mark it as a unsigned value for register writing. + // see https://doc.rust-lang.org/reference/expressions/operator-expr.html#numeric-cast + Ok((value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) as $signed_bin_typ as $unsigned_bin_typ) + } + + #[inline(always)] + /// convert fixed point to float point format + pub fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty { + // It's necessary to cast the unsigned integer to signed integer, before convert it to a float value. + // Since value from register is actually a "signed value", a "as" cast will keep original binary format but mark it as a signed value. + // see https://doc.rust-lang.org/reference/expressions/operator-expr.html#numeric-cast + (value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty) + } + }; +} + +floating_fixed_convert!( + f64_to_q1_31, + q1_31_to_f64, + u32, + i32, + f64, + 31, + 0x3E00_0000_0000_0000u64 // binary form of 1f64^(-31) +); + +floating_fixed_convert!( + f32_to_q1_15, + q1_15_to_f32, + u16, + i16, + f32, + 15, + 0x3800_0000u32 // binary form of 1f32^(-15) +); diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs index 8b826e5ac..99d8b5036 100644 --- a/embassy-stm32/src/lib.rs +++ b/embassy-stm32/src/lib.rs @@ -32,6 +32,9 @@ pub mod timer; pub mod adc; #[cfg(can)] pub mod can; +// FIXME: Cordic driver cause stm32u5a5zj crash +#[cfg(all(cordic, not(any(stm32u5a5, stm32u5a9))))] +pub mod cordic; #[cfg(crc)] pub mod crc; #[cfg(cryp)] @@ -244,7 +247,7 @@ pub fn init(config: Config) -> Peripherals { #[cfg(dbgmcu)] crate::pac::DBGMCU.cr().modify(|cr| { - #[cfg(any(dbgmcu_h5))] + #[cfg(dbgmcu_h5)] { cr.set_stop(config.enable_debug_during_sleep); cr.set_standby(config.enable_debug_during_sleep); diff --git a/examples/stm32h5/src/bin/cordic.rs b/examples/stm32h5/src/bin/cordic.rs new file mode 100644 index 000000000..73e873574 --- /dev/null +++ b/examples/stm32h5/src/bin/cordic.rs @@ -0,0 +1,78 @@ +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_stm32::cordic::{self, utils}; +use {defmt_rtt as _, panic_probe as _}; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let mut dp = embassy_stm32::init(Default::default()); + + let mut cordic = cordic::Cordic::new( + &mut dp.CORDIC, + unwrap!(cordic::Config::new( + cordic::Function::Sin, + Default::default(), + Default::default(), + )), + ); + + // for output buf, the length is not that strict, larger than minimal required is ok. + let mut output_f64 = [0f64; 19]; + let mut output_u32 = [0u32; 21]; + + // tips: + // CORDIC peripheral has some strict on input value, you can also use ".check_argX_fXX()" methods + // to make sure your input values are compatible with current CORDIC setup. + let arg1 = [-1.0, -0.5, 0.0, 0.5, 1.0]; // for trigonometric function, the ARG1 value [-pi, pi] should be map to [-1, 1] + let arg2 = [0.5]; // and for Sin function, ARG2 should be in [0, 1] + + let mut input_buf = [0u32; 9]; + + // convert input from floating point to fixed point + input_buf[0] = unwrap!(utils::f64_to_q1_31(arg1[0])); + input_buf[1] = unwrap!(utils::f64_to_q1_31(arg2[0])); + + // If input length is small, blocking mode can be used to minimize overhead. + let cnt0 = unwrap!(cordic.blocking_calc_32bit( + &input_buf[..2], // input length is strict, since driver use its length to detect calculation count + &mut output_u32, + false, + false + )); + + // convert result from fixed point into floating point + for (&u32_val, f64_val) in output_u32[..cnt0].iter().zip(output_f64.iter_mut()) { + *f64_val = utils::q1_31_to_f64(u32_val); + } + + // convert input from floating point to fixed point + // + // first value from arg1 is used, so truncate to arg1[1..] + for (&f64_val, u32_val) in arg1[1..].iter().zip(input_buf.iter_mut()) { + *u32_val = unwrap!(utils::f64_to_q1_31(f64_val)); + } + + // If calculation is a little longer, async mode can make use of DMA, and let core do some other stuff. + let cnt1 = unwrap!( + cordic + .async_calc_32bit( + &mut dp.GPDMA1_CH0, + &mut dp.GPDMA1_CH1, + &input_buf[..arg1.len() - 1], // limit input buf to its actual length + &mut output_u32, + true, + false + ) + .await + ); + + // convert result from fixed point into floating point + for (&u32_val, f64_val) in output_u32[..cnt1].iter().zip(output_f64[cnt0..cnt0 + cnt1].iter_mut()) { + *f64_val = utils::q1_31_to_f64(u32_val); + } + + println!("result: {}", output_f64[..cnt0 + cnt1]); +} diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml index e42470004..e09083111 100644 --- a/tests/stm32/Cargo.toml +++ b/tests/stm32/Cargo.toml @@ -14,8 +14,8 @@ stm32f429zi = ["embassy-stm32/stm32f429zi", "chrono", "eth", "stop", "can", "not stm32f446re = ["embassy-stm32/stm32f446re", "chrono", "stop", "can", "not-gpdma", "dac", "sdmmc"] stm32f767zi = ["embassy-stm32/stm32f767zi", "chrono", "not-gpdma", "eth", "rng"] stm32g071rb = ["embassy-stm32/stm32g071rb", "cm0", "not-gpdma", "dac", "ucpd"] -stm32g491re = ["embassy-stm32/stm32g491re", "chrono", "stop", "not-gpdma", "rng", "fdcan"] -stm32h563zi = ["embassy-stm32/stm32h563zi", "chrono", "eth", "rng", "hash"] +stm32g491re = ["embassy-stm32/stm32g491re", "chrono", "stop", "not-gpdma", "rng", "fdcan", "cordic"] +stm32h563zi = ["embassy-stm32/stm32h563zi", "chrono", "eth", "rng", "hash", "cordic"] stm32h753zi = ["embassy-stm32/stm32h753zi", "chrono", "not-gpdma", "eth", "rng", "fdcan", "hash", "cryp"] stm32h755zi = ["embassy-stm32/stm32h755zi-cm7", "chrono", "not-gpdma", "eth", "dac", "rng", "fdcan", "hash", "cryp"] stm32h7a3zi = ["embassy-stm32/stm32h7a3zi", "not-gpdma", "rng", "fdcan"] @@ -25,8 +25,8 @@ stm32l496zg = ["embassy-stm32/stm32l496zg", "not-gpdma", "rng"] stm32l4a6zg = ["embassy-stm32/stm32l4a6zg", "chrono", "not-gpdma", "rng", "hash"] stm32l4r5zi = ["embassy-stm32/stm32l4r5zi", "chrono", "not-gpdma", "rng"] stm32l552ze = ["embassy-stm32/stm32l552ze", "not-gpdma", "rng", "hash"] -stm32u585ai = ["embassy-stm32/stm32u585ai", "chrono", "rng", "hash"] -stm32u5a5zj = ["embassy-stm32/stm32u5a5zj", "chrono", "rng", "hash"] +stm32u585ai = ["embassy-stm32/stm32u585ai", "chrono", "rng", "hash", "cordic"] +stm32u5a5zj = ["embassy-stm32/stm32u5a5zj", "chrono", "rng", "hash"] # FIXME: cordic test cause it crash stm32wb55rg = ["embassy-stm32/stm32wb55rg", "chrono", "not-gpdma", "ble", "mac" , "rng"] stm32wba52cg = ["embassy-stm32/stm32wba52cg", "chrono", "rng", "hash"] stm32wl55jc = ["embassy-stm32/stm32wl55jc-cm4", "not-gpdma", "rng", "chrono"] @@ -48,6 +48,7 @@ embassy-stm32-wpan = [] not-gpdma = [] dac = [] ucpd = [] +cordic = ["dep:num-traits"] cm0 = ["portable-atomic/unsafe-assume-single-core"] @@ -83,6 +84,7 @@ chrono = { version = "^0.4", default-features = false, optional = true} sha2 = { version = "0.10.8", default-features = false } hmac = "0.12.1" aes-gcm = {version = "0.10.3", default-features = false, features = ["aes", "heapless"] } +num-traits = {version="0.2", default-features = false,features = ["libm"], optional = true} # BEGIN TESTS # Generated by gen_test.py. DO NOT EDIT. @@ -91,6 +93,11 @@ name = "can" path = "src/bin/can.rs" required-features = [ "can",] +[[bin]] +name = "cordic" +path = "src/bin/cordic.rs" +required-features = [ "rng", "cordic",] + [[bin]] name = "cryp" path = "src/bin/cryp.rs" diff --git a/tests/stm32/gen_test.py b/tests/stm32/gen_test.py index 8ff156c0e..daf714376 100644 --- a/tests/stm32/gen_test.py +++ b/tests/stm32/gen_test.py @@ -14,7 +14,7 @@ for f in sorted(glob('./src/bin/*.rs')): with open(f, 'r') as f: for line in f: if line.startswith('// required-features:'): - features = line.split(':', 2)[1].strip().split(',') + features = [feature.strip() for feature in line.split(':', 2)[1].strip().split(',')] tests[name] = features diff --git a/tests/stm32/src/bin/cordic.rs b/tests/stm32/src/bin/cordic.rs new file mode 100644 index 000000000..400e10207 --- /dev/null +++ b/tests/stm32/src/bin/cordic.rs @@ -0,0 +1,135 @@ +// required-features: rng, cordic + +// Test Cordic driver, with Q1.31 format, Sin function, at 24 iterations (aka PRECISION = 6), using DMA transfer + +#![no_std] +#![no_main] + +#[path = "../common.rs"] +mod common; +use common::*; +use embassy_executor::Spawner; +use embassy_stm32::cordic::utils; +use embassy_stm32::{bind_interrupts, cordic, peripherals, rng}; +use num_traits::Float; +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + RNG => rng::InterruptHandler; +}); + +/* input value control, can be changed */ + +const INPUT_U32_COUNT: usize = 9; +const INPUT_U8_COUNT: usize = 4 * INPUT_U32_COUNT; + +// Assume first calculation needs 2 arguments, the reset needs 1 argument. +// And all calculation generate 2 results. +const OUTPUT_LENGTH: usize = (INPUT_U32_COUNT - 1) * 2; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let dp = embassy_stm32::init(config()); + + // + // use RNG generate random Q1.31 value + // + // we don't generate floating-point value, since not all binary value are valid floating-point value, + // and Q1.31 only accept a fixed range of value. + + let mut rng = rng::Rng::new(dp.RNG, Irqs); + + let mut input_buf_u8 = [0u8; INPUT_U8_COUNT]; + defmt::unwrap!(rng.async_fill_bytes(&mut input_buf_u8).await); + + // convert every [u8; 4] to a u32, for a Q1.31 value + let mut input_q1_31 = unsafe { core::mem::transmute::<[u8; INPUT_U8_COUNT], [u32; INPUT_U32_COUNT]>(input_buf_u8) }; + + // ARG2 for Sin function should be inside [0, 1], set MSB to 0 of a Q1.31 value, will make sure it's no less than 0. + input_q1_31[1] &= !(1u32 << 31); + + // + // CORDIC calculation + // + + let mut output_q1_31 = [0u32; OUTPUT_LENGTH]; + + // setup Cordic driver + let mut cordic = cordic::Cordic::new( + dp.CORDIC, + defmt::unwrap!(cordic::Config::new( + cordic::Function::Sin, + Default::default(), + Default::default(), + )), + ); + + #[cfg(feature = "stm32g491re")] + let (mut write_dma, mut read_dma) = (dp.DMA1_CH4, dp.DMA1_CH5); + + #[cfg(any(feature = "stm32h563zi", feature = "stm32u585ai", feature = "stm32u5a5zj"))] + let (mut write_dma, mut read_dma) = (dp.GPDMA1_CH0, dp.GPDMA1_CH1); + + // calculate first result using blocking mode + let cnt0 = defmt::unwrap!(cordic.blocking_calc_32bit(&input_q1_31[..2], &mut output_q1_31, false, false)); + + // calculate rest results using async mode + let cnt1 = defmt::unwrap!( + cordic + .async_calc_32bit( + &mut write_dma, + &mut read_dma, + &input_q1_31[2..], + &mut output_q1_31[cnt0..], + true, + false, + ) + .await + ); + + // all output value length should be the same as our output buffer size + defmt::assert_eq!(cnt0 + cnt1, output_q1_31.len()); + + let mut cordic_result_f64 = [0.0f64; OUTPUT_LENGTH]; + + for (f64_val, u32_val) in cordic_result_f64.iter_mut().zip(output_q1_31) { + *f64_val = utils::q1_31_to_f64(u32_val); + } + + // + // software calculation + // + + let mut software_result_f64 = [0.0f64; OUTPUT_LENGTH]; + + let arg2 = utils::q1_31_to_f64(input_q1_31[1]); + + for (&arg1, res) in input_q1_31 + .iter() + .enumerate() + .filter_map(|(idx, val)| if idx != 1 { Some(val) } else { None }) + .zip(software_result_f64.chunks_mut(2)) + { + let arg1 = utils::q1_31_to_f64(arg1); + + let (raw_res1, raw_res2) = (arg1 * core::f64::consts::PI).sin_cos(); + (res[0], res[1]) = (raw_res1 * arg2, raw_res2 * arg2); + } + + // + // check result are the same + // + + for (cordic_res, software_res) in cordic_result_f64[..cnt0 + cnt1] + .chunks(2) + .zip(software_result_f64.chunks(2)) + { + for (cord_res, soft_res) in cordic_res.iter().zip(software_res.iter()) { + // 2.0.powi(-19) is the max residual error for Sin function, in q1.31 format, with 24 iterations (aka PRECISION = 6) + defmt::assert!((cord_res - soft_res).abs() <= 2.0.powi(-19)); + } + } + + info!("Test OK"); + cortex_m::asm::bkpt(); +}