diff --git a/embassy-stm32/src/cordic.rs b/embassy-stm32/src/cordic.rs deleted file mode 100644 index 952ee187a..000000000 --- a/embassy-stm32/src/cordic.rs +++ /dev/null @@ -1,460 +0,0 @@ -//! CORDIC co-processor - -use crate::peripherals; -use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef}; - -pub use enums::*; - -mod enums { - /// CORDIC function - #[allow(missing_docs)] - #[derive(Clone, Copy)] - pub enum Function { - Cos = 0, - Sin, - Phase, - Modulus, - Arctan, - Cosh, - Sinh, - Arctanh, - Ln, - Sqrt, - } - - /// CORDIC precision - #[allow(missing_docs)] - #[derive(Clone, Copy)] - pub enum Precision { - Iters4 = 1, - Iters8, - Iters12, - Iters16, - Iters20, - Iters24, - Iters28, - Iters32, - Iters36, - Iters40, - Iters44, - Iters48, - Iters52, - Iters56, - Iters60, - } - - /// CORDIC scale - #[allow(non_camel_case_types)] - #[allow(missing_docs)] - #[derive(Clone, Copy, Default)] - pub enum Scale { - #[default] - A1_R1 = 0, - A1o2_R2, - A1o4_R4, - A1o8_R8, - A1o16_R16, - A1o32_R32, - A1o64_R64, - A1o128_R128, - } - - /// CORDIC argument/result count - #[allow(missing_docs)] - #[derive(Clone, Copy, Default)] - pub enum Count { - #[default] - One, - Two, - } - - /// CORDIC argument/result data width - #[allow(missing_docs)] - #[derive(Clone, Copy)] - pub enum Width { - Bits32, - Bits16, - } - - /// Cordic driver running mode - #[derive(Clone, Copy)] - pub enum Mode { - /// After caculation start, a read to RDATA register will block AHB until the caculation finished - ZeroOverhead, - - /// Use CORDIC interrupt to trigger a read result value - Interrupt, - - /// Use DMA to write/read value - Dma, - } -} - -/// Low-level CORDIC access. -#[cfg(feature = "unstable-pac")] -pub mod low_level { - pub use super::sealed::*; -} - -pub(crate) mod sealed { - use super::*; - use crate::pac::cordic::vals; - - /// Cordic instance - pub trait Instance { - /// 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: Count) { - Self::regs().csr().modify(|v| { - v.set_nargs(match n { - Count::One => vals::Num::NUM1, - Count::Two => vals::Num::NUM2, - }) - }) - } - - /// Set NRES value - fn set_result_count(&self, n: Count) { - Self::regs().csr().modify(|v| { - v.set_nres(match n { - Count::One => vals::Num::NUM1, - Count::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() - } - } -} - -/// CORDIC driver -pub struct Cordic<'d, T: Instance> { - cordic: PeripheralRef<'d, T>, - config: Config, - //state: State, -} - -/// CORDIC instance trait -pub trait Instance: sealed::Instance + Peripheral

+ crate::rcc::RccPeripheral {} - -/// CORDIC configuration -pub struct Config { - function: Function, - precision: Precision, - scale: Scale, - mode: Mode, - first_result: bool, -} - -// CORDIC running state -//struct State { -// input_buf: [u32; 8], -// buf_len: usize, -//} - -impl Config { - /// Create a config for Cordic driver - pub fn new(function: Function, precision: Precision, scale: Option, mode: Mode, first_result: bool) -> Self { - Self { - function, - precision, - scale: scale.unwrap_or_default(), - mode, - first_result, - } - } - - fn check_scale(&self) -> bool { - 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), - } - } -} - -impl<'d, T: Instance> Cordic<'d, T> { - /// Create a Cordic driver instance - /// - /// Note: - /// If you need a periperhal -> CORDIC -> peripehral mode, - /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguemnts with [Self::extra_config] - pub fn new(cordic: impl Peripheral

