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_set_val(regs, rd_index, addr + 4);
75 	if (!ret)
76 		return ret;
77 
78 	ret = rv_insn_reg_get_val(regs, rs1_index, &base_addr);
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