[fio.git] / lib / axmap.c

/*
 * Bitmap of bitmaps, where each layer is number-of-bits-per-word smaller than
 * the previous. Hence an 'axmap', since we axe each previous layer into a
 * much smaller piece. I swear, that is why it's named like that. It has
 * nothing to do with anything remotely narcissistic.
 *
 * A set bit at layer N indicates a full word at layer N-1, and so forth. As
 * the bitmap becomes progressively more full, checking for existence
 * becomes cheaper (since fewer layers are walked, making it a lot more
 * cache friendly) and locating the next free space likewise.
 *
 * Axmaps get pretty close to optimal (1 bit per block) space usage, since
 * layers quickly diminish in size. Doing the size math is straight forward,
 * since we have log64(blocks) layers of maps. For 20000 blocks, overhead
 * is roughly 1.9%, or 1.019 bits per block. The number quickly converges
 * towards 1.0158, or 1.58% of overhead.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include "../arch/arch.h"
#include "axmap.h"
#include "../minmax.h"

#if BITS_PER_LONG == 64
#define UNIT_SHIFT		6
#elif BITS_PER_LONG == 32
#define UNIT_SHIFT		5
#else
#error "Number of arch bits unknown"
#endif

#define BLOCKS_PER_UNIT		(1U << UNIT_SHIFT)
#define BLOCKS_PER_UNIT_MASK	(BLOCKS_PER_UNIT - 1)

static const unsigned long bit_masks[] = {
	0x0000000000000000, 0x0000000000000001, 0x0000000000000003, 0x0000000000000007,
	0x000000000000000f, 0x000000000000001f, 0x000000000000003f, 0x000000000000007f,
	0x00000000000000ff, 0x00000000000001ff, 0x00000000000003ff, 0x00000000000007ff,
	0x0000000000000fff, 0x0000000000001fff, 0x0000000000003fff, 0x0000000000007fff,
	0x000000000000ffff, 0x000000000001ffff, 0x000000000003ffff, 0x000000000007ffff,
	0x00000000000fffff, 0x00000000001fffff, 0x00000000003fffff, 0x00000000007fffff,
	0x0000000000ffffff, 0x0000000001ffffff, 0x0000000003ffffff, 0x0000000007ffffff,
	0x000000000fffffff, 0x000000001fffffff, 0x000000003fffffff, 0x000000007fffffff,
	0x00000000ffffffff,
#if BITS_PER_LONG == 64
	0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff, 0x0000000fffffffff,
	0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff, 0x000000ffffffffff,
	0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff, 0x00000fffffffffff,
	0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff, 0x0000ffffffffffff,
	0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff, 0x000fffffffffffff,
	0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff, 0x00ffffffffffffff,
	0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff, 0x0fffffffffffffff,
	0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff, 0xffffffffffffffff
#endif
};

/**
 * struct axmap_level - a bitmap used to implement struct axmap
 * @level: Level index. Each map has at least one level with index zero. The
 *	higher the level index, the fewer bits a struct axmap_level contains.
 * @map_size: Number of elements of the @map array.
 * @map: A bitmap with @map_size elements.
 */
struct axmap_level {
	int level;
	unsigned long map_size;
	unsigned long *map;
};

/**
 * struct axmap - a set that can store numbers 0 .. @nr_bits - 1
 * @nr_level: Number of elements of the @levels array.
 * @levels: struct axmap_level array in which lower levels contain more bits
 *	than higher levels.
 * @nr_bits: One more than the highest value stored in the set.
 */
struct axmap {
	unsigned int nr_levels;
	struct axmap_level *levels;
	uint64_t nr_bits;
};

static inline unsigned long ulog64(unsigned long val, unsigned int log)
{
	while (log-- && val)
		val >>= UNIT_SHIFT;

	return val;
}

/* Remove all elements from the @axmap set */
void axmap_reset(struct axmap *axmap)
{
	int i;

	for (i = 0; i < axmap->nr_levels; i++) {
		struct axmap_level *al = &axmap->levels[i];

		memset(al->map, 0, al->map_size * sizeof(unsigned long));
	}
}

void axmap_free(struct axmap *axmap)
{
	unsigned int i;

	if (!axmap)
		return;

	for (i = 0; i < axmap->nr_levels; i++)
		free(axmap->levels[i].map);

	free(axmap->levels);
	free(axmap);
}

