1 files changed, 153 insertions, 306 deletions

diff --git a/apps/codecs/libspeex/math_approx.c b/apps/codecs/libspeex/math_approx.c
index 54a3412..21af766 100644
--- a/apps/codecs/libspeex/math_approx.c
+++ b/apps/codecs/libspeex/math_approx.c
@@ -34,181 +34,89 @@
 #include "config.h"
 #endif
 
-
-
 #include "math_approx.h"
 #include "misc.h"
 
-#ifdef FIXED_POINT
-
-/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25723*x^3 (for .25 < x < 1) */
-#define C0 3634
-#define C1 21173
-#define C2 -12627
-#define C3 4215
-
-spx_word16_t spx_sqrt(spx_word32_t x)
+spx_int16_t spx_ilog2(spx_uint32_t x)
 {
-   int k=0;
-   spx_word32_t rt;
-
-   if (x<=0)
-      return 0;
-#if 1
-   if (x>=16777216)
+   int r=0;
+   if (x>=(spx_int32_t)65536)
    {
-      x>>=10;
-      k+=5;
+      x >>= 16;
+      r += 16;
    }
-   if (x>=1048576)
+   if (x>=256)
    {
-      x>>=6;
-      k+=3;
+      x >>= 8;
+      r += 8;
    }
-   if (x>=262144)
+   if (x>=16)
    {
-      x>>=4;
-      k+=2;
+      x >>= 4;
+      r += 4;
    }
-   if (x>=32768)
+   if (x>=4)
    {
-      x>>=2;
-      k+=1;
+      x >>= 2;
+      r += 2;
    }
-   if (x>=16384)
+   if (x>=2)
    {
-      x>>=2;
-      k+=1;
+      r += 1;
    }
-#else
-   while (x>=16384)
+   return r;
+}
+
+spx_int16_t spx_ilog4(spx_uint32_t x)
+{
+   int r=0;
+   if (x>=(spx_int32_t)65536)
    {
-      x>>=2;
-      k++;
-      }
-#endif
-   while (x<4096)
+      x >>= 16;
+      r += 8;
+   }
+   if (x>=256)
    {
-      x<<=2;
-      k--;
+      x >>= 8;
+      r += 4;
    }
-   rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3)))))));
-   if (rt > 16383)
-      rt = 16383;
-   if (k>0)
-      rt <<= k;
-   else
-      rt >>= -k;
-   rt >>=7;
-   return rt;
+   if (x>=16)
+   {
+      x >>= 4;
+      r += 2;
+   }
+   if (x>=4)
+   {
+      r += 1;
+   }
+   return r;
 }
 
