1 // © 2021 Qualcomm Innovation Center, Inc. All rights reserved.
2 //
3 // SPDX-License-Identifier: BSD-3-Clause
4
5 #include <assert.h>
6 #include <hyptypes.h>
7
8 #include <atomic.h>
9 #include <bitmap.h>
10 #include <compiler.h>
11 #include <util.h>
12
13 #define BITMAP_SET_BIT(x) ((register_t)1U << (((x) % BITMAP_WORD_BITS)))
14 #define BITMAP_WORD(x) ((x) / BITMAP_WORD_BITS)
15 #define BITMAP_SIZE_ASSERT(bitmap, bit) \
16 assert((index_t)(compiler_sizeof_object(bitmap) / \
17 sizeof(register_t)) > BITMAP_WORD(bit))
18
19 bool
bitmap_isset(const register_t * bitmap,index_t bit)20 bitmap_isset(const register_t *bitmap, index_t bit)
21 {
22 BITMAP_SIZE_ASSERT(bitmap, bit);
23
24 index_t i = BITMAP_WORD(bit);
25
26 return (bitmap[i] & BITMAP_SET_BIT(bit)) != 0U;
27 }
28
29 void
bitmap_set(register_t * bitmap,index_t bit)30 bitmap_set(register_t *bitmap, index_t bit)
31 {
32 BITMAP_SIZE_ASSERT(bitmap, bit);
33
34 index_t i = BITMAP_WORD(bit);
35
36 bitmap[i] |= BITMAP_SET_BIT(bit);
37 }
38
39 void
bitmap_clear(register_t * bitmap,index_t bit)40 bitmap_clear(register_t *bitmap, index_t bit)
41 {
42 BITMAP_SIZE_ASSERT(bitmap, bit);
43
44 index_t i = BITMAP_WORD(bit);
45
46 bitmap[i] &= ~BITMAP_SET_BIT(bit);
47 }
48
49 register_t
bitmap_extract(const register_t * bitmap,index_t bit,index_t width)50 bitmap_extract(const register_t *bitmap, index_t bit, index_t width)
51 {
52 BITMAP_SIZE_ASSERT(bitmap, bit + width - 1U);
53 assert((width <= BITMAP_WORD_BITS) &&
54 (BITMAP_WORD(bit) == BITMAP_WORD(bit + width - 1U)));
55
56 index_t i = BITMAP_WORD(bit);
57
58 return (bitmap[i] >> (bit % BITMAP_WORD_BITS)) & util_mask(width);
59 }
60
61 void
bitmap_insert(register_t * bitmap,index_t bit,index_t width,register_t value)62 bitmap_insert(register_t *bitmap, index_t bit, index_t width, register_t value)
63 {
64 BITMAP_SIZE_ASSERT(bitmap, bit + width - 1U);
65 assert((width <= BITMAP_WORD_BITS) &&
66 (BITMAP_WORD(bit) == BITMAP_WORD(bit + width - 1U)));
67
68 index_t i = BITMAP_WORD(bit);
69
70 bitmap[i] &= ~(util_mask(width) << (bit % BITMAP_WORD_BITS));
71 bitmap[i] |= (value & util_mask(width)) << (bit % BITMAP_WORD_BITS);
72 }
73
74 bool
bitmap_ffs(const register_t * bitmap,index_t num_bits,index_t * bit)75 bitmap_ffs(const register_t *bitmap, index_t num_bits, index_t *bit)
76 {
77 assert(num_bits > 0U);
78 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
79
80 bool result = false;
81 BITMAP_FOREACH_SET_BEGIN(i, bitmap, num_bits)
82 result = true;
83 *bit = i;
84 break;
85 BITMAP_FOREACH_SET_END
86
87 return result;
88 }
89
90 bool
bitmap_ffc(const register_t * bitmap,index_t num_bits,index_t * bit)91 bitmap_ffc(const register_t *bitmap, index_t num_bits, index_t *bit)
92 {
93 assert(num_bits > 0U);
94 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
95
96 bool result = false;
97 BITMAP_FOREACH_CLEAR_BEGIN(i, bitmap, num_bits)
98 result = true;
99 *bit = i;
100 break;
101 BITMAP_FOREACH_CLEAR_END
102
103 return result;
104 }
105
106 bool
bitmap_empty(const register_t * bitmap,index_t num_bits)107 bitmap_empty(const register_t *bitmap, index_t num_bits)
108 {
109 assert(num_bits > 0U);
110 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
111
112 index_t i;
113 bool result = true;
114
115 for (i = 0U; i < BITMAP_WORD(num_bits); i++) {
116 if (bitmap[i] != 0U) {
117 result = false;
118 break;
119 }
120 }
121
122 if ((i + 1U) == BITMAP_NUM_WORDS(num_bits)) {
123 if ((bitmap[i] & (BITMAP_SET_BIT(num_bits) - 1U)) != 0U) {
124 result = false;
125 }
126 }
127
128 return result;
129 }
130
131 bool
bitmap_full(const register_t * bitmap,index_t num_bits)132 bitmap_full(const register_t *bitmap, index_t num_bits)
133 {
134 assert(num_bits > 0U);
135 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
136
137 index_t i;
138 bool result = true;
139
140 for (i = 0U; i < BITMAP_WORD(num_bits); i++) {
141 if (~bitmap[i] != 0U) {
142 result = false;
143 break;
144 }
145 }
146
147 if ((i + 1U) == BITMAP_NUM_WORDS(num_bits)) {
148 if ((~bitmap[i] & (BITMAP_SET_BIT(num_bits) - 1U)) != 0U) {
149 result = false;
150 }
151 }
152
153 return result;
154 }
155
156 bool
bitmap_atomic_isset(const _Atomic register_t * bitmap,index_t bit,memory_order order)157 bitmap_atomic_isset(const _Atomic register_t *bitmap, index_t bit,
158 memory_order order)
159 {
160 BITMAP_SIZE_ASSERT(bitmap, bit);
161
162 index_t i = BITMAP_WORD(bit);
163
164 return (atomic_load_explicit(&bitmap[i], order) &
165 BITMAP_SET_BIT(bit)) != 0U;
166 }
167
168 bool
bitmap_atomic_test_and_set(_Atomic register_t * bitmap,index_t bit,memory_order order)169 bitmap_atomic_test_and_set(_Atomic register_t *bitmap, index_t bit,
170 memory_order order)
171 {
172 BITMAP_SIZE_ASSERT(bitmap, bit);
173
174 index_t i = BITMAP_WORD(bit);
175 register_t old = atomic_fetch_or_explicit(&bitmap[i],
176 BITMAP_SET_BIT(bit), order);
177
178 return (old & BITMAP_SET_BIT(bit)) != 0U;
179 }
180
181 bool
bitmap_atomic_test_and_clear(_Atomic register_t * bitmap,index_t bit,memory_order order)182 bitmap_atomic_test_and_clear(_Atomic register_t *bitmap, index_t bit,
183 memory_order order)
184 {
185 BITMAP_SIZE_ASSERT(bitmap, bit);
186
187 index_t i = BITMAP_WORD(bit);
188 register_t old = atomic_fetch_and_explicit(&bitmap[i],
189 ~BITMAP_SET_BIT(bit), order);
190
191 return (old & BITMAP_SET_BIT(bit)) != 0U;
192 }
193
194 bool
bitmap_atomic_ffs(const _Atomic register_t * bitmap,index_t num_bits,index_t * bit)195 bitmap_atomic_ffs(const _Atomic register_t *bitmap, index_t num_bits,
196 index_t *bit)
197 {
198 assert(num_bits > 0U);
199 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
200
201 bool result = false;
202 BITMAP_ATOMIC_FOREACH_SET_BEGIN(i, bitmap, num_bits)
203 result = true;
204 *bit = i;
205 break;
206 BITMAP_ATOMIC_FOREACH_SET_END
207
208 return result;
209 }
210
211 bool
bitmap_atomic_ffc(const _Atomic register_t * bitmap,index_t num_bits,index_t * bit)212 bitmap_atomic_ffc(const _Atomic register_t *bitmap, index_t num_bits,
213 index_t *bit)
214 {
215 assert(num_bits > 0U);
216 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
217
218 bool result = false;
219 BITMAP_ATOMIC_FOREACH_CLEAR_BEGIN(i, bitmap, num_bits)
220 result = true;
221 *bit = i;
222 break;
223 BITMAP_ATOMIC_FOREACH_CLEAR_END
224
225 return result;
226 }
227
228 bool
bitmap_atomic_empty(const _Atomic register_t * bitmap,index_t num_bits)229 bitmap_atomic_empty(const _Atomic register_t *bitmap, index_t num_bits)
230 {
231 assert(num_bits > 0U);
232 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
233
234 index_t i;
235 bool result = true;
236
237 for (i = 0U; i < BITMAP_WORD(num_bits); i++) {
238 if (atomic_load_relaxed(&bitmap[i]) != 0U) {
239 result = false;
240 break;
241 }
242 }
243
244 if ((i + 1U) == BITMAP_NUM_WORDS(num_bits)) {
245 if ((atomic_load_relaxed(&bitmap[i]) &
246 (BITMAP_SET_BIT(num_bits) - 1U)) != 0U) {
247 result = false;
248 }
249 }
250
251 return result;
252 }
253
254 bool
bitmap_atomic_full(const _Atomic register_t * bitmap,index_t num_bits)255 bitmap_atomic_full(const _Atomic register_t *bitmap, index_t num_bits)
256 {
257 assert(num_bits > 0U);
258 BITMAP_SIZE_ASSERT(bitmap, num_bits - 1U);
259
260 index_t i;
261 bool result = true;
262
263 for (i = 0U; i < BITMAP_WORD(num_bits); i++) {
264 if (~atomic_load_relaxed(&bitmap[i]) != 0U) {
265 result = false;
266 break;
267 }
268 }
269
270 if ((i + 1U) == BITMAP_NUM_WORDS(num_bits)) {
271 if ((~atomic_load_relaxed(&bitmap[i]) &
272 (BITMAP_SET_BIT(num_bits) - 1U)) != 0U) {
273 result = false;
274 }
275 }
276
277 return result;
278 }
279
280 register_t
bitmap_atomic_extract(const _Atomic register_t * bitmap,index_t bit,index_t width,memory_order order)281 bitmap_atomic_extract(const _Atomic register_t *bitmap, index_t bit,
282 index_t width, memory_order order)
283 {
284 BITMAP_SIZE_ASSERT(bitmap, bit + width - 1U);
285 assert((width <= BITMAP_WORD_BITS) &&
286 (BITMAP_WORD(bit) == BITMAP_WORD(bit + width - 1U)));
287
288 index_t i = BITMAP_WORD(bit);
289
290 return (atomic_load_explicit(&bitmap[i], order) >>
291 (bit % BITMAP_WORD_BITS)) &
292 util_mask(width);
293 }
294
295 void
bitmap_atomic_insert(_Atomic register_t * bitmap,index_t bit,index_t width,register_t value,memory_order order)296 bitmap_atomic_insert(_Atomic register_t *bitmap, index_t bit, index_t width,
297 register_t value, memory_order order)
298 {
299 BITMAP_SIZE_ASSERT(bitmap, bit + width - 1U);
300 assert((width <= BITMAP_WORD_BITS) &&
301 (BITMAP_WORD(bit) == BITMAP_WORD(bit + width - 1U)));
302
303 index_t i = BITMAP_WORD(bit);
304
305 memory_order load_order =
306 (order == memory_order_release) ? memory_order_relaxed
307 : (order == memory_order_acq_rel) ? memory_order_acquire
308 : order;
309
310 register_t old_word = atomic_load_explicit(&bitmap[i], load_order),
311 new_word;
312 do {
313 new_word = (old_word &
314 ~(util_mask(width) << (bit % BITMAP_WORD_BITS))) |
315 ((value & util_mask(width))
316 << (bit % BITMAP_WORD_BITS));
317 } while (!atomic_compare_exchange_weak_explicit(
318 &bitmap[i], &old_word, new_word, order, load_order));
319 }
320