Add search fist non zero bit for bitarray. This implemention is platform independent...
[bertos.git] / bertos / struct / bitarray.c
1 /**
2  * \file
3  * <!--
4  * This file is part of BeRTOS.
5  *
6  * Bertos is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
19  *
20  * As a special exception, you may use this file as part of a free software
21  * library without restriction.  Specifically, if other files instantiate
22  * templates or use macros or inline functions from this file, or you compile
23  * this file and link it with other files to produce an executable, this
24  * file does not by itself cause the resulting executable to be covered by
25  * the GNU General Public License.  This exception does not however
26  * invalidate any other reasons why the executable file might be covered by
27  * the GNU General Public License.
28  *
29  * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
30  * -->
31  *
32  * \brief Bitarray module
33  *
34  * \author Daniele Basile <asterix@develer.com>
35  *
36  */
37
38 #include "bitarray.h"
39
40 #include <string.h>
41
42 // De Bruijn constant coefficents.
43 static const uint8_t DeBruijn_coefficents[32] =
44 {
45   0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8,
46   31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9
47 };
48
49 /**
50  * Return the position of the first bit non zero in bitarray
51  *
52  * \param bitx BitArray context
53  * \param idx Starting bit
54  * \param offset Number of bits to test
55  * \return Position in bitarray when firt bit non zero occur,  -1 otherwise
56  */
57 int bitarray_firstSetBit(BitArray *bitx)
58 {
59         ASSERT(bitx);
60
61         uint32_t *b = (uint32_t *)bitx->array;
62         int pos =  -1;
63         int curr = 0;
64         int i = 0;
65         size_t bytes = bitx->size;
66
67         while (bytes)
68         {
69                 if (bytes < 4)
70                 {
71                         uint32_t data = 0;
72                         memcpy(&data, &b[i], bytes);
73
74                         if (data == 0)
75                                 return -1;
76
77                         return (DeBruijn_coefficents[((uint32_t)((data & -data) * 0x077CB531U)) >> 27] + curr);
78                 }
79
80                 if (b[i])
81                 {
82                         pos = DeBruijn_coefficents[((uint32_t)((b[i] & -b[i]) * 0x077CB531U)) >> 27] + curr;
83                         break;
84                 }
85
86                 bytes -= 4;
87                 curr += 32;
88                 i++;
89         }
90
91         return pos;
92 }