diff options
| -rw-r--r-- | instruction.c | 58 |
1 files changed, 51 insertions, 7 deletions
diff --git a/instruction.c b/instruction.c index f41ad63..4bd216b 100644 --- a/instruction.c +++ b/instruction.c @@ -607,6 +607,30 @@ static int srl(struct nios2 *cpu, uint32_t code) 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) { @@ -759,6 +783,28 @@ static int sub(struct nios2 *cpu, uint32_t code) 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), @@ -781,8 +827,8 @@ static struct instruction r_type_instructions[R_TYPE_COUNT] = { [CMPNE] = INSTRUCTION(cmpne), [SRLI] = INSTRUCTION(srli), [SRL] = INSTRUCTION(srl), - [NEXTPC] = INSTRUCTION_UNIMPLEMENTED(nextpc), - [CALLR] = INSTRUCTION_UNIMPLEMENTED(callr), + [NEXTPC] = INSTRUCTION(nextpc), + [CALLR] = INSTRUCTION(callr), [XOR] = INSTRUCTION(xor), [MULXSS] = INSTRUCTION_UNIMPLEMENTED(mulxss), [CMPEQ] = INSTRUCTION(cmpeq), @@ -799,8 +845,8 @@ static struct instruction r_type_instructions[R_TYPE_COUNT] = { [BREAK] = INSTRUCTION_UNIMPLEMENTED(break), [SYNC] = INSTRUCTION(nop), [SUB] = INSTRUCTION(sub), - [SRAI] = INSTRUCTION_UNIMPLEMENTED(srai), - [SRA] = INSTRUCTION_UNIMPLEMENTED(sra), + [SRAI] = INSTRUCTION(srai), + [SRA] = INSTRUCTION(sra), }; static int handle_r_type_instr(struct nios2 *cpu, uint32_t code) @@ -809,10 +855,8 @@ static int handle_r_type_instr(struct nios2 *cpu, uint32_t code) instruction_handler handle_instr; opx = get_opxcode(code); - if (unlikely(opx >= R_TYPE_COUNT)) { - err("Invalid OPX code %08x\n", opx); + 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; |