From 9a1312e741193778718ed37b3779c2b964649e9d Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Wed, 24 Nov 2010 18:35:39 +0100 Subject: Change exception handling, more instructions implemented --- instruction.c | 73 ++++++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 50 insertions(+), 23 deletions(-) (limited to 'instruction.c') 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; } -- cgit v1.2.3-54-g00ecf