/*
 * 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 "nios2sim-ng.h"
#include "instruction.h"
#include "nios2.h"

static inline uint32_t get_opcode(uint32_t code)
{
	I_TYPE(instr, code);
	return instr->op;
}

static inline uint32_t get_opxcode(uint32_t code)
{
	R_TYPE(instr, code);
	return instr->opx.opx11;
}

/*
 * Used as a placeholder for instructions not yet implemented by the simulator.
 */
static int unimplemented(struct nios2 *cpu, uint32_t code)
{
	info("Unsupported instruction %s (%08x) @ PC %08x\n",
			instruction_get_string(code), code, cpu->pc);

	return PC_INC_NORMAL;
}

/*
 * Used as a placeholder for all instructions which do not have an effect on the
 * simulator (e.g. flush, sync)
 */
static int nop(struct nios2 *cpu, uint32_t code)
{
	/* Nothing to do here */
	return PC_INC_NORMAL;
}

/*
 * J-Type instructions
 */

/*
 * ra <- PC + 4
 * PC <- (PC(31..28) : IMM26 * 4)
 */
static int call(struct nios2 *cpu, uint32_t code)
{
	J_TYPE(instr, code);

	cpu->gp_regs[ra] = cpu->pc + 4;
	cpu->pc = (instr->imm26 * 4) | (cpu->pc & 0xF0000000);

	return PC_INC_BY_INSTR;
}

/* PC <- (PC(31..28) : IMM26 * 4) */
static int jmpi(struct nios2 *cpu, uint32_t code)
{
	J_TYPE(instr, code);

	cpu->pc = (instr->imm26 * 4) | (cpu->pc & 0xF0000000);

	return PC_INC_BY_INSTR;
}

/*
 * I-Type instructions
 */

static int ldbu(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);

	return PC_INC_NORMAL;
}

/* rB <- rA + IMM16 */
static int addi(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] + (int16_t) (instr->imm16);

	return PC_INC_NORMAL;
}

/* PC <- PC + 4 + IMM16 */
static int br(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);

	cpu->pc += 4 + (instr->imm16 & 0xFFFC);

	return PC_INC_NORMAL;
}

static int ldhu(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);

	return PC_INC_NORMAL;
}

/* rB <- rA & IMM16 */
static int andi(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] & instr->imm16;

	return PC_INC_NORMAL;
}

/* rB <- rA | IMM16 */
static int ori(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] | instr->imm16;
	return PC_INC_NORMAL;
}

/*
 * if ((signed) rA < (signed) rB)
 *   PC <- PC + 4 + @(IMM16)
 * else
 *   PC <- PC + 4
 */
static int blt(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (((int32_t) gp_regs[instr->a]) < ((int32_t) gp_regs[instr->b]))
		cpu->pc += 4 + (int16_t)(instr->imm16 & 0xFFFC);
	else
		cpu->pc += 4;

	return PC_INC_BY_INSTR;
}

/* rB <- rA ^ IMM16 */
static int xori(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] ^ instr->imm16;
	return PC_INC_NORMAL;
}

/* rB <- (rA * @(IMM16))(31..0) */
static int muli(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint64_t tmp;

	/* Raise exception if instruction is not implemented */
	if (!cpu->has_mul)
		return INSTR_EXECPTION;

	tmp = gp_regs[instr->a] * ((int16_t) instr->imm16);
	gp_regs[instr->b] = (uint32_t) (tmp & BIT_MASK(32));
	return PC_INC_NORMAL;
}

/*
 * if (rA == rB)
 *   PC <- PC + 4 + @(IMM16)
 * else
 *   PC <- PC + 4
 */
static int beq(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (gp_regs[instr->a] == gp_regs[instr->b])
		cpu->pc += 4 + (int16_t)(instr->imm16 & 0xFFFC);
	else
		cpu->pc += 4;

	return PC_INC_BY_INSTR;
}

/* rB <- rA & (IMM16 : 0x0000) */
static int andhi(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] & (instr->imm16 << 16);

	return PC_INC_NORMAL;
}

/* Prototype only, defined below */
static int handle_r_type_instr(struct nios2 *cpu, uint32_t code);

