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
 10static 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;
 35fail:
 36	return false;
 37}
 38
 39static 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;
 64fail:
 65	return false;
 66}
 67
 68void __kprobes
 69simulate_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
 75void __kprobes
 76simulate_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
 82void __kprobes
 83simulate_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
 92void __kprobes
 93simulate_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
102void __kprobes
103simulate_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
112void __kprobes
113simulate_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
122void __kprobes
123simulate_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
132void __kprobes
133simulate_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
142void __kprobes
143simulate_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
154void __kprobes
155simulate_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
166void __kprobes
167simulate_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
180void __kprobes
181simulate_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
193void __kprobes
194simulate_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
214void __kprobes
215simulate_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
245void __kprobes
246simulate_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
259void __kprobes
260simulate_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
273void __kprobes
274simulate_bnezad32(u32 opcode, long addr, struct pt_regs *regs)
275{
276	unsigned long tmp = opcode & 0x1f;
277	long val;
278
279	csky_insn_reg_get_val(regs, tmp, (unsigned long *)&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, (unsigned long)val);
290}
291
292void __kprobes
293simulate_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 ((long) 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
307void __kprobes
308simulate_bhz32(u32 opcode, long addr, struct pt_regs *regs)
309{
310	unsigned long tmp = opcode & 0x1f;
311	unsigned long val;
312
313	csky_insn_reg_get_val(regs, tmp, &val);
314
315	if ((long) val > 0) {
316		instruction_pointer_set(regs,
317			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
318	} else
319		instruction_pointer_set(regs, addr + 4);
320}
321
322void __kprobes
323simulate_blsz32(u32 opcode, long addr, struct pt_regs *regs)
324{
325	unsigned long tmp = opcode & 0x1f;
326	unsigned long val;
327
328	csky_insn_reg_get_val(regs, tmp, &val);
329
330	if ((long) val <= 0) {
331		instruction_pointer_set(regs,
332			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
333	} else
334		instruction_pointer_set(regs, addr + 4);
335}
336
337void __kprobes
338simulate_blz32(u32 opcode, long addr, struct pt_regs *regs)
339{
340	unsigned long tmp = opcode & 0x1f;
341	unsigned long val;
342
343	csky_insn_reg_get_val(regs, tmp, &val);
344
345	if ((long) val < 0) {
346		instruction_pointer_set(regs,
347			addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
348	} else
349		instruction_pointer_set(regs, addr + 4);
350}
351
352void __kprobes
353simulate_bsr32(u32 opcode, long addr, struct pt_regs *regs)
354{
355	unsigned long tmp;
356
357	tmp = (opcode & 0xffff) << 16;
358	tmp |= (opcode & 0xffff0000) >> 16;
359
360	instruction_pointer_set(regs,
361		addr + sign_extend32((tmp & 0x3ffffff) << 1, 15));
362
363	regs->lr = addr + 4;
364}
365
366void __kprobes
367simulate_jmpi32(u32 opcode, long addr, struct pt_regs *regs)
368{
369	unsigned long val;
370	unsigned long offset = ((opcode & 0xffff0000) >> 14);
371
372	val = *(unsigned int *)
373		((instruction_pointer(regs) + offset) & 0xfffffffc);
374
375	instruction_pointer_set(regs, val);
376}
377
378void __kprobes
379simulate_jsri32(u32 opcode, long addr, struct pt_regs *regs)
380{
381	unsigned long val;
382	unsigned long offset = ((opcode & 0xffff0000) >> 14);
383
384	val = *(unsigned int *)
385		((instruction_pointer(regs) + offset) & 0xfffffffc);
386
387	regs->lr = addr + 4;
388
389	instruction_pointer_set(regs, val);
390}