1/* @title: Fixed Point Arithmetic */
2#pragma once
3#include <types/types.h>
4
5#define FX_ONE ((fx32_32_t) (1LL << 32))
6#define FX_HALF ((fx32_32_t) (1LL << 31))
7#define FX(x) ((fx32_32_t) ((x) * 4294967296.0 + 0.5))
8#define FX_FROM_RATIO(n, d) ((fx32_32_t) (((__int128) (n) << 32) / (d)))
9
10/* Parse a decimal fixed-point string ("3", "-0.5", ".25", ...) into Q32.32.
11 * strtol-style endptr: set past the last consumed char, or to str if nothing
12 * valid was parsed (endptr may be NULL). Defined in kernel/math/fixed.c. */
13fx32_32_t fx_parse(const char *str, char **endptr);
14
15static inline fx32_32_t fx_add(fx32_32_t a, fx32_32_t b) {
16 return (fx32_32_t) (a + b);
17}
18
19static inline fx32_32_t fx_sub(fx32_32_t a, fx32_32_t b) {
20 return (fx32_32_t) (a - b);
21}
22
23static inline fx32_32_t fx_mul(fx32_32_t a, fx32_32_t b) {
24 bool neg = (a ^ b) < 0;
25 uint64_t ua = (uint64_t) (a < 0 ? -a : a);
26 uint64_t ub = (uint64_t) (b < 0 ? -b : b);
27
28 uint64_t a_hi = ua >> 32;
29 uint64_t a_lo = ua & 0xFFFFFFFFULL;
30 uint64_t b_hi = ub >> 32;
31 uint64_t b_lo = ub & 0xFFFFFFFFULL;
32
33 uint64_t p0 = a_lo * b_lo;
34 uint64_t p1 = a_lo * b_hi;
35 uint64_t p2 = a_hi * b_lo;
36 uint64_t p3 = a_hi * b_hi;
37
38 uint64_t mid = (p0 >> 32) + (p1 & 0xFFFFFFFFULL) + (p2 & 0xFFFFFFFFULL);
39 uint64_t high = p3 + (p1 >> 32) + (p2 >> 32) + (mid >> 32);
40
41 uint64_t q = (high << 32) | (mid & 0xFFFFFFFFULL);
42
43 return (fx32_32_t) (neg ? -(int64_t) q : (int64_t) q);
44}
45
46static inline fx32_32_t fx_div(fx32_32_t a, fx32_32_t b) {
47 bool neg = (a ^ b) < 0;
48 uint64_t ua = (uint64_t) (a < 0 ? -a : a);
49 uint64_t ub = (uint64_t) (b < 0 ? -b : b);
50
51 uint64_t num_hi = ua >> 32;
52 uint64_t num_lo = ua << 32;
53 uint64_t q = 0;
54
55 for (int i = 63; i >= 0; i--) {
56 uint64_t bit = num_hi >> 63;
57 num_hi = (num_hi << 1) | (num_lo >> 63);
58 num_lo <<= 1;
59
60 if (bit || num_hi >= ub) {
61 num_hi -= ub;
62 q |= (1ULL << i);
63 }
64 }
65
66 return (fx32_32_t) (neg ? -(int64_t) q : (int64_t) q);
67}
68
69static inline fx32_32_t fx_from_int(int64_t x) {
70 return x << 32;
71}
72
73static inline int64_t fx_to_int(fx32_32_t x) {
74 return x >> 32;
75}
76
77/* COMPILE TIME ONLY! */
78static inline fx32_32_t fx_from_float(double v) {
79 return FX(v);
80}
81
82static inline fx32_32_t fx_min(fx32_32_t a, fx32_32_t b) {
83 return a < b ? a : b;
84}
85
86static inline fx32_32_t fx_max(fx32_32_t a, fx32_32_t b) {
87 return a > b ? a : b;
88}
89
90static inline fx32_32_t fx_clamp(fx32_32_t x, fx32_32_t lo, fx32_32_t hi) {
91 return fx_max(a: lo, b: fx_min(a: x, b: hi));
92}
93
94static inline fx32_32_t fx_pow_i32(fx32_32_t base, int exp) {
95 fx32_32_t result = FX_ONE;
96 if (exp < 0) {
97 exp = -exp;
98 base = fx_div(FX_ONE, b: base);
99 }
100 while (exp) {
101 if (exp & 1)
102 result = fx_mul(a: result, b: base);
103 base = fx_mul(a: base, b: base);
104 exp >>= 1;
105 }
106 return result;
107}
108
109static inline fx32_32_t fx_sqrt(fx32_32_t x) {
110 if (x <= 0)
111 return 0;
112 fx32_32_t r = x;
113 for (int i = 0; i < 8; i++) {
114 r = (r + fx_div(a: x, b: (fx32_32_t) r)) >> 1;
115 }
116 return r;
117}
118
119static inline fx32_32_t fx_ceil(fx32_32_t x) {
120 if (x >= 0) {
121 return (x + FX_ONE - 1) & ~(FX_ONE - 1);
122 } else {
123 return x & ~(FX_ONE - 1);
124 }
125}
126
127static inline fx32_32_t fx_floor(fx32_32_t x) {
128 if (x >= 0) {
129 return x & ~(FX_ONE - 1);
130 } else {
131 return (x - FX_ONE + 1) & ~(FX_ONE - 1);
132 }
133}
134