/* rB <- rA ^ (IMM16 : 0x0000) */
static int xorhi(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->b] = gp_regs[instr->a] ^ (instr->imm16 << 16);

	return PC_INC_NORMAL;
}

static instruction_handler i_type_instr_handlers[I_TYPE_COUNT] = {
	[CALL]		= call,
	[JMPI]		= jmpi,
	[LDBU]		= ldbu,
	[ADDI]		= addi,
	[STB]		= unimplemented,
	[BR]		= br,
	[LDB]		= unimplemented,
	[CMPGEI]	= unimplemented,
	[LDHU]		= ldhu,
	[ANDI]		= andi,
	[STH]		= unimplemented,
	[BGE]		= unimplemented,
	[LDH]		= unimplemented,
	[CMPLTI]	= unimplemented,
	[INITDA]	= unimplemented,
	[ORI]		= ori,
	[STW]		= unimplemented,
	[BLT]		= blt,
	[LDW]		= unimplemented,
	[CMPNEI]	= unimplemented,
	[FLUSHDA]	= nop,
	[XORI]		= xori,
	[BNE]		= unimplemented,
	[CMPEQI]	= unimplemented,
	[LDBUIO]	= unimplemented,
	[MULI]		= muli,
	[STBIO]		= unimplemented,
	[BEQ]		= beq,
	[LDBIO]		= unimplemented,
	[CMPGEUI]	= unimplemented,
	[LDHUIO]	= unimplemented,
	[ANDHI]		= andhi,
	[STHIO]		= unimplemented,
	[BGEU]		= unimplemented,
	[LDHIO]		= unimplemented,
	[CMPLTUI]	= unimplemented,
	[CUSTOM]	= unimplemented,
	[INITD]		= unimplemented,
	[ORHI]		= unimplemented,
	[STWIO]		= unimplemented,
	[BLTU]		= unimplemented,
	[LDWIO]		= unimplemented,
	[R_TYPE]	= handle_r_type_instr,
	[FLUSHD]	= nop,
	[XORHI]		= xorhi,
};

/*
 * R-Type instructions
 */

/* rC <- rA rotated left IMM5 bit positions */
static int roli(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t a = gp_regs[instr->a];
	uint32_t shift_bits = instr->opx.imm5;

	gp_regs[instr->c] = (a  << shift_bits) | (a >> (32 - shift_bits));

	return PC_INC_NORMAL;
}

/* rC <- rA rotated left rB(4..0) bit positions */
static int rol(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t a = gp_regs[instr->a];
	uint32_t shift_bits = gp_regs[instr->b] & BIT_MASK(5);

	gp_regs[instr->c] = (a  << shift_bits) | (a >> (32 - shift_bits));

	return PC_INC_NORMAL;
}

/* rC <- ~(A | rB) */
static int nor(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = ~(gp_regs[instr->a] | gp_regs[instr->b]);

	return PC_INC_NORMAL;
}

/* rC <- rA & rB */
static int and(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] & gp_regs[instr->b];

	return PC_INC_NORMAL;
}

/*
 * if ((signed) rA < (signed) rB)
 *   rC <- 1
 * else
 *   rC <- 0
 */
static int cmplt(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (((int32_t) gp_regs[instr->a]) < ((int32_t) gp_regs[instr->b]))
		gp_regs[instr->c] = 1;
	else
		gp_regs[instr->c] = 0;

	return PC_INC_NORMAL;
}

/* rC <- rA << IMM5 */
static int slli(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] << instr->opx.imm5;

	return PC_INC_NORMAL;
}

/* rC <- rA << rB(4..0) */
static int sll(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] << (gp_regs[instr->b] & BIT_MASK(5));

	return PC_INC_NORMAL;
}

/* rC <- rA | rB */
static int or(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] | gp_regs[instr->b];

	return PC_INC_NORMAL;
}

/*
 * if (rA != rB)
 *   rC <- 1
 * else
 *   rC <- 0
 */
static int cmpne(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (gp_regs[instr->a] != gp_regs[instr->b])
		gp_regs[instr->c] = 1;
	else
		gp_regs[instr->c] = 0;

	return PC_INC_NORMAL;
}

/* rC <- (unsigned) rA >> ((unsigned) IMM5)*/
static int srli(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] >> instr->opx.imm5;

	return PC_INC_NORMAL;
}

