3 files changed, 111 insertions, 65 deletions

diff --git a/apps/codecs/libspeex/speex/speex_stereo.h b/apps/codecs/libspeex/speex/speex_stereo.h
index 904e9b092e..b817a5535e 100644
--- a/apps/codecs/libspeex/speex/speex_stereo.h
+++ b/apps/codecs/libspeex/speex/speex_stereo.h
@@ -46,31 +46,28 @@
 extern "C" {
 #endif
 
-/** State used for decoding (intensity) stereo information */
+/** If you access any of these fields directly, I'll personally come and bite you */
 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
+/** Deprecated. Use speex_stereo_state_init() instead. */
 #define SPEEX_STEREO_STATE_INIT {1,.5,1,1,0,0}
-#else
-#define SPEEX_STEREO_STATE_INIT {65536,16384,16384,16384,0,0}
-#endif
+
+/** Initialise/create a stereo stereo state */
+SpeexStereoState *speex_stereo_state_init();
+
+/** Reset/re-initialise an already allocated stereo state */
+void speex_stereo_state_reset(SpeexStereoState *stereo);
+
+/** Destroy a stereo stereo state */
+void speex_stereo_state_destroy(SpeexStereoState *stereo);
+
 /** 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 695dfe0fb0..c63c974798 100644
--- a/apps/codecs/libspeex/stereo.c
+++ b/apps/codecs/libspeex/stereo.c
@@ -38,6 +38,17 @@
 #include "math_approx.h"
 #include "vq.h"
 #include <math.h>
+#include "os_support.h"
+
+typedef struct RealSpeexStereoState {
+   spx_word32_t balance;      /**< Left/right balance info */
+   spx_word32_t e_ratio;      /**< Ratio of energies: E(left+right)/[E(left)+E(right)]  */
+   spx_word32_t smooth_left;  /**< Smoothed left channel gain */
+   spx_word32_t smooth_right; /**< Smoothed right channel gain */
+   spx_int32_t reserved1;     /**< Reserved for future use */
+   spx_int32_t reserved2;     /**< Reserved for future use */
+} RealSpeexStereoState;
+
 
 /*float e_ratio_quant[4] = {1, 1.26, 1.587, 2};*/
 #ifndef FIXED_POINT
@@ -46,6 +57,50 @@ static const float e_ratio_quant[4] = {.25f, .315f, .397f, .5f};
 static const spx_word16_t e_ratio_quant[4] = {8192, 10332, 13009, 16384};
 #endif
 
+/* This is an ugly compatibility hack that properly resets the stereo state
+   In case it it compiled in fixed-point, but initialised with the deprecated
+   floating point static initialiser */
+#ifdef FIXED_POINT
+#define COMPATIBILITY_HACK(s) do {if ((s)->reserved1 != 0xdeadbeef) speex_stereo_state_init(s); } while (0);
+#else
+#define COMPATIBILITY_HACK(s) 
+#endif
+
+static SpeexStereoState global_stereo_state;
+SpeexStereoState *speex_stereo_state_init()
+{
+   /* SpeexStereoState *stereo = speex_alloc(sizeof(SpeexStereoState)); */
+   SpeexStereoState *stereo = &global_stereo_state;
+   speex_stereo_state_reset(stereo);
+   return stereo;
+}
+
+void speex_stereo_state_reset(SpeexStereoState *_stereo)
+{
+   RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
+#ifdef FIXED_POINT
+   stereo->balance = 65536;
+   stereo->e_ratio = 16384;
+   stereo->smooth_left = 16384;
+   stereo->smooth_right = 16384;
+   stereo->reserved1 = 0xdeadbeef;
+   stereo->reserved2 = 0;
+#else
+   stereo->balance = 1.0f;
+   stereo->e_ratio = .5f;
+   stereo->smooth_left = 1.f;
+   stereo->smooth_right = 1.f;
+   stereo->reserved1 = 0;
+   stereo->reserved2 = 0;
+#endif   
+}
+
+void speex_stereo_state_destroy(SpeexStereoState *stereo)
+{
+   (void)stereo;
+   /* speex_free(stereo); */
+}
+
 #ifndef SPEEX_DISABLE_ENCODER
 void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits)
 {
@@ -79,7 +134,7 @@ void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits)
    
    speex_bits_pack(bits, (int)balance, 5);
    
-   /*Quantize energy ratio*/
+   /* FIXME: Convert properly */
    tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4);
    speex_bits_pack(bits, tmp, 2);
 }
@@ -116,7 +171,7 @@ void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits)
    
    speex_bits_pack(bits, (int)balance, 5);
    
-   /*Quantize energy ratio*/
+   /* FIXME: Convert properly */
    tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4);
    speex_bits_pack(bits, tmp, 2);
 }
@@ -124,73 +179,76 @@ void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits)
 
 /* We don't want to decode to floats yet, disable */
 #if 0
-void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *stereo)
+void speex_decode_stereo(float *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;
+   RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
+   
+   COMPATIBILITY_HACK(stereo);
+   
    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] = (float)MULT16_16_P14(stereo->smooth_left, tmp);
