178 lines
4.4 KiB
C
178 lines
4.4 KiB
C
/* Copyright (C) 2006 David Decotigny
|
|
|
|
This program 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
|
|
USA.
|
|
*/
|
|
|
|
#include <sos/macros.h>
|
|
|
|
#include "bitmap.h"
|
|
|
|
|
|
/**
|
|
* ffs: find first bit set (1 = LSb, 32 = MSb).
|
|
*
|
|
* @return 0 when none found
|
|
*
|
|
* @note: License is GPLv2, origin is Linux Kernel 2.6.14.3
|
|
* (@see include/asm-i386/bitops.h)
|
|
*/
|
|
static inline int word_ffs(sos_ui32_t x)
|
|
{
|
|
if (!x)
|
|
return 0;
|
|
|
|
__asm__("bsfl %1,%0"
|
|
:"=r" (x)
|
|
:"rm" (x));
|
|
return x+1;
|
|
}
|
|
|
|
|
|
/** Set a 32bits mask of nbits '1' starting at LSb */
|
|
static inline sos_ui32_t generate_lsb_mask(int nbits)
|
|
{
|
|
return (1 << nbits) - 1;
|
|
}
|
|
|
|
|
|
sos_size_t sos_bitmap_ffs(const void * area,
|
|
sos_size_t bit_length,
|
|
sos_size_t _start_bit_index)
|
|
{
|
|
sos_size_t start_bit_index = _start_bit_index - 1;
|
|
sos_size_t word_index = start_bit_index >> 5;
|
|
const sos_ui32_t * word = (const sos_ui32_t*)area;
|
|
sos_ui32_t mask = ~ generate_lsb_mask(start_bit_index & 31);
|
|
|
|
while (start_bit_index < bit_length)
|
|
{
|
|
sos_size_t fsb = word_ffs(word[word_index] & mask);
|
|
|
|
if (fsb > 0)
|
|
{
|
|
sos_size_t bit_index = SOS_ALIGN_INF(start_bit_index, 32) + fsb;
|
|
if (bit_index > bit_length)
|
|
return 0;
|
|
|
|
return bit_index;
|
|
}
|
|
|
|
start_bit_index = SOS_ALIGN_INF(start_bit_index, 32) + 32;
|
|
word_index ++;
|
|
mask = ~0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
sos_size_t sos_bitmap_ffc(const void * area,
|
|
sos_size_t bit_length,
|
|
sos_size_t _start_bit_index)
|
|
{
|
|
sos_size_t start_bit_index = _start_bit_index - 1;
|
|
sos_size_t word_index = start_bit_index >> 5;
|
|
const sos_ui32_t * word = (const sos_ui32_t*)area;
|
|
sos_ui32_t mask = ~ generate_lsb_mask(start_bit_index & 31);
|
|
|
|
while (start_bit_index < bit_length)
|
|
{
|
|
sos_size_t fcb = word_ffs((~ word[word_index]) & mask);
|
|
|
|
if (fcb > 0)
|
|
{
|
|
sos_size_t bit_index = SOS_ALIGN_INF(start_bit_index, 32) + fcb;
|
|
if (bit_index > bit_length)
|
|
return 0;
|
|
|
|
return bit_index;
|
|
}
|
|
|
|
start_bit_index = SOS_ALIGN_INF(start_bit_index, 32) + 32;
|
|
word_index ++;
|
|
mask = ~0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
#undef ADDR
|
|
#define ADDR (*(volatile long *) addr)
|
|
|
|
/**
|
|
* test_and_set_bit - Set a bit and return its old value
|
|
* @param nr Bit to set (starting at 0 !)
|
|
* @param addr Address to count from
|
|
*
|
|
* @note: License is GPLv2, origin is Linux Kernel 2.6.14.3
|
|
* (@see include/asm-i386/bitops.h)
|
|
*/
|
|
static inline int test_and_set_bit(int nr, volatile unsigned long * addr)
|
|
{
|
|
int oldbit;
|
|
|
|
__asm__ __volatile__(
|
|
"btsl %2,%1\n\tsbbl %0,%0"
|
|
:"=r" (oldbit),"=m" (ADDR)
|
|
:"Ir" (nr) : "memory");
|
|
return oldbit;
|
|
}
|
|
|
|
|
|
/**
|
|
* test_and_clear_bit - Clear a bit and return its old value
|
|
* @param nr Bit to clear (starting at 0 !)
|
|
* @param addr Address to count from
|
|
*
|
|
* @note: License is GPLv2, origin is Linux Kernel 2.6.14.3
|
|
* (@see include/asm-i386/bitops.h)
|
|
*/
|
|
static inline int test_and_clear_bit(int nr, volatile unsigned long * addr)
|
|
{
|
|
int oldbit;
|
|
|
|
__asm__ __volatile__(
|
|
"btrl %2,%1\n\tsbbl %0,%0"
|
|
:"=r" (oldbit),"=m" (ADDR)
|
|
:"Ir" (nr) : "memory");
|
|
return oldbit;
|
|
}
|
|
|
|
|
|
sos_bool_t sos_bitmap_test_and_set(void * area,
|
|
sos_size_t _bit_index)
|
|
{
|
|
sos_ui32_t * word = (sos_ui32_t*)area;
|
|
sos_size_t bit_index = _bit_index - 1;
|
|
sos_size_t word_index = bit_index >> 5;
|
|
|
|
bit_index &= 31;
|
|
return test_and_set_bit(bit_index, word + word_index);
|
|
}
|
|
|
|
|
|
sos_bool_t sos_bitmap_test_and_clear(void * area,
|
|
sos_size_t _bit_index)
|
|
{
|
|
sos_ui32_t * word = (sos_ui32_t*)area;
|
|
sos_size_t bit_index = _bit_index - 1;
|
|
sos_size_t word_index = bit_index >> 5;
|
|
|
|
bit_index &= 31;
|
|
return test_and_clear_bit(bit_index, word + word_index);
|
|
}
|