#ident "$Id$"
/* ----------------------------------------------------------------------- *
 *   
 *   Copyright 2000-2001 H. Peter Anvin - All Rights Reserved
 *
 *   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, Inc.,
 *   59 Temple Place Ste 330, Boston MA 02111-1307, USA, version 2.1,
 *   incorporated herein by reference.
 *
 * ----------------------------------------------------------------------- */

/*
 * bitops.c
 *
 * Non-inline bitmap operations.
 */

#include <string.h>
#include <assert.h>
#include "bitops.h"

/* Finds a one bit somewhere between [start, start+count) bit indicies.
   Returns the index, or "err" if no such bit is found. */
int lpsm_find_set_bit(const void *ptr, int start, int count, int err)
{
  const bitscan_t *cp, *ecp;
  bitscan_t mask, val;
  int i, end;

  end = start+count-1;

  cp = (bitscan_t *)ptr + (start >> BITSCAN_SHIFT);

  if ( ((start^end) & ~BITSCAN_MASK) == 0 ) {
    /* All falls within a single bitscan_t quantum */
    mask = BITSCAN_1 << (start & BITSCAN_MASK);
    val = *cp;
    for ( i = start ; i <= end ; i++ ) {
      if ( val & mask )
	return i;
      mask <<= 1;
    }
    return err;
  }
  
  mask = ~BITSCAN_0 << (start & BITSCAN_MASK);
  val = *cp++;
  if ( val & mask ) {
    end = start | BITSCAN_MASK;
    mask = BITSCAN_1 << (start & BITSCAN_MASK);
    for ( i = start ; i <= end ; i++ ) {
      if ( val & mask )
	return i;
      mask <<= 1;
    }
    assert(0);			/* Internal error */
  }

  ecp = (bitscan_t *)ptr + (end >> BITSCAN_SHIFT);
  start = (start & ~BITSCAN_MASK) + BITSCAN_BITS;

  while ( cp < ecp ) {
    val = *cp++;
    if ( val ) {
      mask = BITSCAN_1;
      end = start | BITSCAN_MASK;
      for ( i = start ; i <= end ; i++ ) {
	if ( val & mask )
	  return i;
	mask <<= 1;
      }
      assert(0);		/* Internal error */
    }
    start += BITSCAN_BITS;
  }

  /* Scan final word */
  val = *cp;
  mask = BITSCAN_1;
  for ( i = start ; i <= end ; i++ ) {
    if ( val & mask )
      return i;
    mask <<= 1;
  }
  return err;			/* Nothing found */
}