Replace fp_sqrt function with one that only uses shift, or and sub.

Simply extends the current isqrt() to be able to do fractional bits
and improves the initial estimate using clz(). iqrt() itself is
no more and is equivalent to fp_sqrt(x, 0). The original also had
a small bug where the guess comparision should have been >=, not >.

Uses no large integer math or division and is very accurate
(simply returns a truncated fraction).

Change-Id: I2ae26e6505df1770dc01e56220f7385369f90ae9

3 files changed, 55 insertions, 55 deletions

diff --git a/apps/plugins/fft/fft.c b/apps/plugins/fft/fft.c
index 40c251de15..3e88722b23 100644
--- a/apps/plugins/fft/fft.c
+++ b/apps/plugins/fft/fft.c
@@ -624,16 +624,16 @@ static unsigned calc_magnitudes(enum fft_amp_scale scale)
                 }
                 else
                 {
-                    d = isqrt(d); /* linear scaling, nothing
-                                     bad should happen */
+                    d = fp_sqrt(d, 0); /* linear scaling, nothing
+                                          bad should happen */
                     d = fp16_log(d << 16); /* the log function
                                               expects s15.16 values */
                 }
             }
             else
             {
-                d = isqrt(d); /* linear scaling, nothing
-                                 bad should happen */
+                d = fp_sqrt(d, 0); /* linear scaling, nothing
+                                      bad should happen */
             }
         }
 
diff --git a/lib/fixedpoint/fixedpoint.c b/lib/fixedpoint/fixedpoint.c
index b5bbe68a95..4530857df0 100644
--- a/lib/fixedpoint/fixedpoint.c
+++ b/lib/fixedpoint/fixedpoint.c
@@ -25,9 +25,7 @@
 #include <stdbool.h>
 #include <inttypes.h>
 
-#ifndef BIT_N
-#define BIT_N(n) (1U << (n))
-#endif
+#define ULONG_BITS (sizeof (unsigned long)*CHAR_BIT)
 
 /** TAKEN FROM ORIGINAL fixedpoint.h */
 /* Inverse gain of circular cordic rotation in s0.31 format. */
@@ -142,65 +140,72 @@ long fp_sincos(unsigned long phase, long *cos)
     return y;
 }
 
-/**
- * Fixed point square root via Newton-Raphson.
- * @param x square root argument.
- * @param fracbits specifies number of fractional bits in argument.
- * @return Square root of argument in same fixed point format as input.
+/* Accurate sqrt with only elementary operations.
+ * Snagged from:
+ *   http://www.devmaster.net/articles/fixed-point-optimizations/
  *
- * This routine has been modified to run longer for greater precision,
- * but cuts calculation short if the answer is reached sooner. 
+ * Extension to fractions and initial estimate improvement by jethead71
  */
 long fp_sqrt(long x, unsigned int fracbits)
 {
-    unsigned long xfp, b;
-    int n = 8; /* iteration limit (should terminate earlier) */
-
-    if (x <= 0)
+    if (x <= 0) {
         return 0; /* no sqrt(neg), or just sqrt(0) = 0 */
+    }
 
-    /* Increase working precision by one bit */
-    xfp = x << 1;
-    fracbits++;
+    unsigned long g = 0;
+    unsigned long e = x;
 
-    /* Get the midpoint between fracbits index and the highest bit index */
-    b = ((sizeof(xfp)*8-1) - __builtin_clzl(xfp) + fracbits) >> 1;
-    b = BIT_N(b);
+    int intwidth = ULONG_BITS - fracbits;
+    int bshift = __builtin_clzl(e);
 
-    do
-    {
-        unsigned long c = b;
-        b = (fp_div(xfp, b, fracbits) + b) >> 1;
-        if (c == b) break;
+    if (bshift >= intwidth) {
+        bshift = -1;
+    }
+    else {
+        bshift = (intwidth - bshift - 1) >> 1;
     }
-    while (n-- > 0);
 
-    return b >> 1;
-}
+    unsigned long b = 1ul << (bshift + fracbits);
 
-/* Accurate int sqrt with only elementary operations.
- * Snagged from:
- *   http://www.devmaster.net/articles/fixed-point-optimizations/ */
-unsigned long isqrt(unsigned long x)
-{
-    /* Adding CLZ could optimize this further */
-    unsigned long g = 0;
-    int bshift = 15;
-    unsigned long b = 1ul << bshift;
-    
-    do
-    {
-        unsigned long temp = (g + g + b) << bshift;
+    /* integer part */
+    while (e && bshift >= 0) {
+        unsigned long t = ((g << 1) | b) << bshift--;
 
-        if (x > temp)
-        {
-            g += b;
-            x -= temp;
+        if (e >= t) {
+            g |= b;
+            e -= t;
         }
 
         b >>= 1;
     }
-    while (bshift--);
+
+    /* fractional part */
+    while (e && b) {
+        unsigned long t = (g << 1) | b;
+        unsigned long c = e; /* detect carry */
+
+        e <<= 1;
+
+        if (e < c || e >= t) {
+            g |= b;
+            e -= t;
+        }
+
+        b >>= 1;
+    }
+
+#if 0
+    /* round up if the next bit would be a '1' */
+    if (e) {
+        unsigned long c = e; /* detect carry */
+
+        e <<= 1;
+
+        if (e < c || e >= ((g << 1) | 1)) {
+            g++;
+        }
+    }
+#endif
 
     return g;
 }
diff --git a/lib/fixedpoint/fixedpoint.h b/lib/fixedpoint/fixedpoint.h
index 31d60eca4b..bc50ff687d 100644
--- a/lib/fixedpoint/fixedpoint.h
+++ b/lib/fixedpoint/fixedpoint.h
@@ -45,9 +45,6 @@
  *  Take square root of a fixed point number:
  *      fp_sqrt(x, fracbits)
  *
- *  Take the square root of an integer:
- *      isqrt(x)
- *
  *  Calculate sin or cos of an angle (very fast, from a table):
  *      fp14_sin(angle)
  *      fp14_cos(angle)
@@ -88,8 +85,6 @@ long fp14_sin(int val);
 long fp16_log(int x);
 long fp16_exp(int x);
 
-unsigned long isqrt(unsigned long x);
-
 /* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit,
  * whichever is faster for the architecture) */
 #ifdef CPU_ARM