arch/riscv/kernel/probes/simulate-insn.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2
   3 #include <linux/bitops.h>
   4 #include <linux/kernel.h>
   5 #include <linux/kprobes.h>
   6
   7 #include "decode-insn.h"
   8 #include "simulate-insn.h"
   9
  10 static inline bool rv_insn_reg_get_val(struct pt_regs *regs, u32 index,
  11                                        unsigned long *ptr)
  12 {
  13         if (index == 0)
  14                 *ptr = 0;
  15         else if (index <= 31)
  16                 *ptr = *((unsigned long *)regs + index);
  17         else
  18                 return false;
  19
  20         return true;
  21 }
  22
  23 static inline bool rv_insn_reg_set_val(struct pt_regs *regs, u32 index,
  24                                        unsigned long val)
  25 {
  26         if (index == 0)
  27                 return true;
  28         else if (index <= 31)
  29                 *((unsigned long *)regs + index) = val;
  30         else
  31                 return false;
  32
  33         return true;
  34 }
  35
  36 bool __kprobes simulate_jal(u32 opcode, unsigned long addr, struct pt_regs *regs)
  37 {
  38         /*
  39          *     31    30       21    20     19        12 11 7 6      0
  40          * imm [20] | imm[10:1] | imm[11] | imm[19:12] | rd | opcode
  41          *     1         10          1           8       5    JAL/J
  42          */
  43         bool ret;
  44         u32 imm;
  45         u32 index = (opcode >> 7) & 0x1f;
  46
  47         ret = rv_insn_reg_set_val(regs, index, addr + 4);
  48         if (!ret)
  49                 return ret;
  50
  51         imm  = ((opcode >> 21) & 0x3ff) << 1;
  52         imm |= ((opcode >> 20) & 0x1)   << 11;
  53         imm |= ((opcode >> 12) & 0xff)  << 12;
  54         imm |= ((opcode >> 31) & 0x1)   << 20;
  55
  56         instruction_pointer_set(regs, addr + sign_extend32((imm), 20));
  57
  58         return ret;
  59 }
  60
  61 bool __kprobes simulate_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
  62 {
  63         /*
  64          * 31          20 19 15 14 12 11 7 6      0
  65          *  offset[11:0] | rs1 | 010 | rd | opcode
  66          *      12         5      3    5    JALR/JR
  67          */
  68         bool ret;
  69         unsigned long base_addr;
  70         u32 imm = (opcode >> 20) & 0xfff;
  71         u32 rd_index = (opcode >> 7) & 0x1f;
  72         u32 rs1_index = (opcode >> 15) & 0x1f;
  73
  74         ret = rv_insn_reg_get_val(regs, rs1_index, &base_addr);
  75         if (!ret)
  76                 return ret;
  77
  78         ret = rv_insn_reg_set_val(regs, rd_index, addr + 4);
  79         if (!ret)
  80                 return ret;
  81
  82         instruction_pointer_set(regs, (base_addr + sign_extend32((imm), 11))&~1);
  83
  84         return ret;
  85 }
  86
  87 #define auipc_rd_idx(opcode) \
  88         ((opcode >> 7) & 0x1f)
  89
  90 #define auipc_imm(opcode) \
  91         ((((opcode) >> 12) & 0xfffff) << 12)
  92
  93 #if __riscv_xlen == 64
  94 #define auipc_offset(opcode)    sign_extend64(auipc_imm(opcode), 31)
  95 #elif __riscv_xlen == 32
  96 #define auipc_offset(opcode)    auipc_imm(opcode)
  97 #else
  98 #error "Unexpected __riscv_xlen"
  99 #endif
 100
 101 bool __kprobes simulate_auipc(u32 opcode, unsigned long addr, struct pt_regs *regs)
 102 {
 103         /*
 104          * auipc instruction:
 105          *  31        12 11 7 6      0
 106          * | imm[31:12] | rd | opcode |
 107          *        20       5     7
 108          */
 109
 110         u32 rd_idx = auipc_rd_idx(opcode);
 111         unsigned long rd_val = addr + auipc_offset(opcode);
 112
 113         if (!rv_insn_reg_set_val(regs, rd_idx, rd_val))
 114                 return false;
 115
 116         instruction_pointer_set(regs, addr + 4);
 117
 118         return true;
 119 }
 120
 121 #define branch_rs1_idx(opcode) \
 122         (((opcode) >> 15) & 0x1f)
 123
 124 #define branch_rs2_idx(opcode) \
 125         (((opcode) >> 20) & 0x1f)
 126
 127 #define branch_funct3(opcode) \
 128         (((opcode) >> 12) & 0x7)
 129
 130 #define branch_imm(opcode) \
 131         (((((opcode) >>  8) & 0xf ) <<  1) | \
 132          ((((opcode) >> 25) & 0x3f) <<  5) | \
 133          ((((opcode) >>  7) & 0x1 ) << 11) | \
 134          ((((opcode) >> 31) & 0x1 ) << 12))
 135
 136 #define branch_offset(opcode) \
 137         sign_extend32((branch_imm(opcode)), 12)
 138
 139 bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *regs)
 140 {
 141         /*
 142          * branch instructions:
 143          *      31    30       25 24 20 19 15 14    12 11       8    7      6      0
 144          * | imm[12] | imm[10:5] | rs2 | rs1 | funct3 | imm[4:1] | imm[11] | opcode |
 145          *     1           6        5     5      3         4         1         7
 146          *     imm[12|10:5]        rs2   rs1    000       imm[4:1|11]       1100011  BEQ
 147          *     imm[12|10:5]        rs2   rs1    001       imm[4:1|11]       1100011  BNE
 148          *     imm[12|10:5]        rs2   rs1    100       imm[4:1|11]       1100011  BLT
 149          *     imm[12|10:5]        rs2   rs1    101       imm[4:1|11]       1100011  BGE
 150          *     imm[12|10:5]        rs2   rs1    110       imm[4:1|11]       1100011  BLTU
 151          *     imm[12|10:5]        rs2   rs1    111       imm[4:1|11]       1100011  BGEU
 152          */
 153
 154         s32 offset;
 155         s32 offset_tmp;
 156         unsigned long rs1_val;
 157         unsigned long rs2_val;
 158
 159         if (!rv_insn_reg_get_val(regs, branch_rs1_idx(opcode), &rs1_val) ||
 160             !rv_insn_reg_get_val(regs, branch_rs2_idx(opcode), &rs2_val))
 161                 return false;
 162
 163         offset_tmp = branch_offset(opcode);
 164         switch (branch_funct3(opcode)) {
 165         case RVG_FUNCT3_BEQ:
 166                 offset = (rs1_val == rs2_val) ? offset_tmp : 4;
 167                 break;
 168         case RVG_FUNCT3_BNE:
 169                 offset = (rs1_val != rs2_val) ? offset_tmp : 4;
 170                 break;
 171         case RVG_FUNCT3_BLT:
 172                 offset = ((long)rs1_val < (long)rs2_val) ? offset_tmp : 4;
 173                 break;
 174         case RVG_FUNCT3_BGE:
 175                 offset = ((long)rs1_val >= (long)rs2_val) ? offset_tmp : 4;
 176                 break;
 177         case RVG_FUNCT3_BLTU:
 178                 offset = (rs1_val < rs2_val) ? offset_tmp : 4;
 179                 break;
 180         case RVG_FUNCT3_BGEU:
 181                 offset = (rs1_val >= rs2_val) ? offset_tmp : 4;
 182                 break;
 183         default:
 184                 return false;
 185         }
 186
 187         instruction_pointer_set(regs, addr + offset);
 188
 189         return true;
 190 }
 191
 192 bool __kprobes simulate_c_j(u32 opcode, unsigned long addr, struct pt_regs *regs)
 193 {
 194         /*
 195          *  15    13 12                            2 1      0
 196          * | funct3 | offset[11|4|9:8|10|6|7|3:1|5] | opcode |
 197          *     3                   11                    2
 198          */
 199
 200         s32 offset;
 201
 202         offset  = ((opcode >> 3)  & 0x7) << 1;
 203         offset |= ((opcode >> 11) & 0x1) << 4;
 204         offset |= ((opcode >> 2)  & 0x1) << 5;
 205         offset |= ((opcode >> 7)  & 0x1) << 6;
 206         offset |= ((opcode >> 6)  & 0x1) << 7;
 207         offset |= ((opcode >> 9)  & 0x3) << 8;
 208         offset |= ((opcode >> 8)  & 0x1) << 10;
 209         offset |= ((opcode >> 12) & 0x1) << 11;
 210
 211         instruction_pointer_set(regs, addr + sign_extend32(offset, 11));
 212
 213         return true;
 214 }
 215
 216 static bool __kprobes simulate_c_jr_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs,
 217                                          bool is_jalr)
 218 {
 219         /*
 220          *  15    12 11  7 6   2 1  0
 221          * | funct4 | rs1 | rs2 | op |
 222          *     4       5     5    2
 223          */
 224
 225         unsigned long jump_addr;
 226
 227         u32 rs1 = (opcode >> 7) & 0x1f;
 228
 229         if (rs1 == 0) /* C.JR is only valid when rs1 != x0 */
 230                 return false;
 231
 232         if (!rv_insn_reg_get_val(regs, rs1, &jump_addr))
 233                 return false;
 234
 235         if (is_jalr && !rv_insn_reg_set_val(regs, 1, addr + 2))
 236                 return false;
 237
 238         instruction_pointer_set(regs, jump_addr);
 239
 240         return true;
 241 }
 242
 243 bool __kprobes simulate_c_jr(u32 opcode, unsigned long addr, struct pt_regs *regs)
 244 {
 245         return simulate_c_jr_jalr(opcode, addr, regs, false);
 246 }
 247
 248 bool __kprobes simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
 249 {
 250         return simulate_c_jr_jalr(opcode, addr, regs, true);
 251 }
 252
 253 static bool __kprobes simulate_c_bnez_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs,
 254                                            bool is_bnez)
 255 {
 256         /*
 257          *  15    13 12           10 9    7 6                 2 1  0
 258          * | funct3 | offset[8|4:3] | rs1' | offset[7:6|2:1|5] | op |
 259          *     3            3          3             5           2
 260          */
 261
 262         s32 offset;
 263         u32 rs1;
 264         unsigned long rs1_val;
 265
 266         rs1 = 0x8 | ((opcode >> 7) & 0x7);
 267
 268         if (!rv_insn_reg_get_val(regs, rs1, &rs1_val))
 269                 return false;
 270
 271         if ((rs1_val != 0 && is_bnez) || (rs1_val == 0 && !is_bnez)) {
 272                 offset =  ((opcode >> 3)  & 0x3) << 1;
 273                 offset |= ((opcode >> 10) & 0x3) << 3;
 274                 offset |= ((opcode >> 2)  & 0x1) << 5;
 275                 offset |= ((opcode >> 5)  & 0x3) << 6;
 276                 offset |= ((opcode >> 12) & 0x1) << 8;
 277                 offset = sign_extend32(offset, 8);
 278         } else {
 279                 offset = 2;
 280         }
 281
 282         instruction_pointer_set(regs, addr + offset);
 283
 284         return true;
 285 }
 286
 287 bool __kprobes simulate_c_bnez(u32 opcode, unsigned long addr, struct pt_regs *regs)
 288 {
 289         return simulate_c_bnez_beqz(opcode, addr, regs, true);
 290 }
 291
 292 bool __kprobes simulate_c_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs)
 293 {
 294         return simulate_c_bnez_beqz(opcode, addr, regs, false);
 295 }