3 files changed, 46 insertions, 31 deletions

diff --git a/apps/codecs/libspeex/speex/speex_stereo.h b/apps/codecs/libspeex/speex/speex_stereo.h
index ea2f976a26..904e9b092e 100644
--- a/apps/codecs/libspeex/speex/speex_stereo.h
+++ b/apps/codecs/libspeex/speex/speex_stereo.h
@@ -48,17 +48,29 @@ extern "C" {
 
 /** State used for decoding (intensity) stereo information */
 typedef struct SpeexStereoState {
+#ifndef FIXED_POINT
    float balance;      /**< Left/right balance info */
    float e_ratio;      /**< Ratio of energies: E(left+right)/[E(left)+E(right)]  */
    float smooth_left;  /**< Smoothed left channel gain */
    float smooth_right; /**< Smoothed right channel gain */
    float reserved1;    /**< Reserved for future use */
    float reserved2;    /**< Reserved for future use */
+#else
+   spx_int32_t balance;      /**< Left/right balance info */
+   spx_int16_t e_ratio;      /**< Ratio of energies: E(left+right)/[E(left)+E(right)]  */
+   spx_int16_t smooth_left;  /**< Smoothed left channel gain */
+   spx_int16_t smooth_right; /**< Smoothed right channel gain */
+   spx_int32_t reserved1;    /**< Reserved for future use */
+   spx_int32_t reserved2;    /**< Reserved for future use */
+#endif
 } SpeexStereoState;
 
 /** Initialization value for a stereo state */
+#ifndef FIXED_POINT
 #define SPEEX_STEREO_STATE_INIT {1,.5,1,1,0,0}
-
+#else
+#define SPEEX_STEREO_STATE_INIT {65536,16384,16384,16384,0,0}
+#endif
 /** Transforms a stereo frame into a mono frame and stores intensity stereo info in 'bits' */
 void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits);
 
diff --git a/apps/codecs/libspeex/stereo.c b/apps/codecs/libspeex/stereo.c
index b0c65b812f..695dfe0fb0 100644
--- a/apps/codecs/libspeex/stereo.c
+++ b/apps/codecs/libspeex/stereo.c
@@ -35,11 +35,16 @@
 
 #include <speex/speex_stereo.h>
 #include <speex/speex_callbacks.h>
+#include "math_approx.h"
 #include "vq.h"
 #include <math.h>
 
 /*float e_ratio_quant[4] = {1, 1.26, 1.587, 2};*/
+#ifndef FIXED_POINT
 static const float e_ratio_quant[4] = {.25f, .315f, .397f, .5f};
+#else
+static const spx_word16_t e_ratio_quant[4] = {8192, 10332, 13009, 16384};
+#endif
 
 #ifndef SPEEX_DISABLE_ENCODER
 void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits)
@@ -115,8 +120,10 @@ void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits)
    tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4);
    speex_bits_pack(bits, tmp, 2);
 }
-#endif
+#endif /* SPEEX_DISABLE_ENCODER */
 
+/* We don't want to decode to floats yet, disable */
+#if 0
 void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *stereo)
 {
    float balance, e_ratio;
@@ -145,48 +152,46 @@ void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *stereo)
       data[2*i+1] = stereo->smooth_right*ftmp;
    }
 }
