nordic-dev.net/embassy
2
0
Fork 0
mirror of https://github.com/embassy-rs/embassy.git synced 2024-11-25 08:12:30 +00:00
Code Issues Packages Projects Releases Wiki Activity

stm32 CORDIC: ZeroOverhead for q1.31 and q1.15

Browse Source
This commit is contained in:
eZio Pan 2024-03-16 21:20:17 +08:00
parent 5d12f59430
commit c9f759bb21
3 changed files with 278 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
embassy-stm32/src/cordic/enums.rs
View File

@ -68,16 +68,3 @@ 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,
}

468
embassy-stm32/src/cordic/mod.rs
View File

@ -1,8 +1,9 @@
//! CORDIC co-processor
use crate::peripherals;
use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
use crate::peripherals;
mod enums;
pub use enums::*;
@ -10,10 +11,6 @@ pub mod utils;
pub(crate) mod sealed;
// length of pre-allocated [u32] memory for CORDIC input,
// length should be multiple of 2
const INPUT_BUF_LEN: usize = 8;
/// Low-level CORDIC access.
#[cfg(feature = "unstable-pac")]
pub mod low_level {
@ -31,30 +28,16 @@ pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPer
/// CORDIC configuration
pub struct Config {
mode: Mode,
function: Function,
precision: Precision,
scale: Scale,
first_result: bool,
}
// CORDIC running state
struct State {
input_buf: [u32; INPUT_BUF_LEN],
buf_index: usize,
}
impl Config {
/// Create a config for Cordic driver
pub fn new(
mode: Mode,
function: Function,
precision: Option<Precision>,
scale: Option<Scale>,
first_result: bool,
) -> Self {
pub fn new(function: Function, precision: Option<Precision>, scale: Option<Scale>, first_result: bool) -> Self {
Self {
mode,
function,
precision: precision.unwrap_or_default(),
scale: scale.unwrap_or_default(),
@ -133,22 +116,123 @@ impl<'d, T: Instance> Cordic<'d, T> {
} else {
self.peri.set_result_count(Count::Two)
}
}
match self.config.mode {
Mode::ZeroOverhead => (),
Mode::Interrupt => {
self.peri.enable_irq();
}
Mode::Dma => {
self.peri.enable_write_dma();
self.peri.enable_read_dma();
}
fn blocking_read_f32(&mut self) -> (f32, Option<f32>) {
let reg_value = self.peri.read_result();
let res1 = utils::q1_15_to_f32((reg_value & ((1u32 << 16) - 1)) as u16);
// 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.
let res2 = if !self.config.first_result {
Some(utils::q1_15_to_f32((reg_value >> 16) as u16))
} else {
None
};
(res1, res2)
}
}
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 CORDIC calculation
pub fn blocking_calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
if arg1s.is_empty() {
return 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"
);
self.check_input_f64(arg1s, arg2s);
self.peri.disable_irq();
self.peri.disable_write_dma();
self.peri.disable_read_dma();
self.peri.set_result_count(if self.config.first_result {
Count::One
} else {
Count::Two
});
self.peri.set_data_width(Width::Bits32, Width::Bits32);
let mut output_count = 0;
let mut consumed_input_len = 0;
// put double input into cordic
if arg2s.is_some() && !arg2s.expect("It's infailable").is_empty() {
let arg2s = arg2s.expect("It's infailable");
self.peri.set_argument_count(Count::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.
let double_input = arg1s.iter().skip(1).zip(&arg2s[..arg2s.len() - 1]);
// Since we preload 1st value from arg1s, the consumed input length is double_input length + 1.
consumed_input_len = double_input.len() + 1;
// preload first value from arg1 to cordic
self.blocking_write_f64(arg1s[0]);
for (&arg1, &arg2) in double_input {
// Since we manually preload a value before,
// we will write arg2 (from the actual last pair) first, (at this moment, cordic start to calculating,)
// and write arg1 (from the actual next pair), then read the result, to "keep preloading"
self.blocking_write_f64(arg2);
self.blocking_write_f64(arg1);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// write last input value from arg2s, then read out the result
self.blocking_write_f64(arg2s[arg2s.len() - 1]);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// put single input into cordic
let input_left = &arg1s[consumed_input_len..];
if !input_left.is_empty() {
self.peri.set_argument_count(Count::One);
// "preload" value to cordic (at this moment, cordic start to calculating)
self.blocking_write_f64(input_left[0]);
for &arg in input_left.iter().skip(1) {
// this line write arg for next round caculation to cordic,
// and read result from last round
self.blocking_write_f64(arg);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// read the last output
self.blocking_read_f64_to_buf(output, &mut output_count);
}
output_count
}
fn blocking_read_f64(&mut self) -> (f64, Option<f64>) {
let res1 = utils::q1_31_to_f64(self.peri.read_result());
// 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.
let res2 = if !self.config.first_result {
Some(utils::q1_31_to_f64(self.peri.read_result()))
} else {
@ -174,16 +258,14 @@ impl<'d, T: Instance> Cordic<'d, T> {
}
}
impl<'d, T: Instance> Drop for Cordic<'d, T> {
fn drop(&mut self) {
T::disable();
}
}
// q1.31 related
// q1.15 related
impl<'d, T: Instance> Cordic<'d, T> {
/// Run a CORDIC calculation
pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
pub fn blocking_calc_16bit(&mut self, arg1s: &[f32], arg2s: Option<&[f32]>, output: &mut [f32]) -> usize {
if arg1s.is_empty() {
return 0;
}
assert!(
match self.config.first_result {
true => output.len() >= arg1s.len(),
@ -192,180 +274,182 @@ impl<'d, T: Instance> Cordic<'d, T> {
"Output buf length is not long enough"
);
self.check_input_f64(arg1s, arg2s);
self.check_input_f32(arg1s, arg2s);
self.peri.set_result_count(if self.config.first_result {
Count::One
} else {
Count::Two
});
self.peri.disable_irq();
self.peri.disable_write_dma();
self.peri.disable_read_dma();
self.peri.set_data_width(Width::Bits32, Width::Bits32);
// 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_data_width(Width::Bits16, Width::Bits16);
let mut output_count = 0;
let mut consumed_input_len = 0;
// In q1.15 mode, we always fill 1 pair of 16bit value into WDATA register.
// If arg2s is None or empty array, we assume arg2 value always 1.0 (as reset value for ARG2).
// If arg2s has some value, and but not as long as arg1s,
// we fill the reset of arg2 values with last value from arg2s (as q1.31 version does)
match self.config.mode {
Mode::ZeroOverhead => {
// put double input into cordic
if arg2s.is_some() && !arg2s.unwrap().is_empty() {
let arg2s = arg2s.unwrap();
let arg2_default_value = match arg2s {
Some(arg2s) if !arg2s.is_empty() => arg2s[arg2s.len() - 1],
_ => 1.0,
};
self.peri.set_argument_count(Count::Two);
let mut args = arg1s.iter().zip(
arg2s
.unwrap_or(&[])
.iter()
.chain(core::iter::repeat(&arg2_default_value)),
);
// 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.
let double_input = arg1s.iter().skip(1).zip(&arg2s[..arg2s.len() - 1]);
// Since we preload 1st value from arg1s, the consumed input length is double_input length + 1.
consumed_input_len = double_input.len() + 1;
let (&arg1, &arg2) = args
.next()
.expect("This should be infallible, since arg1s is not empty");
// preload first value from arg1 to cordic
self.blocking_write_f64(arg1s[0]);
// preloading 1 pair of arguments
self.blocking_write_f32(arg1, arg2);
for (&arg1, &arg2) in double_input {
// Since we manually preload a value before,
// we will write arg2 (from the actual last pair) first, (at this moment, cordic start to calculating,)
// and write arg1 (from the actual next pair), then read the result, to "keep preloading"
self.blocking_write_f64(arg2);
self.blocking_write_f64(arg1);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// write last input value from arg2s, then read out the result
self.blocking_write_f64(arg2s[arg2s.len() - 1]);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// put single input into cordic
let input_left = &arg1s[consumed_input_len..];
if !input_left.is_empty() {
self.peri.set_argument_count(Count::One);
// "preload" value to cordic (at this moment, cordic start to calculating)
self.blocking_write_f64(input_left[0]);
for &arg in input_left.iter().skip(1) {
// this line write arg for next round caculation to cordic,
// and read result from last round
self.blocking_write_f64(arg);
self.blocking_read_f64_to_buf(output, &mut output_count);
}
// read the last output
self.blocking_read_f64_to_buf(output, &mut output_count);
}
output_count
}
Mode::Interrupt => todo!(),
Mode::Dma => todo!(),
for (&arg1, &arg2) in args {
self.blocking_write_f32(arg1, arg2);
self.blocking_read_f32_to_buf(output, &mut output_count);
}
// read last pair of value from cordic
self.blocking_read_f32_to_buf(output, &mut output_count);
output_count
}
fn check_input_f64(&self, arg1s: &[f64], arg2s: Option<&[f64]>) {
let config = &self.config;
fn blocking_write_f32(&mut self, arg1: f32, arg2: f32) {
let reg_value: u32 = utils::f32_to_q1_15(arg1) as u32 + ((utils::f32_to_q1_15(arg2) as u32) << 16);
self.peri.write_argument(reg_value);
}
use Function::*;
fn blocking_read_f32_to_buf(&mut self, result_buf: &mut [f32], result_index: &mut usize) {
let (res1, res2) = self.blocking_read_f32();
result_buf[*result_index] = res1;
*result_index += 1;
// 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"
),
}
// 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"
);
}
Cosh | Sinh => assert!(
arg1s.iter().all(|v| (-0.559..=0.559).contains(v)),
"ARG1 should be: -0.559 <= ARG1 <= 0.559"
),
Arctanh => assert!(
arg1s.iter().all(|v| (-0.403..=0.403).contains(v)),
"ARG1 should be: -0.403 <= ARG1 <= 0.403"
),
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.4375f64..0.584f64).contains(v)),
"When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584"
),
_ => unreachable!(),
};
}
Function::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!(),
},
}
// 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"
),
Phase | Modulus => assert!(
arg2s.iter().all(|v| (-1.0..=1.0).contains(v)),
"ARG2 should be: -1 <= ARG2 <= 1"
),
_ => (),
}
if let Some(res2) = res2 {
result_buf[*result_index] = res2;
*result_index += 1;
}
}
}
// check input value ARG1, ARG2, SCALE and FUNCTION are compatible with each other
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]>) {
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"
),
}
// 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"
);
}
Cosh | Sinh => assert!(
arg1s.iter().all(|v| (-0.559..=0.559).contains(v)),
"ARG1 should be: -0.559 <= ARG1 <= 0.559"
),
Arctanh => assert!(
arg1s.iter().all(|v| (-0.403..=0.403).contains(v)),
"ARG1 should be: -0.403 <= ARG1 <= 0.403"
),
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!(),
};
}
Function::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!(),
},
}
// 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"
),
Phase | Modulus => assert!(
arg2s.iter().all(|v| (-1.0..=1.0).contains(v)),
"ARG2 should be: -1 <= ARG2 <= 1"
),
_ => (),
}
}
}
}
};
}
check_input_value!(check_input_f64, f64);
check_input_value!(check_input_f32, f32);
foreach_interrupt!(
($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
impl Instance for peripherals::$inst {

4
embassy-stm32/src/cordic/utils.rs
View File

@ -3,7 +3,7 @@
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 {
pub(crate) fn $f_to_q(value: $float_ty) -> $unsigned_bin_typ {
const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
assert!(
@ -31,7 +31,7 @@ macro_rules! floating_fixed_convert {
#[inline(always)]
/// convert fixed point to float point format
pub fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty {
pub(crate) 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)
}