#include "memory.h"
#include "vriscv.h"

uint8_t* memory;
pthread_mutex_t memory_mutex;

#define MMIO_INSIDE(io, addr) (addr >= io->address && addr < io->address + (io->reg_size * io->reg_count))
struct MMIO_ENTRY
{
	uint32_t address;
	uint32_t reg_size;
	uint32_t reg_count;
	void* fn_write;
	void* fn_read;
	struct MMIO_ENTRY* next;
};
struct MMIO_ENTRY* mmio = 0;

void mem_init()
{
	memory = malloc(memory_size);
	pthread_mutex_init(&memory_mutex, 0);
}

void mem_register_mmio(uint32_t address, uint32_t reg_size, uint32_t reg_count, void* fn_write, void* fn_read)
{
	struct MMIO_ENTRY** current = &mmio;
	while(*current)
		current = &(*current)->next;

	*current = malloc(sizeof(struct MMIO_ENTRY));
	(*current)->address = address;
	(*current)->reg_count = reg_count;
	(*current)->reg_size = reg_size;
	(*current)->fn_write = fn_write;
	(*current)->fn_read = fn_read;
	(*current)->next = 0;
}

void mem_write8(uint32_t address, uint8_t value)
{
	// Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 1)
			{
                void (*fn_write)(uint32_t, uint8_t) = io->fn_write;
				fn_write(address, value);
				return;
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 1 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory write
	pthread_mutex_lock(&memory_mutex);
	memory[address] = value;
	pthread_mutex_unlock(&memory_mutex);
}

void mem_write16(uint32_t address, uint16_t value)
{
	// Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 2)
			{
                void (*fn_write)(uint32_t, uint16_t) = io->fn_write;
				fn_write(address, value);
				return;
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 2 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory write
	pthread_mutex_lock(&memory_mutex);
	*((uint16_t*) &memory[address]) = value;
	pthread_mutex_unlock(&memory_mutex);
}

void mem_write32(uint32_t address, uint32_t value)
{
	// Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 4)
			{
                void (*fn_write)(uint32_t, uint32_t) = io->fn_write;
				fn_write(address, value);
				return;
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 4 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory write
	pthread_mutex_lock(&memory_mutex);
	*((uint32_t*) &memory[address]) = value;
	pthread_mutex_unlock(&memory_mutex);
}

uint8_t mem_read8(uint32_t address)
{
    // Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 1)
			{
                uint8_t (*fn_read)(uint32_t) = io->fn_read;
				return fn_read(address);
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 1 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory read
	pthread_mutex_lock(&memory_mutex);
	uint8_t tr = memory[address];
	pthread_mutex_unlock(&memory_mutex);
    return tr;
}

uint16_t mem_read16(uint32_t address)
{
    // Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 2)
			{
                uint16_t (*fn_read)(uint32_t) = io->fn_read;
				return fn_read(address);
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 2 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory read
	pthread_mutex_lock(&memory_mutex);
	uint16_t tr = *((uint16_t*) &memory[address]);
	pthread_mutex_unlock(&memory_mutex);
    return tr;
}

uint32_t mem_read32(uint32_t address)
{
    // Look wether we are on an MMIO region
	struct MMIO_ENTRY* io = mmio;
	while(io)
	{
		if(MMIO_INSIDE(io, address))
		{
			if(io->reg_size == 4)
			{
                uint32_t (*fn_read)(uint32_t) = io->fn_read;
				return fn_read(address);
			}
			else
			{
				fprintf(stderr, "MEMORY: Invalid MMIO access of size 4 in a mapping of %u-sized registers\n", io->reg_size);
				exit(EXIT_FAILURE);
			}
		}
		io = io->next;
	}

	// Proceed with memory read
	pthread_mutex_lock(&memory_mutex);
	uint32_t tr = *((uint32_t*) &memory[address]);
	pthread_mutex_unlock(&memory_mutex);
    return tr;
}