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
  40     #define fixmul32(X,Y) \
  41      ({ \
  42         int32_t low; \
  43         int32_t high; \
  44         asm volatile (                   /* calculates: result = (X*Y)>>32 */ \
  45            "smull  %0,%1,%2,%3 \n\t"     /* 64 = 32x32 multiply */ \
  46            : "=&r"(low), "=&r" (high) \
  47            : "r"(X),"r"(Y)); \
  48         high; \
  49      })
  50 #else
  51     static inline int32_t fixmul16(int32_t x, int32_t y)
  52     {
  53         int64_t temp;
  54         temp = x;
  55         temp *= y;
  56
  57         temp >>= 16;
  58
  59         return (int32_t)temp;
  60     }
  61
  62     static inline int32_t fixmul31(int32_t x, int32_t y)
  63     {
  64         int64_t temp;
  65         temp = x;
  66         temp *= y;
  67
  68         temp >>= 31;        //16+31-16 = 31 bits
  69
  70         return (int32_t)temp;
  71     }
  72
  73     static inline int32_t fixmul32(int32_t x, int32_t y)
  74     {
  75         int64_t temp;
  76         temp = x;
  77         temp *= y;
  78
  79         temp >>= 32;        //16+31-16 = 31 bits
  80
  81         return (int32_t)temp;
  82     }
  83 #endif
  84
  85 static inline int32_t fixdiv16(int32_t x, int32_t y)
  86 {
  87     int64_t temp;
  88     temp = x << 16;
  89     temp /= y;
  90
  91     return (int32_t)temp;
  92 }
  93
  94 /*
  95  * Fast integer square root adapted from algorithm,
  96  * Martin Guy @ UKC, June 1985.
  97  * Originally from a book on programming abaci by Mr C. Woo.
  98  * This is taken from :
  99  * http://wiki.forum.nokia.com/index.php/How_to_use_fixed_point_maths#How_to_get_square_root_for_integers
 100  * with a added shift up of the result by 8 bits to return result in 16.16 fixed-point representation.
 101  */
 102 static inline int32_t fastSqrt(int32_t n)
 103 {
 104    /*
 105     * Logically, these are unsigned.
 106     * We need the sign bit to test
 107     *   whether (op - res - one) underflowed.
 108     */
 109     int32_t op, res, one;
 110     op = n;
 111     res = 0;
 112     /* "one" starts at the highest power of four <= than the argument. */
 113     one = 1 << 30;      /* second-to-top bit set */
 114     while (one > op) one >>= 2;
 115     while (one != 0)
 116     {
 117         if (op >= res + one)
 118         {
 119             op = op - (res + one);
 120             res = res +  (one<<1);
 121         }
 122         res >>= 1;
 123         one >>= 2;
 124     }
 125     return(res << 8);
 126 }