4 files changed, 38 insertions, 5 deletions

diff --git a/apps/codecs/lib/asm_arm.h b/apps/codecs/lib/asm_arm.h
index c0f9440450..629e47b3bd 100644
--- a/apps/codecs/lib/asm_arm.h
+++ b/apps/codecs/lib/asm_arm.h
@@ -52,6 +52,17 @@ static inline int32_t MULT31_SHIFT15(int32_t x, int32_t y) {
   return(hi);
 }
 
+static inline int32_t MULT31_SHIFT16(int32_t x, int32_t y) {
+  int32_t lo,hi;
+  asm volatile("smull   %0, %1, %2, %3\n\t"
+               "movs    %0, %0, lsr #16\n\t"
+               "adc %1, %0, %1, lsl #16\n\t"
+               : "=&r"(lo),"=&r"(hi)
+               : "r"(x),"r"(y)
+               : "cc" );
+  return(hi);
+}
+
 #define XPROD32(a, b, t, v, x, y) \
 { \
   int32_t l; \
diff --git a/apps/codecs/lib/asm_mcf5249.h b/apps/codecs/lib/asm_mcf5249.h
index 49d2ddf7cb..5fb3cff94a 100644
--- a/apps/codecs/lib/asm_mcf5249.h
+++ b/apps/codecs/lib/asm_mcf5249.h
@@ -61,6 +61,21 @@ static inline int32_t MULT31_SHIFT15(int32_t x, int32_t y) {
   return r;
 }
 
+static inline int32_t MULT31_SHIFT16(int32_t x, int32_t y) {
+  int32_t r;
+
+  asm volatile ("mac.l %[x], %[y], %%acc0;"  /* multiply */
+                "mulu.l %[y], %[x];"         /* get lower half, avoid emac stall */
+                "movclr.l %%acc0, %[r];"     /* get higher half */
+                "lsr.l #1, %[r];"            /* hi >> 1, to compensate emac shift */
+                "move.w %[r], %[x];"         /* x = x & 0xffff0000 | r & 0xffff */
+                "swap %[x];"                 /* x = (unsigned)x << 16 | (unsigned)x >> 16 */
+                : [r] "=&d" (r), [x] "+d" (x)
+                : [y] "d" (y)
+                : "cc");
+  return x;
+}
+
 static inline
 void XPROD31(int32_t  a, int32_t  b,
              int32_t  t, int32_t  v,
diff --git a/apps/codecs/lib/codeclib_misc.h b/apps/codecs/lib/codeclib_misc.h
index f3ec209e78..f3b1805e26 100644
--- a/apps/codecs/lib/codeclib_misc.h
+++ b/apps/codecs/lib/codeclib_misc.h
@@ -65,6 +65,7 @@ static inline int32_t MULT32(int32_t x, int32_t y) {
   magic.whole = (int64_t)x * y;
   return magic.halves.hi;
 }
+
 static inline int32_t MULT31(int32_t x, int32_t y) {
   return MULT32(x,y)<<1;
 }
@@ -75,6 +76,12 @@ static inline int32_t MULT31_SHIFT15(int32_t x, int32_t y) {
   return ((uint32_t)(magic.halves.lo)>>15) | ((magic.halves.hi)<<17);
 }
 
+static inline int32_t MULT31_SHIFT16(int32_t x, int32_t y) {
+  union magic magic;
+  magic.whole  = (int64_t)x * y;
+  return ((uint32_t)(magic.halves.lo)>>16) | ((magic.halves.hi)<<16);
+}
+
 #else
 /* 32 bit multiply, more portable but less accurate */
 
diff --git a/apps/codecs/libcook/cook_fixpoint.h b/apps/codecs/libcook/cook_fixpoint.h
index c8a17c6dc6..5c4a5d1a5a 100644
--- a/apps/codecs/libcook/cook_fixpoint.h
+++ b/apps/codecs/libcook/cook_fixpoint.h
@@ -36,7 +36,7 @@
  */
 
 #ifdef ROCKBOX
-/* get definitions of MULT31, MULT31_SHIFT15, vect_add, from codelib */
+/* get definitions of MULT31, MULT31_SHIFT16, vect_add, from codelib */
 #include "codeclib_misc.h"
 #include "codeclib.h"
 #endif
@@ -68,7 +68,7 @@ static inline FIXP fixp_pow2(FIXP x, int i)
  * @param b                     fix point fraction, 0 <= b < 1
  */
 #ifdef ROCKBOX
-#define fixp_mult_su(x,y) (MULT31_SHIFT15(x,y))
+#define fixp_mult_su(x,y) (MULT31_SHIFT16(x,y))
 #else
 static inline FIXP fixp_mult_su(FIXP a, FIXPU b)
 {
@@ -130,18 +130,18 @@ static void scalar_dequant_math(COOKContext *q, int index,
                                 int* subband_coef_sign, REAL_T *mlt_p)
 {
     /* Num. half bits to right shift */
-    const int s = (33 - quant_index + av_log2(q->samples_per_channel)) >> 1;
+    const int s = 33 - quant_index + av_log2(q->samples_per_channel);
     const FIXP *table = quant_tables[s & 1][index];
     FIXP f;
     int i;
 
 
-    if(s >= 32)
+    if(s >= 64)
         memset(mlt_p, 0, sizeof(REAL_T)*SUBBAND_SIZE);
     else 
     {
         for(i=0 ; i<SUBBAND_SIZE ; i++) {
-            f = (table[subband_coef_index[i]])>>s;
+            f = (table[subband_coef_index[i]]) >> (s >> 1);
             /* noise coding if subband_coef_index[i] == 0 */
             if (((subband_coef_index[i] == 0) && cook_random(q)) ||
                 ((subband_coef_index[i] != 0) && subband_coef_sign[i]))