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

#include <sys/mman.h>

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

#define CONFIG_FLASH_SIZE 4 * MIB

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

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

static struct flash_ops stdio_flash_ops = {
	.read = stdio_flash_read,
	.write = stdio_flash_write,
	.erase = stdio_flash_erase,
};

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

	if (addr > priv->size || len > priv->size - addr)
		return -1;

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

	return 0;
}

static int 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;

	if (addr > priv->size || len > priv->size - addr)
		return -1;

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

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

	return 0;
}

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

	if (!IS_ROUND(addr, 4 * KIB) || !IS_ROUND(len, 4 * KIB))
		return -1;

	memset(priv->data + addr, 0xFF, len);

	return 0;
}

struct flash_dev *flash_probe(void)
{
	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->data = mmap(NULL, CONFIG_FLASH_SIZE, PROT_READ | PROT_WRITE,
		MAP_ANONYMOUS | MAP_PRIVATE, -1, 0)))
		goto err_free_priv;

	memset(priv->data, 0xFF, CONFIG_FLASH_SIZE);
	priv->size = CONFIG_FLASH_SIZE;

	dev->priv = priv;
	dev->ops = &stdio_flash_ops;

	return dev;

err_free_priv:
	free(priv);
err_free_dev:
	free(dev);
	return NULL;
}