#include <bitops.h>
#include <flash.h>
#include <macros.h>
#include <mufs.h>

#include "block.h"

int mufs_mark_block(struct mufs *fs, uint32_t block, int status)
{
	struct flash_dev *dev = fs->dev;
	uint32_t data;
	size_t offset, index;

	if (block >= fs->nblocks)
		return -1;

	offset = align(block >> ilog2(8), ilog2(4));
	index = mask(block, ilog2(8) + ilog2(4));

	if (flash_read(fs->dev, (1 << dev->log2_block_size) + offset, &data,
		sizeof data) ==0)
		return -1;

	if (status) {
		data |= 1 << index;
	} else {
		data = data & ~(1 << index);
	}

	if (flash_write(fs->dev, (1 << dev->log2_block_size) + offset, &data,
		sizeof data) == 0)
		return -1;

	if (flash_sync(fs->dev) < 0)
		return -1;

	return 0;
}

static size_t mufs_next_free_block(struct mufs *fs)
{
	struct flash_dev *dev = fs->dev;
	char data[32];
	size_t nblocks = fs->nblocks;
	size_t offset, index, bitlen, len;

	offset = 0;

	while (nblocks) {
		bitlen = min(BIT_SIZE(data), nblocks);
		len = bitlen >> ilog2(8);

		if (flash_read(fs->dev, (1 << dev->log2_block_size) + offset, data,
			len) == 0)
			return SIZE_MAX;

		if ((index = next_clear_bit(data, bitlen)) != SIZE_MAX)
			return (offset >> (ilog2(8) + ilog2(32))) + index;

		offset += len;
		nblocks -= bitlen;
	}

	return SIZE_MAX;
}

int mufs_alloc_block(struct mufs *fs, uint32_t *found)
{
	size_t index;

	if ((index = mufs_next_free_block(fs)) == SIZE_MAX)
		return -1;

	flash_erase(fs->dev, index, 1);

	*found = index;

	return mufs_mark_block(fs, index, 1);
}

int mufs_free_block(struct mufs *fs, uint32_t block)
{
	struct flash_dev *dev = fs->dev;
	uint8_t log2_bits_per_block = dev->log2_block_size + ilog2(8);
	size_t nbitmap_size = align_up(fs->nblocks, log2_bits_per_block) >>
		log2_bits_per_block;

	if (block < 1 + nbitmap_size)
		return -1;

	return mufs_mark_block(fs, block, 0);
}