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