stm32 CORDIC: ZeroOverhead for q1.31 and q1.15
This commit is contained in:
@ -68,16 +68,3 @@ pub enum Width {
|
|||||||
Bits32,
|
Bits32,
|
||||||
Bits16,
|
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,
|
|
||||||
}
|
|
||||||
|
@ -1,8 +1,9 @@
|
|||||||
//! CORDIC co-processor
|
//! CORDIC co-processor
|
||||||
|
|
||||||
use crate::peripherals;
|
|
||||||
use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
|
use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef};
|
||||||
|
|
||||||
|
use crate::peripherals;
|
||||||
|
|
||||||
mod enums;
|
mod enums;
|
||||||
pub use enums::*;
|
pub use enums::*;
|
||||||
|
|
||||||
@ -10,10 +11,6 @@ pub mod utils;
|
|||||||
|
|
||||||
pub(crate) mod sealed;
|
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.
|
/// Low-level CORDIC access.
|
||||||
#[cfg(feature = "unstable-pac")]
|
#[cfg(feature = "unstable-pac")]
|
||||||
pub mod low_level {
|
pub mod low_level {
|
||||||
@ -31,30 +28,16 @@ pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPer
|
|||||||
|
|
||||||
/// CORDIC configuration
|
/// CORDIC configuration
|
||||||
pub struct Config {
|
pub struct Config {
|
||||||
mode: Mode,
|
|
||||||
function: Function,
|
function: Function,
|
||||||
precision: Precision,
|
precision: Precision,
|
||||||
scale: Scale,
|
scale: Scale,
|
||||||
first_result: bool,
|
first_result: bool,
|
||||||
}
|
}
|
||||||
|
|
||||||
// CORDIC running state
|
|
||||||
struct State {
|
|
||||||
input_buf: [u32; INPUT_BUF_LEN],
|
|
||||||
buf_index: usize,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Config {
|
impl Config {
|
||||||
/// Create a config for Cordic driver
|
/// Create a config for Cordic driver
|
||||||
pub fn new(
|
pub fn new(function: Function, precision: Option<Precision>, scale: Option<Scale>, first_result: bool) -> Self {
|
||||||
mode: Mode,
|
|
||||||
function: Function,
|
|
||||||
precision: Option<Precision>,
|
|
||||||
scale: Option<Scale>,
|
|
||||||
first_result: bool,
|
|
||||||
) -> Self {
|
|
||||||
Self {
|
Self {
|
||||||
mode,
|
|
||||||
function,
|
function,
|
||||||
precision: precision.unwrap_or_default(),
|
precision: precision.unwrap_or_default(),
|
||||||
scale: scale.unwrap_or_default(),
|
scale: scale.unwrap_or_default(),
|
||||||
@ -133,45 +116,23 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
} else {
|
} else {
|
||||||
self.peri.set_result_count(Count::Two)
|
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_f64(&mut self) -> (f64, Option<f64>) {
|
fn blocking_read_f32(&mut self) -> (f32, Option<f32>) {
|
||||||
let res1 = utils::q1_31_to_f64(self.peri.read_result());
|
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 {
|
let res2 = if !self.config.first_result {
|
||||||
Some(utils::q1_31_to_f64(self.peri.read_result()))
|
Some(utils::q1_15_to_f32((reg_value >> 16) as u16))
|
||||||
} else {
|
} else {
|
||||||
None
|
None
|
||||||
};
|
};
|
||||||
|
|
||||||
(res1, res2)
|
(res1, res2)
|
||||||
}
|
}
|
||||||
|
|
||||||
fn blocking_read_f64_to_buf(&mut self, result_buf: &mut [f64], result_index: &mut usize) {
|
|
||||||
let (res1, res2) = self.blocking_read_f64();
|
|
||||||
result_buf[*result_index] = res1;
|
|
||||||
*result_index += 1;
|
|
||||||
|
|
||||||
if let Some(res2) = res2 {
|
|
||||||
result_buf[*result_index] = res2;
|
|
||||||
*result_index += 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn blocking_write_f64(&mut self, arg: f64) {
|
|
||||||
self.peri.write_argument(utils::f64_to_q1_31(arg));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<'d, T: Instance> Drop for Cordic<'d, T> {
|
impl<'d, T: Instance> Drop for Cordic<'d, T> {
|
||||||
@ -183,7 +144,11 @@ impl<'d, T: Instance> Drop for Cordic<'d, T> {
|
|||||||
// q1.31 related
|
// q1.31 related
|
||||||
impl<'d, T: Instance> Cordic<'d, T> {
|
impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
/// Run a CORDIC calculation
|
/// Run a CORDIC calculation
|
||||||
pub fn 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]) -> usize {
|
||||||
|
if arg1s.is_empty() {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
assert!(
|
assert!(
|
||||||
match self.config.first_result {
|
match self.config.first_result {
|
||||||
true => output.len() >= arg1s.len(),
|
true => output.len() >= arg1s.len(),
|
||||||
@ -194,6 +159,10 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
|
|
||||||
self.check_input_f64(arg1s, arg2s);
|
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 {
|
self.peri.set_result_count(if self.config.first_result {
|
||||||
Count::One
|
Count::One
|
||||||
} else {
|
} else {
|
||||||
@ -206,11 +175,9 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
|
|
||||||
let mut consumed_input_len = 0;
|
let mut consumed_input_len = 0;
|
||||||
|
|
||||||
match self.config.mode {
|
|
||||||
Mode::ZeroOverhead => {
|
|
||||||
// put double input into cordic
|
// put double input into cordic
|
||||||
if arg2s.is_some() && !arg2s.unwrap().is_empty() {
|
if arg2s.is_some() && !arg2s.expect("It's infailable").is_empty() {
|
||||||
let arg2s = arg2s.unwrap();
|
let arg2s = arg2s.expect("It's infailable");
|
||||||
|
|
||||||
self.peri.set_argument_count(Count::Two);
|
self.peri.set_argument_count(Count::Two);
|
||||||
|
|
||||||
@ -260,12 +227,124 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
|
|
||||||
output_count
|
output_count
|
||||||
}
|
}
|
||||||
Mode::Interrupt => todo!(),
|
|
||||||
Mode::Dma => todo!(),
|
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 {
|
||||||
|
None
|
||||||
|
};
|
||||||
|
|
||||||
|
(res1, res2)
|
||||||
|
}
|
||||||
|
|
||||||
|
fn blocking_read_f64_to_buf(&mut self, result_buf: &mut [f64], result_index: &mut usize) {
|
||||||
|
let (res1, res2) = self.blocking_read_f64();
|
||||||
|
result_buf[*result_index] = res1;
|
||||||
|
*result_index += 1;
|
||||||
|
|
||||||
|
if let Some(res2) = res2 {
|
||||||
|
result_buf[*result_index] = res2;
|
||||||
|
*result_index += 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn check_input_f64(&self, arg1s: &[f64], arg2s: Option<&[f64]>) {
|
fn blocking_write_f64(&mut self, arg: f64) {
|
||||||
|
self.peri.write_argument(utils::f64_to_q1_31(arg));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// q1.15 related
|
||||||
|
impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
|
/// Run a CORDIC calculation
|
||||||
|
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(),
|
||||||
|
false => output.len() >= 2 * arg1s.len(),
|
||||||
|
},
|
||||||
|
"Output buf length is not long enough"
|
||||||
|
);
|
||||||
|
|
||||||
|
self.check_input_f32(arg1s, arg2s);
|
||||||
|
|
||||||
|
self.peri.disable_irq();
|
||||||
|
self.peri.disable_write_dma();
|
||||||
|
self.peri.disable_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_data_width(Width::Bits16, Width::Bits16);
|
||||||
|
|
||||||
|
let mut output_count = 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)
|
||||||
|
|
||||||
|
let arg2_default_value = match arg2s {
|
||||||
|
Some(arg2s) if !arg2s.is_empty() => arg2s[arg2s.len() - 1],
|
||||||
|
_ => 1.0,
|
||||||
|
};
|
||||||
|
|
||||||
|
let mut args = arg1s.iter().zip(
|
||||||
|
arg2s
|
||||||
|
.unwrap_or(&[])
|
||||||
|
.iter()
|
||||||
|
.chain(core::iter::repeat(&arg2_default_value)),
|
||||||
|
);
|
||||||
|
|
||||||
|
let (&arg1, &arg2) = args
|
||||||
|
.next()
|
||||||
|
.expect("This should be infallible, since arg1s is not empty");
|
||||||
|
|
||||||
|
// preloading 1 pair of arguments
|
||||||
|
self.blocking_write_f32(arg1, arg2);
|
||||||
|
|
||||||
|
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 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
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;
|
let config = &self.config;
|
||||||
|
|
||||||
use Function::*;
|
use Function::*;
|
||||||
@ -323,7 +402,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
"When SCALE set to 3, ARG1 should be: 0.375 <= ARG1 < 0.875"
|
"When SCALE set to 3, ARG1 should be: 0.375 <= ARG1 < 0.875"
|
||||||
),
|
),
|
||||||
Scale::A1o16_R16 => assert!(
|
Scale::A1o16_R16 => assert!(
|
||||||
arg1s.iter().all(|v| (0.4375f64..0.584f64).contains(v)),
|
arg1s.iter().all(|v| (0.4375..0.584).contains(v)),
|
||||||
"When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584"
|
"When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584"
|
||||||
),
|
),
|
||||||
_ => unreachable!(),
|
_ => unreachable!(),
|
||||||
@ -365,6 +444,11 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
check_input_value!(check_input_f64, f64);
|
||||||
|
check_input_value!(check_input_f32, f32);
|
||||||
|
|
||||||
foreach_interrupt!(
|
foreach_interrupt!(
|
||||||
($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
|
($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
|
||||||
|
@ -3,7 +3,7 @@
|
|||||||
macro_rules! floating_fixed_convert {
|
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) => {
|
($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
|
/// 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) };
|
const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
|
||||||
|
|
||||||
assert!(
|
assert!(
|
||||||
@ -31,7 +31,7 @@ macro_rules! floating_fixed_convert {
|
|||||||
|
|
||||||
#[inline(always)]
|
#[inline(always)]
|
||||||
/// convert fixed point to float point format
|
/// 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.
|
// 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)
|
-(value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty)
|
||||||
}
|
}
|
||||||
|
Reference in New Issue
Block a user