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 false; 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 } 296