modules/codec/wmafixed/wmafixed.c

   1 /****************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  *
   9  * Copyright (C) 2007 Michael Giacomelli
  10  *
  11  * All files in this archive are subject to the GNU General Public License.
  12  * See the file COPYING in the source tree root for full license agreement.
  13  *
  14  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  15  * KIND, either express or implied.
  16  *
  17  ****************************************************************************/
  18
  19 #include "wmadec.h"
  20 #include "wmafixed.h"
  21
  22 int64_t IntTo64(int x){
  23     int64_t res = 0;
  24     unsigned char *p = (unsigned char *)&res;
  25
  26 #ifdef ROCKBOX_BIG_ENDIAN
  27     p[5] = x & 0xff;
  28     p[4] = (x & 0xff00)>>8;
  29     p[3] = (x & 0xff0000)>>16;
  30     p[2] = (x & 0xff000000)>>24;
  31 #else
  32     p[2] = x & 0xff;
  33     p[3] = (x & 0xff00)>>8;
  34     p[4] = (x & 0xff0000)>>16;
  35     p[5] = (x & 0xff000000)>>24;
  36 #endif
  37     return res;
  38 }
  39
  40 int IntFrom64(int64_t x)
  41 {
  42     int res = 0;
  43     unsigned char *p = (unsigned char *)&x;
  44
  45 #ifdef ROCKBOX_BIG_ENDIAN
  46     res = p[5] | (p[4]<<8) | (p[3]<<16) | (p[2]<<24);
  47 #else
  48     res = p[2] | (p[3]<<8) | (p[4]<<16) | (p[5]<<24);
  49 #endif
  50     return res;
  51 }
  52
  53 int32_t Fixed32From64(int64_t x)
  54 {
  55   return x & 0xFFFFFFFF;
  56 }
  57
  58 int64_t Fixed32To64(int32_t x)
  59 {
  60   return (int64_t)x;
  61 }
  62
  63 /*
  64  * Not performance senstitive code here
  65  */
  66
  67 int64_t fixmul64byfixed(int64_t x, int32_t y)
  68 {
  69     return (x * y);
  70 /*  return (int64_t) fixmul32(Fixed32From64(x),y); */
  71 }
  72
  73 int32_t fixdiv32(int32_t x, int32_t y)
  74 {
  75     int64_t temp;
  76
  77     if(x == 0)
  78         return 0;
  79     if(y == 0)
  80         return 0x7fffffff;
  81     temp = x;
  82     temp <<= PRECISION;
  83     return (int32_t)(temp / y);
  84 }
  85
  86 int64_t fixdiv64(int64_t x, int64_t y)
  87 {
  88     int64_t temp;
  89
  90     if(x == 0)
  91         return 0;
  92     if(y == 0)
  93         return 0x07ffffffffffffffLL;
  94     temp = x;
  95     temp <<= PRECISION64;
  96     return (int64_t)(temp / y);
  97 }
  98
  99 int32_t fixsqrt32(int32_t x)
 100 {
 101     unsigned long r = 0, s, v = (unsigned long)x;
 102
 103 #define STEP(k) s = r + (1 << k * 2); r >>= 1; \
 104     if (s <= v) { v -= s; r |= (1 << k * 2); }
 105
 106     STEP(15);
 107     STEP(14);
 108     STEP(13);
 109     STEP(12);
 110     STEP(11);
 111     STEP(10);
 112     STEP(9);
 113     STEP(8);
 114     STEP(7);
 115     STEP(6);
 116     STEP(5);
 117     STEP(4);
 118     STEP(3);
 119     STEP(2);
 120     STEP(1);
 121     STEP(0);
 122
 123 #undef STEP
 124
 125     return (int32_t)(r << (PRECISION >> 1));
 126 }
 127
 128 /* Inverse gain of circular cordic rotation in s0.31 format. */
 129 static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */
 130
 131 /* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */
 132 static const unsigned long atan_table[] = {
 133     0x1fffffff, /* +0.785398163 (or pi/4) */
 134     0x12e4051d, /* +0.463647609 */
 135     0x09fb385b, /* +0.244978663 */
 136     0x051111d4, /* +0.124354995 */
 137     0x028b0d43, /* +0.062418810 */
 138     0x0145d7e1, /* +0.031239833 */
 139     0x00a2f61e, /* +0.015623729 */
 140     0x00517c55, /* +0.007812341 */
 141     0x0028be53, /* +0.003906230 */
 142     0x00145f2e, /* +0.001953123 */
 143     0x000a2f98, /* +0.000976562 */
 144     0x000517cc, /* +0.000488281 */
 145     0x00028be6, /* +0.000244141 */
 146     0x000145f3, /* +0.000122070 */
 147     0x0000a2f9, /* +0.000061035 */
 148     0x0000517c, /* +0.000030518 */
 149     0x000028be, /* +0.000015259 */
 150     0x0000145f, /* +0.000007629 */
 151     0x00000a2f, /* +0.000003815 */
 152     0x00000517, /* +0.000001907 */
 153     0x0000028b, /* +0.000000954 */
 154     0x00000145, /* +0.000000477 */
 155     0x000000a2, /* +0.000000238 */
 156     0x00000051, /* +0.000000119 */
 157     0x00000028, /* +0.000000060 */
 158     0x00000014, /* +0.000000030 */
 159     0x0000000a, /* +0.000000015 */
 160     0x00000005, /* +0.000000007 */
 161     0x00000002, /* +0.000000004 */
 162     0x00000001, /* +0.000000002 */
 163     0x00000000, /* +0.000000001 */
 164     0x00000000, /* +0.000000000 */
 165 };
 166
 167 /*
 168  *   Below here functions do not use standard fixed precision!
 169  */
 170
 171 /**
 172  * Implements sin and cos using CORDIC rotation.
 173  *
 174  * @param phase has range from 0 to 0xffffffff, representing 0 and
 175  *        2*pi respectively.
 176  * @param cos return address for cos
 177  * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX,
 178  *         representing -1 and 1 respectively.
 179  *
 180  *        Gives at least 24 bits precision (last 2-8 bits or so are probably off)
 181  */
 182 long fsincos(unsigned long phase, int32_t *cos)
 183 {
 184     int32_t x, x1, y, y1;
 185     unsigned long z, z1;
 186     int i;
 187
 188     /* Setup initial vector */
 189     x = cordic_circular_gain;
 190     y = 0;
 191     z = phase;
 192
 193     /* The phase has to be somewhere between 0..pi for this to work right */
 194     if (z < 0xffffffff >> 2) {
 195         /* z in first quadrant, z += pi/2 to correct */
 196         x = -x;
 197         z += 0xffffffff >> 2;
 198     } else if (z < 3 * (0xffffffff >> 2)) {
 199         /* z in third quadrant, z -= pi/2 to correct */
 200         z -= 0xffffffff >> 2;
 201     } else {
 202         /* z in fourth quadrant, z -= 3pi/2 to correct */
 203         x = -x;
 204         z -= 3 * (0xffffffff >> 2);
 205     }
 206
 207     /* Each iteration adds roughly 1-bit of extra precision */
 208     for (i = 0; i < 31; i++) {
 209         x1 = x >> i;
 210         y1 = y >> i;
 211         z1 = atan_table[i];
 212
 213         /* Decided which direction to rotate vector. Pivot point is pi/2 */
 214         if (z >= 0xffffffff >> 2) {
 215             x -= y1;
 216             y += x1;
 217             z -= z1;
 218         } else {
 219             x += y1;
 220             y -= x1;
 221             z += z1;
 222         }
 223     }
 224
 225     if (cos)
 226         *cos = x;
 227
 228     return y;
 229 }