1#include <console/panic.h>
2#include <types/types.h>
3
4typedef union {
5 uint128_t all;
6 struct {
7 uint64_t lo;
8 uint64_t hi;
9 } s;
10} u128_parts;
11
12static inline void mul64_wide(uint64_t a, uint64_t b, uint64_t *hi,
13 uint64_t *lo) {
14 uint64_t a_lo = (uint32_t) a, a_hi = a >> 32;
15 uint64_t b_lo = (uint32_t) b, b_hi = b >> 32;
16
17 uint64_t p0 = a_lo * b_lo;
18 uint64_t p1 = a_lo * b_hi;
19 uint64_t p2 = a_hi * b_lo;
20 uint64_t p3 = a_hi * b_hi;
21
22 uint64_t mid = (p0 >> 32) + (uint32_t) p1 + (uint32_t) p2;
23
24 *lo = (p0 & 0xFFFFFFFFULL) | (mid << 32);
25 *hi = p3 + (p1 >> 32) + (p2 >> 32) + (mid >> 32);
26}
27
28static inline int popcount64(uint64_t x) {
29 x = x - ((x >> 1) & 0x5555555555555555ULL);
30 x = (x & 0x3333333333333333ULL) + ((x >> 2) & 0x3333333333333333ULL);
31 x = (x + (x >> 4)) & 0x0F0F0F0F0F0F0F0FULL;
32 return (int) ((x * 0x0101010101010101ULL) >> 56);
33}
34
35uint128_t __ashlti3(uint128_t a, int b) {
36 u128_parts in = {.all = a}, out;
37
38 if (b == 0)
39 return a;
40
41 if (b >= 64) {
42 out.s.lo = 0;
43 out.s.hi = in.s.lo << (b - 64);
44 } else {
45 out.s.lo = in.s.lo << b;
46 out.s.hi = (in.s.hi << b) | (in.s.lo >> (64 - b));
47 }
48 return out.all;
49}
50
51uint128_t __lshrti3(uint128_t a, int b) {
52 u128_parts in = {.all = a}, out;
53
54 if (b == 0)
55 return a;
56
57 if (b >= 64) {
58 out.s.hi = 0;
59 out.s.lo = in.s.hi >> (b - 64);
60 } else {
61 out.s.hi = in.s.hi >> b;
62 out.s.lo = (in.s.lo >> b) | (in.s.hi << (64 - b));
63 }
64 return out.all;
65}
66
67int128_t __ashrti3(int128_t a, int b) {
68 u128_parts in = {.all = (uint128_t) a}, out;
69 uint64_t fill = (uint64_t) ((int64_t) in.s.hi >> 63);
70
71 if (b == 0)
72 return a;
73
74 if (b >= 64) {
75 out.s.hi = fill;
76 out.s.lo = (uint64_t) ((int64_t) in.s.hi >> (b - 64));
77 } else {
78 out.s.lo = (in.s.lo >> b) | (in.s.hi << (64 - b));
79 out.s.hi = (uint64_t) ((int64_t) in.s.hi >> b);
80 }
81 return (int128_t) out.all;
82}
83
84int128_t __negti2(int128_t a) {
85 u128_parts in = {.all = (uint128_t) a}, out;
86 out.s.lo = ~in.s.lo + 1;
87 out.s.hi = ~in.s.hi + (out.s.lo == 0 ? 1 : 0);
88 return (int128_t) out.all;
89}
90
91int128_t __multi3(int128_t a, int128_t b) {
92 u128_parts ap = {.all = (uint128_t) a}, bp = {.all = (uint128_t) b}, r;
93 uint64_t hi, lo;
94
95 mul64_wide(a: ap.s.lo, b: bp.s.lo, hi: &hi, lo: &lo);
96 r.s.lo = lo;
97 r.s.hi = hi + ap.s.lo * bp.s.hi + ap.s.hi * bp.s.lo;
98 return (int128_t) r.all;
99}
100
101uint128_t __udivmodti4(uint128_t n, uint128_t d, uint128_t *rem) {
102 u128_parts np = {.all = n}, dp = {.all = d};
103
104 if (d == 0)
105 panic("__udivmodti4: 128-bit divide by zero");
106
107 if (np.s.hi == 0 && dp.s.hi == 0) {
108 if (rem)
109 *rem = np.s.lo % dp.s.lo;
110 return np.s.lo / dp.s.lo;
111 }
112
113 if (n < d) {
114 if (rem)
115 *rem = n;
116 return 0;
117 }
118
119 uint128_t q = 0, r = 0;
120 int msb = (np.s.hi != 0) ? (127 - __builtin_clzll(np.s.hi))
121 : (63 - __builtin_clzll(np.s.lo));
122
123 for (int i = msb; i >= 0; i--) {
124 r = (r << 1) | ((n >> i) & 1);
125 if (r >= d) {
126 r -= d;
127 q |= (uint128_t) 1 << i;
128 }
129 }
130
131 if (rem)
132 *rem = r;
133 return q;
134}
135
136uint128_t __udivti3(uint128_t a, uint128_t b) {
137 return __udivmodti4(n: a, d: b, rem: (uint128_t *) 0);
138}
139
140uint128_t __umodti3(uint128_t a, uint128_t b) {
141 uint128_t r;
142 __udivmodti4(n: a, d: b, rem: &r);
143 return r;
144}
145
146int128_t __divmodti4(int128_t a, int128_t b, int128_t *rem) {
147 int neg_q = (a < 0) ^ (b < 0);
148 int neg_r = (a < 0);
149 uint128_t ua = (a < 0) ? -(uint128_t) a : (uint128_t) a;
150 uint128_t ub = (b < 0) ? -(uint128_t) b : (uint128_t) b;
151 uint128_t ur;
152 uint128_t uq = __udivmodti4(n: ua, d: ub, rem: &ur);
153
154 if (rem)
155 *rem = neg_r ? -(int128_t) ur : (int128_t) ur;
156
157 return neg_q ? -(int128_t) uq : (int128_t) uq;
158}
159
160int128_t __divti3(int128_t a, int128_t b) {
161 return __divmodti4(a, b, rem: (int128_t *) 0);
162}
163
164int128_t __modti3(int128_t a, int128_t b) {
165 int128_t r;
166 __divmodti4(a, b, rem: &r);
167 return r;
168}
169
170int __cmpti2(int128_t a, int128_t b) {
171 if (a < b)
172 return 0;
173 if (a == b)
174 return 1;
175 return 2;
176}
177
178int __ucmpti2(uint128_t a, uint128_t b) {
179 if (a < b)
180 return 0;
181 if (a == b)
182 return 1;
183 return 2;
184}
185
186int __clzti2(uint128_t a) {
187 u128_parts p = {.all = a};
188 if (p.s.hi)
189 return __builtin_clzll(p.s.hi);
190 return 64 + __builtin_clzll(p.s.lo);
191}
192
193int __ctzti2(uint128_t a) {
194 u128_parts p = {.all = a};
195 if (p.s.lo)
196 return __builtin_ctzll(p.s.lo);
197 return 64 + __builtin_ctzll(p.s.hi);
198}
199
200int __ffsti2(int128_t a) {
201 u128_parts p = {.all = (uint128_t) a};
202 if (p.s.lo)
203 return __builtin_ctzll(p.s.lo) + 1;
204 if (p.s.hi)
205 return __builtin_ctzll(p.s.hi) + 65;
206 return 0;
207}
208
209/* GCC lowers __builtin_popcount* to these when it can't inline the expansion,
210 * so we'll need to implement these to use it */
211int __popcountsi2(uint32_t a) {
212 return popcount64(x: a);
213}
214
215int __popcountdi2(uint64_t a) {
216 return popcount64(x: a);
217}
218
219int __popcountti2(uint128_t a) {
220 u128_parts p = {.all = a};
221 return popcount64(x: p.s.lo) + popcount64(x: p.s.hi);
222}
223
224int __parityti2(uint128_t a) {
225 u128_parts p = {.all = a};
226 return popcount64(x: p.s.lo ^ p.s.hi) & 1;
227}
228
229int128_t __negvti2(int128_t a) {
230 if (a == INT128_MIN)
231 panic("__negvti2: 128-bit negation overflow");
232 return __negti2(a);
233}
234
235int128_t __absvti2(int128_t a) {
236 if (a == INT128_MIN)
237 panic("__absvti2: 128-bit abs overflow");
238 return a < 0 ? __negti2(a) : a;
239}
240
241int128_t __addvti3(int128_t a, int128_t b) {
242 int128_t r = (int128_t) ((uint128_t) a + (uint128_t) b);
243 if (((a ^ r) & (b ^ r)) < 0)
244 panic("__addvti3: 128-bit addition overflow");
245 return r;
246}
247
248int128_t __subvti3(int128_t a, int128_t b) {
249 int128_t r = (int128_t) ((uint128_t) a - (uint128_t) b);
250 if (((a ^ b) & (a ^ r)) < 0)
251 panic("__subvti3: 128-bit subtraction overflow");
252 return r;
253}
254
255int128_t __mulvti3(int128_t a, int128_t b) {
256 int128_t r = __multi3(a, b);
257 if (a != 0 && (r / a != b || (a == -1 && b == INT128_MIN)))
258 panic("__mulvti3: 128-bit multiplication overflow");
259 return r;
260}
261