/* Allocate memory for a set that can store the numbers 0 .. @nr_bits - 1. */
struct axmap *axmap_new(unsigned long nr_bits)
{
	struct axmap *axmap;
	unsigned int i, levels;

	axmap = malloc(sizeof(*axmap));
	if (!axmap)
		return NULL;

	levels = 1;
	i = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	while (i > 1) {
		i = (i + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
		levels++;
	}

	axmap->nr_levels = levels;
	axmap->levels = calloc(axmap->nr_levels, sizeof(struct axmap_level));
	axmap->nr_bits = nr_bits;

	for (i = 0; i < axmap->nr_levels; i++) {
		struct axmap_level *al = &axmap->levels[i];

		al->level = i;
		al->map_size = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
		al->map = malloc(al->map_size * sizeof(unsigned long));
		if (!al->map)
			goto err;

		nr_bits = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	}

	axmap_reset(axmap);
	return axmap;
err:
	for (i = 0; i < axmap->nr_levels; i++)
		if (axmap->levels[i].map)
			free(axmap->levels[i].map);

	free(axmap->levels);
	free(axmap);
	return NULL;
}

/*
 * Call @func for each level, starting at level zero, until a level is found
 * for which @func returns true. Return false if none of the @func calls
 * returns true.
 */
static bool axmap_handler(struct axmap *axmap, uint64_t bit_nr,
			  bool (*func)(struct axmap_level *, unsigned long, unsigned int,
			  void *), void *data)
{
	struct axmap_level *al;
	uint64_t index = bit_nr;
	int i;

	for (i = 0; i < axmap->nr_levels; i++) {
		unsigned long offset = index >> UNIT_SHIFT;
		unsigned int bit = index & BLOCKS_PER_UNIT_MASK;

		al = &axmap->levels[i];

		if (func(al, offset, bit, data))
			return true;

		if (index)
			index >>= UNIT_SHIFT;
	}

	return false;
}

/*
 * Call @func for each level, starting at the highest level, until a level is
 * found for which @func returns true. Return false if none of the @func calls
 * returns true.
 */
static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
	bool (*func)(struct axmap_level *, unsigned long, unsigned int, void *))
{
	int i;

	for (i = axmap->nr_levels - 1; i >= 0; i--) {
		unsigned long index = ulog64(bit_nr, i);
		unsigned long offset = index >> UNIT_SHIFT;
		unsigned int bit = index & BLOCKS_PER_UNIT_MASK;

		if (func(&axmap->levels[i], offset, bit, NULL))
			return true;
	}

	return false;
}

struct axmap_set_data {
	unsigned int nr_bits;
	unsigned int set_bits;
};

/*
 * Set at most @__data->nr_bits bits in @al at offset @offset. Do not exceed
 * the boundary of the element at offset @offset. Return the number of bits
 * that have been set in @__data->set_bits if @al->level == 0.
 */
static bool axmap_set_fn(struct axmap_level *al, unsigned long offset,
			 unsigned int bit, void *__data)
{
	struct axmap_set_data *data = __data;
	unsigned long mask, overlap;
	unsigned int nr_bits;

	nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);

	mask = bit_masks[nr_bits] << bit;

	/*
	 * Mask off any potential overlap, only sets contig regions
	 */
	overlap = al->map[offset] & mask;
	if (overlap == mask) {
done:
		data->set_bits = 0;
		return true;
	}

	if (overlap) {
		const int __bit = ffz(~overlap);

		nr_bits = __bit - bit;
		if (!nr_bits)
			goto done;

		mask = bit_masks[nr_bits] << bit;
	}

	assert(mask);
	assert(!(al->map[offset] & mask));
	al->map[offset] |= mask;

	if (!al->level)
		data->set_bits = nr_bits;

	/* For the next level */
	data->nr_bits = 1;

	return al->map[offset] != -1UL;
}

/*
 * Set up to @data->nr_bits starting from @bit_nr in @axmap. Start at
 * @bit_nr. If that bit has not yet been set then set it and continue until
 * either @data->nr_bits have been set or a 1 bit is found. Store the number
 * of bits that have been set in @data->set_bits. It is guaranteed that all
 * bits that have been requested to set fit in the same unsigned long word of
 * level 0 of @axmap.
 */
