embassy-mspm0: add CPU accelerated float division function

embassy-mspm0/CHANGELOG.md

@@ -19,4 +19,5 @@ 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)
+- feat: Add module Mathacl & example for mspm0g3507 (#4897)
+- feat: add CPU accelerated float division function (#4966)

embassy-mspm0/src/mathacl.rs

@@ -13,6 +13,8 @@ use micromath::F32Ext;
 use crate::Peri;
 use crate::pac::mathacl::{Mathacl as Regs, vals};
 
+const ERROR_TOLERANCE: f32 = 0.00001;
+
 pub enum Precision {
     High = 31,
     Medium = 15,
@@ -25,7 +27,7 @@ pub enum Precision {
 #[non_exhaustive]
 pub enum Error {
     ValueInWrongRange,
-    NBitsTooBig,
+    DivideByZero,
 }
 
 pub struct Mathacl<'d> {
@@ -60,19 +62,20 @@ impl<'d> Mathacl<'d> {
 
     /// 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;
+        // make division using mathacl
+        let native = self.div(rad, PI)?;
+
+        self.regs.ctl().write(|w| {
+            w.set_func(vals::Func::SINCOS);
+            w.set_numiter(precision as u8);
+        });
 
-        match signed_f32_to_register(native, 0) {
+        // fractional part has to be 31 bits
+        match signed_f32_to_register(native, vals::Qval::Q31) {
             Ok(val) => self.regs.op1().write(|w| {
                 w.set_data(val);
             }),
@@ -83,8 +86,8 @@ impl<'d> Mathacl<'d> {
         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),
+            true => register_to_signed_f32(self.regs.res2().read().data(), vals::Qval::Q31),
+            false => register_to_signed_f32(self.regs.res1().read().data(), vals::Qval::Q31),
         }
     }
 
@@ -97,6 +100,45 @@ impl<'d> Mathacl<'d> {
     pub fn cos(&mut self, rad: f32, precision: Precision) -> Result<f32, Error> {
         self.sincos(rad, precision, false)
     }
+
+    /// Divide function (DIV) computes with a known dividend and divisor.
+    pub fn div(&mut self, dividend: f32, divisor: f32) -> Result<f32, Error> {
+        if -ERROR_TOLERANCE < divisor && divisor < ERROR_TOLERANCE {
+            return Err(Error::DivideByZero);
+        }
+
+        // assume all input data is signed &
+        // has 16 bits of fractional part
+        let optype = true;
+        let q_val = vals::Qval::Q16;
+
+        self.regs.ctl().write(|w| {
+            w.set_func(vals::Func::DIV);
+            w.set_optype(optype);
+            w.set_qval(q_val);
+        });
+
+        match signed_f32_to_register(divisor, q_val) {
+            Ok(val) => self.regs.op2().write(|w| {
+                w.set_data(val);
+            }),
+            Err(er) => return Err(er),
+        };
+
+        match signed_f32_to_register(dividend, q_val) {
+            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 {}
+
+        // read quotient
+        let res1 = self.regs.res1().read().data();
+        register_to_signed_f32(res1, q_val)
+    }
 }
 
 pub(crate) trait SealedInstance {
@@ -120,7 +162,7 @@ macro_rules! impl_mathacl_instance {
 }
 
 /// Convert f32 data to understandable by M0 format.
-fn signed_f32_to_register(data: f32, n_bits: u8) -> Result<u32, Error> {
+fn signed_f32_to_register(data: f32, m_bits: vals::Qval) -> Result<u32, Error> {
     let mut res: u32 = 0;
     // check if negative
     let negative = data < 0.0;
@@ -131,26 +173,21 @@ fn signed_f32_to_register(data: f32, n_bits: u8) -> Result<u32, Error> {
     // 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;
+    let n_bits = total_bits - m_bits as u8;
 
     // 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)
+        ((1u32 << n_bits) - 1, (1u32 << m_bits as u8) - 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;
+    let abs_floor = abs.floor();
+    let n = abs_floor as u32;
+    let mut m = ((abs - abs_floor) * (1u32 << m_bits as u8) as f32).round() as u32;
 
     // Handle trimming integer part
     if n_bits == 0 && n > 0 {
@@ -159,10 +196,10 @@ fn signed_f32_to_register(data: f32, n_bits: u8) -> Result<u32, Error> {
 
     // calculate result
     if n_bits > 0 {
-        res = n << shift & n_mask;
+        res = n << m_bits as u8 & (n_mask << m_bits as u8);
     }
-    if shift > 0 {
-        res = res | m & m_mask;
+    if m_bits as u8 > 0 {
+        res = (m & m_mask) | res;
     }
 
     // if negative, do 2’s compliment
@@ -173,36 +210,31 @@ fn signed_f32_to_register(data: f32, n_bits: u8) -> Result<u32, Error> {
 }
 
 /// 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);
-    }
-
+fn register_to_signed_f32(data: u32, m_bits: vals::Qval) -> Result<f32, Error> {
     // total integer bit count
     let total_bits = 31;
 
     let negative = (data >> 31) == 1;
 
     // number of fractional bits
-    let shift = total_bits - n_bits;
+    let n_bits = total_bits - m_bits as u8;
 
     // 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)
+        ((1u32 << n_bits) - 1, (1u32 << m_bits as u8) - 1)
     };
 
     // Compute n and m
     let mut n = if n_bits == 0 {
         0
-    } else if shift >= 32 {
+    } else if m_bits as u8 >= 32 {
         data & n_mask
     } else {
-        (data >> shift) & n_mask
+        (data >> m_bits as u8) & n_mask
     };
     let mut m = data & m_mask;
 
@@ -212,7 +244,7 @@ fn register_to_signed_f32(data: u32, n_bits: u8) -> Result<f32, Error> {
         m = (!m & m_mask) + 1;
     }
 
-    let mut value = (n as f32) + (m as f32) / (1u32 << shift) as f32;
+    let mut value = (n as f32) + (m as f32) / (1u32 << m_bits as u8) as f32;
     if negative {
         value = -value;
     }
@@ -223,33 +255,41 @@ fn register_to_signed_f32(data: u32, n_bits: u8) -> Result<f32, Error> {
 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);
+        assert_eq!(signed_f32_to_register(test_float, vals::Qval::Q31).unwrap(), 0x7FFFFFFF);
 
         test_float = 0.0;
-        assert_eq!(signed_f32_to_register(test_float, 0).unwrap(), 0x0);
+        assert_eq!(signed_f32_to_register(test_float, vals::Qval::Q31).unwrap(), 0x0);
 
         test_float = -1.0;
-        assert_eq!(signed_f32_to_register(test_float, 0).unwrap(), 0x80000001);
+        assert_eq!(signed_f32_to_register(test_float, vals::Qval::Q31).unwrap(), 0x80000001);
+
+        test_float = 1.666657;
+        assert_eq!(signed_f32_to_register(test_float, vals::Qval::Q16).unwrap(), 0x0001AAAA);
+
+        test_float = -1.666657;
+        assert_eq!(signed_f32_to_register(test_float, vals::Qval::Q16).unwrap(), 0xFFFE5556);
     }
 
     #[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);
+
+        let mut result = register_to_signed_f32(test_u32, vals::Qval::Q31).unwrap();
+        assert!(result < 1.0 + ERROR_TOLERANCE && result > 1.0 - ERROR_TOLERANCE);
 
         test_u32 = 0x0;
-        assert_eq!(register_to_signed_f32(test_u32, 0u8).unwrap(), 0.0);
+        result = register_to_signed_f32(test_u32, vals::Qval::Q31).unwrap();
+        assert!(result < 0.0 + ERROR_TOLERANCE && result > 0.0 - ERROR_TOLERANCE);
+
+        test_u32 = 0x0001AAAA;
+        result = register_to_signed_f32(test_u32, vals::Qval::Q16).unwrap();
+        assert!(result < 1.666657 + ERROR_TOLERANCE && result > 1.666657 - ERROR_TOLERANCE);
 
-        test_u32 = 0x80000001;
-        assert_eq!(register_to_signed_f32(test_u32, 0u8).unwrap(), -1.0);
+        test_u32 = 0xFFFE5556;
+        result = register_to_signed_f32(test_u32, vals::Qval::Q16).unwrap();
+        assert!(result < -1.666657 + ERROR_TOLERANCE && result > -1.666657 - ERROR_TOLERANCE);
     }
 }

examples/mspm0g3507/src/bin/mathacl_ops.rs

@@ -1,6 +1,6 @@
 //! Example of using mathematical calculations performed by the MSPM0G3507 chip.
 //!
-//! It prints the result of basics trigonometric calculation.
+//! It prints the result of basics mathematical calculation.
 
 #![no_std]
 #![no_main]
@@ -22,17 +22,24 @@ async fn main(_spawner: Spawner) -> ! {
     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),
+    let mut op1 = PI * 0.5;
+    match macl.sin(op1, Precision::High) {
+        Ok(res) => info!("sin({}) = {}", op1, res),
         Err(e) => error!("sin Error: {:?}", e),
     }
 
-    match macl.cos(rads, Precision::Medium) {
-        Ok(res) => info!("cos({}) = {}", rads, res),
+    match macl.cos(op1, Precision::Medium) {
+        Ok(res) => info!("cos({}) = {}", op1, res),
         Err(e) => error!("cos Error: {:?}", e),
     }
 
+    op1 = 1.0;
+    let op2 = 3.0;
+    match macl.div(op1, op2) {
+        Ok(res) => info!("{}/{} = {}", op1, op2, res),
+        Err(e) => error!("div Error: {:?}", e),
+    }
+
     loop {
         Timer::after_millis(500).await;
     }