xref: /openbmc/qemu/util/bitmap.c (revision cedb70ea)
1 /*
2  * Bitmap Module
3  *
4  * Stolen from linux/src/lib/bitmap.c
5  *
6  * Copyright (C) 2010 Corentin Chary
7  *
8  * This source code is licensed under the GNU General Public License,
9  * Version 2.
10  */
11 
12 #include "qemu/osdep.h"
13 #include "qemu/bitops.h"
14 #include "qemu/bitmap.h"
15 #include "qemu/atomic.h"
16 
17 /*
18  * bitmaps provide an array of bits, implemented using an
19  * array of unsigned longs.  The number of valid bits in a
20  * given bitmap does _not_ need to be an exact multiple of
21  * BITS_PER_LONG.
22  *
23  * The possible unused bits in the last, partially used word
24  * of a bitmap are 'don't care'.  The implementation makes
25  * no particular effort to keep them zero.  It ensures that
26  * their value will not affect the results of any operation.
27  * The bitmap operations that return Boolean (bitmap_empty,
28  * for example) or scalar (bitmap_weight, for example) results
29  * carefully filter out these unused bits from impacting their
30  * results.
31  *
32  * These operations actually hold to a slightly stronger rule:
33  * if you don't input any bitmaps to these ops that have some
34  * unused bits set, then they won't output any set unused bits
35  * in output bitmaps.
36  *
37  * The byte ordering of bitmaps is more natural on little
38  * endian architectures.
39  */
40 
slow_bitmap_empty(const unsigned long * bitmap,long bits)41 int slow_bitmap_empty(const unsigned long *bitmap, long bits)
42 {
43     long k, lim = bits/BITS_PER_LONG;
44 
45     for (k = 0; k < lim; ++k) {
46         if (bitmap[k]) {
47             return 0;
48         }
49     }
50     if (bits % BITS_PER_LONG) {
51         if (bitmap[k] & BITMAP_LAST_WORD_MASK(bits)) {
52             return 0;
53         }
54     }
55 
56     return 1;
57 }
58 
slow_bitmap_full(const unsigned long * bitmap,long bits)59 int slow_bitmap_full(const unsigned long *bitmap, long bits)
60 {
61     long k, lim = bits/BITS_PER_LONG;
62 
63     for (k = 0; k < lim; ++k) {
64         if (~bitmap[k]) {
65             return 0;
66         }
67     }
68 
69     if (bits % BITS_PER_LONG) {
70         if (~bitmap[k] & BITMAP_LAST_WORD_MASK(bits)) {
71             return 0;
72         }
73     }
74 
75     return 1;
76 }
77 
slow_bitmap_equal(const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)78 int slow_bitmap_equal(const unsigned long *bitmap1,
79                       const unsigned long *bitmap2, long bits)
80 {
81     long k, lim = bits/BITS_PER_LONG;
82 
83     for (k = 0; k < lim; ++k) {
84         if (bitmap1[k] != bitmap2[k]) {
85             return 0;
86         }
87     }
88 
89     if (bits % BITS_PER_LONG) {
90         if ((bitmap1[k] ^ bitmap2[k]) & BITMAP_LAST_WORD_MASK(bits)) {
91             return 0;
92         }
93     }
94 
95     return 1;
96 }
97 
slow_bitmap_complement(unsigned long * dst,const unsigned long * src,long bits)98 void slow_bitmap_complement(unsigned long *dst, const unsigned long *src,
99                             long bits)
100 {
101     long k, lim = bits/BITS_PER_LONG;
102 
103     for (k = 0; k < lim; ++k) {
104         dst[k] = ~src[k];
105     }
106 
107     if (bits % BITS_PER_LONG) {
108         dst[k] = ~src[k] & BITMAP_LAST_WORD_MASK(bits);
109     }
110 }
111 
slow_bitmap_and(unsigned long * dst,const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)112 int slow_bitmap_and(unsigned long *dst, const unsigned long *bitmap1,
113                     const unsigned long *bitmap2, long bits)
114 {
115     long k;
116     long nr = BITS_TO_LONGS(bits);
117     unsigned long result = 0;
118 
119     for (k = 0; k < nr; k++) {
120         result |= (dst[k] = bitmap1[k] & bitmap2[k]);
121     }
122     return result != 0;
123 }
124 
slow_bitmap_or(unsigned long * dst,const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)125 void slow_bitmap_or(unsigned long *dst, const unsigned long *bitmap1,
126                     const unsigned long *bitmap2, long bits)
127 {
128     long k;
129     long nr = BITS_TO_LONGS(bits);
130 
131     for (k = 0; k < nr; k++) {
132         dst[k] = bitmap1[k] | bitmap2[k];
133     }
134 }
135 
slow_bitmap_xor(unsigned long * dst,const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)136 void slow_bitmap_xor(unsigned long *dst, const unsigned long *bitmap1,
137                      const unsigned long *bitmap2, long bits)
138 {
139     long k;
140     long nr = BITS_TO_LONGS(bits);
141 
142     for (k = 0; k < nr; k++) {
143         dst[k] = bitmap1[k] ^ bitmap2[k];
144     }
145 }
146 
slow_bitmap_andnot(unsigned long * dst,const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)147 int slow_bitmap_andnot(unsigned long *dst, const unsigned long *bitmap1,
148                        const unsigned long *bitmap2, long bits)
149 {
150     long k;
151     long nr = BITS_TO_LONGS(bits);
152     unsigned long result = 0;
153 
154     for (k = 0; k < nr; k++) {
155         result |= (dst[k] = bitmap1[k] & ~bitmap2[k]);
156     }
157     return result != 0;
158 }
159 
bitmap_set(unsigned long * map,long start,long nr)160 void bitmap_set(unsigned long *map, long start, long nr)
161 {
162     unsigned long *p = map + BIT_WORD(start);
163     const long size = start + nr;
164     int bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG);
165     unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start);
166 
167     assert(start >= 0 && nr >= 0);
168 
169     while (nr - bits_to_set >= 0) {
170         *p |= mask_to_set;
171         nr -= bits_to_set;
172         bits_to_set = BITS_PER_LONG;
173         mask_to_set = ~0UL;
174         p++;
175     }
176     if (nr) {
177         mask_to_set &= BITMAP_LAST_WORD_MASK(size);
178         *p |= mask_to_set;
179     }
180 }
181 
bitmap_set_atomic(unsigned long * map,long start,long nr)182 void bitmap_set_atomic(unsigned long *map, long start, long nr)
183 {
184     unsigned long *p = map + BIT_WORD(start);
185     const long size = start + nr;
186     int bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG);
187     unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start);
188 
189     assert(start >= 0 && nr >= 0);
190 
191     /* First word */
192     if (nr - bits_to_set > 0) {
193         qatomic_or(p, mask_to_set);
194         nr -= bits_to_set;
195         bits_to_set = BITS_PER_LONG;
196         mask_to_set = ~0UL;
197         p++;
198     }
199 
200     /* Full words */
201     if (bits_to_set == BITS_PER_LONG) {
202         while (nr >= BITS_PER_LONG) {
203             *p = ~0UL;
204             nr -= BITS_PER_LONG;
205             p++;
206         }
207     }
208 
209     /* Last word */
210     if (nr) {
211         mask_to_set &= BITMAP_LAST_WORD_MASK(size);
212         qatomic_or(p, mask_to_set);
213     } else {
214         /* If we avoided the full barrier in qatomic_or(), issue a
215          * barrier to account for the assignments in the while loop.
216          */
217         smp_mb();
218     }
219 }
220 
bitmap_clear(unsigned long * map,long start,long nr)221 void bitmap_clear(unsigned long *map, long start, long nr)
222 {
223     unsigned long *p = map + BIT_WORD(start);
224     const long size = start + nr;
225     int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
226     unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);
227 
228     assert(start >= 0 && nr >= 0);
229 
230     while (nr - bits_to_clear >= 0) {
231         *p &= ~mask_to_clear;
232         nr -= bits_to_clear;
233         bits_to_clear = BITS_PER_LONG;
234         mask_to_clear = ~0UL;
235         p++;
236     }
237     if (nr) {
238         mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
239         *p &= ~mask_to_clear;
240     }
241 }
242 
bitmap_test_and_clear(unsigned long * map,long start,long nr)243 bool bitmap_test_and_clear(unsigned long *map, long start, long nr)
244 {
245     unsigned long *p = map + BIT_WORD(start);
246     const long size = start + nr;
247     int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
248     unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);
249     bool dirty = false;
250 
251     assert(start >= 0 && nr >= 0);
252 
253     /* First word */
254     if (nr - bits_to_clear > 0) {
255         if ((*p) & mask_to_clear) {
256             dirty = true;
257         }
258         *p &= ~mask_to_clear;
259         nr -= bits_to_clear;
260         bits_to_clear = BITS_PER_LONG;
261         p++;
262     }
263 
264     /* Full words */
265     if (bits_to_clear == BITS_PER_LONG) {
266         while (nr >= BITS_PER_LONG) {
267             if (*p) {
268                 dirty = true;
269                 *p = 0;
270             }
271             nr -= BITS_PER_LONG;
272             p++;
273         }
274     }
275 
276     /* Last word */
277     if (nr) {
278         mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
279         if ((*p) & mask_to_clear) {
280             dirty = true;
281         }
282         *p &= ~mask_to_clear;
283     }
284 
285     return dirty;
286 }
287 
bitmap_test_and_clear_atomic(unsigned long * map,long start,long nr)288 bool bitmap_test_and_clear_atomic(unsigned long *map, long start, long nr)
289 {
290     unsigned long *p = map + BIT_WORD(start);
291     const long size = start + nr;
292     int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
293     unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);
294     unsigned long dirty = 0;
295     unsigned long old_bits;
296 
297     assert(start >= 0 && nr >= 0);
298 
299     /* First word */
300     if (nr - bits_to_clear > 0) {
301         old_bits = qatomic_fetch_and(p, ~mask_to_clear);
302         dirty |= old_bits & mask_to_clear;
303         nr -= bits_to_clear;
304         bits_to_clear = BITS_PER_LONG;
305         mask_to_clear = ~0UL;
306         p++;
307     }
308 
309     /* Full words */
310     if (bits_to_clear == BITS_PER_LONG) {
311         while (nr >= BITS_PER_LONG) {
312             if (*p) {
313                 old_bits = qatomic_xchg(p, 0);
314                 dirty |= old_bits;
315             }
316             nr -= BITS_PER_LONG;
317             p++;
318         }
319     }
320 
321     /* Last word */
322     if (nr) {
323         mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
324         old_bits = qatomic_fetch_and(p, ~mask_to_clear);
325         dirty |= old_bits & mask_to_clear;
326     } else {
327         if (!dirty) {
328             smp_mb();
329         }
330     }
331 
332     return dirty != 0;
333 }
334 
bitmap_copy_and_clear_atomic(unsigned long * dst,unsigned long * src,long nr)335 void bitmap_copy_and_clear_atomic(unsigned long *dst, unsigned long *src,
336                                   long nr)
337 {
338     while (nr > 0) {
339         *dst = qatomic_xchg(src, 0);
340         dst++;
341         src++;
342         nr -= BITS_PER_LONG;
343     }
344 }
345 
346 #define ALIGN_MASK(x,mask)      (((x)+(mask))&~(mask))
347 
348 /**
349  * bitmap_find_next_zero_area - find a contiguous aligned zero area
350  * @map: The address to base the search on
351  * @size: The bitmap size in bits
352  * @start: The bitnumber to start searching at
353  * @nr: The number of zeroed bits we're looking for
354  * @align_mask: Alignment mask for zero area
355  *
356  * The @align_mask should be one less than a power of 2; the effect is that
357  * the bit offset of all zero areas this function finds is multiples of that
358  * power of 2. A @align_mask of 0 means no alignment is required.
359  */
bitmap_find_next_zero_area(unsigned long * map,unsigned long size,unsigned long start,unsigned long nr,unsigned long align_mask)360 unsigned long bitmap_find_next_zero_area(unsigned long *map,
361                                          unsigned long size,
362                                          unsigned long start,
363                                          unsigned long nr,
364                                          unsigned long align_mask)
365 {
366     unsigned long index, end, i;
367 again:
368     index = find_next_zero_bit(map, size, start);
369 
370     /* Align allocation */
371     index = ALIGN_MASK(index, align_mask);
372 
373     end = index + nr;
374     if (end > size) {
375         return end;
376     }
377     i = find_next_bit(map, end, index);
378     if (i < end) {
379         start = i + 1;
380         goto again;
381     }
382     return index;
383 }
384 
slow_bitmap_intersects(const unsigned long * bitmap1,const unsigned long * bitmap2,long bits)385 int slow_bitmap_intersects(const unsigned long *bitmap1,
386                            const unsigned long *bitmap2, long bits)
387 {
388     long k, lim = bits/BITS_PER_LONG;
389 
390     for (k = 0; k < lim; ++k) {
391         if (bitmap1[k] & bitmap2[k]) {
392             return 1;
393         }
394     }
395 
396     if (bits % BITS_PER_LONG) {
397         if ((bitmap1[k] & bitmap2[k]) & BITMAP_LAST_WORD_MASK(bits)) {
398             return 1;
399         }
400     }
401     return 0;
402 }
403 
slow_bitmap_count_one(const unsigned long * bitmap,long nbits)404 long slow_bitmap_count_one(const unsigned long *bitmap, long nbits)
405 {
406     long k, lim = nbits / BITS_PER_LONG, result = 0;
407 
408     for (k = 0; k < lim; k++) {
409         result += ctpopl(bitmap[k]);
410     }
411 
412     if (nbits % BITS_PER_LONG) {
413         result += ctpopl(bitmap[k] & BITMAP_LAST_WORD_MASK(nbits));
414     }
415 
416     return result;
417 }
418 
bitmap_to_from_le(unsigned long * dst,const unsigned long * src,long nbits)419 static void bitmap_to_from_le(unsigned long *dst,
420                               const unsigned long *src, long nbits)
421 {
422     long len = BITS_TO_LONGS(nbits);
423 
424 #if HOST_BIG_ENDIAN
425     long index;
426 
427     for (index = 0; index < len; index++) {
428 # if HOST_LONG_BITS == 64
429         dst[index] = bswap64(src[index]);
430 # else
431         dst[index] = bswap32(src[index]);
432 # endif
433     }
434 #else
435     memcpy(dst, src, len * sizeof(unsigned long));
436 #endif
437 }
438 
bitmap_from_le(unsigned long * dst,const unsigned long * src,long nbits)439 void bitmap_from_le(unsigned long *dst, const unsigned long *src,
440                     long nbits)
441 {
442     bitmap_to_from_le(dst, src, nbits);
443 }
444 
bitmap_to_le(unsigned long * dst,const unsigned long * src,long nbits)445 void bitmap_to_le(unsigned long *dst, const unsigned long *src,
446                   long nbits)
447 {
448     bitmap_to_from_le(dst, src, nbits);
449 }
450 
451 /*
452  * Copy "src" bitmap with a positive offset and put it into the "dst"
453  * bitmap.  The caller needs to make sure the bitmap size of "src"
454  * is bigger than (shift + nbits).
455  */
bitmap_copy_with_src_offset(unsigned long * dst,const unsigned long * src,unsigned long shift,unsigned long nbits)456 void bitmap_copy_with_src_offset(unsigned long *dst, const unsigned long *src,
457                                  unsigned long shift, unsigned long nbits)
458 {
459     unsigned long left_mask, right_mask, last_mask;
460 
461     /* Proper shift src pointer to the first word to copy from */
462     src += BIT_WORD(shift);
463     shift %= BITS_PER_LONG;
464 
465     if (!shift) {
466         /* Fast path */
467         bitmap_copy(dst, src, nbits);
468         return;
469     }
470 
471     right_mask = (1ul << shift) - 1;
472     left_mask = ~right_mask;
473 
474     while (nbits >= BITS_PER_LONG) {
475         *dst = (*src & left_mask) >> shift;
476         *dst |= (src[1] & right_mask) << (BITS_PER_LONG - shift);
477         dst++;
478         src++;
479         nbits -= BITS_PER_LONG;
480     }
481 
482     if (nbits > BITS_PER_LONG - shift) {
483         *dst = (*src & left_mask) >> shift;
484         nbits -= BITS_PER_LONG - shift;
485         last_mask = (1ul << nbits) - 1;
486         *dst |= (src[1] & last_mask) << (BITS_PER_LONG - shift);
487     } else if (nbits) {
488         last_mask = (1ul << nbits) - 1;
489         *dst = (*src >> shift) & last_mask;
490     }
491 }
492 
493 /*
494  * Copy "src" bitmap into the "dst" bitmap with an offset in the
495  * "dst".  The caller needs to make sure the bitmap size of "dst" is
496  * bigger than (shift + nbits).
497  */
bitmap_copy_with_dst_offset(unsigned long * dst,const unsigned long * src,unsigned long shift,unsigned long nbits)498 void bitmap_copy_with_dst_offset(unsigned long *dst, const unsigned long *src,
499                                  unsigned long shift, unsigned long nbits)
500 {
501     unsigned long left_mask, right_mask, last_mask;
502 
503     /* Proper shift dst pointer to the first word to copy from */
504     dst += BIT_WORD(shift);
505     shift %= BITS_PER_LONG;
506 
507     if (!shift) {
508         /* Fast path */
509         bitmap_copy(dst, src, nbits);
510         return;
511     }
512 
513     right_mask = (1ul << (BITS_PER_LONG - shift)) - 1;
514     left_mask = ~right_mask;
515 
516     *dst &= (1ul << shift) - 1;
517     while (nbits >= BITS_PER_LONG) {
518         *dst |= (*src & right_mask) << shift;
519         dst[1] = (*src & left_mask) >> (BITS_PER_LONG - shift);
520         dst++;
521         src++;
522         nbits -= BITS_PER_LONG;
523     }
524 
525     if (nbits > BITS_PER_LONG - shift) {
526         *dst |= (*src & right_mask) << shift;
527         nbits -= BITS_PER_LONG - shift;
528         last_mask = ((1ul << nbits) - 1) << (BITS_PER_LONG - shift);
529         dst[1] = (*src & last_mask) >> (BITS_PER_LONG - shift);
530     } else if (nbits) {
531         last_mask = (1ul << nbits) - 1;
532         *dst |= (*src & last_mask) << shift;
533     }
534 }
535