bitmap: Implement bitmap_ffs()

[ccan] / ccan / bitmap / bitmap.c

diff --git a/ccan/bitmap/bitmap.c b/ccan/bitmap/bitmap.c
index a6058117985f8259aee36592993b4fd0e9ab3fce..d812af6a2344f47283f2dd89f358a398cf1cea17 100644 (file)
--- a/ccan/bitmap/bitmap.c
+++ b/ccan/bitmap/bitmap.c
@@ -58,3 +58,68 @@ void bitmap_fill_range(bitmap *bitmap, unsigned long n, unsigned long m)
        if (m > am)
                BITMAP_WORD(bitmap, m) |= bitmap_bswap(tailmask);
 }
+
+static int bitmap_clz(bitmap_word w)
+{
+#if HAVE_BUILTIN_CLZL
+       return __builtin_clzl(w);
+#else
+       int lz = 0;
+       bitmap_word mask = 1UL << (BITMAP_WORD_BITS - 1);
+
+       while (!(w & mask)) {
+               lz++;
+               mask >>= 1;
+       }
+
+       return lz;
+#endif
+}
+
+unsigned long bitmap_ffs(const bitmap *bitmap,
+                        unsigned long n, unsigned long m)
+{
+       unsigned long an = BIT_ALIGN_UP(n);
+       unsigned long am = BIT_ALIGN_DOWN(m);
+       bitmap_word headmask = -1ULL >> (n % BITMAP_WORD_BITS);
+       bitmap_word tailmask = ~(-1ULL >> (m % BITMAP_WORD_BITS));
+
+       assert(m >= n);
+
+       if (am < an) {
+               bitmap_word w = bitmap_bswap(BITMAP_WORD(bitmap, n));
+
+               w &= (headmask & tailmask);
+
+               return w ? am + bitmap_clz(w) : m;
+       }
+
+       if (an > n) {
+               bitmap_word w = bitmap_bswap(BITMAP_WORD(bitmap, n));
+
+               w &= headmask;
+
+               if (w)
+                       return BIT_ALIGN_DOWN(n) + bitmap_clz(w);
+       }
+
+       while (an < am) {
+               bitmap_word w = bitmap_bswap(BITMAP_WORD(bitmap, an));
+
+               if (w)
+                       return an + bitmap_clz(w);
+
+               an += BITMAP_WORD_BITS;
+       }
+
+       if (m > am) {
+               bitmap_word w = bitmap_bswap(BITMAP_WORD(bitmap, m));
+
+               w &= tailmask;
+
+               if (w)
+                       return am + bitmap_clz(w);
+       }
+
+       return m;
+}