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 false;
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
139 #define BRANCH_BEQ 0x0
140 #define BRANCH_BNE 0x1
141 #define BRANCH_BLT 0x4
142 #define BRANCH_BGE 0x5
143 #define BRANCH_BLTU 0x6
144 #define BRANCH_BGEU 0x7
145
simulate_branch(u32 opcode,unsigned long addr,struct pt_regs * regs)146 bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *regs)
147 {
148 /*
149 * branch instructions:
150 * 31 30 25 24 20 19 15 14 12 11 8 7 6 0
151 * | imm[12] | imm[10:5] | rs2 | rs1 | funct3 | imm[4:1] | imm[11] | opcode |
152 * 1 6 5 5 3 4 1 7
153 * imm[12|10:5] rs2 rs1 000 imm[4:1|11] 1100011 BEQ
154 * imm[12|10:5] rs2 rs1 001 imm[4:1|11] 1100011 BNE
155 * imm[12|10:5] rs2 rs1 100 imm[4:1|11] 1100011 BLT
156 * imm[12|10:5] rs2 rs1 101 imm[4:1|11] 1100011 BGE
157 * imm[12|10:5] rs2 rs1 110 imm[4:1|11] 1100011 BLTU
158 * imm[12|10:5] rs2 rs1 111 imm[4:1|11] 1100011 BGEU
159 */
160
161 s32 offset;
162 s32 offset_tmp;
163 unsigned long rs1_val;
164 unsigned long rs2_val;
165
166 if (!rv_insn_reg_get_val(regs, branch_rs1_idx(opcode), &rs1_val) ||
167 !rv_insn_reg_get_val(regs, branch_rs2_idx(opcode), &rs2_val))
168 return false;
169
170 offset_tmp = branch_offset(opcode);
171 switch (branch_funct3(opcode)) {
172 case BRANCH_BEQ:
173 offset = (rs1_val == rs2_val) ? offset_tmp : 4;
174 break;
175 case BRANCH_BNE:
176 offset = (rs1_val != rs2_val) ? offset_tmp : 4;
177 break;
178 case BRANCH_BLT:
179 offset = ((long)rs1_val < (long)rs2_val) ? offset_tmp : 4;
180 break;
181 case BRANCH_BGE:
182 offset = ((long)rs1_val >= (long)rs2_val) ? offset_tmp : 4;
183 break;
184 case BRANCH_BLTU:
185 offset = (rs1_val < rs2_val) ? offset_tmp : 4;
186 break;
187 case BRANCH_BGEU:
188 offset = (rs1_val >= rs2_val) ? offset_tmp : 4;
189 break;
190 default:
191 return false;
192 }
193
194 instruction_pointer_set(regs, addr + offset);
195
196 return true;
197 }
198