/* rC <- (unsigned) rA >> ((unsigned) rB(4..0))*/
static int srl(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] >> (gp_regs[instr->b] & BIT_MASK(5));

	return PC_INC_NORMAL;
}

/* rC <- rA ^ rB */
static int xor(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] ^ gp_regs[instr->b];

	return PC_INC_NORMAL;
}

/*
 * if (rA == rB)
 *   rC <- 1
 * else
 *   rC <- 0
 */
static int cmpeq(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (gp_regs[instr->a] == gp_regs[instr->b])
		gp_regs[instr->c] = 1;
	else
		gp_regs[instr->c] = 0;

	return PC_INC_NORMAL;
}

/* rC <- rA / rB */
static int divu(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	if (!cpu->has_div)
		return INSTR_EXECPTION;

	/* Division by zero? */
	if (gp_regs[instr->b] == 0)
		return INSTR_EXECPTION;

	gp_regs[instr->c] = gp_regs[instr->a] / gp_regs[instr->b];

	return PC_INC_NORMAL;
}

/* rC <- rA / rB */
static int div(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	int32_t a, b;

	if (!cpu->has_div)
		return INSTR_EXECPTION;

	b = (int32_t) gp_regs[instr->b];

	/* Division by zero? */
	if (instr->b == 0)
		return INSTR_EXECPTION;

	a = (int32_t) gp_regs[instr->a];
	gp_regs[instr->c] = (uint32_t)(a / b);

	return PC_INC_NORMAL;
}

/* rC <- ctlN */
static int rdctl(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);

	/* Instruction only allowed in supervisor mode */
	if (!nios2_in_supervisor_mode(cpu))
		return INSTR_EXECPTION;

	cpu->gp_regs[instr->c] = cpu->ctrl_regs[instr->opx.imm5];

	return PC_INC_NORMAL;
}

/* rC <- rA + rB */
static int add(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] + gp_regs[instr->b];

	return PC_INC_NORMAL;
}

/* rC <- rA - rB */
static int sub(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;

	gp_regs[instr->c] = gp_regs[instr->a] - gp_regs[instr->b];

	return PC_INC_NORMAL;
}

static instruction_handler r_type_instr_handlers[R_TYPE_COUNT] = {
	[ERET]		= unimplemented,
	[ROLI]		= roli,
	[ROL]		= rol,
	[FLUSHP]	= nop,
	[RET]		= unimplemented,
	[NOR]		= nor,
	[MULXUU]	= unimplemented,
	[CMPGE]		= unimplemented,
	[BRET]		= unimplemented,
	[ROR]		= unimplemented,
	[FLUSHI]	= nop,
	[JMP]		= unimplemented,
	[AND]		= and,
	[CMPLT]		= cmplt,
	[SLLI]		= slli,
	[SLL]		= sll,
	[OR]		= or,
	[MULXSU]	= unimplemented,
	[CMPNE]		= cmpne,
	[SRLI]		= srli,
	[SRL]		= srl,
	[NEXTPC]	= unimplemented,
	[CALLR]		= unimplemented,
	[XOR]		= xor,
	[MULXSS]	= unimplemented,
	[CMPEQ]		= cmpeq,
	[DIVU]		= divu,
	[DIV]		= div,
	[RDCTL]		= rdctl,
	[MUL]		= unimplemented,
	[CMPGEU]	= unimplemented,
	[INITI]		= unimplemented,
	[TRAP]		= unimplemented,
	[WRCTL]		= unimplemented,
	[CMPLTU]	= unimplemented,
	[ADD]		= add,
	[BREAK]		= unimplemented,
	[SYNC]		= nop,
	[SUB]		= sub,
	[SRAI]		= unimplemented,
	[SRA]		= unimplemented,
};

static int handle_r_type_instr(struct nios2 *cpu, uint32_t code)
{
	uint32_t opx;
	instruction_handler handle_instr;

	opx = get_opxcode(code);
	if (unlikely(opx >= R_TYPE_COUNT)) {
		err("Invalid OPX code %08x\n", opx);
		return INSTR_ERR;
	}

	handle_instr = r_type_instr_handlers[opx];
	if (unlikely(handle_instr == NULL)) {
		err("Invalid instruction %08x\n", code);
		return INSTR_ERR;
	}

	return handle_instr(cpu, code);
}

