apps/codecs/libatrac/fixp_math.h

   1 #include <stdlib.h>
   2 #include <inttypes.h>
   3
   4 /* Macros for converting between various fixed-point representations and floating point. */
   5 #define ONE_16 (1L << 16)
   6 #define fixtof64(x)       (float)((float)(x) / (float)(1 << 16))        //does not work on int64_t!
   7 #define ftofix32(x)       ((int32_t)((x) * (float)(1 << 16) + ((x) < 0 ? -0.5 : 0.5)))
   8 #define ftofix31(x)       ((int32_t)((x) * (float)(1 << 31) + ((x) < 0 ? -0.5 : 0.5)))
   9 #define fix31tof64(x)     (float)((float)(x) / (float)(1 << 31))
  10
  11 /* Fixed point math routines for use in atrac3.c */
  12
  13 #if defined(CPU_ARM)
  14     #define fixmul16(X,Y) \
  15      ({ \
  16         int32_t low; \
  17         int32_t high; \
  18         asm volatile (                   /* calculates: result = (X*Y)>>16 */ \
  19            "smull  %0,%1,%2,%3 \n\t"     /* 64 = 32x32 multiply */ \
  20            "mov %0, %0, lsr #16 \n\t"    /* %0 = %0 >> 16 */ \
  21            "orr %0, %0, %1, lsl #16 \n\t"/* result = %0 OR (%1 << 16) */ \
  22            : "=&r"(low), "=&r" (high) \
  23            : "r"(X),"r"(Y)); \
  24         low; \
  25      })
  26
  27     #define fixmul31(X,Y) \
  28      ({ \
  29         int32_t low; \
  30         int32_t high; \
  31         asm volatile (                   /* calculates: result = (X*Y)>>31 */ \
  32            "smull  %0,%1,%2,%3 \n\t"     /* 64 = 32x32 multiply */ \
  33            "mov %0, %0, lsr #31 \n\t"    /* %0 = %0 >> 31 */ \
  34            "orr %0, %0, %1, lsl #1 \n\t" /* result = %0 OR (%1 << 1) */ \
  35            : "=&r"(low), "=&r" (high) \
  36            : "r"(X),"r"(Y)); \
  37         low; \
  38      })
  39 #elif defined(CPU_COLDFIRE)
  40     #define fixmul16(X,Y) \
  41     ({ \
  42         int32_t t1, t2; \
  43         asm volatile ( \
  44             "mac.l   %[x],%[y],%%acc0\n\t" /* multiply */ \
  45             "mulu.l  %[y],%[x]   \n\t"     /* get lower half, avoid emac stall */ \
  46             "movclr.l %%acc0,%[t1]   \n\t" /* get higher half */ \
  47             "moveq.l #15,%[t2]   \n\t" \
  48             "asl.l   %[t2],%[t1] \n\t"     /* hi <<= 15, plus one free */ \
  49             "moveq.l #16,%[t2]   \n\t" \
  50             "lsr.l   %[t2],%[x]  \n\t"     /* (unsigned)lo >>= 16 */ \
  51             "or.l    %[x],%[t1]  \n\t"     /* combine result */ \
  52             : /* outputs */ \
  53             [t1]"=&d"(t1), \
  54             [t2]"=&d"(t2) \
  55             : /* inputs */ \
  56             [x] "d" ((X)), \
  57             [y] "d" ((Y))); \
  58         t1; \
  59     })
  60
  61     #define fixmul31(X,Y) \
  62     ({ \
  63        int32_t t; \
  64        asm volatile ( \
  65           "mac.l %[x], %[y], %%acc0\n\t"   /* multiply */ \
  66           "movclr.l %%acc0, %[t]\n\t"      /* get higher half as result */ \
  67           : [t] "=d" (t) \
  68           : [x] "r" ((X)), [y] "r" ((Y))); \
  69        t; \
  70     })
  71 #else
  72     static inline int32_t fixmul16(int32_t x, int32_t y)
  73     {
  74         int64_t temp;
  75         temp = x;
  76         temp *= y;
  77
  78         temp >>= 16;
  79
  80         return (int32_t)temp;
  81     }
  82
  83     static inline int32_t fixmul31(int32_t x, int32_t y)
  84     {
  85         int64_t temp;
  86         temp = x;
  87         temp *= y;
  88
  89         temp >>= 31;        //16+31-16 = 31 bits
  90
  91         return (int32_t)temp;
  92     }
  93 #endif
  94
  95 static inline int32_t fixdiv16(int32_t x, int32_t y)
  96 {
  97     int64_t temp;
  98     temp = x << 16;
  99     temp /= y;
 100
 101     return (int32_t)temp;
 102 }
 103
 104 /*
 105  * Fast integer square root adapted from algorithm,
 106  * Martin Guy @ UKC, June 1985.
 107  * Originally from a book on programming abaci by Mr C. Woo.
 108  * This is taken from :
 109  * http://wiki.forum.nokia.com/index.php/How_to_use_fixed_point_maths#How_to_get_square_root_for_integers
 110  * with a added shift up of the result by 8 bits to return result in 16.16 fixed-point representation.
 111  */
 112 static inline int32_t fastSqrt(int32_t n)
 113 {
 114    /*
 115     * Logically, these are unsigned.
 116     * We need the sign bit to test
 117     * whether (op - res - one) underflowed.
 118     */
 119     int32_t op, res, one;
 120     op = n;
 121     res = 0;
 122     /* "one" starts at the highest power of four <= than the argument. */
 123     one = 1 << 30; /* second-to-top bit set */
 124     while (one > op) one >>= 2;
 125     while (one != 0)
 126     {
 127         if (op >= res + one)
 128         {
 129             op = op - (res + one);
 130             res = res +  (one<<1);
 131         }
 132         res >>= 1;
 133         one >>= 2;
 134     }
 135     return(res << 8);
 136 }