/*
 * Copyright (C) 2010 Tobias Klauser <tklauser@distanz.ch>
 *
 * This file is part of nios2sim-ng.
 *
 * This file is subject to the terms and conditions of the GNU General Public
 * License. See the file "COPYING" in the main directory of this archive
 * for more details.
 */

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

#include "nios2sim-ng.h"
#include "nios2.h"
#include "memory.h"
#include "device.h"

void nios2_cpu_reset(struct nios2 *cpu)
{
	/* Reset registers */
	memset(cpu->gp_regs, 0x00, NIOS2_GP_REG_COUNT * sizeof(uint32_t));
	memset(cpu->ctrl_regs, 0x00, NIOS2_CTRL_REG_COUNT * sizeof(uint32_t));

	cpu->pc = 0;
	cpu->mode = NIOS2_SUPERVISOR_MODE;
	cpu->exception_cause = NIOS2_EX_NONE;
}

void nios2_cpu_init(struct nios2 *cpu)
{
	/* Set PC to entry address of program */
	cpu->pc = cpu->mem->image_base;
}

void nios2_cpu_inc_pc(struct nios2 *cpu)
{
	cpu->pc += 4;
}

void nios2_simulate(struct nios2 *cpu)
{
	/* Did anyone write to r0? */
	if (cpu->gp_regs[zero]) {
		warn("Written to r0\n");
		cpu->gp_regs[zero] = 0;
	}
}

bool nios2_in_user_mode(struct nios2 *cpu)
{
	if (!cpu->has_mmu)
		return false;
	else if (cpu->mode != NIOS2_USER_MODE)
		return false;
	else
		return true;
}

bool nios2_in_supervisor_mode(struct nios2 *cpu)
{
	return !nios2_in_user_mode(cpu);
}

void nios2_handle_exception(struct nios2 *cpu)
{
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t *ctrl_regs = cpu->ctrl_regs;

	if (cpu->exception_cause == NIOS2_EX_NONE)
		return;

	/* Store the exception cause */
	ctrl_regs[exception] = (cpu->exception_cause << NIOS2_EXCEPTION_CAUSE_OFF)
			       & NIOS2_EXCEPTION_CAUSE_MASK;
	/* Clear status.PIE */
	ctrl_regs[status] &= ~NIOS2_STATUS_PIE_MASK;
	/* Clear status.U */
	ctrl_regs[status] &= ~NIOS2_STATUS_U_MASK;

	if (!cpu->has_mmu
	    || (cpu->has_mmu && !(ctrl_regs[status] & NIOS2_STATUS_EH_MASK))) {
		/* Copy status control register to estatus control register */
		ctrl_regs[estatus] = cpu->ctrl_regs[status];
		/* Save address of the instruction following the exception to ea */
		gp_regs[ea] = cpu->pc + 4;
	}

	/* TODO */

}

uint32_t nios2_fetch_instr(struct nios2 *cpu)
{
	struct memory *mem = cpu->mem;

	return mem->base[cpu->pc / 4];
}

int nios2_execute_instr(struct nios2 *cpu, uint32_t instr)
{
	instruction_handler handle_instr = instruction_get_handler(instr);

	if (unlikely(handle_instr == NULL))
		return INSTR_ERR;

	return handle_instr(cpu, instr);
}

static bool is_mem_addr(struct memory *mem, uint32_t addr)
{
	if (addr >= mem->image_base && addr < (mem->image_base + mem->size))
		return true;
	else
		return false;
}

int nios2_load(struct nios2 *cpu, uint32_t addr, void *data, size_t size)
{
	struct memory *mem = cpu->mem;

//	dbg("load%zu %08x\n", size * 8, addr);

	if (is_mem_addr(mem, addr)) {
//		dbg("load MEM\n");
		switch (size) {
		case 1:
			*((uint8_t *) data) = memory_get_byte(mem, addr);
			break;
		case 2:
			*((uint16_t *) data) = memory_get_halfword(mem, addr);
			break;
		case 4:
			*((uint32_t *) data) = memory_get_word(mem, addr);
			break;
		default:
			err("Invalid size for load at %08x: %zu\n", addr, size);
			return -1;
		}
	} else {	/* must be I/O */
		struct device *dev = device_get_by_addr(addr);
		dbg("load I/O\n");

		if (unlikely(dev == NULL)) {
			err("Reading data outside of device range (0x%08x)\n", addr);
			return -1;
		}

		if (dev->read(dev, addr, data, size) != 1) {
			err("Read from device '%s' failed\n", dev->name);
			return -1;
		}
	}

	return 0;
}

int nios2_store(struct nios2 *cpu, uint32_t addr, void *data, size_t size)
{
	struct memory *mem = cpu->mem;

//	dbg("store%zu %08x\n", size, addr);

	if (is_mem_addr(mem, addr)) {
//		dbg("store MEM\n");
		switch (size) {
		case 1:
			memory_set_byte(mem, addr, *((uint8_t *) data));
			break;
		case 2:
			memory_set_halfword(mem, addr, *((uint16_t *) data));
			break;
		case 4:
			memory_set_word(mem, addr, *((uint32_t *) data));
			break;
		default:
			err("Invalid size for store at %08x: %zu\n", addr, size);
			return -1;
		}
	} else {	/* must be I/O */
		struct device *dev = device_get_by_addr(addr);
		dbg("store I/O\n");

		if (unlikely(dev == NULL)) {
			err("Writing data outside of device range (0x%08x)\n", addr);
			return -1;
		}

		if (dev->write(dev, addr, data, size) != 1) {
			err("Write to device '%s' failed\n", dev->name);
			return -1;
		}
	}

	return 0;
}

static const char *nios2_gp_registers[] = {
	"zero",
	"at",
	"r2",
	"r3",
	"r4",
	"r5",
	"r6",
	"r7",
	"r8",
	"r9",
	"r10",
	"r11",
	"r12",
	"r13",
	"r14",
	"r15",
	"r16",
	"r17",
	"r18",
	"r19",
	"r20",
	"r21",
	"r22",
	"r23",
	"et",
	"bt",
	"gp",
	"sp",
	"fp",
	"ea",
	"ba",
	"ra",
};

static const char *nios2_ctrl_registers[] = {
	"status",
	"estatus",
	"bstatus",
	"ienable",
	"ipending",
	"cpuid",
	"reserved",
	"exception",
	"pteaddr",
	"tlbacc",
	"tlbmisc",
	"reserved",
	"badaddr",
	"config",
	"mpubase",
	"mpuacc"
};

void nios2_dump_registers(struct nios2 *cpu)
{
	unsigned int i;
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t *ctrl_regs = cpu->ctrl_regs;

	info("General-purpose registers:\n");
	for (i = 0; i < NIOS2_GP_REG_COUNT; i++) {
		info(" %10s 0x%08x", nios2_gp_registers[i], gp_regs[i]);
		if ((i + 1) % 4 == 0)
			info("\n");
	}

	info("Control registers:\n");
	for (i = 0; i < NIOS2_CTRL_REG_COUNT; i++) {
		info(" %10s 0x%08x", nios2_ctrl_registers[i], ctrl_regs[i]);
		if ((i + 1) % 4 == 0)
			info("\n");
	}

}