static void __axmap_set(struct axmap *axmap, uint64_t bit_nr,
			 struct axmap_set_data *data)
{
	unsigned int set_bits, nr_bits = data->nr_bits;

	if (bit_nr > axmap->nr_bits)
		return;
	else if (bit_nr + nr_bits > axmap->nr_bits)
		nr_bits = axmap->nr_bits - bit_nr;

	set_bits = 0;
	while (nr_bits) {
		axmap_handler(axmap, bit_nr, axmap_set_fn, data);
		set_bits += data->set_bits;

		if (!data->set_bits ||
		    data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
			break;

		nr_bits -= data->set_bits;
		bit_nr += data->set_bits;

		data->nr_bits = nr_bits;
	}

	data->set_bits = set_bits;
}

void axmap_set(struct axmap *axmap, uint64_t bit_nr)
{
	struct axmap_set_data data = { .nr_bits = 1, };

	__axmap_set(axmap, bit_nr, &data);
}

/*
 * Set up to @nr_bits starting from @bit in @axmap. Start at @bit. If that
 * bit has not yet been set then set it and continue until either @nr_bits
 * have been set or a 1 bit is found. Return the number of bits that have been
 * set.
 */
unsigned int axmap_set_nr(struct axmap *axmap, uint64_t bit_nr,
			  unsigned int nr_bits)
{
	unsigned int set_bits = 0;

	do {
		struct axmap_set_data data = { .nr_bits = nr_bits, };
		unsigned int max_bits, this_set;

		max_bits = BLOCKS_PER_UNIT - (bit_nr & BLOCKS_PER_UNIT_MASK);
		if (nr_bits > max_bits)
			data.nr_bits = max_bits;

		this_set = data.nr_bits;
		__axmap_set(axmap, bit_nr, &data);
		set_bits += data.set_bits;
		if (data.set_bits != this_set)
			break;

		nr_bits -= data.set_bits;
		bit_nr += data.set_bits;
	} while (nr_bits);

	return set_bits;
}

static bool axmap_isset_fn(struct axmap_level *al, unsigned long offset,
			   unsigned int bit, void *unused)
{
	return (al->map[offset] & (1UL << bit)) != 0;
}

bool axmap_isset(struct axmap *axmap, uint64_t bit_nr)
{
	if (bit_nr <= axmap->nr_bits)
		return axmap_handler_topdown(axmap, bit_nr, axmap_isset_fn);

	return false;
}

/*
 * Find the first free bit that is at least as large as bit_nr.  Return
 * -1 if no free bit is found before the end of the map.
 */
static uint64_t axmap_find_first_free(struct axmap *axmap, uint64_t bit_nr)
{
	int i;
	unsigned long temp;
	unsigned int bit;
	uint64_t offset, base_index, index;
	struct axmap_level *al;

	index = 0;
	for (i = axmap->nr_levels - 1; i >= 0; i--) {
		al = &axmap->levels[i];

		/* Shift previously calculated index for next level */
		index <<= UNIT_SHIFT;

		/*
		 * Start from an index that's at least as large as the
		 * originally passed in bit number.
		 */
		base_index = bit_nr >> (UNIT_SHIFT * i);
		if (index < base_index)
			index = base_index;

		/* Get the offset and bit for this level */
		offset = index >> UNIT_SHIFT;
		bit = index & BLOCKS_PER_UNIT_MASK;

		/*
		 * If the previous level had unused bits in its last
		 * word, the offset could be bigger than the map at
		 * this level. That means no free bits exist before the
		 * end of the map, so return -1.
		 */
		if (offset >= al->map_size)
			return -1ULL;

		/* Check the first word starting with the specific bit */
		temp = ~bit_masks[bit] & ~al->map[offset];
		if (temp)
			goto found;

		/*
		 * No free bit in the first word, so iterate
		 * looking for a word with one or more free bits.
		 */
		for (offset++; offset < al->map_size; offset++) {
			temp = ~al->map[offset];
			if (temp)
				goto found;
		}

		/* Did not find a free bit */
		return -1ULL;

found:
		/* Compute the index of the free bit just found */
		index = (offset << UNIT_SHIFT) + ffz(~temp);
	}

	/* If found an unused bit in the last word of level 0, return -1 */
	if (index >= axmap->nr_bits)
		return -1ULL;

	return index;
}

/*
 * 'bit_nr' is already set. Find the next free bit after this one.
 * Return -1 if no free bits found.
 */
uint64_t axmap_next_free(struct axmap *axmap, uint64_t bit_nr)
{
	uint64_t ret;
	uint64_t next_bit = bit_nr + 1;
	unsigned long temp;
	uint64_t offset;
	unsigned int bit;

	if (bit_nr >= axmap->nr_bits)
		return -1ULL;

	/* If at the end of the map, wrap-around */
	if (next_bit == axmap->nr_bits)
		next_bit = 0;

	offset = next_bit >> UNIT_SHIFT;
	bit = next_bit & BLOCKS_PER_UNIT_MASK;

	/*
	 * As an optimization, do a quick check for a free bit
	 * in the current word at level 0. If not found, do
	 * a topdown search.
	 */
	temp = ~bit_masks[bit] & ~axmap->levels[0].map[offset];
	if (temp) {
		ret = (offset << UNIT_SHIFT) + ffz(~temp);

		/* Might have found an unused bit at level 0 */
		if (ret >= axmap->nr_bits)
			ret = -1ULL;
	} else
		ret = axmap_find_first_free(axmap, next_bit);

	/*
	 * If there are no free bits starting at next_bit and going
	 * to the end of the map, wrap around by searching again
	 * starting at bit 0.
	 */
	if (ret == -1ULL && next_bit != 0)
		ret = axmap_find_first_free(axmap, 0);
	return ret;
}
Commit	Line	Data
7ebd796f JA	1	/*
	2	* Bitmap of bitmaps, where each layer is number-of-bits-per-word smaller than
	3	* the previous. Hence an 'axmap', since we axe each previous layer into a
	4	* much smaller piece. I swear, that is why it's named like that. It has
	5	* nothing to do with anything remotely narcissistic.
	6	*
	7	* A set bit at layer N indicates a full word at layer N-1, and so forth. As
de8f6de9	8	* the bitmap becomes progressively more full, checking for existence
7ebd796f JA	9	* becomes cheaper (since fewer layers are walked, making it a lot more
	10	* cache friendly) and locating the next free space likewise.
	11	*
	12	* Axmaps get pretty close to optimal (1 bit per block) space usage, since
	13	* layers quickly diminish in size. Doing the size math is straight forward,
	14	* since we have log64(blocks) layers of maps. For 20000 blocks, overhead
	15	* is roughly 1.9%, or 1.019 bits per block. The number quickly converges
	16	* towards 1.0158, or 1.58% of overhead.
	17	*/
	18	#include <stdio.h>
	19	#include <stdlib.h>
	20	#include <string.h>
	21	#include <assert.h>
	22
	23	#include "../arch/arch.h"
	24	#include "axmap.h"
7ebd796f JA	25	#include "../minmax.h"
	26
	27	#if BITS_PER_LONG == 64
	28	#define UNIT_SHIFT 6
	29	#elif BITS_PER_LONG == 32
	30	#define UNIT_SHIFT 5
	31	#else
	32	#error "Number of arch bits unknown"
	33	#endif
	34
e6f735f0	35	#define BLOCKS_PER_UNIT (1U << UNIT_SHIFT)
7ebd796f JA	36	#define BLOCKS_PER_UNIT_MASK (BLOCKS_PER_UNIT - 1)
7ebd796f JA	37
76dd186e JA	38	static const unsigned long bit_masks[] = {
	39	0x0000000000000000, 0x0000000000000001, 0x0000000000000003, 0x0000000000000007,
	40	0x000000000000000f, 0x000000000000001f, 0x000000000000003f, 0x000000000000007f,
	41	0x00000000000000ff, 0x00000000000001ff, 0x00000000000003ff, 0x00000000000007ff,
	42	0x0000000000000fff, 0x0000000000001fff, 0x0000000000003fff, 0x0000000000007fff,
	43	0x000000000000ffff, 0x000000000001ffff, 0x000000000003ffff, 0x000000000007ffff,
	44	0x00000000000fffff, 0x00000000001fffff, 0x00000000003fffff, 0x00000000007fffff,
	45	0x0000000000ffffff, 0x0000000001ffffff, 0x0000000003ffffff, 0x0000000007ffffff,
	46	0x000000000fffffff, 0x000000001fffffff, 0x000000003fffffff, 0x000000007fffffff,
	47	0x00000000ffffffff,
	48	#if BITS_PER_LONG == 64
	49	0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff, 0x0000000fffffffff,
	50	0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff, 0x000000ffffffffff,
	51	0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff, 0x00000fffffffffff,
	52	0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff, 0x0000ffffffffffff,
	53	0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff, 0x000fffffffffffff,
	54	0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff, 0x00ffffffffffffff,
	55	0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff, 0x0fffffffffffffff,
	56	0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff, 0xffffffffffffffff
	57	#endif
	58	};
	59
c7143018 BVA	60	/**
	61	* struct axmap_level - a bitmap used to implement struct axmap
	62	* @level: Level index. Each map has at least one level with index zero. The
	63	* higher the level index, the fewer bits a struct axmap_level contains.
	64	* @map_size: Number of elements of the @map array.
	65	* @map: A bitmap with @map_size elements.
	66	*/
7ebd796f JA	67	struct axmap_level {
	68	int level;
	69	unsigned long map_size;
	70	unsigned long *map;
	71	};
	72
c7143018 BVA	73	/**
	74	* struct axmap - a set that can store numbers 0 .. @nr_bits - 1
	75	* @nr_level: Number of elements of the @levels array.
	76	* @levels: struct axmap_level array in which lower levels contain more bits
	77	* than higher levels.
	78	* @nr_bits: One more than the highest value stored in the set.
	79	*/
7ebd796f JA	80	struct axmap {
	81	unsigned int nr_levels;
	82	struct axmap_level *levels;
47d94b0b	83	uint64_t nr_bits;
7ebd796f JA	84	};
7ebd796f JA	85
76dd186e	86	static inline unsigned long ulog64(unsigned long val, unsigned int log)
7ebd796f JA	87	{
	88	while (log-- && val)
	89	val >>= UNIT_SHIFT;
	90
	91	return val;
	92	}
	93
c7143018	94	/* Remove all elements from the @axmap set */
7ebd796f JA	95	void axmap_reset(struct axmap *axmap)
	96	{
	97	int i;
	98
	99	for (i = 0; i < axmap->nr_levels; i++) {
	100	struct axmap_level *al = &axmap->levels[i];
	101
	102	memset(al->map, 0, al->map_size * sizeof(unsigned long));
	103	}
	104	}
	105
	106	void axmap_free(struct axmap *axmap)
	107	{
	108	unsigned int i;
	109
	110	if (!axmap)
	111	return;
	112
	113	for (i = 0; i < axmap->nr_levels; i++)
aded30f7	114	free(axmap->levels[i].map);
7ebd796f	115
aded30f7 JA	116	free(axmap->levels);
aded30f7 JA	117	free(axmap);
7ebd796f JA	118	}
7ebd796f JA	119
c7143018	120	/* Allocate memory for a set that can store the numbers 0 .. @nr_bits - 1. */
7ebd796f JA	121	struct axmap *axmap_new(unsigned long nr_bits)
	122	{
	123	struct axmap *axmap;
	124	unsigned int i, levels;
	125
aded30f7	126	axmap = malloc(sizeof(*axmap));
7ebd796f JA	127	if (!axmap)
	128	return NULL;
	129
	130	levels = 1;
	131	i = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	132	while (i > 1) {
	133	i = (i + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	134	levels++;
	135	}
	136
	137	axmap->nr_levels = levels;
2404b2c3	138	axmap->levels = calloc(axmap->nr_levels, sizeof(struct axmap_level));
47d94b0b	139	axmap->nr_bits = nr_bits;
7ebd796f JA	140
	141	for (i = 0; i < axmap->nr_levels; i++) {
	142	struct axmap_level *al = &axmap->levels[i];
	143
	144	al->level = i;
	145	al->map_size = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
aded30f7	146	al->map = malloc(al->map_size * sizeof(unsigned long));
7ebd796f JA	147	if (!al->map)
	148	goto err;
	149
	150	nr_bits = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	151	}
	152
	153	axmap_reset(axmap);
	154	return axmap;
	155	err:
	156	for (i = 0; i < axmap->nr_levels; i++)
	157	if (axmap->levels[i].map)
aded30f7	158	free(axmap->levels[i].map);
7ebd796f	159
aded30f7	160	free(axmap->levels);
bb1116fe	161	free(axmap);
7ebd796f JA	162	return NULL;
	163	}
	164
c7143018 BVA	165	/*
	166	* Call @func for each level, starting at level zero, until a level is found
	167	* for which @func returns true. Return false if none of the @func calls
	168	* returns true.
	169	*/
e39c0676 JA	170	static bool axmap_handler(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	171	bool (func)(struct axmap_level , unsigned long, unsigned int,
7ebd796f JA	172	void ), void data)
	173	{
	174	struct axmap_level *al;
edfca254	175	uint64_t index = bit_nr;
7ebd796f JA	176	int i;
	177
	178	for (i = 0; i < axmap->nr_levels; i++) {
7ebd796f JA	179	unsigned long offset = index >> UNIT_SHIFT;
	180	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	181
	182	al = &axmap->levels[i];
	183
	184	if (func(al, offset, bit, data))
e39c0676	185	return true;
edfca254 JA	186
	187	if (index)
	188	index >>= UNIT_SHIFT;
7ebd796f JA	189	}
7ebd796f JA	190
e39c0676	191	return false;
7ebd796f JA	192	}
7ebd796f JA	193
c7143018 BVA	194	/*
	195	* Call @func for each level, starting at the highest level, until a level is
	196	* found for which @func returns true. Return false if none of the @func calls
	197	* returns true.
	198	*/
e39c0676	199	static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
c406445a	200	bool (func)(struct axmap_level , unsigned long, unsigned int, void *))
7ebd796f	201	{
76dd186e	202	int i;
7ebd796f JA	203
7ebd796f JA	204	for (i = axmap->nr_levels - 1; i >= 0; i--) {
76dd186e	205	unsigned long index = ulog64(bit_nr, i);
7ebd796f JA	206	unsigned long offset = index >> UNIT_SHIFT;
	207	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	208
41264298	209	if (func(&axmap->levels[i], offset, bit, NULL))
e39c0676	210	return true;
7ebd796f JA	211	}
7ebd796f JA	212
e39c0676	213	return false;
7ebd796f JA	214	}
7ebd796f JA	215
7ebd796f JA	216	struct axmap_set_data {
	217	unsigned int nr_bits;
	218	unsigned int set_bits;
7ebd796f JA	219	};
7ebd796f JA	220
c7143018 BVA	221	/*
	222	* Set at most @__data->nr_bits bits in @al at offset @offset. Do not exceed
	223	* the boundary of the element at offset @offset. Return the number of bits
	224	* that have been set in @__data->set_bits if @al->level == 0.
	225	*/
e39c0676	226	static bool axmap_set_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	227	unsigned int bit, void *__data)
	228	{
	229	struct axmap_set_data *data = __data;
	230	unsigned long mask, overlap;
	231	unsigned int nr_bits;
	232
	233	nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);
	234
	235	mask = bit_masks[nr_bits] << bit;
	236
	237	/*
	238	* Mask off any potential overlap, only sets contig regions
	239	*/
	240	overlap = al->map[offset] & mask;
