/*
 * 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->opx6;
}

/*
 * 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 INSTR_UNIMPL;
}

/*
 * 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 __unused, uint32_t code __unused)
{
	/* 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 = (cpu->pc & 0xF0000000) | (instr->imm26 * 4);

	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 = (cpu->pc & 0xF0000000) | (instr->imm26 * 4);

	return PC_INC_BY_INSTR;
}

/*
 * I-Type instructions
 */

/* rB <- 0x000000 : Mem8[rA + @(IMM16)] */
static int ldbu(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16;
	uint8_t byte;

	if (nios2_load(cpu, addr, &byte, 1))
		return INSTR_ERR;

	gp_regs[instr->b] = (uint32_t) byte;

	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;
}

/* Mem8[rA + @(IMM16)] <- rB(7..0) */
static int stb(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16;
	uint8_t data = gp_regs[instr->b] & 0xFF;

	if (nios2_store(cpu, addr, &data, 1))
		return INSTR_ERR;

	return PC_INC_NORMAL;
}

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

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

	return PC_INC_NORMAL;
}

/* rB <- @(Mem8[rA + @(IMM16)]) */
static int ldb(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16;
	uint8_t byte;

	if (nios2_load(cpu, addr, &byte, 1))
		return INSTR_ERR;

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

	return PC_INC_NORMAL;
}

/*
 * if ((signed) rA >= (signed) @(IMM16))
 *   rB <- 1
 * else
 *   rB <- 0
 */
static int cmpgei(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) ((int16_t) instr->imm16))
		gp_regs[instr->b] = 1;
	else
		gp_regs[instr->b] = 0;

	return PC_INC_NORMAL;
}

/* rB <- 0x0000 : Mem16[rA + @IMM16)] */
static int ldhu(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16;
	uint16_t halfword;

	if (nios2_load(cpu, addr, &halfword, 2))
		return INSTR_ERR;

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

	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;
}

/* */
static int sth(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */
	return PC_INC_NORMAL;
}

/*
 * if ((signed) rA >= (signed) rB)
 *   PC <- PC + 4 + @(IMM16)
 * else
 *   PC <- PC + 4
 */
static int bge(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 + (instr->imm16 & 0xFFFC);
	else
		cpu->pc += 4;

	return PC_INC_BY_INSTR;
}

/* */
static int ldh(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */
	return PC_INC_NORMAL;
}

/*
 * if ((signed) rA < (signed) @(IMM16))
 *   rB <- 1
 * else
 *   rB <- 0
 */
static int cmplti(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) ((int16_t) instr->imm16))
		gp_regs[instr->b] = 1;
	else
		gp_regs[instr->b] = 0;

	return PC_INC_NORMAL;
}

/* Initializes the data cache line currently caching address rA + @(IMM16) */
static int initda(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */
	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;
}