-  static int intSqrt(int x) {
-    int xn;
-   static int sqrt_table[256] = {
-     0,    16,  22,  27,  32,  35,  39,  42,  45,  48,  50,  53,  55,  57,
-     59,   61,  64,  65,  67,  69,  71,  73,  75,  76,  78,  80,  81,  83,
-     84,   86,  87,  89,  90,  91,  93,  94,  96,  97,  98,  99, 101, 102,
-     103, 104, 106, 107, 108, 109, 110, 112, 113, 114, 115, 116, 117, 118,
-     119, 120, 121, 122, 123, 124, 125, 126, 128, 128, 129, 130, 131, 132,
-     133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 144, 145,
-     146, 147, 148, 149, 150, 150, 151, 152, 153, 154, 155, 155, 156, 157,
-     158, 159, 160, 160, 161, 162, 163, 163, 164, 165, 166, 167, 167, 168,
-     169, 170, 170, 171, 172, 173, 173, 174, 175, 176, 176, 177, 178, 178,
-     179, 180, 181, 181, 182, 183, 183, 184, 185, 185, 186, 187, 187, 188,
-     189, 189, 190, 191, 192, 192, 193, 193, 194, 195, 195, 196, 197, 197,
-     198, 199, 199, 200, 201, 201, 202, 203, 203, 204, 204, 205, 206, 206,
-     207, 208, 208, 209, 209, 210, 211, 211, 212, 212, 213, 214, 214, 215,
-     215, 216, 217, 217, 218, 218, 219, 219, 220, 221, 221, 222, 222, 223,
-     224, 224, 225, 225, 226, 226, 227, 227, 228, 229, 229, 230, 230, 231,
-     231, 232, 232, 233, 234, 234, 235, 235, 236, 236, 237, 237, 238, 238,
-     239, 240, 240, 241, 241, 242, 242, 243, 243, 244, 244, 245, 245, 246,
-     246, 247, 247, 248, 248, 249, 249, 250, 250, 251, 251, 252, 252, 253,
-     253, 254, 254, 255
-  };
-    if (x >= 0x10000) {
-      if (x >= 0x1000000) {
-        if (x >= 0x10000000) {
-          if (x >= 0x40000000) {
-            xn = sqrt_table[x >> 24] << 8;
-          } else {
-            xn = sqrt_table[x >> 22] << 7;
-          }
-        } else {
-          if (x >= 0x4000000) {
-            xn = sqrt_table[x >> 20] << 6;
-          } else {
-            xn = sqrt_table[x >> 18] << 5;
-          }
-        }
-
-        xn = (xn + 1 + (x / xn)) >> 1;
-        xn = (xn + 1 + (x / xn)) >> 1;
-        return ((xn * xn) > x) ? --xn : xn;
-      } else {
-        if (x >= 0x100000) {
-          if (x >= 0x400000) {
-            xn = sqrt_table[x >> 16] << 4;
-          } else {
-            xn = sqrt_table[x >> 14] << 3;
-          }
-        } else {
-          if (x >= 0x40000) {
-            xn = sqrt_table[x >> 12] << 2;
-          } else {
-            xn = sqrt_table[x >> 10] << 1;
-          }
-        }
-
-        xn = (xn + 1 + (x / xn)) >> 1;
+#ifdef FIXED_POINT
 
-        return ((xn * xn) > x) ? --xn : xn;
-      }
-    } else {
-      if (x >= 0x100) {
-        if (x >= 0x1000) {
-          if (x >= 0x4000) {
-            xn = (sqrt_table[x >> 8]) + 1;
-          } else {
-            xn = (sqrt_table[x >> 6] >> 1) + 1;
-          }
-        } else {
-          if (x >= 0x400) {
-            xn = (sqrt_table[x >> 4] >> 2) + 1;
-          } else {
-            xn = (sqrt_table[x >> 2] >> 3) + 1;
-          }
-        }
+/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25723*x^3 (for .25 < x < 1) */
+/*#define C0 3634
+#define C1 21173
+#define C2 -12627
+#define C3 4215*/
 
-        return ((xn * xn) > x) ? --xn : xn;
-      } else {
-        if (x >= 0) {
-          return sqrt_table[x] >> 4;
-        }
-      }
-    }
-    
-    return -1;
-  }
+/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */
+#define C0 3634
+#define C1 21173
+#define C2 -12627
+#define C3 4204
 
-float spx_sqrtf(float arg)
+spx_word16_t spx_sqrt(spx_word32_t x)
 {
-	if(arg==0.0)
-		return 0.0;
-	else if(arg==1.0)
-		return 1.0;
-	else if(arg==2.0)
-		return 1.414;
-	else if(arg==3.27)
-		return 1.8083;
-	//printf("Sqrt:%f:%f:%f\n",arg,(((float)intSqrt((int)(arg*10000)))/100)+0.0055,(float)spx_sqrt((spx_word32_t)arg));
-	//return ((float)fastSqrt((int)(arg*2500)))/50;
-	//LOGF("Sqrt:%d:%d\n",arg,(intSqrt((int)(arg*2500)))/50);
-	return (((float)intSqrt((int)(arg*10000)))/100)+0.0055;//(float)spx_sqrt((spx_word32_t)arg);
-	//return 1;
+   int k;
+   spx_word32_t rt;
+   k = spx_ilog4(x)-6;
+   x = VSHR32(x, (k<<1));
+   rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3)))))));
+   rt = VSHR32(rt,7-k);
+   return rt;
 }
 
