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
rv_insn_reg_get_val(struct pt_regs * regs,u32 index,unsigned long * ptr)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
rv_insn_reg_set_val(struct pt_regs * regs,u32 index,unsigned long val)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
simulate_jal(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_auipc(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_branch(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_c_j(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_c_jr_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs,bool is_jalr)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
simulate_c_jr(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_c_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_c_bnez_beqz(u32 opcode,unsigned long addr,struct pt_regs * regs,bool is_bnez)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
simulate_c_bnez(u32 opcode,unsigned long addr,struct pt_regs * regs)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
simulate_c_beqz(u32 opcode,unsigned long addr,struct pt_regs * regs)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