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 __divti3(int128_t a, int128_t b) {
147 int neg = (a < 0) ^ (b < 0);
148 uint128_t ua = (a < 0) ? -(uint128_t) a : (uint128_t) a;
149 uint128_t ub = (b < 0) ? -(uint128_t) b : (uint128_t) b;
150 uint128_t q = __udivmodti4(n: ua, d: ub, rem: (uint128_t *) 0);
151 return neg ? -(int128_t) q : (int128_t) q;
152}
153
154int128_t __modti3(int128_t a, int128_t b) {
155 uint128_t ua = (a < 0) ? -(uint128_t) a : (uint128_t) a;
156 uint128_t ub = (b < 0) ? -(uint128_t) b : (uint128_t) b;
157 uint128_t r;
158 __udivmodti4(n: ua, d: ub, rem: &r);
159 return (a < 0) ? -(int128_t) r : (int128_t) r;
160}
161
162int __cmpti2(int128_t a, int128_t b) {
163 if (a < b)
164 return 0;
165 if (a == b)
166 return 1;
167 return 2;
168}
169
170int __ucmpti2(uint128_t a, uint128_t b) {
171 if (a < b)
172 return 0;
173 if (a == b)
174 return 1;
175 return 2;
176}
177
178int __clzti2(uint128_t a) {
179 u128_parts p = {.all = a};
180 if (p.s.hi)
181 return __builtin_clzll(p.s.hi);
182 return 64 + __builtin_clzll(p.s.lo);
183}
184
185int __ctzti2(uint128_t a) {
186 u128_parts p = {.all = a};
187 if (p.s.lo)
188 return __builtin_ctzll(p.s.lo);
189 return 64 + __builtin_ctzll(p.s.hi);
190}
191
192int __ffsti2(int128_t a) {
193 u128_parts p = {.all = (uint128_t) a};
194 if (p.s.lo)
195 return __builtin_ctzll(p.s.lo) + 1;
196 if (p.s.hi)
197 return __builtin_ctzll(p.s.hi) + 65;
198 return 0;
199}
200
201int __popcountti2(uint128_t a) {
202 u128_parts p = {.all = a};
203 return popcount64(x: p.s.lo) + popcount64(x: p.s.hi);
204}
205
206int __parityti2(uint128_t a) {
207 u128_parts p = {.all = a};
208 return popcount64(x: p.s.lo ^ p.s.hi) & 1;
209}
210
211int128_t __negvti2(int128_t a) {
212 if (a == INT128_MIN)
213 panic("__negvti2: 128-bit negation overflow");
214 return __negti2(a);
215}
216
217int128_t __absvti2(int128_t a) {
218 if (a == INT128_MIN)
219 panic("__absvti2: 128-bit abs overflow");
220 return a < 0 ? __negti2(a) : a;
221}
222
223int128_t __addvti3(int128_t a, int128_t b) {
224 int128_t r = (int128_t) ((uint128_t) a + (uint128_t) b);
225 if (((a ^ r) & (b ^ r)) < 0)
226 panic("__addvti3: 128-bit addition overflow");
227 return r;
228}
229
230int128_t __subvti3(int128_t a, int128_t b) {
231 int128_t r = (int128_t) ((uint128_t) a - (uint128_t) b);
232 if (((a ^ b) & (a ^ r)) < 0)
233 panic("__subvti3: 128-bit subtraction overflow");
234 return r;
235}
236
237int128_t __mulvti3(int128_t a, int128_t b) {
238 int128_t r = __multi3(a, b);
239 if (a != 0 && (r / a != b || (a == -1 && b == INT128_MIN)))
240 panic("__mulvti3: 128-bit multiplication overflow");
241 return r;
242}
243