Merge pull request #4897 from bespsm/MSPM0-MATHACL

MSPSM0: module MATHACL

6 files changed, 301 insertions, 2 deletions

diff --git a/embassy-mspm0/CHANGELOG.md b/embassy-mspm0/CHANGELOG.md
index f0b5868f4..6972a8472 100644
--- a/embassy-mspm0/CHANGELOG.md
+++ b/embassy-mspm0/CHANGELOG.md
@@ -19,3 +19,4 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - feat: Add i2c target implementation (#4605)
 - fix: group irq handlers must check for NO_INTR (#4785)
 - feat: Add read_reset_cause function
+- feat: Add module Mathacl & example for mspm0g3507 (#4897)
\ No newline at end of file
diff --git a/embassy-mspm0/Cargo.toml b/embassy-mspm0/Cargo.toml
index b76bc7e41..254e0209b 100644
--- a/embassy-mspm0/Cargo.toml
+++ b/embassy-mspm0/Cargo.toml
@@ -70,9 +70,10 @@ log = { version = "0.4.14", optional = true }
 cortex-m-rt = ">=0.6.15,<0.8"
 cortex-m = "0.7.6"
 critical-section = "1.2.0"
+micromath = "2.0.0"
 
 # mspm0-metapac = { version = "" }
-mspm0-metapac = { git = "https://github.com/mspm0-rs/mspm0-data-generated/", tag = "mspm0-data-8542f260cc89645a983b7f1a874c87b21822279e" }
+mspm0-metapac = { git = "https://github.com/mspm0-rs/mspm0-data-generated/", tag = "mspm0-data-f21b04e9de074af4965bf67ec3646cb9fe1b9852" }
 
 [build-dependencies]
 proc-macro2 = "1.0.94"
@@ -80,7 +81,7 @@ quote = "1.0.40"
 cfg_aliases = "0.2.1"
 
 # mspm0-metapac = { version = "", default-features = false, features = ["metadata"] }
-mspm0-metapac = { git = "https://github.com/mspm0-rs/mspm0-data-generated/", tag = "mspm0-data-8542f260cc89645a983b7f1a874c87b21822279e", default-features = false, features = ["metadata"] }
+mspm0-metapac = { git = "https://github.com/mspm0-rs/mspm0-data-generated/", tag = "mspm0-data-f21b04e9de074af4965bf67ec3646cb9fe1b9852", default-features = false, features = ["metadata"] }
 
 [features]
 default = ["rt"]
diff --git a/embassy-mspm0/build.rs b/embassy-mspm0/build.rs
index 4942364aa..0fe056c4e 100644
--- a/embassy-mspm0/build.rs
+++ b/embassy-mspm0/build.rs
@@ -591,6 +591,7 @@ fn generate_peripheral_instances() -> TokenStream {
             "i2c" => Some(quote! { impl_i2c_instance!(#peri, #fifo_size); }),
             "wwdt" => Some(quote! { impl_wwdt_instance!(#peri); }),
             "adc" => Some(quote! { impl_adc_instance!(#peri); }),
+            "mathacl" => Some(quote! { impl_mathacl_instance!(#peri); }),
             _ => None,
         };
 
diff --git a/embassy-mspm0/src/lib.rs b/embassy-mspm0/src/lib.rs
index 9f3e4d5e8..c43c81853 100644
--- a/embassy-mspm0/src/lib.rs
+++ b/embassy-mspm0/src/lib.rs
@@ -19,6 +19,8 @@ pub mod dma;
 pub mod gpio;
 pub mod i2c;
 pub mod i2c_target;
+#[cfg(any(mspm0g150x, mspm0g151x, mspm0g350x, mspm0g351x))]
+pub mod mathacl;
 pub mod timer;
 pub mod uart;
 pub mod wwdt;
diff --git a/embassy-mspm0/src/mathacl.rs b/embassy-mspm0/src/mathacl.rs
new file mode 100644
index 000000000..e29f4a59e
--- /dev/null
+++ b/embassy-mspm0/src/mathacl.rs
@@ -0,0 +1,255 @@
+//! MATHACL
+//!
+//! This HAL implements mathematical calculations performed by the CPU.
+
+#![macro_use]
+
+use core::f32::consts::PI;
+use core::marker::PhantomData;
+
+use embassy_hal_internal::PeripheralType;
+use micromath::F32Ext;
+
+use crate::Peri;
+use crate::pac::mathacl::{Mathacl as Regs, vals};
+
+pub enum Precision {
+    High = 31,
+    Medium = 15,
+    Low = 1,
+}
+
+/// Serial error
+#[derive(Debug, Eq, PartialEq, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[non_exhaustive]
+pub enum Error {
+    ValueInWrongRange,
+    NBitsTooBig,
+}
+
+pub struct Mathacl<'d> {
+    regs: &'static Regs,
+    _phantom: PhantomData<&'d mut ()>,
+}
+
+impl<'d> Mathacl<'d> {
+    /// Mathacl initialization.
+    pub fn new<T: Instance>(_instance: Peri<'d, T>) -> Self {
+        // Init power
+        T::regs().gprcm(0).rstctl().write(|w| {
+            w.set_resetstkyclr(vals::Resetstkyclr::CLR);
+            w.set_resetassert(vals::Resetassert::ASSERT);
+            w.set_key(vals::ResetKey::KEY);
+        });
+
+        // Enable power
+        T::regs().gprcm(0).pwren().write(|w| {
+            w.set_enable(true);
+            w.set_key(vals::PwrenKey::KEY);
+        });
+
+        // init delay, 16 cycles
+        cortex_m::asm::delay(16);
+
+        Self {
+            regs: T::regs(),
+            _phantom: PhantomData,
+        }
+    }
+
+    /// Internal helper SINCOS function.
+    fn sincos(&mut self, rad: f32, precision: Precision, sin: bool) -> Result<f32, Error> {
+        self.regs.ctl().write(|w| {
+            w.set_func(vals::Func::SINCOS);
+            w.set_numiter(precision as u8);
+        });
+
+        if rad > PI || rad < -PI {
+            return Err(Error::ValueInWrongRange);
+        }
+
+        // TODO: make f32 division on CPU
+        let native = rad / PI;
+
+        match signed_f32_to_register(native, 0) {
+            Ok(val) => self.regs.op1().write(|w| {
+                w.set_data(val);
+            }),
+            Err(er) => return Err(er),
+        };
+
+        // check if done
+        while self.regs.status().read().busy() == vals::Busy::NOTDONE {}
+
+        match sin {
+            true => register_to_signed_f32(self.regs.res2().read().data(), 0),
+            false => register_to_signed_f32(self.regs.res1().read().data(), 0),
+        }
+    }
+
+    /// Calsulates trigonometric sine operation in the range [-1,1) with a give precision.
+    pub fn sin(&mut self, rad: f32, precision: Precision) -> Result<f32, Error> {
+        self.sincos(rad, precision, true)
+    }
+
+    /// Calsulates trigonometric cosine operation in the range [-1,1) with a give precision.
+    pub fn cos(&mut self, rad: f32, precision: Precision) -> Result<f32, Error> {
+        self.sincos(rad, precision, false)
+    }
+}
+
+pub(crate) trait SealedInstance {
+    fn regs() -> &'static Regs;
+}
+
+/// Mathacl instance trait
+#[allow(private_bounds)]
+pub trait Instance: SealedInstance + PeripheralType {}
+
+macro_rules! impl_mathacl_instance {
+    ($instance: ident) => {
+        impl crate::mathacl::SealedInstance for crate::peripherals::$instance {
+            fn regs() -> &'static crate::pac::mathacl::Mathacl {
+                &crate::pac::$instance
+            }
+        }
+
+        impl crate::mathacl::Instance for crate::peripherals::$instance {}
+    };
+}
+
+/// Convert f32 data to understandable by M0 format.
+fn signed_f32_to_register(data: f32, n_bits: u8) -> Result<u32, Error> {
+    let mut res: u32 = 0;
+    // check if negative
+    let negative = data < 0.0;
+
+    // absolute value for extraction
+    let abs = data.abs();
+
+    // total integer bit count
+    let total_bits = 31;
+
+    // Validate n_bits
+    if n_bits > 31 {
+        return Err(Error::NBitsTooBig);
+    }
+
+    // number of fractional bits
+    let shift = total_bits - n_bits;
+
+    // Compute masks
+    let (n_mask, m_mask) = if n_bits == 0 {
+        (0, 0x7FFFFFFF)
+    } else if n_bits == 31 {
+        (0x7FFFFFFF, 0)
+    } else {
+        ((1u32 << n_bits) - 1, (1u32 << shift) - 1)
+    };
+
+    // calc. integer(n) & fractional(m) parts
+    let n = abs.floor() as u32;
+    let mut m = ((abs - abs.floor()) * (1u32 << shift) as f32).round() as u32;
+
+    // Handle trimming integer part
+    if n_bits == 0 && n > 0 {
+        m = 0x7FFFFFFF;
+    }
+
+    // calculate result
+    if n_bits > 0 {
+        res = n << shift & n_mask;
+    }
+    if shift > 0 {
+        res = res | m & m_mask;
+    }
+
+    // if negative, do 2’s compliment
+    if negative {
+        res = !res + 1;
+    }
+    Ok(res)
+}
+
+/// Reversely converts M0-register format to native f32.
+fn register_to_signed_f32(data: u32, n_bits: u8) -> Result<f32, Error> {
+    // Validate n_bits
+    if n_bits > 31 {
+        return Err(Error::NBitsTooBig);
+    }
+
+    // total integer bit count
+    let total_bits = 31;
+
+    let negative = (data >> 31) == 1;
+
+    // number of fractional bits
+    let shift = total_bits - n_bits;
+
+    // Compute masks
+    let (n_mask, m_mask) = if n_bits == 0 {
+        (0, 0x7FFFFFFF)
+    } else if n_bits == 31 {
+        (0x7FFFFFFF, 0)
+    } else {
+        ((1u32 << n_bits) - 1, (1u32 << shift) - 1)
+    };
+
+    // Compute n and m
+    let mut n = if n_bits == 0 {
+        0
+    } else if shift >= 32 {
+        data & n_mask
+    } else {
+        (data >> shift) & n_mask
+    };
+    let mut m = data & m_mask;
+
+    // if negative, do 2’s compliment
+    if negative {
+        n = !n & n_mask;
+        m = (!m & m_mask) + 1;
+    }
+
+    let mut value = (n as f32) + (m as f32) / (1u32 << shift) as f32;
+    if negative {
+        value = -value;
+    }
+    return Ok(value);
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn mathacl_convert_func_errors() {
+        assert_eq!(signed_f32_to_register(0.0, 32), Err(Error::NBitsTooBig));
+        assert_eq!(register_to_signed_f32(0, 32), Err(Error::NBitsTooBig));
+    }
+
+    #[test]
+    fn mathacl_signed_f32_to_register() {
+        let mut test_float = 1.0;
+        assert_eq!(signed_f32_to_register(test_float, 0).unwrap(), 0x7FFFFFFF);
+
+        test_float = 0.0;
+        assert_eq!(signed_f32_to_register(test_float, 0).unwrap(), 0x0);
+
+        test_float = -1.0;
+        assert_eq!(signed_f32_to_register(test_float, 0).unwrap(), 0x80000001);
+    }
+
+    #[test]
+    fn mathacl_register_to_signed_f32() {
+        let mut test_u32: u32 = 0x7FFFFFFF;
+        assert_eq!(register_to_signed_f32(test_u32, 0u8).unwrap(), 1.0);
+
+        test_u32 = 0x0;
+        assert_eq!(register_to_signed_f32(test_u32, 0u8).unwrap(), 0.0);
+
+        test_u32 = 0x80000001;
+        assert_eq!(register_to_signed_f32(test_u32, 0u8).unwrap(), -1.0);
+    }
+}
diff --git a/examples/mspm0g3507/src/bin/mathacl_ops.rs b/examples/mspm0g3507/src/bin/mathacl_ops.rs
new file mode 100644
index 000000000..25d74b29b
--- /dev/null
+++ b/examples/mspm0g3507/src/bin/mathacl_ops.rs
@@ -0,0 +1,39 @@
+//! Example of using mathematical calculations performed by the MSPM0G3507 chip.
+//!
+//! It prints the result of basics trigonometric calculation.
+
+#![no_std]
+#![no_main]
+
+use core::f32::consts::PI;
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_mspm0::mathacl::{Mathacl, Precision};
+use embassy_time::Timer;
+use {defmt_rtt as _, panic_halt as _};
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) -> ! {
+    info!("Hello world!");
+
+    let d = embassy_mspm0::init(Default::default());
+
+    let mut macl = Mathacl::new(d.MATHACL);
+
+    // value radians [-PI; PI]
+    let rads = PI * 0.5;
+    match macl.sin(rads, Precision::High) {
+        Ok(res) => info!("sin({}) = {}", rads, res),
+        Err(e) => error!("sin Error: {:?}", e),
+    }
+
+    match macl.cos(rads, Precision::Medium) {
+        Ok(res) => info!("cos({}) = {}", rads, res),
+        Err(e) => error!("cos Error: {:?}", e),
+    }
+
+    loop {
+        Timer::after_millis(500).await;
+    }
+}