/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser 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.
 *
 * (c) Copyright 2006,2007,2008 Jari OS Core Team <http://jarios.org>
 * (c) Copyright 2008 Dmitry Gromada <gromada82@gmail.com>
 * (c) Copyright 2010 Alexander Vdolainen <vdo@askele.com>
 *
 *
 * Implements bitwise operations with multiword bitmaps
 */

#include <stdint.h>
#include <sys/types.h>
#include <stdlib.h>
#include <string.h>
#include <tdata/bitwise.h>

/* operation equivalent to division modulo number of power 2 */
#define MOD_POW2(var, pow) ((var) & ((pow) - 1))

int init_bitmap(bitmap_t *bitmap, int nbits)
{
  int ret = 0;
  int size = nbits >> TYPE_LONG_SHIFT;

  if (nbits < 0)
    return -1;

  if (size << TYPE_LONG_SHIFT != nbits)
     size++;

  bitmap->map = (unsigned long*)malloc(size * sizeof(unsigned long));
  if (bitmap->map == NULL)
    ret = -1;
  else
     bitmap->nwords = size;

  memset(bitmap->map, 0, size * sizeof(unsigned long));

  return ret;
}


void free_bitmap(bitmap_t *bitmap)
{
  if (bitmap->map) {
    free(bitmap->map);
    bitmap->map = NULL;
  }
  bitmap->nwords = 0;
}


void bit_set_multi(bitmap_t *bitmap, int bitno)
{
  int i = bitno >> TYPE_LONG_SHIFT;

  return bit_set( bitmap->map + i, MOD_POW2(bitno, BITS_PER_LONG) );
}

int bit_test_and_set_multi(bitmap_t *bitmap, int bitno)
{
  int i = bitno >> TYPE_LONG_SHIFT;

  return bit_test_and_set( bitmap->map + i, MOD_POW2(bitno, BITS_PER_LONG) );
}

int bit_test_and_reset_multi(bitmap_t *bitmap, int bitno)
{
  int i = bitno >> TYPE_LONG_SHIFT;

  return bit_test_and_reset( bitmap->map + i, MOD_POW2(bitno, BITS_PER_LONG) );
}

void bit_clear_multi(bitmap_t *bitmap, int bitno)
{
  int i = bitno >> TYPE_LONG_SHIFT;

  return bit_clear( bitmap->map + i, MOD_POW2(bitno, BITS_PER_LONG) );
}

int bit_find_msf_multi(bitmap_t *bitmap, int mbit)
{
  int i, r;
  unsigned long word;

  if (mbit < 0)
    return -1;
  else if (mbit >= bitmap->nwords * BITS_PER_LONG)
    mbit = bitmap->nwords * BITS_PER_LONG - 1;

  i = mbit >> TYPE_LONG_SHIFT;
  word = bitmap->map[i] & (~0UL >> ( BITS_PER_LONG - 1 - MOD_POW2(mbit, BITS_PER_LONG) ));
  r = bit_find_msf(word);

  while ((r == -1) && (--i >= 0)) {
    if (bitmap->map[i])
      r = bit_find_msf(bitmap->map[i]);
  }

  r = (r == -1) ? r : (i * BITS_PER_LONG + r);

  return r;
}

int bit_find_lsf_multi(bitmap_t *bitmap, int lbit)
{
  int i, r;
  unsigned long word;

  if (lbit >= bitmap->nwords * BITS_PER_LONG)
    return -1;
  else if (lbit < 0)
    lbit = 0;

  i = lbit >> TYPE_LONG_SHIFT;
  word = bitmap->map[i] & (~0UL << MOD_POW2(lbit, BITS_PER_LONG));
  r = bit_find_lsf(word);

  while ((r == -1) && (++i < bitmap->nwords)) {
    if (bitmap->map[i])
      r = bit_find_lsf(bitmap->map[i]);
  }

  r = (r == -1) ? r : (i * BITS_PER_LONG + r);

  return r;
}

int bit_test_multi(bitmap_t *bitmap, int bitno)
{
  int i = bitno >> TYPE_LONG_SHIFT;

  return bit_test( bitmap->map + i, MOD_POW2(bitno, BITS_PER_LONG) );
}

void bitrange_set_multi(bitmap_t *bitmap, int start_bit, int end_bit)
{
  int i1, i2;
  unsigned long mask1, mask2, *p = bitmap->map;

  if (start_bit > end_bit)
    return;

  i1 = start_bit >> TYPE_LONG_SHIFT;
  mask1 = ~0UL << MOD_POW2(start_bit, BITS_PER_LONG);

  i2 = end_bit >> TYPE_LONG_SHIFT;
  mask2 = ~0UL >> (BITS_PER_LONG - 1 - MOD_POW2(end_bit, BITS_PER_LONG));

  if (i1 == i2)
    p[i1] |= mask1 & mask2;
  else {
    p[i1++] |= mask1;

    if (i2 - i1 > 0)
      memset( p + i1, 0xFF, (i2 - i1) * sizeof(unsigned long) );

    p[i2] |= mask2;
  }
}

void bitrange_clear_multi(bitmap_t *bitmap, int start_bit, int end_bit)
{
  int          i1, i2;
  unsigned long mask1, mask2, *p = bitmap->map;

  if (start_bit > end_bit)
    return;

  i1 = start_bit >> TYPE_LONG_SHIFT;
  mask1 = ( 1UL << (start_bit & MOD_POW2(start_bit, BITS_PER_LONG)) ) - 1;

  i2 = end_bit >> TYPE_LONG_SHIFT;
  mask2 = ~0UL << (MOD_POW2(end_bit, BITS_PER_LONG) + 1);

  if (i1 == i2)
    p[i1] &= mask1 | mask2;
  else {
    p[i1++] &= mask1;

    if (i2 - i1 > 0)
      memset(p + i1, 0, (i2 - i1) * sizeof(unsigned long));

    p[i2] &= mask2;
  }
}