CMSIS-DSP: Reworked sqrt q15 and q31

Include/arm_common_tables.h

@@ -498,10 +498,20 @@ extern "C"
     extern const q15_t sinTable_q15[FAST_MATH_TABLE_SIZE + 1];
   #endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
 
+  /* Fast vector sqrt */
   #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
      #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_FAST_SQRT_Q31_MVE)
        extern const q31_t sqrtTable_Q31[256];
      #endif /* !defined(ARM_DSP_CONFIG_TABLES) defined(ARM_ALL_FAST_TABLES) */
+  #endif 
+
+  /* Accurate scalar sqrt */
+  #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q31) 
+       extern const q31_t sqrt_initial_lut_q31[32];
+  #endif
+
+  #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q15) 
+       extern const q15_t sqrt_initial_lut_q15[16];
   #endif
 
   #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)

Source/CommonTables/arm_common_tables.c

@@ -70507,8 +70507,36 @@ const q15_t sqrtTable_Q15[256] = {
      #endif
 #endif /* defined(ARM_MATH_MVEI) */
 
+#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q31)
+/*
+ClearAll[tofix];
+tofix[q_][a_] := With[{r = Round[a*2^q]},
+   If[r > (2^q - 1), 2^q - 1, r]
+   ];
+
+(* For q = format, 2^nb is length of the table *)
+With[{q = 15, nb = 4, q12quarter = 16^^2000},
+  With[{shift = Echo[q - nb]},
+   Table[tofix[q][1.0/Sqrt[1.0*i/2^q]/8.0], {i, 2^(q - 2), 
+     2^q + q12quarter - 1, 2^shift}]]
+  ] // CopyToClipboard
+
+*/
+const q31_t sqrt_initial_lut_q31[32]={536870912, 506166750, 480191942, 457845052, 438353264, 421156193, \
+405836263, 392075079, 379625062, 368290407, 357913941, 348367849, \
+339546978, 331363921, 323745341, 316629190, 309962566, 303700050, \
+297802400, 292235509, 286969573, 281978417, 277238947, 272730696, \
+268435456, 264336964, 260420644, 256673389, 253083375, 249639903, \
+246333269, 243154642};
+#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q31) */
+
+#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q15)
+const q15_t sqrt_initial_lut_q15[16]={8192, 7327, 6689, 6193, 5793, 5461, 5181, 4940, 4730, 4544, 4379, \
+4230, 4096, 3974, 3862, 3759};
+#endif /* !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q15) */
+
 
-#endif /* if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FAST_TABLES) */
+#endif /* #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FAST_ALLOW_TABLES) */
 
 #if (defined(ARM_MATH_MVEF) || defined(ARM_MATH_HELIUM)) && !defined(ARM_MATH_AUTOVECTORIZE)
 const float32_t exp_tab[8] = {

Source/FastMathFunctions/CMakeLists.txt

@@ -41,8 +41,14 @@ if (NOT CONFIGTABLE OR ALLFAST OR ARM_SIN_Q31)
 target_sources(CMSISDSPFastMath PRIVATE arm_sin_q31.c)
 endif()
 
-target_sources(CMSISDSPFastMath PRIVATE arm_sqrt_q15.c)
+if (NOT CONFIGTABLE OR ALLFAST OR ARM_SQRT_Q31)
 target_sources(CMSISDSPFastMath PRIVATE arm_sqrt_q31.c)
+endif()
+
+if (NOT CONFIGTABLE OR ALLFAST OR ARM_SQRT_Q15)
+target_sources(CMSISDSPFastMath PRIVATE arm_sqrt_q15.c)
+endif()
+
 target_sources(CMSISDSPFastMath PRIVATE arm_vlog_f32.c)
 target_sources(CMSISDSPFastMath PRIVATE arm_vlog_f64.c)
 target_sources(CMSISDSPFastMath PRIVATE arm_vexp_f32.c)

Source/FastMathFunctions/FastMathFunctions.c

@@ -52,10 +52,16 @@
 #include "arm_sin_q31.c"
 #endif 
 
-#endif
+#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q31)
+#include "arm_sqrt_q31.c"
+#endif 
 
+#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_SQRT_Q15)
 #include "arm_sqrt_q15.c"
-#include "arm_sqrt_q31.c"
+#endif 
+
+#endif
+
 #include "arm_vexp_f32.c"
 #include "arm_vexp_f64.c"
 #include "arm_vlog_f32.c"

Source/FastMathFunctions/arm_divide_q31.c

@@ -57,7 +57,6 @@
   to the saturated negative or positive value.
  */
 
-#include <stdio.h>
 arm_status arm_divide_q31(q31_t numerator,
   q31_t denominator,
   q31_t *quotient,

Source/FastMathFunctions/arm_sqrt_q15.c

@@ -47,18 +47,12 @@
                    - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
  */
 
+#define Q12QUARTER 0x2000
 arm_status arm_sqrt_q15(
   q15_t in,
   q15_t * pOut)
 {
-  q31_t bits_val1;
+  q15_t number, var1, signBits1,temp;
-  q15_t number, temp1, var1, signBits1, half;
-  float32_t temp_float1;
-  union
-  {
-    q31_t fracval;
-    float32_t floatval;
-  } tempconv;
 
   number = in;
 
@@ -76,46 +70,30 @@ arm_status arm_sqrt_q15(
     {
       number = number << (signBits1 - 1);
     }
+    /* Start value for 1/sqrt(x) for the Newton iteration */
+    var1 = sqrt_initial_lut_q15[(number>> 11) - (Q12QUARTER >> 11)];
 
-    /* Calculate half value of the number */
+    /* 0.5 var1 * (3 - number * var1 * var1) */
-    half = number >> 1;
-    /* Store the number for later use */
-    temp1 = number;
-
-    /* Convert to float */
-    temp_float1 = number * 3.051757812500000e-005f;
-    /* Store as integer */
-    tempconv.floatval = temp_float1;
-    bits_val1 = tempconv.fracval;
-    /* Subtract the shifted value from the magic number to give intial guess */
-    bits_val1 = 0x5f3759df - (bits_val1 >> 1);  /* gives initial guess */
-    /* Store as float */
-    tempconv.fracval = bits_val1;
-    temp_float1 = tempconv.floatval;
-    /* Convert to integer format */
-    var1 = (q31_t) (temp_float1 * 16384);
-
     /* 1st iteration */
-    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+
-                                     ((q15_t)
+   temp = ((q31_t) var1 * var1) >> 12;
-                                      ((((q15_t)
+   temp = ((q31_t) number * temp) >> 15;
-                                         (((q31_t) var1 * var1) >> 15)) *
+   temp = 0x3000 - temp; 
-                                        (q31_t) half) >> 15))) >> 15)) << 2;
+   var1 = ((q31_t) var1 * temp) >> 13;
-    /* 2nd iteration */
+
-    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+   temp = ((q31_t) var1 * var1) >> 12;
-                                     ((q15_t)
+   temp = ((q31_t) number * temp) >> 15;
-                                      ((((q15_t)
+   temp = 0x3000 - temp; 
-                                         (((q31_t) var1 * var1) >> 15)) *
+   var1 = ((q31_t) var1 * temp) >> 13;
-                                        (q31_t) half) >> 15))) >> 15)) << 2;
+
-    /* 3rd iteration */
+   temp = ((q31_t) var1 * var1) >> 12;
-    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+   temp = ((q31_t) number * temp) >> 15;
-                                     ((q15_t)
+   temp = 0x3000 - temp; 
-                                      ((((q15_t)
+   var1 = ((q31_t) var1 * temp) >> 13;
-                                         (((q31_t) var1 * var1) >> 15)) *
-                                        (q31_t) half) >> 15))) >> 15)) << 2;
 
     /* Multiply the inverse square root with the original value */
