2 files changed, 227 insertions, 42 deletions

diff --git a/embassy-stm32/src/cordic/enums.rs b/embassy-stm32/src/cordic/enums.rs
index 4697a1df1..3e1c47f7f 100644
--- a/embassy-stm32/src/cordic/enums.rs
+++ b/embassy-stm32/src/cordic/enums.rs
@@ -16,14 +16,15 @@ pub enum Function {
 
 /// CORDIC precision
 #[allow(missing_docs)]
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Default)]
 pub enum Precision {
     Iters4 = 1,
     Iters8,
     Iters12,
     Iters16,
     Iters20,
-    Iters24,
+    #[default]
+    Iters24, // this value is recomended by Reference Manual
     Iters28,
     Iters32,
     Iters36,
@@ -38,7 +39,7 @@ pub enum Precision {
 /// CORDIC scale
 #[allow(non_camel_case_types)]
 #[allow(missing_docs)]
-#[derive(Clone, Copy, Default)]
+#[derive(Clone, Copy, Default, PartialEq)]
 pub enum Scale {
     #[default]
     A1_R1 = 0,
diff --git a/embassy-stm32/src/cordic/mod.rs b/embassy-stm32/src/cordic/mod.rs
index c0a69b757..b15521ca6 100644
--- a/embassy-stm32/src/cordic/mod.rs
+++ b/embassy-stm32/src/cordic/mod.rs
@@ -10,6 +10,10 @@ 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 {
@@ -20,7 +24,7 @@ pub mod low_level {
 pub struct Cordic<'d, T: Instance> {
     cordic: PeripheralRef<'d, T>,
     config: Config,
-    //state: State,
+    state: State,
 }
 
 /// CORDIC instance trait
@@ -28,27 +32,33 @@ pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPer
 
 /// CORDIC configuration
 pub struct Config {
+    mode: Mode,
     function: Function,
     precision: Precision,
     scale: Scale,
-    mode: Mode,
     first_result: bool,
 }
 
 // CORDIC running state
-//struct State {
-//    input_buf: [u32; 8],
-//    buf_len: usize,
-//}
+struct State {
+    input_buf: [u32; INPUT_BUF_LEN],
+    buf_index: usize,
+}
 
 impl Config {
     /// Create a config for Cordic driver
-    pub fn new(function: Function, precision: Precision, scale: Option<Scale>, mode: Mode, first_result: bool) -> Self {
+    pub fn new(
+        mode: Mode,
+        function: Function,
+        precision: Option<Precision>,
+        scale: Option<Scale>,
+        first_result: bool,
+    ) -> Self {
         Self {
+            mode,
             function,
-            precision,
+            precision: precision.unwrap_or_default(),
             scale: scale.unwrap_or_default(),
-            mode,
             first_result,
         }
     }
@@ -66,6 +76,7 @@ impl Config {
     }
 }
 
+// common method
 impl<'d, T: Instance> Cordic<'d, T> {
     /// Create a Cordic driver instance
     ///
@@ -84,10 +95,10 @@ impl<'d, T: Instance> Cordic<'d, T> {
         let mut instance = Self {
             cordic,
             config,
-            // state: State {
-            //     input_buf: [0u32; 8],
-            //     buf_len: 0,
-            // },
+            state: State {
+                input_buf: [0u32; 8],
+                buf_index: 0,
+            },
         };
 
         instance.reconfigure();
@@ -128,6 +139,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
         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 {
@@ -145,55 +157,227 @@ impl<'d, T: Instance> Cordic<'d, T> {
             }
         }
 
-        //self.state.input_buf.fill(0u32);
+        self.state.input_buf.fill(0u32);
+        self.state.buf_index = 0;
+    }
+}
+
+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 calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
-        match self.config.mode {
+        let peri = &self.cordic;
+        let config = &self.config;
+
+        assert!(
+            match 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);
+
+        peri.set_result_count(if config.first_result { Count::One } else { Count::Two });
+        peri.set_data_width(Width::Bits32, Width::Bits32);
+
+        let state = &mut self.state;
+
+        let mut output_count = 0;
+
+        let mut consumed_input_len = 0;
+
+        match config.mode {
             Mode::ZeroOverhead => {
-                if arg2s.is_none() {
-                    self.cordic.set_argument_count(Count::One);
+                // put double input into cordic
+                if arg2s.is_some() && !arg2s.unwrap().is_empty() {
+                    let arg2s = arg2s.unwrap();
+
+                    peri.set_argument_count(Count::Two);
+
+                    let double_value = arg1s.iter().zip(arg2s);
+                    consumed_input_len = double_value.len();
+
+                    for (arg1, arg2) in double_value {
+                        // if input_buf is full, send values to cordic
+                        if state.buf_index == INPUT_BUF_LEN - 1 {
+                            for arg in state.input_buf.chunks(2) {
+                                peri.write_argument(arg[0]);
+                                peri.write_argument(arg[1]);
 
-                    self.cordic.set_result_count(if self.config.first_result {
-                        if output.len() < arg1s.len() {
-                            panic!("Output buf length is not long enough")
+                                output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                                output_count += 1;
+
+                                if !config.first_result {
+                                    output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                                    output_count += 1;
+                                }
+                            }
+
+                            state.buf_index = 0;
                         }
-                        Count::One
-                    } else {
-                        if output.len() < 2 * arg1s.len() {
-                            panic!("Output buf length is not long enough")
+
+                        for &&arg in [arg1, arg2].iter() {
+                            state.input_buf[state.buf_index] = utils::f64_to_q1_31(arg);
+                            state.buf_index += 1;
                         }
-                        Count::Two
-                    });
+                    }
+
+                    // put left paired args into cordic
+                    if state.buf_index > 0 {
+                        for arg in state.input_buf[..state.buf_index].chunks(2) {
+                            peri.write_argument(arg[0]);
+                            peri.write_argument(arg[1]);
 
-                    let mut cnt = 0;
+                            output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                            output_count += 1;
+
+                            if !config.first_result {
+                                output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                                output_count += 1;
+                            }
+                        }
 
-                    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;
+                        state.buf_index = 0;
                     }
+                }
+
+                // put single input into cordic
+                let input_left = &arg1s[consumed_input_len..];
+
+                if !input_left.is_empty() {
+                    peri.set_argument_count(Count::One);
+
+                    for &arg in input_left.iter() {
+                        peri.write_argument(utils::f64_to_q1_31(arg));
 
-                    cnt
-                } else {
-                    todo!()
+                        output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                        output_count += 1;
+
+                        if !config.first_result {
+                            output[output_count] = utils::q1_31_to_f64(peri.read_result());
+                            output_count += 1;
+                        }
+                    }
                 }
+
+                output_count
             }
             Mode::Interrupt => todo!(),
             Mode::Dma => todo!(),
         }
     }
-}
 
-impl<'d, T: Instance> Drop for Cordic<'d, T> {
-    fn drop(&mut self) {
-        T::disable();
+    fn check_input_f64(&self, arg1s: &[f64], arg2s: Option<&[f64]>) {
+        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.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"
+                ),
+
+                _ => (),
+            }
+        }
     }
 }
 
 foreach_interrupt!(
-    ($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => {
+    ($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
         impl Instance for peripherals::$inst {
         }