From ba475b8dcf3242a134c4e6856569939ebdeed1c2 Mon Sep 17 00:00:00 2001
From: Cyrill Gorcunov <gorcunov@openvz.org>
Date: Mon, 9 Nov 2015 21:54:18 +0300
Subject: [PATCH] bitmap -- Add few helpers for bits manipulations

Grabbed from kernel. Probably worth to gather
all bits manipulators here in future.

Signed-off-by: Cyrill Gorcunov <gorcunov@openvz.org>
Signed-off-by: Pavel Emelyanov <xemul@parallels.com>
---
 Makefile.crtools |  1 +
 bitmap.c         | 54 ++++++++++++++++++++++++++++++++++++++++++++++++
 include/bitmap.h |  7 +++++++
 3 files changed, 62 insertions(+)
 create mode 100644 bitmap.c
 create mode 100644 include/bitmap.h

diff --git a/Makefile.crtools b/Makefile.crtools
index ea813e539..847b11daf 100644
--- a/Makefile.crtools
+++ b/Makefile.crtools
@@ -48,6 +48,7 @@ obj-y	+= fsnotify.o
 obj-y	+= irmap.o
 obj-y	+= signalfd.o
 obj-y	+= pstree.o
+obj-y	+= bitmap.o
 obj-y	+= protobuf.o
 obj-y	+= protobuf-desc.o
 obj-y	+= tty.o
diff --git a/bitmap.c b/bitmap.c
new file mode 100644
index 000000000..65a501e72
--- /dev/null
+++ b/bitmap.c
@@ -0,0 +1,54 @@
+#include "asm/bitsperlong.h"
+
+#define BIT_WORD(nr)		((nr) / BITS_PER_LONG)
+
+#define BITMAP_FIRST_WORD_MASK(start) (~0ul << ((start) % BITS_PER_LONG))
+
+#define BITMAP_LAST_WORD_MASK(nbits)					\
+(									\
+	((nbits) % BITS_PER_LONG) ?					\
+		(1ul << ((nbits) % BITS_PER_LONG)) - 1 : ~0ul		\
+)
+
+#define small_const_nbits(nbits)					\
+	(__builtin_constant_p(nbits) && (nbits) <= BITS_PER_LONG)
+
+void bitmap_set(unsigned long *map, int start, int nr)
+{
+	unsigned long *p = map + BIT_WORD(start);
+	const int size = start + nr;
+	int bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG);
+	unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start);
+
+	while (nr - bits_to_set >= 0) {
+		*p |= mask_to_set;
+		nr -= bits_to_set;
+		bits_to_set = BITS_PER_LONG;
+		mask_to_set = ~0UL;
+		p++;
+	}
+	if (nr) {
+		mask_to_set &= BITMAP_LAST_WORD_MASK(size);
+		*p |= mask_to_set;
+	}
+}
+
+void bitmap_clear(unsigned long *map, int start, int nr)
+{
+	unsigned long *p = map + BIT_WORD(start);
+	const int size = start + nr;
+	int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
+	unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);
+
+	while (nr - bits_to_clear >= 0) {
+		*p &= ~mask_to_clear;
+		nr -= bits_to_clear;
+		bits_to_clear = BITS_PER_LONG;
+		mask_to_clear = ~0UL;
+		p++;
+	}
+	if (nr) {
+		mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
+		*p &= ~mask_to_clear;
+	}
+}
diff --git a/include/bitmap.h b/include/bitmap.h
new file mode 100644
index 000000000..9e701b66c
--- /dev/null
+++ b/include/bitmap.h
@@ -0,0 +1,7 @@
+#ifndef __CR_BITMAP_H__
+#define __CR_BITMAP_H__
+
+extern void bitmap_set(unsigned long *map, int start, int nr);
+extern void bitmap_clear(unsigned long *map, int start, int nr);
+
+#endif /* __CR_BITMAP_H__ */