+#endif
 
 void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *stereo)
 {
-   float balance, e_ratio;
    int i;
-   float e_tot=0, e_left, e_right, e_sum;
+   spx_word32_t balance;
+   spx_word16_t e_left, e_right, e_ratio;
 
    balance=stereo->balance;
    e_ratio=stereo->e_ratio;
-   for (i=frame_size-1;i>=0;i--)
-   {
-      e_tot += ((float)data[i])*data[i];
-   }
-   e_sum=e_tot/e_ratio;
-   e_left  = e_sum*balance / (1+balance);
-   e_right = e_sum-e_left;
 
-   e_left  = sqrt(e_left/(e_tot+.01));
-   e_right = sqrt(e_right/(e_tot+.01));
+   /* These two are Q14, with max value just below 2. */
+   e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance))));
+   e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8);
 
    for (i=frame_size-1;i>=0;i--)
    {
-      float ftmp=data[i];
-      stereo->smooth_left  = .98*stereo->smooth_left  + .02*e_left;
-      stereo->smooth_right = .98*stereo->smooth_right + .02*e_right;
-      data[2*i] = stereo->smooth_left*ftmp;
-      data[2*i+1] = stereo->smooth_right*ftmp;
+      spx_word16_t tmp=data[i];
+      stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15));
+      stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15));
+      data[2*i] = MULT16_16_P14(stereo->smooth_left, tmp);
+      data[2*i+1] = MULT16_16_P14(stereo->smooth_right, tmp);
    }
 }
 
 int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data)
 {
    SpeexStereoState *stereo;
-   float sign=1;
+   spx_word16_t sign=1;
    int tmp;
 
    stereo = (SpeexStereoState*)data;
    if (speex_bits_unpack_unsigned(bits, 1))
       sign=-1;
    tmp = speex_bits_unpack_unsigned(bits, 5);
+#ifndef FIXED_POINT
    stereo->balance = exp(sign*.25*tmp);
-
+#else
+   stereo->balance = spx_exp(MULT16_16(sign, SHL16(tmp, 9)));
+#endif
    tmp = speex_bits_unpack_unsigned(bits, 2);
    stereo->e_ratio = e_ratio_quant[tmp];
 
diff --git a/apps/codecs/speex.c b/apps/codecs/speex.c
index 7edda4b6db..2cd547a84d 100644
--- a/apps/codecs/speex.c
+++ b/apps/codecs/speex.c
@@ -26,7 +26,8 @@
 #include "libspeex/speex/speex_config_types.h"
 #include "codeclib.h"
 
-#define MAX_FRAME_SIZE 2000
+// Room for one stereo frame of max size, 2*640
+#define MAX_FRAME_SIZE 1280
 #define CHUNKSIZE 10000  /*2kb*/
 #define SEEK_CHUNKSIZE 7*CHUNKSIZE
 
@@ -346,7 +347,7 @@ static void *process_header(spx_ogg_packet *op,
     speex_decoder_ctl(st, SPEEX_SET_ENH, &enh_enabled);
     speex_decoder_ctl(st, SPEEX_GET_FRAME_SIZE, frame_size);
 
-    if (!(*channels==1)){
+    if (*channels!=1){
         callback.callback_id = SPEEX_INBAND_STEREO;
         callback.func = speex_std_stereo_request_handler;
         callback.data = stereo;
@@ -381,7 +382,8 @@ enum codec_status codec_main(void)
     int enh_enabled = 1;
     int nframes = 2;
     int eos = 0;
-    SpeexStereoState stereo = SPEEX_STEREO_STATE_INIT;
+    static const SpeexStereoState stereo_init = SPEEX_STEREO_STATE_INIT;
+    SpeexStereoState stereo = stereo_init;
     int channels = -1;
     int rate = 0, samplerate = 0;
     int extra_headers = 0;
@@ -531,13 +533,11 @@ next_page:
                         if (channels == 2)
                             speex_decode_stereo_int(output, frame_size, &stereo);
 
-                        int new_frame_size = frame_size;
-
-                        if (new_frame_size > 0) {  
-                            ci->pcmbuf_insert(output, NULL, new_frame_size);
+                        if (frame_size > 0) {
+                            ci->pcmbuf_insert(output, NULL, frame_size);
 
                             /* 2 bytes/sample */
-                            cur_granule += new_frame_size / 2;
+                            cur_granule += frame_size / 2;
 
                             ci->set_offset((long) ci->curpos);
 
@@ -566,9 +566,7 @@ done:
         cur_granule = stream_init = rate = samplerate = headerssize 
             = packet_count = eos = 0;
 
-        stereo.balance = stereo.smooth_left = stereo.smooth_right = 1;
-        stereo.e_ratio = .5;
-        stereo.reserved1 = stereo.reserved2 = 0;
+        stereo = stereo_init;
 
         goto next_track;
     }