apps/codecs/libwma/wmafixed.c

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