+ 'd, config: Config) -> Self { - T::enable_and_reset(); - - into_ref!(cordic); - - if !config.check_scale() { - panic!("Scale value is not compatible with Function") - } - - let mut instance = Self { - cordic, - config, - // state: State { - // input_buf: [0u32; 8], - // buf_len: 0, - // }, - }; - - 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: Count, arg_width: Width, res_width: Width) { - let peri = &self.cordic; - peri.set_argument_count(arg_cnt); - peri.set_data_width(arg_width, res_width); - } - - fn reconfigure(&mut self) { - let peri = &self.cordic; - let config = &self.config; - - if peri.ready_to_read() { - warn!("At least 1 result hasn't been read, reconfigure will cause DATA LOST"); - }; - - peri.disable_irq(); - peri.disable_write_dma(); - peri.disable_read_dma(); - - // clean RRDY flag - while peri.ready_to_read() { - peri.read_result(); - } - - peri.set_func(config.function); - peri.set_precision(config.precision); - peri.set_scale(config.scale); - if config.first_result { - peri.set_result_count(Count::One) - } else { - peri.set_result_count(Count::Two) - } - - match config.mode { - Mode::ZeroOverhead => (), - Mode::Interrupt => { - peri.enable_irq(); - } - Mode::Dma => { - peri.enable_write_dma(); - peri.enable_read_dma(); - } - } - - //self.state.input_buf.fill(0u32); - } - - /// Run a CORDIC calculation - pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize { - match self.config.mode { - Mode::ZeroOverhead => { - if arg2s.is_none() { - self.cordic.set_argument_count(Count::One); - - self.cordic.set_result_count(if self.config.first_result { - if output.len() < arg1s.len() { - panic!("Output buf length is not long enough") - } - Count::One - } else { - if output.len() < 2 * arg1s.len() { - panic!("Output buf length is not long enough") - } - Count::Two - }); - - let mut cnt = 0; - - for &arg in arg1s.iter() { - self.cordic.write_argument(f64_to_q1_31(arg)); - output[cnt] = q1_31_to_f64(self.cordic.read_result()); - cnt += 1; - } - - cnt - } else { - todo!() - } - } - Mode::Interrupt => todo!(), - Mode::Dma => todo!(), - } - } -} - -impl<'d, T: Instance> Drop for Cordic<'d, T> { - fn drop(&mut self) { - T::disable(); - } -} - -foreach_interrupt!( - ($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => { - impl Instance for peripherals::$inst { - } - - impl sealed::Instance for peripherals::$inst { - fn regs() -> crate::pac::cordic::Cordic { - crate::pac::$inst - } - } - }; -); - -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) -> $unsigned_bin_typ { - const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) }; - - assert!( - (-1.0 as $float_ty) <= value, - "input value {} should be equal or greater than -1", - value - ); - - let value = if value == 1.0 as $float_ty{ - (1.0 as $float_ty) - MIN_POSITIVE - } else { - assert!( - value <= (1.0 as $float_ty) - MIN_POSITIVE, - "input value {} should be equal or less than 1-2^(-{})", - value, $offset - ); - value - }; - - (value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) 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 needed to convert from unsigned to signed first, for correct result. - -(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/cordic/enums.rs b/embassy-stm32/src/cordic/enums.rs new file mode 100644 index 000000000..4697a1df1 --- /dev/null +++ b/embassy-stm32/src/cordic/enums.rs @@ -0,0 +1,82 @@ +/// CORDIC function +#[allow(missing_docs)] +#[derive(Clone, Copy)] +pub enum Function { + Cos = 0, + Sin, + Phase, + Modulus, + Arctan, + Cosh, + Sinh, + Arctanh, + Ln, + Sqrt, +} + +/// CORDIC precision +#[allow(missing_docs)] +#[derive(Clone, Copy)] +pub enum Precision { + Iters4 = 1, + Iters8, + Iters12, + Iters16, + Iters20, + Iters24, + Iters28, + Iters32, + Iters36, + Iters40, + Iters44, + Iters48, + Iters52, + Iters56, + Iters60, +} + +/// CORDIC scale +#[allow(non_camel_case_types)] +#[allow(missing_docs)] +#[derive(Clone, Copy, Default)] +pub enum Scale { + #[default] + A1_R1 = 0, + A1o2_R2, + A1o4_R4, + A1o8_R8, + A1o16_R16, + A1o32_R32, + A1o64_R64, + A1o128_R128, +} + +/// CORDIC argument/result count +#[allow(missing_docs)] +#[derive(Clone, Copy, Default)] +pub enum Count { + #[default] + One, + Two, +} + +/// CORDIC argument/result data width +#[allow(missing_docs)] +#[derive(Clone, Copy)] +pub enum Width { + Bits32, + Bits16, +} + +/// Cordic driver running mode +#[derive(Clone, Copy)] +pub enum Mode { + /// After caculation start, a read to RDATA register will block AHB until the caculation finished + ZeroOverhead, + + /// Use CORDIC interrupt to trigger a read result value + Interrupt, + + /// Use DMA to write/read value + Dma, +} diff --git a/embassy-stm32/src/cordic/mod.rs b/embassy-stm32/src/cordic/mod.rs new file mode 100644 index 000000000..c0a69b757 --- /dev/null +++ b/embassy-stm32/src/cordic/mod.rs @@ -0,0 +1,206 @@ +//! CORDIC co-processor + +use crate::peripherals; +use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef}; + +mod enums; +pub use enums::*; + +pub mod utils; + +pub(crate) mod sealed; + +/// Low-level CORDIC access. +#[cfg(feature = "unstable-pac")] +pub mod low_level { + pub use super::sealed::*; +} + +/// CORDIC driver +pub struct Cordic<'d, T: Instance> { + cordic: PeripheralRef<'d, T>, + config: Config, + //state: State, +} + +/// CORDIC instance trait +pub trait Instance: sealed::Instance + Peripheral

