summaryrefslogtreecommitdiff
path: root/instruction.c
diff options
context:
space:
mode:
Diffstat (limited to 'instruction.c')
-rw-r--r--instruction.c73
1 files changed, 50 insertions, 23 deletions
diff --git a/instruction.c b/instruction.c
index 6fd06dc..8b10093 100644
--- a/instruction.c
+++ b/instruction.c
@@ -14,6 +14,12 @@
#include "instruction.h"
#include "nios2.h"
+#if 1
+# define instr_dbg(fmt, args...) dbg(fmt, ##args)
+#else
+# define instr_dbg(fmt, args...)
+#endif
+
static inline uint32_t get_opcode(uint32_t code)
{
I_TYPE(instr, code);
@@ -62,6 +68,11 @@ static int call(struct nios2 *cpu, uint32_t code)
cpu->gp_regs[ra] = cpu->pc + 4;
cpu->pc = (cpu->pc & 0xF0000000) | (instr->imm26 * 4);
+ if (cpu->pc == 0) {
+ err("PC = 0\n");
+ return INSTR_ERR;
+ }
+
return PC_INC_BY_INSTR;
}
@@ -340,7 +351,7 @@ static int muli(struct nios2 *cpu, uint32_t code)
/* Raise exception if instruction is not implemented */
if (!cpu->has_mul)
- return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+ return EXCEPTION(cpu, NIOS2_EX_UNIMPLEMENTED);
tmp = gp_regs[instr->a] * ((int16_t) instr->imm16);
gp_regs[instr->b] = (uint32_t) (tmp & BIT_MASK_FILL(32));
@@ -377,6 +388,25 @@ static int andhi(struct nios2 *cpu, uint32_t code)
return PC_INC_NORMAL;
}
+/*
+ * if (rA >= rB)
+ * PC <- PC + 4 + @(IMM16)
+ * else
+ * PC <- PC + 4
+ */
+static int bgeu(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;
+}
+
/* */
static int initd(struct nios2 *cpu __unused, uint32_t code __unused)
{
@@ -463,7 +493,7 @@ static struct instruction i_type_instructions[I_TYPE_COUNT] = {
[LDHUIO] = INSTRUCTION_UNIMPLEMENTED(ldhuio),
[ANDHI] = INSTRUCTION(andhi),
[STHIO] = INSTRUCTION_UNIMPLEMENTED(sthio),
- [BGEU] = INSTRUCTION_UNIMPLEMENTED(bgeu),
+ [BGEU] = INSTRUCTION(bgeu),
[LDHIO] = INSTRUCTION_UNIMPLEMENTED(ldhio),
[CMPLTUI] = INSTRUCTION_UNIMPLEMENTED(cmpltui),
[CUSTOM] = INSTRUCTION_UNIMPLEMENTED(custom),
@@ -742,11 +772,11 @@ static int divu(struct nios2 *cpu, uint32_t code)
uint32_t *gp_regs = cpu->gp_regs;
if (!cpu->has_div)
- return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+ return EXCEPTION(cpu, NIOS2_EX_UNIMPLEMENTED);
/* Division by zero? */
if (gp_regs[instr->b] == 0)
- return EXCEPTION(NIOS2_EX_DIV_ERR);
+ return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
gp_regs[instr->c] = gp_regs[instr->a] / gp_regs[instr->b];
@@ -761,17 +791,17 @@ static int div(struct nios2 *cpu, uint32_t code)
int32_t a, b;
if (!cpu->has_div)
- return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+ return EXCEPTION(cpu, 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);
+ return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
/* Divide overflow? */
if ((uint32_t) a == 0x80000000 && (uint32_t) b == 0xFFFFFFFF)
- return EXCEPTION(NIOS2_EX_DIV_ERR);
+ return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
gp_regs[instr->c] = (uint32_t)(a / b);
@@ -785,7 +815,7 @@ static int rdctl(struct nios2 *cpu, uint32_t code)
/* Instruction only allowed in supervisor mode */
if (!nios2_in_supervisor_mode(cpu))
- return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
+ return EXCEPTION(cpu, NIOS2_EX_SUPERVISOR_ONLY_I);
cpu->gp_regs[instr->c] = cpu->ctrl_regs[instr->imm5];
@@ -807,18 +837,9 @@ static int initi(struct nios2 *cpu __unused, uint32_t code __unused)
* ea <- PC + 4
* PC <- exception handler address
*/
-static int trap(struct nios2 *cpu, uint32_t code __unused)
+static int trap(struct nios2 *cpu __unused, 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;
+ return EXCEPTION(cpu, NIOS2_EX_TRAP);
}
/* ctlN <- rA */
@@ -828,7 +849,7 @@ static int wrctl(struct nios2 *cpu, uint32_t code)
/* Instruction only allowed in supervisor mode */
if (!nios2_in_supervisor_mode(cpu))
- return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
+ return EXCEPTION(cpu, NIOS2_EX_SUPERVISOR_ONLY_I);
cpu->ctrl_regs[instr->imm5] = cpu->gp_regs[instr->a];
@@ -846,6 +867,12 @@ static int add(struct nios2 *cpu, uint32_t code)
return PC_INC_NORMAL;
}
+/* */
+static int __break(struct nios2 *cpu, uint32_t code)
+{
+ return INSTR_BREAK;
+}
+
/* rC <- rA - rB */
static int sub(struct nios2 *cpu, uint32_t code)
{
@@ -916,7 +943,7 @@ static struct instruction r_type_instructions[R_TYPE_COUNT] = {
[WRCTL] = INSTRUCTION(wrctl),
[CMPLTU] = INSTRUCTION_UNIMPLEMENTED(cmpltu),
[ADD] = INSTRUCTION(add),
- [BREAK] = INSTRUCTION_UNIMPLEMENTED(break),
+ [BREAK] = { "break", __break },
[SYNC] = INSTRUCTION(nop),
[SUB] = INSTRUCTION(sub),
[SRAI] = INSTRUCTION(srai),
@@ -932,7 +959,7 @@ static int handle_r_type_instr(struct nios2 *cpu, uint32_t code)
if (unlikely(opx >= R_TYPE_COUNT))
return INSTR_ERR;
-// dbg(" R: %s (%08x)\n", r_type_instructions[opx].name, code);
+ instr_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;
@@ -946,7 +973,7 @@ instruction_handler instruction_get_handler(uint32_t code)
if (unlikely(op >= I_TYPE_COUNT))
return NULL;
-// dbg("I: %s (%08x)\n", i_type_instructions[op].name, code);
+ instr_dbg("I: %s (%08x)\n", i_type_instructions[op].name, code);
return i_type_instructions[op].handler;
}