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

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 {
	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 inline 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 = 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;
}

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 *))
{
	struct axmap_level *al;
	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;

		al = &axmap->levels[i];

		if (func(al, offset, bit, NULL))
			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 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;

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

		nr_bits = __bit - bit;
		if (!nr_bits)
			return true;

		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;

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

	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
76dd186e JA	40	static const unsigned long bit_masks[] = {
	41	0x0000000000000000, 0x0000000000000001, 0x0000000000000003, 0x0000000000000007,
	42	0x000000000000000f, 0x000000000000001f, 0x000000000000003f, 0x000000000000007f,
	43	0x00000000000000ff, 0x00000000000001ff, 0x00000000000003ff, 0x00000000000007ff,
	44	0x0000000000000fff, 0x0000000000001fff, 0x0000000000003fff, 0x0000000000007fff,
	45	0x000000000000ffff, 0x000000000001ffff, 0x000000000003ffff, 0x000000000007ffff,
	46	0x00000000000fffff, 0x00000000001fffff, 0x00000000003fffff, 0x00000000007fffff,
	47	0x0000000000ffffff, 0x0000000001ffffff, 0x0000000003ffffff, 0x0000000007ffffff,
	48	0x000000000fffffff, 0x000000001fffffff, 0x000000003fffffff, 0x000000007fffffff,
	49	0x00000000ffffffff,
	50	#if BITS_PER_LONG == 64
	51	0x00000001ffffffff, 0x00000003ffffffff, 0x00000007ffffffff, 0x0000000fffffffff,
	52	0x0000001fffffffff, 0x0000003fffffffff, 0x0000007fffffffff, 0x000000ffffffffff,
	53	0x000001ffffffffff, 0x000003ffffffffff, 0x000007ffffffffff, 0x00000fffffffffff,
	54	0x00001fffffffffff, 0x00003fffffffffff, 0x00007fffffffffff, 0x0000ffffffffffff,
	55	0x0001ffffffffffff, 0x0003ffffffffffff, 0x0007ffffffffffff, 0x000fffffffffffff,
	56	0x001fffffffffffff, 0x003fffffffffffff, 0x007fffffffffffff, 0x00ffffffffffffff,
	57	0x01ffffffffffffff, 0x03ffffffffffffff, 0x07ffffffffffffff, 0x0fffffffffffffff,
	58	0x1fffffffffffffff, 0x3fffffffffffffff, 0x7fffffffffffffff, 0xffffffffffffffff
	59	#endif
	60	};
	61
7ebd796f JA	62	struct axmap_level {
	63	int level;
	64	unsigned long map_size;
	65	unsigned long *map;
	66	};
	67
	68	struct axmap {
	69	unsigned int nr_levels;
	70	struct axmap_level *levels;
	71	uint64_t first_free;
47d94b0b	72	uint64_t nr_bits;
7ebd796f JA	73	};
7ebd796f JA	74
76dd186e	75	static inline unsigned long ulog64(unsigned long val, unsigned int log)
7ebd796f JA	76	{
	77	while (log-- && val)
	78	val >>= UNIT_SHIFT;
	79
	80	return val;
	81	}
	82
	83	void axmap_reset(struct axmap *axmap)
	84	{
	85	int i;
	86
	87	for (i = 0; i < axmap->nr_levels; i++) {
	88	struct axmap_level *al = &axmap->levels[i];
	89
	90	memset(al->map, 0, al->map_size * sizeof(unsigned long));
	91	}
17f0fd39 JA	92
17f0fd39 JA	93	axmap->first_free = 0;
7ebd796f JA	94	}
	95
	96	void axmap_free(struct axmap *axmap)
	97	{
	98	unsigned int i;
	99
	100	if (!axmap)
	101	return;
	102
	103	for (i = 0; i < axmap->nr_levels; i++)
aded30f7	104	free(axmap->levels[i].map);
7ebd796f	105
aded30f7 JA	106	free(axmap->levels);
aded30f7 JA	107	free(axmap);
7ebd796f JA	108	}
	109
	110	struct axmap *axmap_new(unsigned long nr_bits)
	111	{
	112	struct axmap *axmap;
	113	unsigned int i, levels;
	114
aded30f7	115	axmap = malloc(sizeof(*axmap));
7ebd796f JA	116	if (!axmap)
	117	return NULL;
	118
	119	levels = 1;
	120	i = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	121	while (i > 1) {
	122	i = (i + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	123	levels++;
	124	}
	125
	126	axmap->nr_levels = levels;
2404b2c3	127	axmap->levels = calloc(axmap->nr_levels, sizeof(struct axmap_level));
47d94b0b	128	axmap->nr_bits = nr_bits;
7ebd796f JA	129
	130	for (i = 0; i < axmap->nr_levels; i++) {
	131	struct axmap_level *al = &axmap->levels[i];
	132
	133	al->level = i;
	134	al->map_size = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
aded30f7	135	al->map = malloc(al->map_size * sizeof(unsigned long));
7ebd796f JA	136	if (!al->map)
	137	goto err;
	138
	139	nr_bits = (nr_bits + BLOCKS_PER_UNIT - 1) >> UNIT_SHIFT;
	140	}
	141
	142	axmap_reset(axmap);
	143	return axmap;
	144	err:
	145	for (i = 0; i < axmap->nr_levels; i++)
	146	if (axmap->levels[i].map)
aded30f7	147	free(axmap->levels[i].map);
7ebd796f	148
aded30f7	149	free(axmap->levels);
bb1116fe	150	free(axmap);
7ebd796f JA	151	return NULL;
	152	}
	153
e39c0676 JA	154	static bool axmap_handler(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	155	bool (func)(struct axmap_level , unsigned long, unsigned int,
7ebd796f JA	156	void ), void data)
	157	{
	158	struct axmap_level *al;
	159	int i;
	160
	161	for (i = 0; i < axmap->nr_levels; i++) {
	162	unsigned long index = ulog64(bit_nr, i);
	163	unsigned long offset = index >> UNIT_SHIFT;
	164	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	165
	166	al = &axmap->levels[i];
	167
	168	if (func(al, offset, bit, data))
e39c0676	169	return true;
7ebd796f JA	170	}
7ebd796f JA	171
e39c0676	172	return false;
7ebd796f JA	173	}
7ebd796f JA	174
e39c0676	175	static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
c406445a	176	bool (func)(struct axmap_level , unsigned long, unsigned int, void *))
7ebd796f JA	177	{
7ebd796f JA	178	struct axmap_level *al;
76dd186e	179	int i;
7ebd796f JA	180
7ebd796f JA	181	for (i = axmap->nr_levels - 1; i >= 0; i--) {
76dd186e	182	unsigned long index = ulog64(bit_nr, i);
7ebd796f JA	183	unsigned long offset = index >> UNIT_SHIFT;
	184	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	185
	186	al = &axmap->levels[i];
	187
c406445a	188	if (func(al, offset, bit, NULL))
e39c0676	189	return true;
7ebd796f JA	190	}
7ebd796f JA	191
e39c0676	192	return false;
7ebd796f JA	193	}
7ebd796f JA	194
e39c0676	195	static bool axmap_clear_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	196	unsigned int bit, void *unused)
	197	{
	198	if (!(al->map[offset] & (1UL << bit)))
e39c0676	199	return true;
7ebd796f JA	200
7ebd796f JA	201	al->map[offset] &= ~(1UL << bit);
e39c0676	202	return false;
7ebd796f JA	203	}
	204
	205	void axmap_clear(struct axmap *axmap, uint64_t bit_nr)
	206	{
	207	axmap_handler(axmap, bit_nr, axmap_clear_fn, NULL);
2b2fa7f5 JA	208
	209	if (bit_nr < axmap->first_free)
	210	axmap->first_free = bit_nr;
7ebd796f JA	211	}
	212
	213	struct axmap_set_data {
	214	unsigned int nr_bits;
	215	unsigned int set_bits;
7ebd796f JA	216	};
7ebd796f JA	217
e39c0676	218	static bool axmap_set_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	219	unsigned int bit, void *__data)
	220	{
	221	struct axmap_set_data *data = __data;
	222	unsigned long mask, overlap;
	223	unsigned int nr_bits;
	224
	225	nr_bits = min(data->nr_bits, BLOCKS_PER_UNIT - bit);
	226
	227	mask = bit_masks[nr_bits] << bit;
	228
	229	/*
	230	* Mask off any potential overlap, only sets contig regions
	231	*/
	232	overlap = al->map[offset] & mask;
09d6bf09	233	if (overlap == mask)
e39c0676	234	return true;
7ebd796f	235
8cd5a2fd JA	236	if (overlap) {
8cd5a2fd JA	237	const int __bit = ffz(~overlap);
7ebd796f	238
c2e48e0e JA	239	nr_bits = __bit - bit;
c2e48e0e JA	240	if (!nr_bits)
8cd5a2fd JA	241	return true;
8cd5a2fd JA	242
8cd5a2fd	243	mask = bit_masks[nr_bits] << bit;
7ebd796f JA	244	}
	245
	246	assert(mask);
	247	assert(!(al->map[offset] & mask));
7ebd796f JA	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	330	if (bit_nr <= axmap->nr_bits)
c406445a	331	return axmap_handler_topdown(axmap, bit_nr, axmap_isset_fn);
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	}