-
 /* log(x) ~= -2.18151 + 4.20592*x - 2.88938*x^2 + 0.86535*x^3 (for .5 < x < 1) */
 
 
@@ -259,6 +167,101 @@ spx_word16_t spx_cos(spx_word16_t x)
    }
 }
 
+#define L1 32767
+#define L2 -7651
+#define L3 8277
+#define L4 -626
+
+static inline spx_word16_t _spx_cos_pi_2(spx_word16_t x)
+{
+   spx_word16_t x2;
+   
+   x2 = MULT16_16_P15(x,x);
+   return ADD16(1,MIN16(32766,ADD32(SUB16(L1,x2), MULT16_16_P15(x2, ADD32(L2, MULT16_16_P15(x2, ADD32(L3, MULT16_16_P15(L4, x2))))))));
+}
+
+spx_word16_t spx_cos_norm(spx_word32_t x)
+{
+   x = x&0x0001ffff;
+   if (x>SHL32(EXTEND32(1), 16))
+      x = SUB32(SHL32(EXTEND32(1), 17),x);
+   if (x&0x00007fff)
+   {
+      if (x<SHL32(EXTEND32(1), 15))
+      {
+         return _spx_cos_pi_2(EXTRACT16(x));
+      } else {
+         return NEG32(_spx_cos_pi_2(EXTRACT16(65536-x)));
+      }
+   } else {
+      if (x&0x0000ffff)
+         return 0;
+      else if (x&0x0001ffff)
+         return -32767;
+      else
+         return 32767;
+   }
+}
+
+/*
+ K0 = 1
+ K1 = log(2)
+ K2 = 3-4*log(2)
+ K3 = 3*log(2) - 2
+*/
+#define D0 16384
+#define D1 11356
+#define D2 3726
+#define D3 1301
+/* Input in Q11 format, output in Q16 */
+static spx_word32_t spx_exp2(spx_word16_t x)
+{
+   int integer;
+   spx_word16_t frac;
+   integer = SHR16(x,11);
+   if (integer>14)
+      return 0x7fffffff;
+   else if (integer < -15)
+      return 0;
+   frac = SHL16(x-SHL16(integer,11),3);
+   frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac))))));
+   return VSHR32(EXTEND32(frac), -integer-2);
+}
+
+/* Input in Q11 format, output in Q16 */
+spx_word32_t spx_exp(spx_word16_t x)
+{
+   if (x>21290)
+      return 0x7fffffff;
+   else if (x<-21290)
+      return 0;
+   else
+      return spx_exp2(MULT16_16_P14(23637,x));
+}
+#define M1 32767
+#define M2 -21
+#define M3 -11943
+#define M4 4936
+
+static inline spx_word16_t spx_atan01(spx_word16_t x)
+{
+   return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x)))))));
+}
+
+/* Input in Q15, output in Q14 */
+spx_word16_t spx_atan(spx_word32_t x)
+{
+   if (x <= 32767)
+   {
+      return SHR16(spx_atan01(x),1);
+   } else {
+      int e = spx_ilog2(x);
+      if (e>=29)
+         return 25736;
+      x = DIV32_16(SHL32(EXTEND32(32767),29-e), EXTRACT16(SHR32(x, e-14)));
+      return SUB16(25736, SHR16(spx_atan01(x),1));
+   }
+}
 #else
 
 #ifndef M_PI
@@ -284,161 +287,5 @@ spx_word16_t spx_cos(spx_word16_t x)
       return NEG16(C1 + x*(C2+x*(C3+C4*x)));
    }
 }
