Instruction updates

1 files changed, 45 insertions, 12 deletions

diff --git a/instruction.c b/instruction.c
index ecdaa45..4e288db 100644
--- a/instruction.c
+++ b/instruction.c
@@ -23,7 +23,7 @@ static inline uint32_t get_opcode(uint32_t code)
 static inline uint32_t get_opxcode(uint32_t code)
 {
 	R_TYPE(instr, code);
-	return instr->opx.opx11;
+	return instr->opx6;
 }
 
 /*
@@ -62,6 +62,7 @@ static int call(struct nios2 *cpu, uint32_t code)
 	cpu->gp_regs[ra] = cpu->pc + 4;
 	cpu->pc = (instr->imm26 * 4) | (cpu->pc & 0xF0000000);
 
+	dbg("pc after call: %08x\n", cpu->pc);
 	return PC_INC_BY_INSTR;
 }
 
@@ -112,6 +113,7 @@ static int br(struct nios2 *cpu, uint32_t code)
 	return PC_INC_NORMAL;
 }
 
+/* */
 static int ldhu(struct nios2 *cpu __unused, uint32_t code __unused)
 {
 	/* TODO */
@@ -200,7 +202,7 @@ static int ori(struct nios2 *cpu, uint32_t code)
 }
 
 /* */
-static int stw(struct nios2 *cpu, uint32_t code)
+static int stw(struct nios2 *cpu __unused, uint32_t code __unused)
 {
 	/* TODO */
 
@@ -302,13 +304,43 @@ static int andhi(struct nios2 *cpu, uint32_t code)
 }
 
 /* */
-static int initd(struct nios2 *cpu, uint32_t code)
+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);
 
@@ -362,9 +394,9 @@ static struct instruction i_type_instructions[I_TYPE_COUNT] = {
 	[CMPLTUI]	= INSTRUCTION_UNIMPLEMENTED(cmpltui),
 	[CUSTOM]	= INSTRUCTION_UNIMPLEMENTED(custom),
 	[INITD]		= INSTRUCTION(initd),
-	[ORHI]		= INSTRUCTION_UNIMPLEMENTED(orhi),
+	[ORHI]		= INSTRUCTION(orhi),
 	[STWIO]		= INSTRUCTION_UNIMPLEMENTED(stwio),
-	[BLTU]		= INSTRUCTION_UNIMPLEMENTED(bltu),
+	[BLTU]		= INSTRUCTION(bltu),
 	[LDWIO]		= INSTRUCTION_UNIMPLEMENTED(ldwio),
 	[R_TYPE]	= { "<R-type instruction>", handle_r_type_instr },
 	[FLUSHD]	= INSTRUCTION_NOP(flushd),
@@ -393,7 +425,7 @@ 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;
+	uint32_t shift_bits = instr->imm5;
 
 	gp_regs[instr->c] = (a  << shift_bits) | (a >> (32 - shift_bits));
 
@@ -454,7 +486,7 @@ static int ror(struct nios2 *cpu, uint32_t code)
 }
 
 /* */
-static int flushi(struct nios2 *cpu, uint32_t code)
+static int flushi(struct nios2 *cpu __unused, uint32_t code __unused)
 {
 	/* TODO */
 
@@ -507,7 +539,7 @@ 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;
+	gp_regs[instr->c] = gp_regs[instr->a] << instr->imm5;
 
 	return PC_INC_NORMAL;
 }
@@ -559,7 +591,7 @@ 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;
+	gp_regs[instr->c] = gp_regs[instr->a] >> instr->imm5;
 
 	return PC_INC_NORMAL;
 }
@@ -640,7 +672,7 @@ static int div(struct nios2 *cpu, uint32_t code)
 	if (b == 0)
 		return EXCEPTION(NIOS2_EX_DIV_ERR);
 	/* Divide overflow? */
-	if (a == 0x80000000 && b == 0xFFFFFFFF)
+	if ((uint32_t) a == 0x80000000 && (uint32_t) b == 0xFFFFFFFF)
 		return EXCEPTION(NIOS2_EX_DIV_ERR);
 
 	gp_regs[instr->c] = (uint32_t)(a / b);
@@ -657,7 +689,7 @@ static int rdctl(struct nios2 *cpu, uint32_t code)
 	if (!nios2_in_supervisor_mode(cpu))
 		return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
 
-	cpu->gp_regs[instr->c] = cpu->ctrl_regs[instr->opx.imm5];
+	cpu->gp_regs[instr->c] = cpu->ctrl_regs[instr->imm5];
 
 	return PC_INC_NORMAL;
 }
@@ -700,7 +732,7 @@ static int wrctl(struct nios2 *cpu, uint32_t code)
 	if (!nios2_in_supervisor_mode(cpu))
 		return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
 
-	cpu->ctrl_regs[instr->opx.imm5] = cpu->gp_regs[instr->a];
+	cpu->ctrl_regs[instr->imm5] = cpu->gp_regs[instr->a];
 
 	return PC_INC_NORMAL;
 }
@@ -797,6 +829,7 @@ instruction_handler instruction_get_handler(uint32_t code)
 
 	if (unlikely(op >= I_TYPE_COUNT))
 		return NULL;
+	dbg("%s (%08x)\n", i_type_instructions[op].name, code);
 	return i_type_instructions[op].handler;
 }