riscv: kprobes: simulate c.beqz and c.bnez

kprobes currently rejects instruction c.beqz and c.bnez. Implement them.

Signed-off-by: Nam Cao <namcaov@gmail.com>
Reviewed-by: Charlie Jenkins <charlie@rivosinc.com>
Link: https://lore.kernel.org/r/1d879dba4e4ee9a82e27625d6483b5c9cfed684f.1690704360.git.namcaov@gmail.com
Signed-off-by: Palmer Dabbelt <palmer@rivosinc.com>

diff --git a/arch/riscv/kernel/probes/decode-insn.c b/arch/riscv/kernel/probes/decode-insn.c
index 6dba23a..65d9590 100644 (file)
--- a/arch/riscv/kernel/probes/decode-insn.c
+++ b/arch/riscv/kernel/probes/decode-insn.c
@@ -30,13 +30,13 @@ riscv_probe_decode_insn(probe_opcode_t *addr, struct arch_probe_insn *api)
         */
 #ifdef CONFIG_RISCV_ISA_C
        RISCV_INSN_REJECTED(c_jal,              insn);
-       RISCV_INSN_REJECTED(c_beqz,             insn);
-       RISCV_INSN_REJECTED(c_bnez,             insn);
        RISCV_INSN_REJECTED(c_ebreak,           insn);
 
        RISCV_INSN_SET_SIMULATE(c_j,            insn);
        RISCV_INSN_SET_SIMULATE(c_jr,           insn);
        RISCV_INSN_SET_SIMULATE(c_jalr,         insn);
+       RISCV_INSN_SET_SIMULATE(c_beqz,         insn);
+       RISCV_INSN_SET_SIMULATE(c_bnez,         insn);
 #endif
 
        RISCV_INSN_SET_SIMULATE(jal,            insn);

diff --git a/arch/riscv/kernel/probes/simulate-insn.c b/arch/riscv/kernel/probes/simulate-insn.c
index 1ead6f4..d3099d6 100644 (file)
--- a/arch/riscv/kernel/probes/simulate-insn.c
+++ b/arch/riscv/kernel/probes/simulate-insn.c
@@ -249,3 +249,47 @@ bool __kprobes simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *r
 {
        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);
+}

diff --git a/arch/riscv/kernel/probes/simulate-insn.h b/arch/riscv/kernel/probes/simulate-insn.h
index 472a194..44ebbc4 100644 (file)
--- a/arch/riscv/kernel/probes/simulate-insn.h
+++ b/arch/riscv/kernel/probes/simulate-insn.h
@@ -27,5 +27,7 @@ bool simulate_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs);
 bool simulate_c_j(u32 opcode, unsigned long addr, struct pt_regs *regs);
 bool simulate_c_jr(u32 opcode, unsigned long addr, struct pt_regs *regs);
 bool simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs);
+bool simulate_c_bnez(u32 opcode, unsigned long addr, struct pt_regs *regs);
+bool simulate_c_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs);
 
 #endif /* _RISCV_KERNEL_PROBES_SIMULATE_INSN_H */