-    var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
+
+    var1 = ((q15_t) (((q31_t) number * var1) >> 12));
 
     /* Shift the output down accordingly */
     if ((signBits1 % 2) == 0)
@@ -128,6 +106,7 @@ arm_status arm_sqrt_q15(
     }
     *pOut = var1;
 
+
     return (ARM_MATH_SUCCESS);
   }
   /* If the number is a negative number then store zero as its square root value */

Source/FastMathFunctions/arm_sqrt_q31.c

@@ -46,20 +46,14 @@
                    - \ref ARM_MATH_SUCCESS        : input value is positive
                    - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0
  */
+#define Q28QUARTER 0x20000000 
 
 arm_status arm_sqrt_q31(
   q31_t in,
   q31_t * pOut)
 {
-  q31_t bits_val1;
+  q31_t number, var1, signBits1 ,temp;
-  q31_t number, temp1, var1, signBits1, half;
+ 
-  float32_t temp_float1;
-  union
-  {
-    q31_t fracval;
-    float32_t floatval;
-  } tempconv;
-
   number = in;
 
   /* If the input is a positive number then compute the signBits. */
@@ -77,45 +71,33 @@ arm_status arm_sqrt_q31(
       number = number << (signBits1 - 1);
     }
 
-    /* Calculate half value of the number */
+    /* Start value for 1/sqrt(x) for the Newton iteration */
-    half = number >> 1;
+    var1 = sqrt_initial_lut_q31[(number>> 26) - (Q28QUARTER >> 26)];
-    /* Store the number for later use */
+
-    temp1 = number;
+    /* 0.5 var1 * (3 - number * var1 * var1) */
-
-    /* Convert to float */
-    temp_float1 = number * 4.6566128731e-010f;
-    /* Store as integer */
-    tempconv.floatval = temp_float1;
-    bits_val1 = tempconv.fracval;
-    /* Subtract the shifted value from the magic number to give intial guess */
-    bits_val1 = 0x5f3759df - (bits_val1 >> 1);  /* gives initial guess */
-    /* Store as float */
-    tempconv.fracval = bits_val1;
-    temp_float1 = tempconv.floatval;
-    /* Convert to integer format */
-    var1 = (q31_t) (temp_float1 * 1073741824);
 
     /* 1st iteration */
-    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+
-                                     ((q31_t)
+    temp = ((q63_t) var1 * var1) >> 28;
-                                      ((((q31_t)
+    temp = ((q63_t) number * temp) >> 31;
-                                         (((q63_t) var1 * var1) >> 31)) *
+    temp = 0x30000000 - temp; 
-                                        (q63_t) half) >> 31))) >> 31)) << 2;
+    var1 = ((q63_t) var1 * temp) >> 29;
+
+    
     /* 2nd iteration */
