Initial commit

tasks/mm0/tests/idpool_test/bit.c

@@ -0,0 +1,98 @@
+/*
+ * Bit manipulation functions.
+ *
+ * Copyright (C) 2007 Bahadir Balban
+ */
+#include "bit.h"
+#include <stdio.h>
+
+/* Emulation of ARM's CLZ (count leading zeroes) instruction */
+unsigned int __clz(unsigned int bitvector)
+{
+	unsigned int x = 0;
+	while((!(bitvector & ((unsigned)1 << 31))) && (x < 32)) {
+		bitvector <<= 1;
+		x++;
+	}
+	return x;
+}
+
+int find_and_set_first_free_bit(u32 *word, unsigned int limit)
+{
+	int success = 0;
+	int i;
+
+	for(i = 0; i < limit; i++) {
+		/* Find first unset bit */
+		if (!(word[BITWISE_GETWORD(i)] & BITWISE_GETBIT(i))) {
+			/* Set it */
+			word[BITWISE_GETWORD(i)] |= BITWISE_GETBIT(i);
+			success = 1;
+			break;
+		}
+	}
+
+	/* Return bit just set */
+	if (success)
+		return i;
+	else
+		return -1;
+}
+
+int find_and_set_first_free_contig_bits(u32 *word, unsigned int limit,
+					int nbits)
+{
+	int i = 0, first = 0, last = 0, found = 0;
+
+	/* Can't allocate more than the limit */
+	if (nbits > limit)
+		return -1;
+
+	/* This is a state machine that checks n contiguous free bits. */
+	while (i < limit) {
+		first = i;
+		last  = i;
+		while (!(word[BITWISE_GETWORD(last)] & BITWISE_GETBIT(last))) {
+			last++;
+			i++;
+			if (last == first + nbits) {
+				found = 1;
+				break;
+			}
+			if (i == limit)
+				break;
+		}
+		if (found)
+			break;
+		i++;
+	}
+
+	/* If found, set the bits */
+	if (found) {
+		for (int x = first; x < first + nbits; x++)
+			word[BITWISE_GETWORD(x)] |= BITWISE_GETBIT(x);
+		return first;
+	} else
+		return -1;
+}
+
+int check_and_clear_bit(u32 *word, int bit)
+{
+	/* Check that bit was set */
+	if (word[BITWISE_GETWORD(bit)] & BITWISE_GETBIT(bit)) {
+		word[BITWISE_GETWORD(bit)] &= ~BITWISE_GETBIT(bit);
+		return 0;
+	} else {
+		printf("Trying to clear already clear bit\n");
+		return -1;
+	}
+}
+
+int check_and_clear_contig_bits(u32 *word, int first, int nbits)
+{
+	for (int i = first; i < first + nbits; i++)
+		if (check_and_clear_bit(word, i) < 0)
+			return -1;
+	return 0;
+}
+