/* */
static int stw(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */

	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 <- @(Mem32[rA + @(IMM16)]) */
static int ldw(struct nios2 *cpu, uint32_t code)
{
	I_TYPE(instr, code);
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16;

	if (nios2_load(cpu, addr, &gp_regs[instr->b], 4))
		return INSTR_ERR;

	return PC_INC_NORMAL;
}

/* 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;
}

/*
 * if (rA != rB)
 *   PC <- PC + 4 + @(IMM16)
 * else
 *   PC <- PC + 4
 */
static int bne(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))(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 EXCEPTION(NIOS2_EX_UNIMPLEMENTED);

	tmp = gp_regs[instr->a] * ((int16_t) instr->imm16);
	gp_regs[instr->b] = (uint32_t) (tmp & BIT_MASK_FILL(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;
}

/* */
static int initd(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */

	return PC_INC_NORMAL;
}

/* rB <- rA | (IMM16 : 0x0000) */
static int orhi(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;
}

/*
 * if ((unsigned) rA < (unsigned) rB)
 *   PC <- PC + 4 + @(IMM16)
 * else
 *   PC <- PC + 4
 */
static int bltu(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;
}

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

/*
 * R-Type instructions
 */

/*
 * status <- estatus
 * PC <- ea
 */
static int eret(struct nios2 *cpu, uint32_t code __unused)
{
	cpu->ctrl_regs[status] = cpu->ctrl_regs[estatus];
	cpu->pc = cpu->gp_regs[ea];

	return PC_INC_BY_INSTR;
}

/* 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->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_FILL(5);

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

	return PC_INC_NORMAL;
}

/* */
static int flushp(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */

	return PC_INC_NORMAL;
}

/* PC <- ra */
static int ret(struct nios2 *cpu, uint32_t code __unused)
{
	cpu->pc = cpu->gp_regs[ra];

	return PC_INC_BY_INSTR;
}

/* 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 rotated right rb(4..0) bit positions */
static int ror(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_FILL(5);

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

	return PC_INC_NORMAL;
}

/* */
static int flushi(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */

	return PC_INC_NORMAL;
}

/* PC <- rA */
static int jmp(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);

	cpu->pc = cpu->gp_regs[instr->a];

	return PC_INC_BY_INSTR;
}

/* 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->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_FILL(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->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_FILL(5));

	return PC_INC_NORMAL;
}

/* rC <- PC + 4 */
static int nextpc(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);

	cpu->gp_regs[instr->c] = cpu->pc + 4;

	return PC_INC_NORMAL;
}

/*
 * ra <- PC + 4
 * PC <- rA
 */
static int callr(struct nios2 *cpu, uint32_t code)
{
	R_TYPE(instr, code);

	cpu->gp_regs[ra] = cpu->pc + 4;
	cpu->pc = cpu->gp_regs[instr->a];

	return PC_INC_BY_INSTR;
}

/* 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 EXCEPTION(NIOS2_EX_UNIMPLEMENTED);

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

	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 EXCEPTION(NIOS2_EX_UNIMPLEMENTED);

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

	/* Division by zero? */
	if (b == 0)
		return EXCEPTION(NIOS2_EX_DIV_ERR);
	/* Divide overflow? */
	if ((uint32_t) a == 0x80000000 && (uint32_t) b == 0xFFFFFFFF)
		return EXCEPTION(NIOS2_EX_DIV_ERR);

	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 EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);

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

	return PC_INC_NORMAL;
}

/* */
static int initi(struct nios2 *cpu __unused, uint32_t code __unused)
{
	/* TODO */

	return PC_INC_NORMAL;
}

/*
 * estatus <- status
 * PIE <- 0
 * U <- 0
 * ea <- PC + 4
 * PC <- exception handler address
 */
static int trap(struct nios2 *cpu, uint32_t code __unused)
{
	uint32_t *gp_regs = cpu->gp_regs;
	uint32_t *ctrl_regs = cpu->ctrl_regs;

	ctrl_regs[estatus] = ctrl_regs[status];
	clear_bit(NIOS2_STATUS_PIE, &ctrl_regs[status]);
	clear_bit(NIOS2_STATUS_U, &ctrl_regs[status]);
	gp_regs[ea] = cpu->pc + 4;
	cpu->pc = cpu->exception_handler_addr;

	return PC_INC_BY_INSTR;
}

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

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

	cpu->ctrl_regs[instr->imm5] = cpu->gp_regs[instr->a];

	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;
}

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

	gp_regs[instr->c] = ((int32_t) gp_regs[instr->a]) >> instr->imm5;

	return PC_INC_NORMAL;
}

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

	gp_regs[instr->c] = ((int32_t) gp_regs[instr->a]) >> (gp_regs[instr->b] & 0x1F);

	return PC_INC_NORMAL;
}

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

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))
		return INSTR_ERR;

//	dbg("  R: %s (%08x)\n", r_type_instructions[opx].name, code);
	handle_instr = r_type_instructions[opx].handler;
	if (unlikely(handle_instr == NULL))
		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;
//	dbg("I: %s (%08x)\n", i_type_instructions[op].name, code);
	return i_type_instructions[op].handler;
}

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_instructions[opx].name;
	} else
		return i_type_instructions[op].name;
}