tbm-mcu/source/drivers/sandbox_flash.c

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <sys/mman.h>
#include <sys/stat.h>

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

#define CONFIG_FLASH_SIZE 4 * MIB

struct stdio_flash_priv {
	FILE *fp;
	char *data;
	size_t size;
};

static size_t stdio_flash_get_size(struct flash_dev *dev);
static size_t stdio_flash_read(struct flash_dev *dev, uint32_t addr, void *data,
	size_t len);
static size_t stdio_flash_write(struct flash_dev *dev, uint32_t addr,
	const void *data, size_t len);
static int stdio_flash_copy(struct flash_dev *dev, uint32_t dst, uint32_t src,
	size_t len);
static int stdio_flash_erase(struct flash_dev *dev, uint32_t addr);
static void stdio_flash_release(struct flash_dev *dev);

static struct flash_ops stdio_flash_ops = {
	.release = stdio_flash_release,
	.get_size = stdio_flash_get_size,
	.read = stdio_flash_read,
	.write = stdio_flash_write,
	.copy = stdio_flash_copy,
	.is_erased = default_flash_is_erased,
	.erase = stdio_flash_erase,
};

static size_t stdio_flash_get_size(struct flash_dev *dev)
{
	struct stdio_flash_priv *priv = dev->priv;

	return priv->size;
}

static size_t stdio_flash_read(struct flash_dev *dev, uint32_t addr, void *data,
	size_t len)
{
	struct stdio_flash_priv *priv = dev->priv;

	addr = min(addr, priv->size);
	len = min(len, priv->size - addr);

	if (!len)
		return 0;

	memcpy(data, priv->data + addr, len);

	return len;
}

static size_t stdio_flash_write(struct flash_dev *dev, uint32_t addr,
	const void *data, size_t len)
{
	struct stdio_flash_priv *priv = dev->priv;
	const char *src;
	char *dst;
	size_t i;

	addr = min(addr, priv->size);
	len = min(len, priv->size - addr);

	if (!len)
		return 0;

	src = data;
	dst = priv->data + addr;

	for (i = 0; i < len; ++i) {
		dst[i] &= src[i];
	}

	return len;
}

static int stdio_flash_copy(struct flash_dev *dev, uint32_t dst, uint32_t src,
	size_t len)
{
	uint8_t data[64];
	size_t nbytes;

	while (len) {
		nbytes = min(sizeof data, len);

		if (stdio_flash_read(dev, src, data, nbytes) == 0)
			return -1;

		if (stdio_flash_write(dev, dst, data, nbytes) == 0)
			return -1;

		dst += nbytes;
		src += nbytes;
		len -= nbytes;
	}

	return 0;
}

static int stdio_flash_erase(struct flash_dev *dev, uint32_t addr)
{
	struct stdio_flash_priv *priv = dev->priv;

	memset(priv->data + (addr << dev->log2_block_size), 0xff,
		1 << dev->log2_block_size);

	return 0;
}

struct flash_dev *flash_probe(void)
{
	struct stat st;
	struct flash_dev *dev;
	struct stdio_flash_priv *priv;

	if (!(dev = malloc(sizeof *dev)))
		return NULL;

	if (!(priv = malloc(sizeof *priv)))
		goto err_free_dev;

	if (!(priv->fp = fopen("flash.img", "r+b")))
		goto err_free_priv;

	if (fstat(fileno(priv->fp), &st) < 0)
		goto err_close_fp;

	priv->size = st.st_size;

	if (!(priv->data = mmap(NULL, priv->size, PROT_READ | PROT_WRITE,
		MAP_SHARED, fileno(priv->fp), 0)))
		goto err_close_fp;

	dev->priv = priv;
	dev->ops = &stdio_flash_ops;
	dev->log2_block_size = ilog2(4 * KIB);

	return dev;

err_close_fp:
	fclose(priv->fp);
err_free_priv:
	free(priv);
err_free_dev:
	free(dev);
	return NULL;
}

static void stdio_flash_release(struct flash_dev *dev)
{
	struct stdio_flash_priv *priv;

	if (!dev)
		return;

	priv = dev->priv;

	munmap(priv->data, priv->size);
	fclose(priv->fp);
	free(dev->priv);
	free(dev);
}