bd71edce JA	241	if (overlap == mask) {
	242	done:
	243	data->set_bits = 0;
e39c0676	244	return true;
bd71edce	245	}
7ebd796f	246
8cd5a2fd JA	247	if (overlap) {
8cd5a2fd JA	248	const int __bit = ffz(~overlap);
7ebd796f	249
c2e48e0e JA	250	nr_bits = __bit - bit;
c2e48e0e JA	251	if (!nr_bits)
bd71edce	252	goto done;
8cd5a2fd	253
8cd5a2fd	254	mask = bit_masks[nr_bits] << bit;
7ebd796f JA	255	}
	256
	257	assert(mask);
	258	assert(!(al->map[offset] & mask));
7ebd796f JA	259	al->map[offset] \|= mask;
	260
	261	if (!al->level)
	262	data->set_bits = nr_bits;
	263
c7143018	264	/* For the next level */
7ebd796f	265	data->nr_bits = 1;
c7143018	266
7ebd796f JA	267	return al->map[offset] != -1UL;
	268	}
	269
c7143018 BVA	270	/*
	271	* Set up to @data->nr_bits starting from @bit_nr in @axmap. Start at
	272	* @bit_nr. If that bit has not yet been set then set it and continue until
	273	* either @data->nr_bits have been set or a 1 bit is found. Store the number
	274	* of bits that have been set in @data->set_bits. It is guaranteed that all
	275	* bits that have been requested to set fit in the same unsigned long word of
	276	* level 0 of @axmap.
	277	*/
