3 files changed, 147 insertions, 5 deletions

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.
@@ -92,6 +94,11 @@ 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"
 required-features = [ "cryp",]
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<peripherals::RNG>;
+});
+
+/* 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();
+}