-#endif
-
-inline float spx_floor(float x){
-	return ((float)(((int)x)));
-}
-
-#define FP_BITS         (14)
-#define FP_MASK         ((1 << FP_BITS) - 1)
-#define FP_ONE          (1 << FP_BITS)
-#define FP_TWO          (2 << FP_BITS)
-#define FP_HALF         (1 << (FP_BITS - 1))
-#define FP_LN2          ( 45426 >> (16 - FP_BITS))
-#define FP_LN2_INV      ( 94548 >> (16 - FP_BITS))
-#define FP_EXP_ZERO     ( 10922 >> (16 - FP_BITS))
-#define FP_EXP_ONE      (  -182 >> (16 - FP_BITS))
-#define FP_EXP_TWO      (     4 >> (16 - FP_BITS))
-// #define FP_INF          (0x7fffffff)
-// #define FP_LN10         (150902 >> (16 - FP_BITS))
-
-#define FP_MAX_DIGITS       (4)
-#define FP_MAX_DIGITS_INT   (10000)
 
-
-// #define FP_FAST_MUL_DIV
-
-// #ifdef FP_FAST_MUL_DIV
-
-/* These macros can easily overflow, but they are good enough for our uses,
- * and saves some code.
- */
-#define fp_mul(x, y) (((x) * (y)) >> FP_BITS)
-#define fp_div(x, y) (((x) << FP_BITS) / (y))
-
-#ifndef abs
-	#define abs(x) (((x)<0)?((x)*-1):(x))
 #endif
