3 files changed, 33 insertions, 0 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 */