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 csky_insn_reg_get_val(struct pt_regs *regs,
11 					 unsigned long index,
12 					 unsigned long *ptr)
13 {
14 	if (index < 14)
15 		*ptr = *(&regs->a0 + index);
16 
17 	if (index > 15 && index < 31)
18 		*ptr = *(&regs->exregs[0] + index - 16);
19 
20 	switch (index) {
21 	case 14:
22 		*ptr = regs->usp;
23 		break;
24 	case 15:
25 		*ptr = regs->lr;
26 		break;
27 	case 31:
28 		*ptr = regs->tls;
29 		break;
30 	default:
31 		goto fail;
32 	}
33 
34 	return true;
35 fail:
36 	return false;
37 }
38 
39 static inline bool csky_insn_reg_set_val(struct pt_regs *regs,
40 					 unsigned long index,
41 					 unsigned long val)
42 {
43 	if (index < 14)
44 		*(&regs->a0 + index) = val;
45 
46 	if (index > 15 && index < 31)
47 		*(&regs->exregs[0] + index - 16) = val;
48 
49 	switch (index) {
50 	case 14:
51 		regs->usp = val;
52 		break;
53 	case 15:
54 		regs->lr = val;
55 		break;
56 	case 31:
57 		regs->tls = val;
58 		break;
59 	default:
60 		goto fail;
61 	}
62 
63 	return true;
64 fail:
65 	return false;
66 }
67 
68 void __kprobes
69 simulate_br16(u32 opcode, long addr, struct pt_regs *regs)
70 {
71 	instruction_pointer_set(regs,
72 		addr + sign_extend32((opcode & 0x3ff) << 1, 9));
73 }
74 
75 void __kprobes
76 simulate_br32(u32 opcode, long addr, struct pt_regs *regs)
77 {
78 	instruction_pointer_set(regs,
79 		addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
80 }
81 
82 void __kprobes
83 simulate_bt16(u32 opcode, long addr, struct pt_regs *regs)
84 {
85 	if (regs->sr & 1)
86 		instruction_pointer_set(regs,
87 			addr + sign_extend32((opcode & 0x3ff) << 1, 9));
88 	else
89 		instruction_pointer_set(regs, addr + 2);
90 }
91 
92 void __kprobes
93 simulate_bt32(u32 opcode, long addr, struct pt_regs *regs)
94 {
95 	if (regs->sr & 1)
96 		instruction_pointer_set(regs,
97 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
98 	else
99 		instruction_pointer_set(regs, addr + 4);
100 }
101 
102 void __kprobes
103 simulate_bf16(u32 opcode, long addr, struct pt_regs *regs)
104 {
105 	if (!(regs->sr & 1))
106 		instruction_pointer_set(regs,
107 			addr + sign_extend32((opcode & 0x3ff) << 1, 9));
108 	else
109 		instruction_pointer_set(regs, addr + 2);
110 }
111 
112 void __kprobes
113 simulate_bf32(u32 opcode, long addr, struct pt_regs *regs)
114 {
115 	if (!(regs->sr & 1))
116 		instruction_pointer_set(regs,
117 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
118 	else
119 		instruction_pointer_set(regs, addr + 4);
120 }
121 
122 void __kprobes
123 simulate_jmp16(u32 opcode, long addr, struct pt_regs *regs)
124 {
125 	unsigned long tmp = (opcode >> 2) & 0xf;
126 
127 	csky_insn_reg_get_val(regs, tmp, &tmp);
128 
129 	instruction_pointer_set(regs, tmp & 0xfffffffe);
130 }
131 
132 void __kprobes
133 simulate_jmp32(u32 opcode, long addr, struct pt_regs *regs)
134 {
135 	unsigned long tmp = opcode & 0x1f;
136 
137 	csky_insn_reg_get_val(regs, tmp, &tmp);
138 
139 	instruction_pointer_set(regs, tmp & 0xfffffffe);
140 }
141 
142 void __kprobes
143 simulate_jsr16(u32 opcode, long addr, struct pt_regs *regs)
144 {
145 	unsigned long tmp = (opcode >> 2) & 0xf;
146 
147 	csky_insn_reg_get_val(regs, tmp, &tmp);
148 
149 	regs->lr = addr + 2;
150 
151 	instruction_pointer_set(regs, tmp & 0xfffffffe);
152 }
153 
154 void __kprobes
155 simulate_jsr32(u32 opcode, long addr, struct pt_regs *regs)
156 {
157 	unsigned long tmp = opcode & 0x1f;
158 
159 	csky_insn_reg_get_val(regs, tmp, &tmp);
160 
161 	regs->lr = addr + 4;
162 
163 	instruction_pointer_set(regs, tmp & 0xfffffffe);
164 }
165 
166 void __kprobes
167 simulate_lrw16(u32 opcode, long addr, struct pt_regs *regs)
168 {
169 	unsigned long val;
170 	unsigned long tmp = (opcode & 0x300) >> 3;
171 	unsigned long offset = ((opcode & 0x1f) | tmp) << 2;
172 
173 	tmp = (opcode & 0xe0) >> 5;
174 
175 	val = *(unsigned int *)(instruction_pointer(regs) + offset);
176 
177 	csky_insn_reg_set_val(regs, tmp, val);
178 }
179 
180 void __kprobes
181 simulate_lrw32(u32 opcode, long addr, struct pt_regs *regs)
182 {
183 	unsigned long val;
184 	unsigned long offset = (opcode & 0xffff0000) >> 14;
185 	unsigned long tmp = opcode & 0x0000001f;
186 
187 	val = *(unsigned int *)
188 		((instruction_pointer(regs) + offset) & 0xfffffffc);
189 
190 	csky_insn_reg_set_val(regs, tmp, val);
191 }
192 
193 void __kprobes
194 simulate_pop16(u32 opcode, long addr, struct pt_regs *regs)
195 {
196 	unsigned long *tmp = (unsigned long *)regs->usp;
197 	int i;
198 
199 	for (i = 0; i < (opcode & 0xf); i++) {
200 		csky_insn_reg_set_val(regs, i + 4, *tmp);
201 		tmp += 1;
202 	}
203 
204 	if (opcode & 0x10) {
205 		csky_insn_reg_set_val(regs, 15, *tmp);
206 		tmp += 1;
207 	}
208 
209 	regs->usp = (unsigned long)tmp;
210 
211 	instruction_pointer_set(regs, regs->lr);
212 }
213 
214 void __kprobes
215 simulate_pop32(u32 opcode, long addr, struct pt_regs *regs)
216 {
217 	unsigned long *tmp = (unsigned long *)regs->usp;
218 	int i;
219 
220 	for (i = 0; i < ((opcode & 0xf0000) >> 16); i++) {
221 		csky_insn_reg_set_val(regs, i + 4, *tmp);
222 		tmp += 1;
223 	}
224 
225 	if (opcode & 0x100000) {
226 		csky_insn_reg_set_val(regs, 15, *tmp);
227 		tmp += 1;
228 	}
229 
230 	for (i = 0; i < ((opcode & 0xe00000) >> 21); i++) {
231 		csky_insn_reg_set_val(regs, i + 16, *tmp);
232 		tmp += 1;
233 	}
234 
235 	if (opcode & 0x1000000) {
236 		csky_insn_reg_set_val(regs, 29, *tmp);
237 		tmp += 1;
238 	}
239 
240 	regs->usp = (unsigned long)tmp;
241 
242 	instruction_pointer_set(regs, regs->lr);
243 }
244 
245 void __kprobes
246 simulate_bez32(u32 opcode, long addr, struct pt_regs *regs)
247 {
248 	unsigned long tmp = opcode & 0x1f;
249 
250 	csky_insn_reg_get_val(regs, tmp, &tmp);
251 
252 	if (tmp == 0) {
253 		instruction_pointer_set(regs,
254 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
255 	} else
256 		instruction_pointer_set(regs, addr + 4);
257 }
258 
259 void __kprobes
260 simulate_bnez32(u32 opcode, long addr, struct pt_regs *regs)
261 {
262 	unsigned long tmp = opcode & 0x1f;
263 
264 	csky_insn_reg_get_val(regs, tmp, &tmp);
265 
266 	if (tmp != 0) {
267 		instruction_pointer_set(regs,
268 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
269 	} else
270 		instruction_pointer_set(regs, addr + 4);
271 }
272 
273 void __kprobes
274 simulate_bnezad32(u32 opcode, long addr, struct pt_regs *regs)
275 {
276 	unsigned long tmp = opcode & 0x1f;
277 	unsigned long val;
278 
279 	csky_insn_reg_get_val(regs, tmp, &val);
280 
281 	val -= 1;
282 
283 	if (val > 0) {
284 		instruction_pointer_set(regs,
285 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
286 	} else
287 		instruction_pointer_set(regs, addr + 4);
288 
289 	csky_insn_reg_set_val(regs, tmp, val);
290 }
291 
292 void __kprobes
293 simulate_bhsz32(u32 opcode, long addr, struct pt_regs *regs)
294 {
295 	unsigned long tmp = opcode & 0x1f;
296 	unsigned long val;
297 
298 	csky_insn_reg_get_val(regs, tmp, &val);
299 
300 	if (val >= 0) {
301 		instruction_pointer_set(regs,
302 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
303 	} else
304 		instruction_pointer_set(regs, addr + 4);
305 
306 	csky_insn_reg_set_val(regs, tmp, val);
307 }
308 
309 void __kprobes
310 simulate_bhz32(u32 opcode, long addr, struct pt_regs *regs)
311 {
312 	unsigned long tmp = opcode & 0x1f;
313 	unsigned long val;
314 
315 	csky_insn_reg_get_val(regs, tmp, &val);
316 
317 	if (val > 0) {
318 		instruction_pointer_set(regs,
319 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
320 	} else
321 		instruction_pointer_set(regs, addr + 4);
322 
323 	csky_insn_reg_set_val(regs, tmp, val);
324 }
325 
326 void __kprobes
327 simulate_blsz32(u32 opcode, long addr, struct pt_regs *regs)
328 {
329 	unsigned long tmp = opcode & 0x1f;
330 	unsigned long val;
331 
332 	csky_insn_reg_get_val(regs, tmp, &val);
333 
334 	if (val <= 0) {
335 		instruction_pointer_set(regs,
336 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
337 	} else
338 		instruction_pointer_set(regs, addr + 4);
339 
340 	csky_insn_reg_set_val(regs, tmp, val);
341 }
342 
343 void __kprobes
344 simulate_blz32(u32 opcode, long addr, struct pt_regs *regs)
345 {
346 	unsigned long tmp = opcode & 0x1f;
347 	unsigned long val;
348 
349 	csky_insn_reg_get_val(regs, tmp, &val);
350 
351 	if (val < 0) {
352 		instruction_pointer_set(regs,
353 			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
354 	} else
355 		instruction_pointer_set(regs, addr + 4);
356 
357 	csky_insn_reg_set_val(regs, tmp, val);
358 }
359 
360 void __kprobes
361 simulate_bsr32(u32 opcode, long addr, struct pt_regs *regs)
362 {
363 	unsigned long tmp;
364 
365 	tmp = (opcode & 0xffff) << 16;
366 	tmp |= (opcode & 0xffff0000) >> 16;
367 
368 	instruction_pointer_set(regs,
369 		addr + sign_extend32((tmp & 0x3ffffff) << 1, 15));
370 
371 	regs->lr = addr + 4;
372 }
373 
374 void __kprobes
375 simulate_jmpi32(u32 opcode, long addr, struct pt_regs *regs)
376 {
377 	unsigned long val;
378 	unsigned long offset = ((opcode & 0xffff0000) >> 14);
379 
380 	val = *(unsigned int *)
381 		((instruction_pointer(regs) + offset) & 0xfffffffc);
382 
383 	instruction_pointer_set(regs, val);
384 }
385 
386 void __kprobes
387 simulate_jsri32(u32 opcode, long addr, struct pt_regs *regs)
388 {
389 	unsigned long val;
390 	unsigned long offset = ((opcode & 0xffff0000) >> 14);
391 
392 	val = *(unsigned int *)
393 		((instruction_pointer(regs) + offset) & 0xfffffffc);
394 
395 	regs->lr = addr + 4;
396 
397 	instruction_pointer_set(regs, val);
398 }
399