[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 = 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, 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;

	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
	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;
2404b2c3	105	axmap->levels = calloc(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	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	}