From 51767a4e8dc762e230683773b7dadb87d40da456 Mon Sep 17 00:00:00 2001
From: asterix <asterix@38d2e660-2303-0410-9eaa-f027e97ec537>
Date: Wed, 31 Aug 2011 15:29:44 +0000
Subject: [PATCH] Add search fist non zero bit for bitarray. This implemention
 is platform independent using the De Bruijin algorithm. Signed off by Lafras
 Henning <lafras@xietel.com>.

git-svn-id: https://src.develer.com/svnoss/bertos/trunk@5005 38d2e660-2303-0410-9eaa-f027e97ec537
---
 bertos/struct/bitarray.c      | 92 +++++++++++++++++++++++++++++++++++
 bertos/struct/bitarray.h      |  2 +
 bertos/struct/bitarray_test.c | 46 ++++++++++++++++++
 3 files changed, 140 insertions(+)
 create mode 100644 bertos/struct/bitarray.c

diff --git a/bertos/struct/bitarray.c b/bertos/struct/bitarray.c
new file mode 100644
index 00000000..971a3139
--- /dev/null
+++ b/bertos/struct/bitarray.c
@@ -0,0 +1,92 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction.  Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License.  This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
+ * -->
+ *
+ * \brief Bitarray module
+ *
+ * \author Daniele Basile <asterix@develer.com>
+ *
+ */
+
+#include "bitarray.h"
+
+#include <string.h>
+
+// De Bruijn constant coefficents.
+static const uint8_t DeBruijn_coefficents[32] =
+{
+  0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8,
+  31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9
+};
+
+/**
+ * Return the position of the first bit non zero in bitarray
+ *
+ * \param bitx BitArray context
+ * \param idx Starting bit
+ * \param offset Number of bits to test
+ * \return Position in bitarray when firt bit non zero occur,  -1 otherwise
+ */
+int bitarray_firstSetBit(BitArray *bitx)
+{
+	ASSERT(bitx);
+
+	uint32_t *b = (uint32_t *)bitx->array;
+	int pos =  -1;
+	int curr = 0;
+	int i = 0;
+	size_t bytes = bitx->size;
+
+	while (bytes)
+	{
+		if (bytes < 4)
+		{
+			uint32_t data = 0;
+			memcpy(&data, &b[i], bytes);
+
+			if (data == 0)
+				return -1;
+
+			return (DeBruijn_coefficents[((uint32_t)((data & -data) * 0x077CB531U)) >> 27] + curr);
+		}
+
+		if (b[i])
+		{
+			pos = DeBruijn_coefficents[((uint32_t)((b[i] & -b[i]) * 0x077CB531U)) >> 27] + curr;
+			break;
+		}
+
+		bytes -= 4;
+		curr += 32;
+		i++;
+	}
+
+	return pos;
+}
diff --git a/bertos/struct/bitarray.h b/bertos/struct/bitarray.h
index 83a0ef16..791d4676 100644
--- a/bertos/struct/bitarray.h
+++ b/bertos/struct/bitarray.h
@@ -238,6 +238,8 @@ INLINE void bitarray_dump(BitArray *bitx)
 		kprintf("..%02x [%d]\n", bitx->array[i / 8], i);
 }
 
+int bitarray_firstSetBit(BitArray *bitx);
+
 /**
  * Init a BitArray.
  *
diff --git a/bertos/struct/bitarray_test.c b/bertos/struct/bitarray_test.c
index b917d0f4..0dad3a27 100644
--- a/bertos/struct/bitarray_test.c
+++ b/bertos/struct/bitarray_test.c
@@ -45,18 +45,30 @@
 
 #define TEST1_LEN   31
 #define TEST2_LEN   17
+#define TEST3_LEN   16
+#define TEST4_LEN   23
+#define TEST5_LEN   72
 
 BITARRAY_ALLOC(test1, TEST1_LEN);
 BITARRAY_ALLOC(test2, TEST2_LEN);
+BITARRAY_ALLOC(test3, TEST3_LEN);
+BITARRAY_ALLOC(test4, TEST4_LEN);
+BITARRAY_ALLOC(test5, TEST5_LEN);
 
 BitArray bitx1;
 BitArray bitx2;
+BitArray bitx3;
+BitArray bitx4;
+BitArray bitx5;
 
 int bitarray_testSetup(void)
 {
 	kdbg_init();
 	bitarray_init(&bitx1, TEST1_LEN, test1, sizeof(test1));
 	bitarray_init(&bitx2, TEST2_LEN, test2, sizeof(test2));
+	bitarray_init(&bitx3, TEST3_LEN, test3, sizeof(test3));
+	bitarray_init(&bitx4, TEST4_LEN, test4, sizeof(test4));
+	bitarray_init(&bitx5, TEST5_LEN, test5, sizeof(test5));
 	return 0;
 }
 
@@ -117,6 +129,40 @@ int bitarray_testRun(void)
 	if (bitarray_isFull(&bitx2))
 		goto error;
 
+	kprintf("Test 3\n");
+	bitarray_set(&bitx3, 12);
+	bitarray_dump(&bitx3);
+	int pos = 0;
+	pos = bitarray_firstSetBit(&bitx3);
+	if (pos != 12)
+		goto error;
+
+	kprintf("Test 4\n");
+	bitarray_set(&bitx4, TEST4_LEN);
+	bitarray_dump(&bitx4);
+	pos = 0;
+	pos = bitarray_firstSetBit(&bitx4);
+	if (pos != 23)
+		goto error;
+
+	kprintf("Test 5\n");
+	bitarray_set(&bitx5, 71);
+	bitarray_dump(&bitx5);
+	pos = 0;
+	pos = bitarray_firstSetBit(&bitx5);
+	kprintf("pos %d\n", pos);
+	if (pos != 71)
+		goto error;
+
+	kprintf("Test 6\n");
+	bitarray_clear(&bitx5, 71);
+	bitarray_set(&bitx5, 5);
+	bitarray_dump(&bitx5);
+	pos = 0;
+	pos = bitarray_firstSetBit(&bitx5);
+	if (pos != 5)
+		goto error;
+
 	return 0;
 
 error:
-- 
2.25.1