-    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+    temp = ((q63_t) var1 * var1) >> 28;
-                                     ((q31_t)
+    temp = ((q63_t) number * temp) >> 31;
-                                      ((((q31_t)
+    temp = 0x30000000 - temp; 
-                                         (((q63_t) var1 * var1) >> 31)) *
+    var1 = ((q63_t) var1 * temp) >> 29;
-                                        (q63_t) half) >> 31))) >> 31)) << 2;
+
-    /* 3rd iteration */
+    /* 3nd iteration */
-    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+    temp = ((q63_t) var1 * var1) >> 28;
-                                     ((q31_t)
+    temp = ((q63_t) number * temp) >> 31;
-                                      ((((q31_t)
+    temp = 0x30000000 - temp; 
-                                         (((q63_t) var1 * var1) >> 31)) *
+    var1 = ((q63_t) var1 * temp) >> 29;
-                                        (q63_t) half) >> 31))) >> 31)) << 2;
 
     /* Multiply the inverse square root with the original value */
-    var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
+    var1 = ((q31_t) (((q63_t) number * var1) >> 28));
 
     /* Shift the output down accordingly */
     if ((signBits1 % 2) == 0)

Source/interpol.cmake

@@ -1,5 +1,13 @@
 function(interpol PROJECT)
 
