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

/*
 * 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>

////////////////////////////////////
void bitmap_init( bitmap_t * bitmap,
                  uint32_t   len )
{
    uint32_t word;
    uint32_t nwords = BITMAP_SIZE( len );
    for( word = 0 ; word < nwords ; word++ )
    {
        bitmap[word] = 0;
    }
}  // end bitmap_init()

//////////////////////////////////////////
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 )) != 0;
}

//////////////////////////////////////////
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;
                }
        }
}  // bitmap_set_range()

///////////////////////////////////////////
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;
                }
        }
}  // bitmap_clear_range()

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

        max_word = ( (size-1) >>5 ) + 1;

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

        return -1;

}  // bitmap_ffs2()

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

        max_word = ( (size-1) >>5 ) + 1;

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

        return -1;

}  // bitmap_ffc2()

//////////////////////////////////////
uint32_t bitmap_ffs( bitmap_t * bitmap,
                     uint32_t   size )
{
    uint32_t max_word;
        uint32_t word;
        uint32_t bit;

    if( size )
    {
        max_word = ( (size-1) >>5 ) + 1;

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

        return -1;

}  // bitmap_ffs()

//////////////////////////////////////
uint32_t bitmap_ffc( bitmap_t * bitmap, 
                     uint32_t   size )
{
    uint32_t max_word;
        uint32_t word;
        uint32_t bit;
 
    if( size )
    {
        max_word = ( (size-1) >>5 ) + 1;

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

        return -1;

}  // bitmap_ffc()