From a8f26919a2ecb204d6d5f120709c52b6cd320835 Mon Sep 17 00:00:00 2001
From: Jonathan Teh
Date: Tue, 22 Feb 2022 23:57:22 +0000
Subject: dsp1: Use clz CPU instruction when normalising fixed-point numbers

Use the gcc built-in function, making the operation constant-time and
branch-free.
---
 source/dsp1emu.c | 41 +++++++++++++++++++++++++++++++++++++++--
 1 file changed, 39 insertions(+), 2 deletions(-)

diff --git a/source/dsp1emu.c b/source/dsp1emu.c
index 76df362..d83e078 100644
--- a/source/dsp1emu.c
+++ b/source/dsp1emu.c
@@ -187,11 +187,19 @@ void DSP1_Inverse(int16_t Coefficient, int16_t Exponent, int16_t* iCoefficient,
       }
 
       /* Step Three: Normalize */
+#ifdef __GNUC__
+      {
+          const int shift = __builtin_clz(Coefficient) - (8 * sizeof(int) - 15);
+          Coefficient <<= shift;
+          Exponent -= shift;
+      }
+#else
       while (Coefficient < 0x4000)
       {
          Coefficient <<= 1;
          Exponent--;
       }
+#endif
 
       /* Step Four: Special Case */
       if (Coefficient == 0x4000)
@@ -336,9 +344,18 @@ int16_t DSP1_Cos(int16_t Angle)
 
 void DSP1_Normalize(int16_t m, int16_t* Coefficient, int16_t* Exponent)
 {
-   int16_t i = 0x4000;
    int16_t e = 0;
 
+#ifdef __GNUC__
+   int16_t n = m < 0 ? ~m : m;
+
+   if (n == 0)
+      e = 15;
+   else
+      e = __builtin_clz(n) - (8 * sizeof(int) - 15);
+#else
+   int16_t i = 0x4000;
+
    if (m < 0)
    {
       while ((m & i) && i)
@@ -355,6 +372,7 @@ void DSP1_Normalize(int16_t m, int16_t* Coefficient, int16_t* Exponent)
          e++;
       }
    }
+#endif
 
    if (e > 0)
       *Coefficient = m * DSP1ROM[0x21 + e] << 1;
@@ -368,9 +386,18 @@ void DSP1_NormalizeDouble(int32_t Product, int16_t* Coefficient, int16_t* Expone
 {
    int16_t n = Product & 0x7fff;
    int16_t m = Product >> 15;
-   int16_t i = 0x4000;
    int16_t e = 0;
 
+#ifdef __GNUC__
+   int16_t t = m < 0 ? ~m : m;
+
+   if (t == 0)
+      e = 15;
+   else
+      e = __builtin_clz(t) - (8 * sizeof(int) - 15);
+#else
+   int16_t i = 0x4000;
+
    if (m < 0)
    {
       while ((m & i) && i)
@@ -387,6 +414,7 @@ void DSP1_NormalizeDouble(int32_t Product, int16_t* Coefficient, int16_t* Expone
          e++;
       }
    }
+#endif
 
    if (e > 0)
    {
@@ -396,6 +424,14 @@ void DSP1_NormalizeDouble(int32_t Product, int16_t* Coefficient, int16_t* Expone
          *Coefficient += n * DSP1ROM[0x0040 - e] >> 15;
       else
       {
+#ifdef __GNUC__
+         t = m < 0 ? ~(n | 0x8000) : n;
+
+         if (t == 0)
+            e += 15;
+         else
+            e += __builtin_clz(t) - (8 * sizeof(int) - 15);
+#else
          i = 0x4000;
 
          if (m < 0)
@@ -414,6 +450,7 @@ void DSP1_NormalizeDouble(int32_t Product, int16_t* Coefficient, int16_t* Expone
                e++;
             }
          }
+#endif
 
          if (e > 15)
             *Coefficient = n * DSP1ROM[0x0012 + e] << 1;
-- 
cgit v1.2.3