+if (CONFIGTABLE AND ARM_SQRT_Q31) 
+    target_compile_definitions(${PROJECT} PUBLIC ARM_TABLE_SQRT_Q31)
+endif()
+
+if (CONFIGTABLE AND ARM_SQRT_Q15) 
+    target_compile_definitions(${PROJECT} PUBLIC ARM_TABLE_SQRT_Q15)
+endif()
+
 if (CONFIGTABLE AND ARM_COS_F32)
     target_compile_definitions(${PROJECT} PUBLIC ARM_TABLE_SIN_F32)
 endif()

Testing/FrameworkSource/Error.cpp

@@ -89,7 +89,7 @@ void assert_near_equal(unsigned long nb,q63_t pa, q63_t pb, q63_t threshold)
     if (abs(pa - pb) > threshold)
     {
          char details[200];
-         sprintf(details,"diff %lld > %lld (%016llX,%016llX)",abs(pa - pb) , threshold,pa,pb);
+         sprintf(details,"diff %lld > %lld (0x%016llX,0x%016llX)",abs(pa - pb) , threshold,pa,pb);
          throw (Error(EQUAL_ERROR,nb,details));
     }
 };
@@ -100,7 +100,7 @@ void assert_near_equal(unsigned long nb,q31_t pa, q31_t pb, q31_t threshold)
     if (abs(pa - pb) > threshold)
     {
          char details[200];
-         sprintf(details,"diff %d > %d (%08X,%08X)",abs(pa - pb) , threshold,pa,pb);
+         sprintf(details,"diff %d > %d (0x%08X,0x%08X)",abs(pa - pb) , threshold,pa,pb);
          throw (Error(EQUAL_ERROR,nb,details));
     }
 };
@@ -111,7 +111,7 @@ void assert_near_equal(unsigned long nb,q15_t pa, q15_t pb, q15_t threshold)
     if (abs(pa - pb) > threshold)
     {
          char details[200];
-         sprintf(details,"diff %d > %d (%04X,%04X)",abs(pa - pb) , threshold,pa,pb);
+         sprintf(details,"diff %d > %d (0x%04X,0x%04X)",abs(pa - pb) , threshold,pa,pb);
          throw (Error(EQUAL_ERROR,nb,details));
     }
 };
@@ -122,7 +122,7 @@ void assert_near_equal(unsigned long nb,q7_t pa, q7_t pb, q7_t threshold)
     if (abs(pa - pb) > threshold)
     {
          char details[200];
-         sprintf(details,"diff %d > %d (%02X,%02X)",abs(pa - pb) , threshold,pa,pb);
+         sprintf(details,"diff %d > %d (0x%02X,0x%02X)",abs(pa - pb) , threshold,pa,pb);
          throw (Error(EQUAL_ERROR,nb,details));
     }
 };

Testing/PatternGeneration/FastMath.py

@@ -104,7 +104,7 @@ def writeTests(config,format):
     refsin = np.sin(angles)
 
 
-    vals=np.array([0.0, 0.0, 0.1,1.0,2.0,3.0,3.5,3.6])
+    vals=np.linspace(0.0,1.0,1024)
     sqrtvals=np.sqrt(vals)
 
     # Negative values in CMSIS are giving 0
@@ -115,10 +115,15 @@ def writeTests(config,format):
         angles=np.concatenate((a1,a2,a1))
         angles = angles / (2*math.pi)
     config.writeInput(1, angles,"Angles")
+    
+    config.setOverwrite(True)
     config.writeInput(1, vals,"SqrtInput")
