tbm-mcu/source/fs/mufs/block.c

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

#include <fs/mufs.h>

#include "block.h"

static int mufs_mark_block(struct mufs *fs, uint32_t block, int status)
{
	char data[1 << fs->dev->log2_block_size];
	uint8_t log2_bits_per_block = fs->dev->log2_block_size + ilog2(8);
	size_t index;

	index = block & ((UINT32_C(1) << log2_bits_per_block) - 1);
	block >>= log2_bits_per_block;

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

	if (status) {
		set_bit(data, index);
	} else {
		clear_bit(data, index);
	}

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

	return 0;
}

int mufs_alloc_block(struct mufs *fs, uint32_t *found)
{
	char data[1 << fs->dev->log2_block_size];
	uint32_t nblocks = fs->nblocks;
	uint32_t nbits;
	uint32_t block = 0;
	size_t index;
	uint8_t log2_bits_per_block = fs->dev->log2_block_size + ilog2(8);
	size_t nbitmap_size = align_up(fs->nblocks, log2_bits_per_block) >>
		log2_bits_per_block;

	while (nblocks) {
		if (flash_read(fs->dev, (1 + block) << fs->dev->log2_block_size, data,
			sizeof data) == 0)
			return -1;

		nbits = min(nblocks, UINT32_C(1) << log2_bits_per_block);

		if ((index = next_clear_bit(data, nbits)) == SIZE_MAX) {
			++block;
			nblocks -= nbits;
			continue;
		}

		index += block << log2_bits_per_block;
		*found = 1 + nbitmap_size + index;

		return mufs_mark_block(fs, index, 1);
	}

	return -1;
}

int mufs_free_block(struct mufs *fs, uint32_t block)
{
	uint8_t log2_bits_per_block = fs->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;

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

	return mufs_mark_block(fs, block - 1 - nbitmap_size, 0);
}