[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 *),
	void *data)
{
	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, 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 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, 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
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 JA	175	static bool axmap_handler_topdown(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	176	bool (func)(struct axmap_level , unsigned long, unsigned int, void *),
7ebd796f JA	177	void *data)
	178	{
	179	struct axmap_level *al;
76dd186e	180	int i;
7ebd796f JA	181
7ebd796f JA	182	for (i = axmap->nr_levels - 1; i >= 0; i--) {
76dd186e	183	unsigned long index = ulog64(bit_nr, i);
7ebd796f JA	184	unsigned long offset = index >> UNIT_SHIFT;
	185	unsigned int bit = index & BLOCKS_PER_UNIT_MASK;
	186
	187	al = &axmap->levels[i];
	188
	189	if (func(al, offset, bit, data))
e39c0676	190	return true;
7ebd796f JA	191	}
7ebd796f JA	192
e39c0676	193	return false;
7ebd796f JA	194	}
7ebd796f JA	195
e39c0676	196	static bool axmap_clear_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	197	unsigned int bit, void *unused)
	198	{
	199	if (!(al->map[offset] & (1UL << bit)))
e39c0676	200	return true;
7ebd796f JA	201
7ebd796f JA	202	al->map[offset] &= ~(1UL << bit);
e39c0676	203	return false;
7ebd796f JA	204	}
	205
	206	void axmap_clear(struct axmap *axmap, uint64_t bit_nr)
	207	{
	208	axmap_handler(axmap, bit_nr, axmap_clear_fn, NULL);
2b2fa7f5 JA	209
	210	if (bit_nr < axmap->first_free)
	211	axmap->first_free = bit_nr;
7ebd796f JA	212	}
	213
	214	struct axmap_set_data {
	215	unsigned int nr_bits;
	216	unsigned int set_bits;
7ebd796f JA	217	};
7ebd796f JA	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	236
8cd5a2fd JA	237	if (overlap) {
8cd5a2fd JA	238	const int __bit = ffz(~overlap);
7ebd796f	239
c2e48e0e JA	240	nr_bits = __bit - bit;
c2e48e0e JA	241	if (!nr_bits)
8cd5a2fd JA	242	return true;
8cd5a2fd JA	243
8cd5a2fd	244	mask = bit_masks[nr_bits] << bit;
7ebd796f JA	245	}
	246
	247	assert(mask);
	248	assert(!(al->map[offset] & mask));
7ebd796f JA	249	al->map[offset] \|= mask;
	250
	251	if (!al->level)
	252	data->set_bits = nr_bits;
	253
	254	data->nr_bits = 1;
	255	return al->map[offset] != -1UL;
	256	}
	257
	258	static void __axmap_set(struct axmap *axmap, uint64_t bit_nr,
	259	struct axmap_set_data *data)
	260	{
	261	unsigned int set_bits, nr_bits = data->nr_bits;
	262
	263	if (axmap->first_free >= bit_nr &&
	264	axmap->first_free < bit_nr + data->nr_bits)
	265	axmap->first_free = -1ULL;
	266
47d94b0b JA	267	if (bit_nr > axmap->nr_bits)
	268	return;
	269	else if (bit_nr + nr_bits > axmap->nr_bits)
	270	nr_bits = axmap->nr_bits - bit_nr;
	271
7ebd796f JA	272	set_bits = 0;
	273	while (nr_bits) {
	274	axmap_handler(axmap, bit_nr, axmap_set_fn, data);
	275	set_bits += data->set_bits;
	276
0bfdf9f1 JA	277	if (!data->set_bits \|\|
0bfdf9f1 JA	278	data->set_bits != (BLOCKS_PER_UNIT - nr_bits))
7ebd796f JA	279	break;
	280
	281	nr_bits -= data->set_bits;
	282	bit_nr += data->set_bits;
	283
	284	data->nr_bits = nr_bits;
7ebd796f JA	285	}
	286
	287	data->set_bits = set_bits;
	288	}
	289
	290	void axmap_set(struct axmap *axmap, uint64_t bit_nr)
	291	{
	292	struct axmap_set_data data = { .nr_bits = 1, };
	293
	294	__axmap_set(axmap, bit_nr, &data);
	295	}
	296
e39c0676 JA	297	unsigned int axmap_set_nr(struct axmap *axmap, uint64_t bit_nr,
e39c0676 JA	298	unsigned int nr_bits)
7ebd796f	299	{
0bfdf9f1 JA	300	unsigned int set_bits = 0;
	301
	302	do {
09d6bf09	303	struct axmap_set_data data = { .nr_bits = nr_bits, };
0bfdf9f1 JA	304	unsigned int max_bits, this_set;
	305
	306	max_bits = BLOCKS_PER_UNIT - (bit_nr & BLOCKS_PER_UNIT_MASK);
	307	if (max_bits < nr_bits)
	308	data.nr_bits = max_bits;
	309
	310	this_set = data.nr_bits;
	311	__axmap_set(axmap, bit_nr, &data);
	312	set_bits += data.set_bits;
	313	if (data.set_bits != this_set)
	314	break;
7ebd796f	315
0bfdf9f1 JA	316	nr_bits -= data.set_bits;
	317	bit_nr += data.set_bits;
	318	} while (nr_bits);
	319
	320	return set_bits;
7ebd796f JA	321	}
7ebd796f JA	322
e39c0676 JA	323	static bool axmap_isset_fn(struct axmap_level *al, unsigned long offset,
e39c0676 JA	324	unsigned int bit, void *unused)
7ebd796f JA	325	{
	326	return (al->map[offset] & (1UL << bit)) != 0;
	327	}
	328