+      data[2*i+1] = (float)MULT16_16_P14(stereo->smooth_right, tmp);
    }
 }
 #endif
 
-void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *stereo)
+void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo)
 {
    int i;
    spx_word32_t balance;
    spx_word16_t e_left, e_right, e_ratio;
+   RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
 
+   COMPATIBILITY_HACK(stereo);
+   
    balance=stereo->balance;
    e_ratio=stereo->e_ratio;
-
+   
    /* 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--)
    {
-      spx_word16_t tmp=data[i];
+      spx_int16_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);
+      data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp);
+      data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp);
    }
 }
 
 int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data)
 {
-   SpeexStereoState *stereo;
-   spx_word16_t sign=1;
+   RealSpeexStereoState *stereo;
+   spx_word16_t sign=1, dexp;
    int tmp;
 
-   stereo = (SpeexStereoState*)data;
+   stereo = (RealSpeexStereoState*)data;
+   
+   COMPATIBILITY_HACK(stereo);
+
    if (speex_bits_unpack_unsigned(bits, 1))
       sign=-1;
-   tmp = speex_bits_unpack_unsigned(bits, 5);
+   dexp = speex_bits_unpack_unsigned(bits, 5);
 #ifndef FIXED_POINT
-   stereo->balance = exp(sign*.25*tmp);
+   stereo->balance = exp(sign*.25*dexp);
 #else
-   stereo->balance = spx_exp(MULT16_16(sign, SHL16(tmp, 9)));
+   stereo->balance = spx_exp(MULT16_16(sign, SHL16(dexp, 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 724160b152..c3f1fc8408 100644
--- a/apps/codecs/speex.c
+++ b/apps/codecs/speex.c
@@ -206,9 +206,7 @@ int speex_seek_page_granule(spx_int64_t pos, spx_int64_t curpos,
            the bitrate is relativly constant.
          */
 
-        curoffset = (int)((((float)(*curbyteoffset-(headerssize)) *
-                          (float)pos)/(float)curpos)*0.98);
-
+        curoffset = (((*curbyteoffset-headerssize) * pos)/curpos)*98/100;
         if (curoffset < 0)
             curoffset=0;
 
@@ -347,12 +345,14 @@ 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 (header->nb_channels!=1){
         callback.callback_id = SPEEX_INBAND_STEREO;
         callback.func = speex_std_stereo_request_handler;
         callback.data = stereo;
         speex_decoder_ctl(st, SPEEX_SET_HANDLER, &callback);
     }
+    *channels = header->nb_channels;
+
     if (!*rate)
         *rate = header->rate;
 
@@ -360,9 +360,6 @@ static void *process_header(spx_ogg_packet *op,
 
     *nframes = header->frames_per_packet;
 
-    if (*channels == -1)
-        *channels = header->nb_channels;
-
     *extra_headers = header->extra_headers;
 
     return st;
@@ -382,27 +379,25 @@ enum codec_status codec_main(void)
     int enh_enabled = 1;
     int nframes = 2;
     int eos = 0;
-    static const SpeexStereoState stereo_init = SPEEX_STEREO_STATE_INIT;
-    SpeexStereoState stereo = stereo_init;
+    SpeexStereoState *stereo;
     int channels = -1;
     int rate = 0, samplerate = 0;
     int extra_headers = 0;
     int stream_init = 0;
     int page_nb_packets, frame_size, packet_count = 0;
-    int lookahead = 0;
     int headerssize = -1;
     unsigned long strtoffset = 0;
     void *st = NULL;
     int j = 0;
 
-    /* We need to flush reserver memory every track load. */
+    /* Ogg handling still uses mallocs, so reset the malloc buffer per track */
 next_track:
 
     if (codec_init()) {
         error = CODEC_ERROR;
         goto exit;
     }
-
+    stereo = speex_stereo_state_init();
     strtoffset = ci->id3->offset;
 
     while (!*ci->taginfo_ready && !ci->stop_codec)
@@ -467,9 +462,7 @@ next_page:
                 if (packet_count==0){
                     st = process_header(&op, enh_enabled, &frame_size,
                                          &samplerate, &nframes, &channels,
-                                         &stereo, &extra_headers);
-
-                    speex_decoder_ctl(st, SPEEX_GET_LOOKAHEAD, &lookahead);
+                                         stereo, &extra_headers);
 
                     if (!nframes)
                         nframes=1;
@@ -531,7 +524,7 @@ next_page:
                             break;
 
                         if (channels == 2)
-                            speex_decode_stereo_int(output, frame_size, &stereo);
+                            speex_decode_stereo_int(output, frame_size, stereo);
 
                         if (frame_size > 0) {
                             ci->pcmbuf_insert(output, NULL, frame_size);
@@ -566,8 +559,6 @@ done:
         cur_granule = stream_init = rate = samplerate = headerssize 
             = packet_count = eos = 0;
 
-        stereo = stereo_init;
-
         goto next_track;
     }