source: trunk/kernel/libk/bits.c @ 2

/*
 * bits.c - bits manipulation functions implementation
 *
 * Author  Ghassan Almaless (2008,2009,2010,2011,2012)
 *         Alain Greiner    (2016)
 *
 * Copyright (c) UPMC Sorbonne Universites
 *
 * This file is part of ALMOS-MKH.
 *
 * ALMOS-MKH 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; version 2.0 of the License.
 *
 * ALMOS-MKH 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 ALMOS-MKH; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <hal_types.h>
#include <bits.h>

//////////////////////////////////////////
inline void bitmap_set( bitmap_t * bitmap,
                        uint32_t   index )
{
        uint32_t  word = index / 32;
        uint32_t  bit  = index % 32;

        bitmap[word] |= ( 1 << bit );
}

////////////////////////////////////////////
inline void bitmap_clear( bitmap_t * bitmap, 
                          uint32_t   index )
{
        uint32_t  word = index / 32;
        uint32_t  bit  = index % 32;

        bitmap[word] &= ~( 1 << bit );
}

//////////////////////////////////////////////
inline bool_t bitmap_state( bitmap_t * bitmap, 
                            uint32_t   index )
{
        uint32_t  word = index / 32;
        uint32_t  bit  = index % 32;

        return (bitmap[word] & ( 1 << bit )) && 1;
}

//////////////////////////////////////////
void bitmap_set_range( bitmap_t * bitmap,
                       uint32_t   index, 
                       uint32_t   len )
{
        uint32_t val;
        uint32_t word = index / 32;
        uint32_t bit  = index % 32;
  
        while((len > 0))
        {
                if((len + bit) >= 32)
                {
                        val = (bit == 0) ? 0xFFFFFFFF : (1 << (32 - bit)) - 1;
                        bitmap[word] |= (val << bit);
                        word++;
                        len -= (32 - bit);
                        bit = 0;
                }
                else
                {
                        bitmap[word] |= (((1 << len ) - 1) << bit);
                        break;
                }
        }
}

///////////////////////////////////////////
void bitmap_clear_range( bitmap_t * bitmap,
                         uint32_t   index, 
                         uint32_t   len )
{
    uint32_t val;
        uint32_t word = index / 32;
        uint32_t bit  = index % 32;

        while((len > 0))
        {
                if((len + bit) >= 32)
                {
                        val = (bit == 0) ? 0xFFFFFFFF : (1 << (32 - bit)) - 1;
                        bitmap[word] &= ~(val << bit);
                        word++;
                        len -= (32 - bit);
                        bit = 0;
                }
                else
                {
                    bitmap[word] &= ~(((1 << len ) - 1) << bit);
                        break;
                }
        }
}

///////////////////////////////////////
uint32_t bitmap_ffs2( bitmap_t * bitmap,
                      uint32_t   index,
                      uint32_t   size )
{
        uint32_t word = index / 32;  
        uint32_t bit  = index % 32; 
 
        if(bit != 0)
        {
                for(; bit < 32; bit++)
                {
                        if(bitmap[word] & (1 << bit)) return (word*32 + bit);
                }
                word++;
        }

        for(; word < size/32; word++)
        {
                if(bitmap[word] != 0)
                {
                        for(bit = 0; bit < 32; bit++)
                        {
                                if(bitmap[word] & (1 << bit)) return (word*32 + bit);
                        }
                }
        }
        return 0xFFFFFFFF;
}

///////////////////////////////////////
uint32_t bitmap_ffc2( bitmap_t * bitmap,
                      uint32_t   index,
                      uint32_t   size )
{
        uint32_t word = index / 32;
        uint32_t bit  = index % 32;
 
        if(bit != 0)
        {
                for(; bit < 32; bit++)
                {
                        if((bitmap[word] & (1 << bit)) == 0) return (word*32 + bit);
                }
                word++;
        }

        for(; word < size/32; word++)
        {
                if(bitmap[word] != 0xFFFFFFFF)
                {
                        for(bit = 0; bit < 32; bit++)
                        {
                                if((bitmap[word] & (1 << bit)) == 0) return (word*32 + bit);
                        }
                }
        }
        return 0xFFFFFFFF;
}

//////////////////////////////////////
uint32_t bitmap_ffs( bitmap_t * bitmap,
                     uint32_t   size )
{
        uint32_t word;
        uint32_t bit;
 
        for(word = 0 ; word < size/32 ; word++)
        {
                if(bitmap[word] != 0)
                {
                        for(bit = 0 ; bit < 32 ; bit++)
                        {
                                if( bitmap[word] & (1 << bit) ) return (word*32 + bit);
                        }
                }
        }
        return 0xFFFFFFFF;
}

//////////////////////////////////////
uint32_t bitmap_ffc( bitmap_t * bitmap, 
                     uint32_t   size )
{
        uint32_t word;
        uint32_t bit;
 
        for(word = 0 ; word < size/32 ; word++)
        {
                if(bitmap[word] != 0)
                {
                        for(bit = 0 ; bit < 32 ; bit++)
                        {
                                if( (bitmap[word] & (1 << bit)) == 0 ) return (word*32 + bit);
                        }
                }
        }
        return 0xFFFFFFFF;
}