[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)

#define firstfree_valid(b)	((b)->first_free != (uint64_t) -1)

struct axmap_level {
	int level;
	unsigned long map_size;
	unsigned long *map;
};

struct axmap {
	unsigned int nr_levels;
	struct axmap_level *levels;
	uint64_t first_free;
	uint64_t nr_bits;
};

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

	return val;
}

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

	axmap->first_free = 0;
}

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

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 = malloc(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;
}

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;
	int i;

	for (i = 0; i < axmap->nr_levels; i++) {
		unsigned long index = ulog64(bit_nr, 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;
	}

	return false;
}

static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
	bool (*func)(struct axmap_level *, unsigned long, unsigned int, void *),
	void *data)
{
	struct axmap_level *al;
	int i, level = axmap->nr_levels;

	for (i = axmap->nr_levels - 1; i >= 0; i--) {
		unsigned long index = ulog64(bit_nr, --level);
		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;
	}

	return false;
}

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

	al->map[offset] &= ~(1UL << bit);
	return false;
}

void axmap_clear(struct axmap *axmap, uint64_t bit_nr)
{
	axmap_handler(axmap, bit_nr, axmap_clear_fn, NULL);

	if (bit_nr < axmap->first_free)
		axmap->first_free = bit_nr;
}

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

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

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)
		return true;

	while (overlap) {
		unsigned long clear_mask = ~(1UL << ffz(~overlap));

		mask &= clear_mask;
		overlap &= clear_mask;
		nr_bits--;
	}

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

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

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

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 (axmap->first_free >= bit_nr &&
	    axmap->first_free < bit_nr + data->nr_bits)
		axmap->first_free = -1ULL;

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

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 (max_bits < nr_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, NULL);

	return false;
}

static uint64_t axmap_find_first_free(struct axmap *axmap, unsigned int level,
				       uint64_t index)
{
	uint64_t ret = -1ULL;
	unsigned long j;
	int i;

	/*
	 * Start at the bottom, then converge towards first free bit at the top
	 */
	for (i = level; i >= 0; i--) {
		struct axmap_level *al = &axmap->levels[i];

		/*
		 * Clear 'ret', this is a bug condition.
		 */
		if (index >= al->map_size) {
			ret = -1ULL;
			break;
		}

		for (j = index; j < al->map_size; j++) {
			if (al->map[j] == -1UL)
				continue;

			/*
			 * First free bit here is our index into the first
			 * free bit at the next higher level
			 */
			ret = index = (j << UNIT_SHIFT) + ffz(al->map[j]);
			break;
		}
	}

	if (ret < axmap->nr_bits)
		return ret;

	return (uint64_t) -1ULL;
}

static uint64_t axmap_first_free(struct axmap *axmap)
{
	if (!firstfree_valid(axmap))
		axmap->first_free = axmap_find_first_free(axmap, axmap->nr_levels - 1, 0);

	return axmap->first_free;
}

struct axmap_next_free_data {
	unsigned int level;
	unsigned long offset;
	uint64_t bit;
};

static bool axmap_next_free_fn(struct axmap_level *al, unsigned long offset,
			       unsigned int bit, void *__data)
{
	struct axmap_next_free_data *data = __data;
	uint64_t mask = ~bit_masks[(data->bit + 1) & BLOCKS_PER_UNIT_MASK];

	if (!(mask & ~al->map[offset]))
		return false;

	if (al->map[offset] != -1UL) {
		data->level = al->level;
		data->offset = offset;
		return true;
	}

	data->bit = (data->bit + BLOCKS_PER_UNIT - 1) / BLOCKS_PER_UNIT;
	return false;
}

/*
 * 'bit_nr' is already set. Find the next free bit after this one.
 */