e39c0676	329	bool axmap_isset(struct axmap *axmap, uint64_t bit_nr)
7ebd796f	330	{
ac569176 JA	331	if (bit_nr <= axmap->nr_bits)
ac569176 JA	332	return axmap_handler_topdown(axmap, bit_nr, axmap_isset_fn, NULL);
47d94b0b	333
e39c0676	334	return false;
7ebd796f JA	335	}
	336
	337	static uint64_t axmap_find_first_free(struct axmap *axmap, unsigned int level,
	338	uint64_t index)
	339	{
731ef4c7	340	uint64_t ret = -1ULL;
7ebd796f	341	unsigned long j;
731ef4c7	342	int i;
7ebd796f JA	343
	344	/*
	345	* Start at the bottom, then converge towards first free bit at the top
	346	*/
	347	for (i = level; i >= 0; i--) {
	348	struct axmap_level *al = &axmap->levels[i];
	349
731ef4c7 JA	350	/*
	351	* Clear 'ret', this is a bug condition.
	352	*/
7ebd796f	353	if (index >= al->map_size) {
731ef4c7	354	ret = -1ULL;
7ebd796f JA	355	break;
	356	}
	357
	358	for (j = index; j < al->map_size; j++) {
	359	if (al->map[j] == -1UL)
	360	continue;
	361
	362	/*
	363	* First free bit here is our index into the first
	364	* free bit at the next higher level
	365	*/
731ef4c7	366	ret = index = (j << UNIT_SHIFT) + ffz(al->map[j]);
7ebd796f JA	367	break;
	368	}
	369	}
	370
47d94b0b JA	371	if (ret < axmap->nr_bits)
	372	return ret;
	373
	374	return (uint64_t) -1ULL;
7ebd796f JA	375	}
7ebd796f JA	376
99b9c534	377	static uint64_t axmap_first_free(struct axmap *axmap)
7ebd796f	378	{
2b2fa7f5 JA	379	if (!firstfree_valid(axmap))
2b2fa7f5 JA	380	axmap->first_free = axmap_find_first_free(axmap, axmap->nr_levels - 1, 0);
7ebd796f	381
ac569176	382	return axmap->first_free;
7ebd796f JA	383	}
	384
	385	struct axmap_next_free_data {
	386	unsigned int level;
	387	unsigned long offset;
	388	uint64_t bit;
	389	};
	390
e39c0676	391	static bool axmap_next_free_fn(struct axmap_level *al, unsigned long offset,
7ebd796f JA	392	unsigned int bit, void *__data)
	393	{
	394	struct axmap_next_free_data *data = __data;
53737ae0	395	uint64_t mask = ~bit_masks[(data->bit + 1) & BLOCKS_PER_UNIT_MASK];
7ebd796f	396
53737ae0	397	if (!(mask & ~al->map[offset]))
e39c0676	398	return false;
7ebd796f JA	399
	400	if (al->map[offset] != -1UL) {
	401	data->level = al->level;
	402	data->offset = offset;
e39c0676	403	return true;
7ebd796f JA	404	}
	405
	406	data->bit = (data->bit + BLOCKS_PER_UNIT - 1) / BLOCKS_PER_UNIT;
e39c0676	407	return false;
7ebd796f JA	408	}
	409
	410	/*
	411	* 'bit_nr' is already set. Find the next free bit after this one.
	412	*/
	413	uint64_t axmap_next_free(struct axmap *axmap, uint64_t bit_nr)
	414	{
	415	struct axmap_next_free_data data = { .level = -1U, .bit = bit_nr, };
53737ae0	416	uint64_t ret;
7ebd796f	417
ac569176 JA	418	if (firstfree_valid(axmap) && bit_nr < axmap->first_free)
ac569176 JA	419	return axmap->first_free;
12bde369	420
ac569176 JA	421	if (!axmap_handler(axmap, bit_nr, axmap_next_free_fn, &data))
ac569176 JA	422	return axmap_first_free(axmap);
7ebd796f JA	423
	424	assert(data.level != -1U);
	425
53737ae0 JA	426	/*
	427	* In the rare case that the map is unaligned, we might end up
	428	* finding an offset that's beyond the valid end. For that case,
	429	* find the first free one, the map is practically full.
	430	*/
	431	ret = axmap_find_first_free(axmap, data.level, data.offset);
ac569176	432	if (ret != -1ULL)
53737ae0 JA	433	return ret;
53737ae0 JA	434
ac569176	435	return axmap_first_free(axmap);
7ebd796f	436	}