instruction_handler instruction_get_handler(uint32_t code)
{
	uint32_t op = get_opcode(code);

	if (unlikely(op >= I_TYPE_COUNT))
		return NULL;
	return i_type_instr_handlers[op];
}

static const char *i_type_instr_strings[I_TYPE_COUNT] = {
	[CALL]		= "call",
	[JMPI]		= "jmpi",
	[LDBU]		= "ldbu",
	[ADDI]		= "addi",
	[STB]		= "stb",
	[BR]		= "br",
	[LDB]		= "ldb",
	[CMPGEI]	= "cmpgei",
	[LDHU]		= "ldhu",
	[ANDI]		= "andi",
	[STH]		= "sth",
	[BGE]		= "bge",
	[LDH]		= "ldh",
	[CMPLTI]	= "cmplti",
	[INITDA]	= "initda",
	[ORI]		= "ori",
	[STW]		= "stw",
	[BLT]		= "blt",
	[LDW]		= "ldw",
	[CMPNEI]	= "cmpnei",
	[FLUSHDA]	= "nop",
	[XORI]		= "xori",
	[BNE]		= "bne",
	[CMPEQI]	= "cmpeqi",
	[LDBUIO]	= "ldbuio",
	[MULI]		= "muli",
	[STBIO]		= "stbio",
	[BEQ]		= "beq",
	[LDBIO]		= "ldbio",
	[CMPGEUI]	= "cmpgeui",
	[LDHUIO]	= "ldhuio",
	[ANDHI]		= "andhi",
	[STHIO]		= "sthio",
	[BGEU]		= "bgeu",
	[LDHIO]		= "ldhio",
	[CMPLTUI]	= "cmpltui",
	[CUSTOM]	= "custom",
	[INITD]		= "initd",
	[ORHI]		= "orhi",
	[STWIO]		= "stwio",
	[BLTU]		= "bltu",
	[LDWIO]		= "ldwio",
	[R_TYPE]	= "<R-type instruction>",
	[FLUSHD]	= "flushd",
	[XORHI]		= "xorhi",
};

static const char *r_type_instr_strings[R_TYPE_COUNT] = {
	[ERET]		= "eret",
	[ROLI]		= "roli",
	[ROL]		= "rol",
	[FLUSHP]	= "flushp",
	[RET]		= "ret",
	[NOR]		= "nor",
	[MULXUU]	= "mulxuu",
	[CMPGE]		= "cmpge",
	[BRET]		= "bret",
	[ROR]		= "ror",
	[FLUSHI]	= "flushi",
	[JMP]		= "jmp",
	[AND]		= "and",
	[CMPLT]		= "cmplt",
	[SLLI]		= "slli",
	[SLL]		= "sll",
	[OR]		= "or",
	[MULXSU]	= "mulxsu",
	[CMPNE]		= "cmpne",
	[SRLI]		= "srli",
	[SRL]		= "srl",
	[NEXTPC]	= "nextpc",
	[CALLR]		= "callr",
	[XOR]		= "xor",
	[MULXSS]	= "mulxss",
	[CMPEQ]		= "cmpeq",
	[DIVU]		= "divu",
	[DIV]		= "div",
	[RDCTL]		= "rdctl",
	[MUL]		= "mul",
	[CMPGEU]	= "cmpgeu",
	[INITI]		= "initi",
	[TRAP]		= "trap",
	[WRCTL]		= "wrctl",
	[CMPLTU]	= "cmpltu",
	[ADD]		= "add",
	[BREAK]		= "break",
	[SYNC]		= "sync",
	[SUB]		= "sub",
	[SRAI]		= "srai",
	[SRA]		= "sra",
};

const char *instruction_get_string(uint32_t code)
{
	uint32_t op = get_opcode(code);

	if (unlikely(op >= I_TYPE_COUNT))
		return "";
	else if (op == R_TYPE) {
		uint32_t opx = get_opxcode(code);
		if (unlikely(opx >= R_TYPE_COUNT))
			return "";
		return r_type_instr_strings[opx];
	}else
		return i_type_instr_strings[op];
}