1 files changed, 188 insertions, 14 deletions

diff --git a/instruction.c b/instruction.c
index ef7e9f9..b6671d1 100644
--- a/instruction.c
+++ b/instruction.c
@@ -8,24 +8,45 @@
  * 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;
+}
+
+static int unsupported(struct nios2 *cpu, uint32_t code)
+{
+	info("Unsupported instructtion %08x @ PC %08x\n", code, cpu->pc);
+
+	return PC_INC_NORMAL;
+}
+
 /*
  * J-Type instructions
  */
 
-uint32_t call(struct nios2 *cpu, uint32_t opcode)
+static int call(struct nios2 *cpu, uint32_t code)
 {
-	J_TYPE(instr, opcode);
+	J_TYPE(instr, code);
 
 	return PC_INC_BY_INSTR;
 }
 
-uint32_t jmpi(struct nios2 *cpu, uint32_t opcode)
+static int jmpi(struct nios2 *cpu, uint32_t code)
 {
-	J_TYPE(instr, opcode);
+	J_TYPE(instr, code);
 
 	return PC_INC_BY_INSTR;
 }
@@ -34,26 +55,179 @@ uint32_t jmpi(struct nios2 *cpu, uint32_t opcode)
  * I-Type instructions
  */
 
-uint32_t ldbu(struct nios2 *cpu, uint32_t opcode)
+static int ldbu(struct nios2 *cpu, uint32_t code)
 {
-	I_TYPE(instr, opcode);
+	I_TYPE(instr, code);
 
 	return PC_INC_NORMAL;
 }
 
-uint32_t addi(struct nios2 *cpu, uint32_t opcode)
+static int addi(struct nios2 *cpu, uint32_t code)
 {
-	I_TYPE(instr, opcode);
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
 
 	/* rB <- rA + IMM16 */
-	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] + (int16_t) (instr->imm16);
+	gp_regs[instr->b] = gp_regs[instr->a] + (int16_t) (instr->imm16);
+	return PC_INC_NORMAL;
+}
+
+static int ldhu(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+
+	return PC_INC_NORMAL;
+}
+
+static int andi(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA & IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] & (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+static int ori(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA | IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] | (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+static int xori(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA ^ IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] ^ (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+/*
+ * R-Type instructions
+ */
+
+static int and(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA & rB */
+	gp_regs[instr->c] = gp_regs[instr->a] & gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int or(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA | rB */
+	gp_regs[instr->c] = gp_regs[instr->a] | gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int add(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA + rB */
+	gp_regs[instr->c] = gp_regs[instr->a] + gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int sync(struct nios2 *cpu, uint32_t code)
+{
+	/* Nothing to do here */
+	return PC_INC_NORMAL;
+}
 
+static int sub(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA - rB */
+	gp_regs[instr->c] = gp_regs[instr->a] - gp_regs[instr->b];
 	return PC_INC_NORMAL;
 }
 
-instruction_handler i_type_inst_handlers[I_TYPE_COUNT] = {
-	[CALL]	= call,
-	[JMPI]	= jmpi,
-	[LDBU]	= ldbu,
-	[ADDI]	= addi,
+static instruction_handler r_type_instr_handlers[R_TYPE_COUNT] = {
+	[AND]	= and,
+	[OR]	= or,
+	[ADD]	= add,
+
+	[BREAK]	= unsupported,
+	[SYNC]	= sync,
+	[SUB]	= sub,
+	[SRAI]	= unsupported,
+	[SRA]	= unsupported,
+};
+
+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);
+}
+
+static instruction_handler i_type_instr_handlers[I_TYPE_COUNT] = {
+	[CALL]		= call,
+	[JMPI]		= jmpi,
+	[LDBU]		= ldbu,
+	[ADDI]		= addi,
+	[STB]		= unsupported,
+	[BR]		= unsupported,
+	[LDB]		= unsupported,
+	[CMPGEI]	= unsupported,
+	[LDHU]		= ldhu,
+	[ANDI]		= andi,
+	[STH]		= unsupported,
+	[BGE]		= unsupported,
+	[LDH]		= unsupported,
+	[CMPLTI]	= unsupported,
+	[INITDA]	= unsupported,
+	[ORI]		= ori,
+	[STW]		= unsupported,
+	[BLT]		= unsupported,
+	[LDW]		= unsupported,
+	[CMPNEI]	= unsupported,
+	[FLUSHDA]	= unsupported,
+	[XORI]		= xori,
+	[BNE]		= unsupported,
+
+	[BLTU]		= unsupported,
+	[LDWIO]		= unsupported,
+	[R_TYPE]	= handle_r_type_instr,
+	[FLUSHD]	= unsupported,
+	[XORHI]		= unsupported,
 };
+
+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];
+}