From 104769cec02772d23d8248ea97e421604d5077f1 Mon Sep 17 00:00:00 2001
From: jSTE0 <98854293+jSTE0@users.noreply.github.com>
Date: Fri, 4 Feb 2022 22:55:06 +0000
Subject: [PATCH] dsp1: Use clz CPU instruction when normalising fixed-point
 numbers

Use the gcc built-in GCC function, making the operation constant-time
and branch-free. Verified on x86 and ARM.
---
 dsp1.cpp | 39 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 37 insertions(+), 2 deletions(-)

diff --git a/dsp1.cpp b/dsp1.cpp
index 66c038d0..af089d92 100644
--- a/dsp1.cpp
+++ b/dsp1.cpp
@@ -316,11 +316,17 @@ static void DSP1_Inverse (int16 Coefficient, int16 Exponent, int16 *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)
@@ -398,9 +404,18 @@ static int16 DSP1_Cos (int16 Angle)
 
 static void DSP1_Normalize (int16 m, int16 *Coefficient, int16 *Exponent)
 {
-	int16	i = 0x4000;
 	int16	e = 0;
 
+#ifdef __GNUC__
+	int16	n = m < 0 ? ~m : m;
+
+	if (n == 0)
+		e = 15;
+	else
+		e = __builtin_clz(n) - (8 * sizeof(int) - 15);
+#else
+	int16	i = 0x4000;
+
 	if (m < 0)
 	{
 		while ((m & i) && i)
@@ -417,6 +432,7 @@ static void DSP1_Normalize (int16 m, int16 *Coefficient, int16 *Exponent)
 			e++;
 		}
 	}
+#endif
 
 	if (e > 0)
 		*Coefficient = m * DSP1ROM[0x21 + e] << 1;
@@ -430,9 +446,18 @@ static void DSP1_NormalizeDouble (int32 Product, int16 *Coefficient, int16 *Expo
 {
 	int16	n = Product & 0x7fff;
 	int16	m = Product >> 15;
-	int16	i = 0x4000;
 	int16	e = 0;
 
+#ifdef __GNUC__
+	int16	t = m < 0 ? ~m : m;
+
+	if (t == 0)
+		e = 15;
+	else
+		e = __builtin_clz(t) - (8 * sizeof(int) - 15);
+#else
+	int16	i = 0x4000;
+
 	if (m < 0)
 	{
 		while ((m & i) && i)
@@ -449,6 +474,7 @@ static void DSP1_NormalizeDouble (int32 Product, int16 *Coefficient, int16 *Expo
 			e++;
 		}
 	}
+#endif
 
 	if (e > 0)
 	{
@@ -458,6 +484,14 @@ static void DSP1_NormalizeDouble (int32 Product, int16 *Coefficient, int16 *Expo
 			*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)
@@ -476,6 +510,7 @@ static void DSP1_NormalizeDouble (int32 Product, int16 *Coefficient, int16 *Expo
 					e++;
 				}
 			}
+#endif
 
 			if (e > 15)
 				*Coefficient = n * DSP1ROM[0x0012 + e] << 1;