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