riscv: kprobes: implement the branch instructions

This has been tested by probing a module that contains each of the
flavors of branches we have.

Signed-off-by: Chen Lifu <chenlifu@huawei.com>
[Palmer: commit message, fix kconfig errors]
Signed-off-by: Palmer Dabbelt <palmerdabbelt@google.com>

diff --git a/arch/riscv/kernel/probes/decode-insn.c b/arch/riscv/kernel/probes/decode-insn.c
index 5eb03fb..64f6183 100644 (file)
--- a/arch/riscv/kernel/probes/decode-insn.c
+++ b/arch/riscv/kernel/probes/decode-insn.c
@@ -38,11 +38,10 @@ riscv_probe_decode_insn(probe_opcode_t *addr, struct arch_probe_insn *api)
        RISCV_INSN_REJECTED(c_ebreak,           insn);
 #endif
 
-       RISCV_INSN_REJECTED(branch,             insn);
-
        RISCV_INSN_SET_SIMULATE(jal,            insn);
        RISCV_INSN_SET_SIMULATE(jalr,           insn);
        RISCV_INSN_SET_SIMULATE(auipc,          insn);
+       RISCV_INSN_SET_SIMULATE(branch,         insn);
 
        return INSN_GOOD;
 }

diff --git a/arch/riscv/kernel/probes/simulate-insn.c b/arch/riscv/kernel/probes/simulate-insn.c
index b817195..d73e96f 100644 (file)
--- a/arch/riscv/kernel/probes/simulate-insn.c
+++ b/arch/riscv/kernel/probes/simulate-insn.c
@@ -117,3 +117,81 @@ bool __kprobes simulate_auipc(u32 opcode, unsigned long addr, struct pt_regs *re
 
        return true;
 }
+
+#define branch_rs1_idx(opcode) \
+       (((opcode) >> 15) & 0x1f)
+
+#define branch_rs2_idx(opcode) \
+       (((opcode) >> 20) & 0x1f)
+
+#define branch_funct3(opcode) \
+       (((opcode) >> 12) & 0x7)
+
+#define branch_imm(opcode) \
+       (((((opcode) >>  8) & 0xf ) <<  1) | \
+        ((((opcode) >> 25) & 0x3f) <<  5) | \
+        ((((opcode) >>  7) & 0x1 ) << 11) | \
+        ((((opcode) >> 31) & 0x1 ) << 12))
+
+#define branch_offset(opcode) \
+       sign_extend32((branch_imm(opcode)), 12)
+
+#define BRANCH_BEQ     0x0
+#define BRANCH_BNE     0x1
+#define BRANCH_BLT     0x4
+#define BRANCH_BGE     0x5
+#define BRANCH_BLTU    0x6
+#define BRANCH_BGEU    0x7
+
+bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *regs)
+{
+       /*
+        * branch instructions:
+        *      31    30       25 24 20 19 15 14    12 11       8    7      6      0
+        * | imm[12] | imm[10:5] | rs2 | rs1 | funct3 | imm[4:1] | imm[11] | opcode |
+        *     1           6        5     5      3         4         1         7
+        *     imm[12|10:5]        rs2   rs1    000       imm[4:1|11]       1100011  BEQ
+        *     imm[12|10:5]        rs2   rs1    001       imm[4:1|11]       1100011  BNE
+        *     imm[12|10:5]        rs2   rs1    100       imm[4:1|11]       1100011  BLT
+        *     imm[12|10:5]        rs2   rs1    101       imm[4:1|11]       1100011  BGE
+        *     imm[12|10:5]        rs2   rs1    110       imm[4:1|11]       1100011  BLTU
+        *     imm[12|10:5]        rs2   rs1    111       imm[4:1|11]       1100011  BGEU
+        */
+
+       s32 offset;
+       s32 offset_tmp;
+       unsigned long rs1_val;
+       unsigned long rs2_val;
+
+       if (!rv_insn_reg_get_val(regs, branch_rs1_idx(opcode), &rs1_val) ||
+           !rv_insn_reg_get_val(regs, branch_rs2_idx(opcode), &rs2_val))
+               return false;
+
+       offset_tmp = branch_offset(opcode);
+       switch (branch_funct3(opcode)) {
+       case BRANCH_BEQ:
+               offset = (rs1_val == rs2_val) ? offset_tmp : 4;
+               break;
+       case BRANCH_BNE:
+               offset = (rs1_val != rs2_val) ? offset_tmp : 4;
+               break;
+       case BRANCH_BLT:
+               offset = ((long)rs1_val < (long)rs2_val) ? offset_tmp : 4;
+               break;
+       case BRANCH_BGE:
+               offset = ((long)rs1_val >= (long)rs2_val) ? offset_tmp : 4;
+               break;
+       case BRANCH_BLTU:
+               offset = (rs1_val < rs2_val) ? offset_tmp : 4;
+               break;
+       case BRANCH_BGEU:
+               offset = (rs1_val >= rs2_val) ? offset_tmp : 4;
+               break;
+       default:
+               return false;
+       }
+
+       instruction_pointer_set(regs, addr + offset);
+
+       return true;
+}