7ebd796f JA	278	static void __axmap_set(struct axmap *axmap, uint64_t bit_nr,
	279	struct axmap_set_data *data)
	280	{
	281	unsigned int set_bits, nr_bits = data->nr_bits;
	282
47d94b0b JA	283	if (bit_nr > axmap->nr_bits)
	284	return;
	285	else if (bit_nr + nr_bits > axmap->nr_bits)
	286	nr_bits = axmap->nr_bits - bit_nr;
	287
7ebd796f JA	288	set_bits = 0;
	289	while (nr_bits) {
	290	axmap_handler(axmap, bit_nr, axmap_set_fn, data);
	291	set_bits += data->set_bits;
	292
0bfdf9f1 JA	293	if (!data->set_bits \|\|
0bfdf9f1 JA	294	data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
7ebd796f JA	295	break;
	296
	297	nr_bits -= data->set_bits;
	298	bit_nr += data->set_bits;
	299
	300	data->nr_bits = nr_bits;
7ebd796f JA	301	}
	302
	303	data->set_bits = set_bits;
	304	}
	305
	306	void axmap_set(struct axmap *axmap, uint64_t bit_nr)
	307	{
	308	struct axmap_set_data data = { .nr_bits = 1, };
	309
	310	__axmap_set(axmap, bit_nr, &data);
	311	}
	312
c7143018 BVA	313	/*
	314	* Set up to @nr_bits starting from @bit in @axmap. Start at @bit. If that
	315	* bit has not yet been set then set it and continue until either @nr_bits
	316	* have been set or a 1 bit is found. Return the number of bits that have been
	317	* set.
	318	*/
e39c0676 JA	319	unsigned int axmap_set_nr(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	320	unsigned int nr_bits)
7ebd796f	321	{
0bfdf9f1 JA	322	unsigned int set_bits = 0;
	323
	324	do {
09d6bf09	325	struct axmap_set_data data = { .nr_bits = nr_bits, };
0bfdf9f1 JA	326	unsigned int max_bits, this_set;
	327
	328	max_bits = BLOCKS_PER_UNIT - (bit_nr & BLOCKS_PER_UNIT_MASK);
bd71edce	329	if (nr_bits > max_bits)
0bfdf9f1 JA	330	data.nr_bits = max_bits;
	331
	332	this_set = data.nr_bits;
	333	__axmap_set(axmap, bit_nr, &data);
	334	set_bits += data.set_bits;
	335	if (data.set_bits != this_set)
	336	break;
7ebd796f	337
0bfdf9f1 JA	338	nr_bits -= data.set_bits;
	339	bit_nr += data.set_bits;
	340	} while (nr_bits);
	341
	342	return set_bits;
7ebd796f JA	343	}
7ebd796f JA	344
e39c0676 JA	345	static bool axmap_isset_fn(struct axmap_level *al, unsigned long offset,
e39c0676 JA	346	unsigned int bit, void *unused)
7ebd796f JA	347	{
	348	return (al->map[offset] & (1UL << bit)) != 0;
	349	}
	350
e39c0676	351	bool axmap_isset(struct axmap *axmap, uint64_t bit_nr)
7ebd796f	352	{
ac569176	353	if (bit_nr <= axmap->nr_bits)
c406445a	354	return axmap_handler_topdown(axmap, bit_nr, axmap_isset_fn);
47d94b0b	355
e39c0676	356	return false;
7ebd796f JA	357	}
7ebd796f JA	358
b9304c5b MK	359	/*
	360	* Find the first free bit that is at least as large as bit_nr. Return
	361	* -1 if no free bit is found before the end of the map.
	362	*/
	363	static uint64_t axmap_find_first_free(struct axmap *axmap, uint64_t bit_nr)
7ebd796f	364	{
731ef4c7	365	int i;
b9304c5b MK	366	unsigned long temp;
	367	unsigned int bit;
	368	uint64_t offset, base_index, index;
	369	struct axmap_level *al;
7ebd796f	370
b9304c5b MK	371	index = 0;
	372	for (i = axmap->nr_levels - 1; i >= 0; i--) {
	373	al = &axmap->levels[i];
7ebd796f	374
b9304c5b MK	375	/* Shift previously calculated index for next level */
	376	index <<= UNIT_SHIFT;
	377
	378	/*
	379	* Start from an index that's at least as large as the
	380	* originally passed in bit number.
	381	*/
	382	base_index = bit_nr >> (UNIT_SHIFT * i);
	383	if (index < base_index)
	384	index = base_index;
	385
	386	/* Get the offset and bit for this level */
	387	offset = index >> UNIT_SHIFT;
	388	bit = index & BLOCKS_PER_UNIT_MASK;
	389
	390	/*
	391	* If the previous level had unused bits in its last
	392	* word, the offset could be bigger than the map at
	393	* this level. That means no free bits exist before the
	394	* end of the map, so return -1.
	395	*/
	396	if (offset >= al->map_size)
	397	return -1ULL;
	398
	399	/* Check the first word starting with the specific bit */
	400	temp = ~bit_masks[bit] & ~al->map[offset];
	401	if (temp)
	402	goto found;
	403
	404	/*
	405	* No free bit in the first word, so iterate
	406	* looking for a word with one or more free bits.
	407	*/
	408	for (offset++; offset < al->map_size; offset++) {
	409	temp = ~al->map[offset];
	410	if (temp)
	411	goto found;
7ebd796f	412	}
47d94b0b	413
b9304c5b MK	414	/* Did not find a free bit */
b9304c5b MK	415	return -1ULL;
7ebd796f	416
b9304c5b MK	417	found:
	418	/* Compute the index of the free bit just found */
	419	index = (offset << UNIT_SHIFT) + ffz(~temp);
7ebd796f JA	420	}
7ebd796f JA	421
b9304c5b MK	422	/* If found an unused bit in the last word of level 0, return -1 */
	423	if (index >= axmap->nr_bits)
	424	return -1ULL;
	425
	426	return index;
7ebd796f JA	427	}
	428
	429	/*
	430	* 'bit_nr' is already set. Find the next free bit after this one.
b9304c5b	431	* Return -1 if no free bits found.
7ebd796f JA	432	*/
	433	uint64_t axmap_next_free(struct axmap *axmap, uint64_t bit_nr)
	434	{
53737ae0	435	uint64_t ret;
b9304c5b MK	436	uint64_t next_bit = bit_nr + 1;
	437	unsigned long temp;
	438	uint64_t offset;
	439	unsigned int bit;
7ebd796f	440
b9304c5b MK	441	if (bit_nr >= axmap->nr_bits)
b9304c5b MK	442	return -1ULL;
12bde369	443
b9304c5b MK	444	/* If at the end of the map, wrap-around */
	445	if (next_bit == axmap->nr_bits)
	446	next_bit = 0;
7ebd796f	447
b9304c5b MK	448	offset = next_bit >> UNIT_SHIFT;
b9304c5b MK	449	bit = next_bit & BLOCKS_PER_UNIT_MASK;
7ebd796f	450
53737ae0	451	/*
b9304c5b MK	452	* As an optimization, do a quick check for a free bit
	453	* in the current word at level 0. If not found, do
	454	* a topdown search.
53737ae0	455	*/
b9304c5b MK	456	temp = ~bit_masks[bit] & ~axmap->levels[0].map[offset];
	457	if (temp) {
	458	ret = (offset << UNIT_SHIFT) + ffz(~temp);
53737ae0	459
b9304c5b MK	460	/* Might have found an unused bit at level 0 */
	461	if (ret >= axmap->nr_bits)
	462	ret = -1ULL;
	463	} else
	464	ret = axmap_find_first_free(axmap, next_bit);
	465
	466	/*
	467	* If there are no free bits starting at next_bit and going
	468	* to the end of the map, wrap around by searching again
	469	* starting at bit 0.
	470	*/
	471	if (ret == -1ULL && next_bit != 0)
	472	ret = axmap_find_first_free(axmap, 0);
	473	return ret;
7ebd796f	474	}