1 /*
2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License as published by
4 * the Free Software Foundation; either version 2 of the License, or
5 * (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software
14 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
15 */
16
17 // Fixed-point math routines (signed 19.12)
18 // Largest positive value: 524287.999755859375
19 // Smallest positive value: 0.000244140625
20
21 #define FRAC_BITS 12
22 #define INT_MASK 0x7FFFF000 // 20 bits
23 #define FIXED_1 4096 // (1 << FRAC_BITS)
24 #define FIXED_255 1044480 // (255 << FRAC_BITS)
25 #define FIXED_HALF 2048 // (fixed_t)(0.5 * (float)(1L << FRAC_BITS) + 0.5)
26 #define FIXED_EPSILON 1
27
28 #define ROUND_FIXED_TO_INT(x) ((int)(x < 0 ? 0 : (x > FIXED_255) ? 255 : fixed_to_int(x + FIXED_HALF)))
29
30 typedef int32_t fixed_t;
31
int_to_fixed(int32_t x)32 static inline fixed_t int_to_fixed(int32_t x) {
33 return x << FRAC_BITS;
34 }
35
fixed_to_int(fixed_t x)36 static inline int32_t fixed_to_int(fixed_t x) {
37 return x >> FRAC_BITS;
38 }
39
float_to_fixed(float x)40 static inline fixed_t float_to_fixed(float x) {
41 return ((fixed_t)((x) * (float)(1L << FRAC_BITS) + 0.5));
42 }
43
fixed_to_float(fixed_t x)44 static inline float fixed_to_float(fixed_t x) {
45 return ((float)((x) / (float)(1L << FRAC_BITS)));
46 }
47
48 #if defined(__GNUC__)
49 # if defined(__arm__)
fixed_mul(fixed_t x,fixed_t y)50 static inline fixed_t fixed_mul(fixed_t x, fixed_t y) {
51 fixed_t __hi, __lo, __result;
52 __asm__ __volatile__(
53 "smull %0, %1, %3, %4\n\t"
54 "movs %0, %0, lsr %5\n\t"
55 "add %2, %0, %1, lsl %6"
56 : "=&r" (__lo), "=&r" (__hi), "=r" (__result)
57 : "%r" (x), "r" (y), "M" (FRAC_BITS), "M" (32 - (FRAC_BITS))
58 : "cc"
59 );
60 return __result;
61 }
62 # elif defined(__i386__) || defined(__x86_64__)
63 // This improves fixed-point performance about 15-20% on x86
fixed_mul(fixed_t x,fixed_t y)64 static inline fixed_t fixed_mul(fixed_t x, fixed_t y)
65 {
66 fixed_t __hi, __lo;
67 __asm__ __volatile__(
68 "imull %3\n"
69 "shrdl %4, %1, %0"
70 : "=a"(__lo), "=d"(__hi)
71 : "%a"(x), "rm"(y), "I"(FRAC_BITS)
72 : "cc"
73 );
74 return __lo;
75 }
76 # elif defined(PADRE) // Sparc ReadyNAS
fixed_mul(fixed_t x,fixed_t y)77 static inline fixed_t fixed_mul(fixed_t x, fixed_t y)
78 {
79 fixed_t __hi, __lo, __result;
80 __asm__ __volatile__(
81 " nop\n"
82 " nop\n"
83 " smul %3, %4, %0\n"
84 " mov %%y, %1\n"
85 " srl %0, %5, %0\n"
86 " sll %1, %6, %1\n"
87 " add %0, %1, %2\n"
88 : "=&r" (__lo), "=&r" (__hi), "=r" (__result)
89 : "%r" (x), "r" (y), "M" (FRAC_BITS), "M" (32 - (FRAC_BITS))
90 : "cc"
91 );
92 return __result;
93 }
94 # else // Other gcc platform
fixed_mul(fixed_t x,fixed_t y)95 static inline fixed_t fixed_mul(fixed_t x, fixed_t y) {
96 return (fixed_t)(((int64_t)x * y) >> FRAC_BITS);
97 }
98 # endif
99 #elif defined(_MSC_VER) // x86 Windows
fixed_mul(fixed_t x,fixed_t y)100 static inline fixed_t fixed_mul(fixed_t x, fixed_t y) {
101 enum {
102 fracbits = FRAC_BITS
103 };
104
105 __asm {
106 mov eax, x
107 imul y
108 shrd eax, edx, fracbits
109 }
110 // eax is returned automatically
111 }
112 #else // Other non-gcc platform
fixed_mul(fixed_t x,fixed_t y)113 static inline fixed_t fixed_mul(fixed_t x, fixed_t y) {
114 return (fixed_t)(((int64_t)x * y) >> FRAC_BITS);
115 }
116 #endif
117
118 // XXX ARM version from http://me.henri.net/fp-div.html ?
fixed_div(fixed_t x,fixed_t y)119 static inline fixed_t fixed_div(fixed_t x, fixed_t y) {
120 return (fixed_t)(((int64_t)x << FRAC_BITS) / y);
121 }
122
fixed_floor(fixed_t x)123 static inline fixed_t fixed_floor(fixed_t x) {
124 return x & INT_MASK;
125 }
126