uint64_t axmap_next_free(struct axmap *axmap, uint64_t bit_nr)
{
	struct axmap_next_free_data data = { .level = -1U, .bit = bit_nr, };
	uint64_t ret;

	if (firstfree_valid(axmap) && bit_nr < axmap->first_free)
		return axmap->first_free;

	if (!axmap_handler(axmap, bit_nr, axmap_next_free_fn, &data))
		return axmap_first_free(axmap);

	assert(data.level != -1U);

	/*
	 * In the rare case that the map is unaligned, we might end up
	 * finding an offset that's beyond the valid end. For that case,
	 * find the first free one, the map is practically full.
	 */
	ret = axmap_find_first_free(axmap, data.level, data.offset);
	if (ret != -1ULL)
		return ret;

	return axmap_first_free(axmap);
}
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)
	37
	38	#define firstfree_valid(b) ((b)->first_free != (uint64_t) -1)
	39
	40	struct axmap_level {
	41	int level;
	42	unsigned long map_size;
	43	unsigned long *map;
	44	};
	45
	46	struct axmap {
	47	unsigned int nr_levels;
	48	struct axmap_level *levels;
	49	uint64_t first_free;
47d94b0b	50	uint64_t nr_bits;
7ebd796f JA	51	};
	52
	53	static unsigned long ulog64(unsigned long val, unsigned int log)
	54	{
	55	while (log-- && val)
	56	val >>= UNIT_SHIFT;
	57
	58	return val;
	59	}
	60
	61	void axmap_reset(struct axmap *axmap)
	62	{
	63	int i;
	64
	65	for (i = 0; i < axmap->nr_levels; i++) {
	66	struct axmap_level *al = &axmap->levels[i];
	67
	68	memset(al->map, 0, al->map_size * sizeof(unsigned long));
	69	}
17f0fd39 JA	70
17f0fd39 JA	71	axmap->first_free = 0;
7ebd796f JA	72	}
	73
	74	void axmap_free(struct axmap *axmap)
	75	{
	76	unsigned int i;
	77
	78	if (!axmap)
	79	return;
	80
	81	for (i = 0; i < axmap->nr_levels; i++)
aded30f7	82	free(axmap->levels[i].map);
7ebd796f	83
aded30f7 JA	84	free(axmap->levels);
aded30f7 JA	85	free(axmap);
7ebd796f JA	86	}
	87
	88	struct axmap *axmap_new(unsigned long nr_bits)
	89	{
	90	struct axmap *axmap;
	91	unsigned int i, levels;
	92
aded30f7	93	axmap = malloc(sizeof(*axmap));
7ebd796f JA	94	if (!axmap)
	95	return NULL;
	96
	97	levels = 1;
	98	i = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	99	while (i > 1) {
	100	i = (i + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	101	levels++;
	102	}
	103
	104	axmap->nr_levels = levels;
aded30f7	105	axmap->levels = malloc(axmap->nr_levels * sizeof(struct axmap_level));
47d94b0b	106	axmap->nr_bits = nr_bits;
7ebd796f JA	107
	108	for (i = 0; i < axmap->nr_levels; i++) {
	109	struct axmap_level *al = &axmap->levels[i];
	110
	111	al->level = i;
	112	al->map_size = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
aded30f7	113	al->map = malloc(al->map_size * sizeof(unsigned long));
7ebd796f JA	114	if (!al->map)
	115	goto err;
	116
	117	nr_bits = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	118	}
	119
	120	axmap_reset(axmap);
	121	return axmap;
	122	err:
	123	for (i = 0; i < axmap->nr_levels; i++)
	124	if (axmap->levels[i].map)
aded30f7	125	free(axmap->levels[i].map);
7ebd796f	126
aded30f7	127	free(axmap->levels);
bb1116fe	128	free(axmap);
7ebd796f JA	129	return NULL;
	130	}
	131
e39c0676 JA	132	static bool axmap_handler(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	133	bool (func)(struct axmap_level , unsigned long, unsigned int,
7ebd796f JA	134	void ), void data)
	135	{
	136	struct axmap_level *al;
	137	int i;
	138
	139	for (i = 0; i < axmap->nr_levels; i++) {
	140	unsigned long index = ulog64(bit_nr, i);
	141	unsigned long offset = index >> UNIT_SHIFT;
	142	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	143
	144	al = &axmap->levels[i];
	145
	146	if (func(al, offset, bit, data))
e39c0676	147	return true;
7ebd796f JA	148	}
7ebd796f JA	149
e39c0676	150	return false;
7ebd796f JA	151	}
7ebd796f JA	152
e39c0676 JA	153	static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	154	bool (func)(struct axmap_level , unsigned long, unsigned int, void *),
7ebd796f JA	155	void *data)
	156	{
	157	struct axmap_level *al;
	158	int i, level = axmap->nr_levels;
	159
	160	for (i = axmap->nr_levels - 1; i >= 0; i--) {
	161	unsigned long index = ulog64(bit_nr, --level);
	162	unsigned long offset = index >> UNIT_SHIFT;
	163	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	164
	165	al = &axmap->levels[i];
	166
	167	if (func(al, offset, bit, data))
e39c0676	168	return true;
7ebd796f JA	169	}
7ebd796f JA	170
e39c0676	171	return false;
7ebd796f JA	172	}
7ebd796f JA	173
e39c0676	174	static bool axmap_clear_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	175	unsigned int bit, void *unused)
	176	{
	177	if (!(al->map[offset] & (1UL << bit)))
e39c0676	178	return true;
7ebd796f JA	179
7ebd796f JA	180	al->map[offset] &= ~(1UL << bit);
e39c0676	181	return false;
7ebd796f JA	182	}
	183
	184	void axmap_clear(struct axmap *axmap, uint64_t bit_nr)
	185	{
	186	axmap_handler(axmap, bit_nr, axmap_clear_fn, NULL);
2b2fa7f5 JA	187
	188	if (bit_nr < axmap->first_free)
	189	axmap->first_free = bit_nr;
7ebd796f JA	190	}
	191
	192	struct axmap_set_data {
	193	unsigned int nr_bits;
	194	unsigned int set_bits;
7ebd796f JA	195	};
7ebd796f JA	196
2b2fa7f5	197	static const unsigned long bit_masks[] = {
7ebd796f JA	198	0x0000000000000000, 0x0000000000000001, 0x0000000000000003, 0x0000000000000007,
	199	0x000000000000000f, 0x000000000000001f, 0x000000000000003f, 0x000000000000007f,
	200	0x00000000000000ff, 0x00000000000001ff, 0x00000000000003ff, 0x00000000000007ff,
	201	0x0000000000000fff, 0x0000000000001fff, 0x0000000000003fff, 0x0000000000007fff,
	202	0x000000000000ffff, 0x000000000001ffff, 0x000000000003ffff, 0x000000000007ffff,
	203	0x00000000000fffff, 0x00000000001fffff, 0x00000000003fffff, 0x00000000007fffff,
	204	0x0000000000ffffff, 0x0000000001ffffff, 0x0000000003ffffff, 0x0000000007ffffff,
	205	0x000000000fffffff, 0x000000001fffffff, 0x000000003fffffff, 0x000000007fffffff,
	206	0x00000000ffffffff,
	207	#if BITS_PER_LONG == 64
	208	0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff, 0x0000000fffffffff,
	209	0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff, 0x000000ffffffffff,
	210	0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff, 0x00000fffffffffff,
	211	0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff, 0x0000ffffffffffff,
	212	0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff, 0x000fffffffffffff,
	213	0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff, 0x00ffffffffffffff,
	214	0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff, 0x0fffffffffffffff,
	215	0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff, 0xffffffffffffffff
	216	#endif
	217	};
	218