+    config.writeReference(1, sqrtvals,"Sqrt")
+    config.setOverwrite(False)
+
     config.writeReference(1, refcos,"Cos")
     config.writeReference(1, refsin,"Sin")
-    config.writeReference(1, sqrtvals,"Sqrt")
+
 
     # For benchmarks
     samples=np.random.randn(NBSAMPLES)

Testing/Source/Tests/FastMathQ15.cpp

@@ -12,6 +12,8 @@ Reference patterns are generated with
 a double precision computation.
 
 */
+#define ABS_SQRT_ERROR ((q15_t)6)
+
 #define ABS_ERROR ((q15_t)10)
 
 #define LOG_ABS_ERROR ((q15_t)3)
@@ -92,14 +94,16 @@ a double precision computation.
         arm_status status;
         unsigned long i;
 
+
         for(i=0; i < ref.nbSamples(); i++)
         {
+            
            status=arm_sqrt_q15(inp[i],&outp[i]);
            ASSERT_TRUE((status == ARM_MATH_SUCCESS) || ((inp[i] <= 0) && (status == ARM_MATH_ARGUMENT_ERROR)));
         }
 
         ASSERT_SNR(ref,output,(float32_t)SNR_THRESHOLD);
-        ASSERT_NEAR_EQ(ref,output,ABS_ERROR);
+        ASSERT_NEAR_EQ(ref,output,ABS_SQRT_ERROR);
 
     }
 

Testing/Source/Tests/FastMathQ31.cpp

@@ -11,6 +11,8 @@ Reference patterns are generated with
 a double precision computation.
 
 */
+#define ABS_SQRT_ERROR ((q31_t)7)
+
 #define ABS_ERROR ((q31_t)2200)
 #define ABS_DIV_ERROR ((q31_t)1)
 
@@ -100,8 +102,8 @@ a double precision computation.
            ASSERT_TRUE((status == ARM_MATH_SUCCESS) || ((inp[i] <= 0) && (status == ARM_MATH_ARGUMENT_ERROR)));
         }
 
-        ASSERT_SNR(ref,output,(float32_t)SNR_THRESHOLD);
+        //ASSERT_SNR(ref,output,(float32_t)SNR_THRESHOLD);
-        ASSERT_NEAR_EQ(ref,output,ABS_ERROR);
+        ASSERT_NEAR_EQ(ref,output,ABS_SQRT_ERROR);
 
     }
 

cmsisdspconfig.py

@@ -40,6 +40,7 @@ config["SIN_Q31"]=False
 config["SIN_Q15"]=False
 config["SIN_COS_F32"]=False
 config["SIN_COS_Q31"]=False
+config["SQRT_Q31"]=False
 config["LMS_NORM_Q31"]=False
 config["LMS_NORM_Q15"]=False
 config["CMPLX_MAG_Q31"]=False
@@ -82,6 +83,7 @@ realname["SIN_Q31"]="ARM_SIN_Q31"
 realname["SIN_Q15"]="ARM_SIN_Q15"
 realname["SIN_COS_F32"]="ARM_SIN_COS_F32"
 realname["SIN_COS_Q31"]="ARM_SIN_COS_Q31"
+realname["SQRT_Q31"]="ARM_SQRT_Q31"
 realname["LMS_NORM_Q31"]="ARM_LMS_NORM_Q31"
 realname["LMS_NORM_Q15"]="ARM_LMS_NORM_Q15"
 realname["CMPLX_MAG_Q31"]="ARM_CMPLX_MAG_Q31"
@@ -362,6 +364,9 @@ def interpretCmakeOptions(cmake):
     if test(cmake,"ARM_SIN_COS_Q31"):
         r.append("-DARM_TABLE_SIN_Q31")
 
+    if test(cmake,"ARM_SQRT_Q31"):
+        r.append("-DARM_TABLE_SQRT_Q31")
+
     if test(cmake,"ARM_LMS_NORM_Q31"):
         r.append("-DARM_TABLE_RECIP_Q31")