+ crate::rcc::RccPeripheral {} + +/// CORDIC configuration +pub struct Config { + function: Function, + precision: Precision, + scale: Scale, + mode: Mode, + first_result: bool, +} + +// CORDIC running state +//struct State { +// input_buf: [u32; 8], +// buf_len: usize, +//} + +impl Config { + /// Create a config for Cordic driver + pub fn new(function: Function, precision: Precision, scale: Option, mode: Mode, first_result: bool) -> Self { + Self { + function, + precision, + scale: scale.unwrap_or_default(), + mode, + first_result, + } + } + + fn check_scale(&self) -> bool { + 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), + } + } +} + +impl<'d, T: Instance> Cordic<'d, T> { + /// Create a Cordic driver instance + /// + /// Note: + /// If you need a periperhal -> CORDIC -> peripehral mode, + /// you may want to set Cordic into [Mode::ZeroOverhead] mode, and add extra arguemnts with [Self::extra_config] + pub fn new(cordic: impl Peripheral

+ 'd, config: Config) -> Self { + T::enable_and_reset(); + + into_ref!(cordic); + + if !config.check_scale() { + panic!("Scale value is not compatible with Function") + } + + let mut instance = Self { + cordic, + config, + // state: State { + // input_buf: [0u32; 8], + // buf_len: 0, + // }, + }; + + 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: Count, arg_width: Width, res_width: Width) { + let peri = &self.cordic; + peri.set_argument_count(arg_cnt); + peri.set_data_width(arg_width, res_width); + } + + fn reconfigure(&mut self) { + let peri = &self.cordic; + let config = &self.config; + + if peri.ready_to_read() { + warn!("At least 1 result hasn't been read, reconfigure will cause DATA LOST"); + }; + + peri.disable_irq(); + peri.disable_write_dma(); + peri.disable_read_dma(); + + // clean RRDY flag + while peri.ready_to_read() { + peri.read_result(); + } + + peri.set_func(config.function); + peri.set_precision(config.precision); + peri.set_scale(config.scale); + if config.first_result { + peri.set_result_count(Count::One) + } else { + peri.set_result_count(Count::Two) + } + + match config.mode { + Mode::ZeroOverhead => (), + Mode::Interrupt => { + peri.enable_irq(); + } + Mode::Dma => { + peri.enable_write_dma(); + peri.enable_read_dma(); + } + } + + //self.state.input_buf.fill(0u32); + } + + /// Run a CORDIC calculation + pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize { + match self.config.mode { + Mode::ZeroOverhead => { + if arg2s.is_none() { + self.cordic.set_argument_count(Count::One); + + self.cordic.set_result_count(if self.config.first_result { + if output.len() < arg1s.len() { + panic!("Output buf length is not long enough") + } + Count::One + } else { + if output.len() < 2 * arg1s.len() { + panic!("Output buf length is not long enough") + } + Count::Two + }); + + let mut cnt = 0; + + for &arg in arg1s.iter() { + self.cordic.write_argument(utils::f64_to_q1_31(arg)); + output[cnt] = utils::q1_31_to_f64(self.cordic.read_result()); + cnt += 1; + } + + cnt + } else { + todo!() + } + } + Mode::Interrupt => todo!(), + Mode::Dma => todo!(), + } + } +} + +impl<'d, T: Instance> Drop for Cordic<'d, T> { + fn drop(&mut self) { + T::disable(); + } +} + +foreach_interrupt!( + ($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => { + impl Instance for peripherals::$inst { + } + + impl sealed::Instance for peripherals::$inst { + fn regs() -> crate::pac::cordic::Cordic { + crate::pac::$inst + } + } + }; +); diff --git a/embassy-stm32/src/cordic/sealed.rs b/embassy-stm32/src/cordic/sealed.rs new file mode 100644 index 000000000..0f00e380c --- /dev/null +++ b/embassy-stm32/src/cordic/sealed.rs @@ -0,0 +1,116 @@ +use super::*; +use crate::pac::cordic::vals; + +/// Cordic instance +pub trait Instance { + /// 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: Count) { + Self::regs().csr().modify(|v| { + v.set_nargs(match n { + Count::One => vals::Num::NUM1, + Count::Two => vals::Num::NUM2, + }) + }) + } + + /// Set NRES value + fn set_result_count(&self, n: Count) { + Self::regs().csr().modify(|v| { + v.set_nres(match n { + Count::One => vals::Num::NUM1, + Count::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..3f055c34b --- /dev/null +++ b/embassy-stm32/src/cordic/utils.rs @@ -0,0 +1,59 @@ +//! Common match utils + +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) -> $unsigned_bin_typ { + const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) }; + + assert!( + (-1.0 as $float_ty) <= value, + "input value {} should be equal or greater than -1", + value + ); + + + let value = if value == 1.0 as $float_ty{ + // make a exception for user specifing exact 1.0 float point, + // convert 1.0 to max representable value of q1.x format + (1.0 as $float_ty) - MIN_POSITIVE + } else { + assert!( + value <= (1.0 as $float_ty) - MIN_POSITIVE, + "input value {} should be equal or less than 1-2^(-{})", + value, $offset + ); + value + }; + + (value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) 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 needed to convert from unsigned to signed first, for correct result. + -(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) +);