e39c0676	219	static bool axmap_set_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	220	unsigned int bit, void *__data)
	221	{
	222	struct axmap_set_data *data = __data;
	223	unsigned long mask, overlap;
	224	unsigned int nr_bits;
	225
	226	nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);
	227
	228	mask = bit_masks[nr_bits] << bit;
	229
	230	/*
	231	* Mask off any potential overlap, only sets contig regions
	232	*/
	233	overlap = al->map[offset] & mask;
09d6bf09	234	if (overlap == mask)
e39c0676	235	return true;
7ebd796f JA	236
	237	while (overlap) {
	238	unsigned long clear_mask = ~(1UL << ffz(~overlap));
	239
	240	mask &= clear_mask;
	241	overlap &= clear_mask;
	242	nr_bits--;
	243	}
	244
	245	assert(mask);
	246	assert(!(al->map[offset] & mask));
	247
	248	al->map[offset] \|= mask;
	249
	250	if (!al->level)
	251	data->set_bits = nr_bits;
	252
	253	data->nr_bits = 1;
	254	return al->map[offset] != -1UL;
	255	}
	256
	257	static void __axmap_set(struct axmap *axmap, uint64_t bit_nr,
	258	struct axmap_set_data *data)
	259	{
	260	unsigned int set_bits, nr_bits = data->nr_bits;
	261
	262	if (axmap->first_free >= bit_nr &&
	263	axmap->first_free < bit_nr + data->nr_bits)
	264	axmap->first_free = -1ULL;
	265
47d94b0b JA	266	if (bit_nr > axmap->nr_bits)
	267	return;
	268	else if (bit_nr + nr_bits > axmap->nr_bits)
	269	nr_bits = axmap->nr_bits - bit_nr;
	270
7ebd796f JA	271	set_bits = 0;
	272	while (nr_bits) {
	273	axmap_handler(axmap, bit_nr, axmap_set_fn, data);
	274	set_bits += data->set_bits;
	275
0bfdf9f1 JA	276	if (!data->set_bits \|\|
0bfdf9f1 JA	277	data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
7ebd796f JA	278	break;
	279
	280	nr_bits -= data->set_bits;
	281	bit_nr += data->set_bits;
	282
	283	data->nr_bits = nr_bits;
7ebd796f JA	284	}
	285
	286	data->set_bits = set_bits;
	287	}
	288
	289	void axmap_set(struct axmap *axmap, uint64_t bit_nr)
	290	{
	291	struct axmap_set_data data = { .nr_bits = 1, };
	292
	293	__axmap_set(axmap, bit_nr, &data);
	294	}
	295
e39c0676 JA	296	unsigned int axmap_set_nr(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	297	unsigned int nr_bits)
7ebd796f	298	{
0bfdf9f1 JA	299	unsigned int set_bits = 0;
	300
	301	do {
09d6bf09	302	struct axmap_set_data data = { .nr_bits = nr_bits, };
0bfdf9f1 JA	303	unsigned int max_bits, this_set;
	304
	305	max_bits = BLOCKS_PER_UNIT - (bit_nr & BLOCKS_PER_UNIT_MASK);
	306	if (max_bits < nr_bits)
	307	data.nr_bits = max_bits;
	308
	309	this_set = data.nr_bits;
	310	__axmap_set(axmap, bit_nr, &data);
	311	set_bits += data.set_bits;
	312	if (data.set_bits != this_set)
	313	break;
7ebd796f	314
0bfdf9f1 JA	315	nr_bits -= data.set_bits;
	316	bit_nr += data.set_bits;
	317	} while (nr_bits);
	318
	319	return set_bits;
7ebd796f JA	320	}
7ebd796f JA	321
e39c0676 JA	322	static bool axmap_isset_fn(struct axmap_level *al, unsigned long offset,
e39c0676 JA	323	unsigned int bit, void *unused)
7ebd796f JA	324	{
	325	return (al->map[offset] & (1UL << bit)) != 0;
	326	}
	327
e39c0676	328	bool axmap_isset(struct axmap *axmap, uint64_t bit_nr)
7ebd796f	329	{
ac569176 JA	330	if (bit_nr <= axmap->nr_bits)
ac569176 JA	331	return axmap_handler_topdown(axmap, bit_nr, axmap_isset_fn, NULL);
47d94b0b	332
e39c0676	333	return false;
7ebd796f JA	334	}
	335
	336	static uint64_t axmap_find_first_free(struct axmap *axmap, unsigned int level,
	337	uint64_t index)
	338	{
731ef4c7	339	uint64_t ret = -1ULL;
7ebd796f	340	unsigned long j;
731ef4c7	341	int i;
7ebd796f JA	342
	343	/*
	344	* Start at the bottom, then converge towards first free bit at the top
	345	*/
	346	for (i = level; i >= 0; i--) {
	347	struct axmap_level *al = &axmap->levels[i];
	348
731ef4c7 JA	349	/*
	350	* Clear 'ret', this is a bug condition.
	351	*/
7ebd796f	352	if (index >= al->map_size) {
731ef4c7	353	ret = -1ULL;
7ebd796f JA	354	break;
	355	}
	356
	357	for (j = index; j < al->map_size; j++) {
	358	if (al->map[j] == -1UL)
	359	continue;
	360
	361	/*
	362	* First free bit here is our index into the first
	363	* free bit at the next higher level
	364	*/
731ef4c7	365	ret = index = (j << UNIT_SHIFT) + ffz(al->map[j]);
7ebd796f JA	366	break;
	367	}
	368	}
	369
47d94b0b JA	370	if (ret < axmap->nr_bits)
	371	return ret;
	372
	373	return (uint64_t) -1ULL;
7ebd796f JA	374	}
7ebd796f JA	375
99b9c534	376	static uint64_t axmap_first_free(struct axmap *axmap)
7ebd796f	377	{
2b2fa7f5 JA	378	if (!firstfree_valid(axmap))
2b2fa7f5 JA	379	axmap->first_free = axmap_find_first_free(axmap, axmap->nr_levels - 1, 0);
7ebd796f	380
ac569176	381	return axmap->first_free;
7ebd796f JA	382	}
	383
	384	struct axmap_next_free_data {
	385	unsigned int level;
	386	unsigned long offset;
	387	uint64_t bit;
	388	};
	389
e39c0676	390	static bool axmap_next_free_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	391	unsigned int bit, void *__data)
	392	{
	393	struct axmap_next_free_data *data = __data;
53737ae0	394	uint64_t mask = ~bit_masks[(data->bit + 1) & BLOCKS_PER_UNIT_MASK];
7ebd796f	395
53737ae0	396	if (!(mask & ~al->map[offset]))
e39c0676	397	return false;
7ebd796f JA	398
	399	if (al->map[offset] != -1UL) {
	400	data->level = al->level;
	401	data->offset = offset;
e39c0676	402	return true;
7ebd796f JA	403	}
	404
	405	data->bit = (data->bit + BLOCKS_PER_UNIT - 1) / BLOCKS_PER_UNIT;
e39c0676	406	return false;
7ebd796f JA	407	}
	408
	409	/*
	410	* 'bit_nr' is already set. Find the next free bit after this one.
	411	*/
	412	uint64_t axmap_next_free(struct axmap *axmap, uint64_t bit_nr)
	413	{
	414	struct axmap_next_free_data data = { .level = -1U, .bit = bit_nr, };
53737ae0	415	uint64_t ret;
7ebd796f	416
ac569176 JA	417	if (firstfree_valid(axmap) && bit_nr < axmap->first_free)
ac569176 JA	418	return axmap->first_free;
12bde369	419
ac569176 JA	420	if (!axmap_handler(axmap, bit_nr, axmap_next_free_fn, &data))
ac569176 JA	421	return axmap_first_free(axmap);
7ebd796f JA	422
	423	assert(data.level != -1U);
	424
53737ae0 JA	425	/*
	426	* In the rare case that the map is unaligned, we might end up
	427	* finding an offset that's beyond the valid end. For that case,
	428	* find the first free one, the map is practically full.
	429	*/
	430	ret = axmap_find_first_free(axmap, data.level, data.offset);
ac569176	431	if (ret != -1ULL)
53737ae0 JA	432	return ret;
53737ae0 JA	433
ac569176	434	return axmap_first_free(axmap);
7ebd796f	435	}