Implement nextpc, callr, srai, sra instructions

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;