-
-float spx_sqrt2(float xf) {
-	long x=(xf*(2.0*FP_BITS));
-	int i=0, s = (x + FP_ONE) >> 1;
-	for (; i < 8; i++) {
-		s = (s + fp_div(x, s)) >> 1;
-	}
-	return s/((float)(2*FP_BITS));
-}
-
-static int exp_s16p16(int x)
-{
-	int t;
-	int y = 0x00010000;
-	
-	if (x < 0) x += 0xb1721,            y >>= 16;
-	t = x - 0x58b91; if (t >= 0) x = t, y <<= 8;
-	t = x - 0x2c5c8; if (t >= 0) x = t, y <<= 4;
-	t = x - 0x162e4; if (t >= 0) x = t, y <<= 2;
-	t = x - 0x0b172; if (t >= 0) x = t, y <<= 1;
-	t = x - 0x067cd; if (t >= 0) x = t, y += y >> 1;
-	t = x - 0x03920; if (t >= 0) x = t, y += y >> 2;
-	t = x - 0x01e27; if (t >= 0) x = t, y += y >> 3;
-	t = x - 0x00f85; if (t >= 0) x = t, y += y >> 4;
-	t = x - 0x007e1; if (t >= 0) x = t, y += y >> 5;
-	t = x - 0x003f8; if (t >= 0) x = t, y += y >> 6;
-	t = x - 0x001fe; if (t >= 0) x = t, y += y >> 7;
-	y += ((y >> 8) * x) >> 8;
-	
-	return y;
-}
-float spx_expB(float xf) {
-	return exp_s16p16(xf*32)/32;
-}
-float spx_expC(float xf){
-  long x=xf*(2*FP_BITS);
-/*
-static long fp_exp(long x)
-{*/
-    long k;
-    long z;
-    long R;
-    long xp;
-
-    if (x == 0)
-    {
-        return FP_ONE;
-    }
-
-    k = (fp_mul(abs(x), FP_LN2_INV) + FP_HALF) & ~FP_MASK;
-
-    if (x < 0)
-    {
-        k = -k;
-    }
-
-    x -= fp_mul(k, FP_LN2);
-    z = fp_mul(x, x);
-    R = FP_TWO + fp_mul(z, FP_EXP_ZERO + fp_mul(z, FP_EXP_ONE
-        + fp_mul(z, FP_EXP_TWO)));
-    xp = FP_ONE + fp_div(fp_mul(FP_TWO, x), R - x);
-
-    if (k < 0)
-    {
-        k = FP_ONE >> (-k >> FP_BITS);
-    }
-    else
-    {
-        k = FP_ONE << (k >> FP_BITS);
-    }
-
-    return fp_mul(k, xp)/(2*FP_BITS);
-}
-/*To generate (ruby code): (0...33).each { |idx| puts Math.exp((idx-10) / 8.0).to_s + "," } */
-const float exp_lookup_int[33]={0.28650479686019,0.32465246735835,0.367879441171442,0.416862019678508,0.472366552741015,0.53526142851899,0.606530659712633,0.687289278790972,0.778800783071405,0.882496902584595,1.0,1.13314845306683,1.28402541668774,1.4549914146182,1.64872127070013,1.86824595743222,2.11700001661267,2.3988752939671,2.71828182845905,3.08021684891803,3.49034295746184,3.95507672292058,4.48168907033806,5.07841903718008,5.75460267600573,6.52081912033011,7.38905609893065,8.37289748812726,9.48773583635853,10.7510131860764,12.1824939607035,13.8045741860671,15.6426318841882};
-/*To generate (ruby code): (0...32).each { |idx| puts Math.exp((idx-16.0) / 4.0).to_s+","} */
-static const float exp_table[32]={0.0183156388887342,0.0235177458560091,0.0301973834223185,0.038774207831722,0.0497870683678639,0.0639278612067076,0.0820849986238988,0.105399224561864,0.135335283236613,0.173773943450445,0.22313016014843,0.28650479686019,0.367879441171442,0.472366552741015,0.606530659712633,0.778800783071405,1.0,1.28402541668774,1.64872127070013,2.11700001661267,2.71828182845905,3.49034295746184,4.48168907033806,5.75460267600573,7.38905609893065,9.48773583635853,12.1824939607035,15.6426318841882,20.0855369231877,25.7903399171931,33.1154519586923,42.5210820000628};
-/**Returns exp(x) Range x=-4-+4 {x.0,x.25,x.5,x.75} */
-float spx_exp(float xf){
-	float flt=spx_floor(xf);
-	if(-4<xf&&4>xf&&(abs(xf-flt)==0.0||abs(xf-flt)==0.25||abs(xf-flt)==0.5||abs(xf-flt)==0.75||abs(xf-flt)==1.0)){
-#ifdef SIMULATOR 				
-/*		printf("NtbSexp:%f,%d,%f:%f,%f,%f:%d,%d:%d\n",
-			exp_sqrt_table[(int)((xf+4.0)*4.0)],
-			(int)((xf-4.0)*4.0),
-			(xf-4.0)*4.0,
-			xf,
-			flt,
-			xf-flt,
-			-4<xf,
-			4>xf,
-			abs(xf-flt)
-		);*/
-#endif
-		return exp_table[(int)((xf+4.0)*4.0)];
-	} else if (-4<xf&&4>xf){
-#ifdef SIMULATOR 		
-/*		printf("NtbLexp:%f,%f,%f:%d,%d:%d\n",xf,flt,xf-flt,-4<xf,4>xf,abs(xf-flt)); */
-#endif
-
-		return exp_table[(int)((xf+4.0)*4.0)];
-	}
-	
-#ifdef SIMULATOR 		
-/*	printf("NTBLexp:%f,%f,%f:%d,%d:%d\n",xf,flt,xf-flt,-4<xf,4>xf,abs(xf-flt)); */
-#endif
-	return spx_expB(xf);	
-	//return exp(xf);
-}
-//Placeholders (not fixed point, only used when encoding):
-float pow(float a,float b){
-	return 0;
-}
-float log(float l){
-	return 0;
-}
-float fabs(float a){
-	return 0;
-}
-float sin(float a){
-	return 0;
-}