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