1 files changed, 105 insertions, 0 deletions
diff --git a/arch/riscv/kernel/probes/simulate-insn.c b/arch/riscv/kernel/probes/simulate-insn.c
index 7441ac8a6843..d3099d67816d 100644
--- a/arch/riscv/kernel/probes/simulate-insn.c
+++ b/arch/riscv/kernel/probes/simulate-insn.c
@@ -188,3 +188,108 @@ bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *r
 
 	return true;
 }
+
+bool __kprobes simulate_c_j(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+	/*
+	 *  15    13 12                            2 1      0
+	 * | funct3 | offset[11|4|9:8|10|6|7|3:1|5] | opcode |
+	 *     3                   11                    2
+	 */
+
+	s32 offset;
+
+	offset  = ((opcode >> 3)  & 0x7) << 1;
+	offset |= ((opcode >> 11) & 0x1) << 4;
+	offset |= ((opcode >> 2)  & 0x1) << 5;
+	offset |= ((opcode >> 7)  & 0x1) << 6;
+	offset |= ((opcode >> 6)  & 0x1) << 7;
+	offset |= ((opcode >> 9)  & 0x3) << 8;
+	offset |= ((opcode >> 8)  & 0x1) << 10;
+	offset |= ((opcode >> 12) & 0x1) << 11;
+
+	instruction_pointer_set(regs, addr + sign_extend32(offset, 11));
+
+	return true;
+}
+
+static bool __kprobes simulate_c_jr_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs,
+					 bool is_jalr)
+{
+	/*
+	 *  15    12 11  7 6   2 1  0
+	 * | funct4 | rs1 | rs2 | op |
+	 *     4       5     5    2
+	 */
+
+	unsigned long jump_addr;
+
+	u32 rs1 = (opcode >> 7) & 0x1f;
+
+	if (rs1 == 0) /* C.JR is only valid when rs1 != x0 */
+		return false;
+
+	if (!rv_insn_reg_get_val(regs, rs1, &jump_addr))
+		return false;
+
+	if (is_jalr && !rv_insn_reg_set_val(regs, 1, addr + 2))
+		return false;
+
+	instruction_pointer_set(regs, jump_addr);
+
+	return true;
+}
+
+bool __kprobes simulate_c_jr(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+	return simulate_c_jr_jalr(opcode, addr, regs, false);
+}
+
+bool __kprobes simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+	return simulate_c_jr_jalr(opcode, addr, regs, true);
+}
+
+static bool __kprobes simulate_c_bnez_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs,
+					   bool is_bnez)
+{
+	/*
+	 *  15    13 12           10 9    7 6                 2 1  0
+	 * | funct3 | offset[8|4:3] | rs1' | offset[7:6|2:1|5] | op |
+	 *     3            3          3             5           2
+	 */
+
+	s32 offset;
+	u32 rs1;
+	unsigned long rs1_val;
+
+	rs1 = 0x8 | ((opcode >> 7) & 0x7);
+
+	if (!rv_insn_reg_get_val(regs, rs1, &rs1_val))
+		return false;
+
+	if ((rs1_val != 0 && is_bnez) || (rs1_val == 0 && !is_bnez)) {
+		offset =  ((opcode >> 3)  & 0x3) << 1;
+		offset |= ((opcode >> 10) & 0x3) << 3;
+		offset |= ((opcode >> 2)  & 0x1) << 5;
+		offset |= ((opcode >> 5)  & 0x3) << 6;
+		offset |= ((opcode >> 12) & 0x1) << 8;
+		offset = sign_extend32(offset, 8);
+	} else {
+		offset = 2;
+	}
+
+	instruction_pointer_set(regs, addr + offset);
+
+	return true;
+}
+
+bool __kprobes simulate_c_bnez(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+	return simulate_c_bnez_beqz(opcode, addr, regs, true);
+}
+
+bool __kprobes simulate_c_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+	return simulate_c_bnez_beqz(opcode, addr, regs, false);
+}