From 1d6736cff62ccf5894edb862617553f1835b8049 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 4 Jul 2021 15:39:26 +0200
Subject: [PATCH] Add modules::math for specialized math functions

Currently include reduced-precision 8x16 and 16x16 multiplication
functions used by the speed lookup tables.
---
 src/modules/math.h | 78 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 src/modules/math.h

diff --git a/src/modules/math.h b/src/modules/math.h
new file mode 100644
index 0000000..65697fe
--- /dev/null
+++ b/src/modules/math.h
@@ -0,0 +1,78 @@
+#pragma once
+#include "../config/config.h"
+
+namespace modules {
+
+/// Specialized math operations
+namespace math {
+
+/// (intIn1 * intIn2) >> 8
+static inline uint16_t mulU8X16toH16(const uint8_t charIn1, const uint16_t intIn2) {
+    uint16_t intRes;
+#if !defined(__AVR__) || defined(NO_ASM)
+    intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8;
+#else
+    asm volatile(
+        "clr r26        \n\t"
+        "mul %A1, %B2   \n\t"
+        "movw %A0, r0   \n\t"
+        "mul %A1, %A2   \n\t"
+        "add %A0, r1    \n\t"
+        "adc %B0, r26   \n\t"
+        "lsr r0         \n\t"
+        "adc %A0, r26   \n\t"
+        "adc %B0, r26   \n\t"
+        "clr r1         \n\t"
+        : "=&r"(intRes)
+        : "d"(charIn1), "d"(intIn2)
+        : "r26");
+#endif
+    return intRes;
+}
+
+/// (longIn1 * longIn2) >> 24
+static inline uint16_t mulU24X24toH16(const uint32_t &longIn1, const uint32_t &longIn2) {
+    uint16_t intRes;
+#if !defined(__AVR__) || defined(NO_ASM)
+    intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
+#else
+    asm volatile(
+        "clr r26         \n\t"
+        "mul %A1, %B2    \n\t"
+        "mov r27, r1     \n\t"
+        "mul %B1, %C2    \n\t"
+        "movw %A0, r0    \n\t"
+        "mul %C1, %C2    \n\t"
+        "add %B0, r0     \n\t"
+        "mul %C1, %B2    \n\t"
+        "add %A0, r0     \n\t"
+        "adc %B0, r1     \n\t"
+        "mul %A1, %C2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %B1, %B2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %C1, %A2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %B1, %A2    \n\t"
+        "add r27, r1     \n\t"
+        "adc %A0, r26    \n\t"
+        "adc %B0, r26    \n\t"
+        "lsr r27         \n\t"
+        "adc %A0, r26    \n\t"
+        "adc %B0, r26    \n\t"
+        "clr r1          \n\t"
+        : "=&r"(intRes)
+        : "d"(longIn1), "d"(longIn2)
+        : "r26", "r27");
+#endif
+    return intRes;
+}
+
+} // namespace math
+} // namespace modules