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