simulate-insn.c

  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 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
 23static 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
 36bool __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	s32 imm;
 45	u32 index = RV_EXTRACT_RD_REG(opcode);
 46
 47	ret = rv_insn_reg_set_val(regs, index, addr + 4);
 48	if (!ret)
 49		return ret;
 50
 51	imm = RV_EXTRACT_JTYPE_IMM(opcode);
 52
 53	instruction_pointer_set(regs, addr + imm);
 54
 55	return ret;
 56}
 57
 58bool __kprobes simulate_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
 59{
 60	/*
 61	 * 31          20 19 15 14 12 11 7 6      0
 62	 *  offset[11:0] | rs1 | 010 | rd | opcode
 63	 *      12         5      3    5    JALR/JR
 64	 */
 65	bool ret;
 66	unsigned long base_addr;
 67	u32 imm = RV_EXTRACT_ITYPE_IMM(opcode);
 68	u32 rd_index = RV_EXTRACT_RD_REG(opcode);
 69	u32 rs1_index = RV_EXTRACT_RS1_REG(opcode);
 70
 71	ret = rv_insn_reg_get_val(regs, rs1_index, &base_addr);
 72	if (!ret)
 73		return ret;
 74
 75	ret = rv_insn_reg_set_val(regs, rd_index, addr + 4);
 76	if (!ret)
 77		return ret;
 78
 79	instruction_pointer_set(regs, (base_addr + sign_extend32((imm), 11))&~1);
 80
 81	return ret;
 82}
 83
 84bool __kprobes simulate_auipc(u32 opcode, unsigned long addr, struct pt_regs *regs)
 85{
 86	/*
 87	 * auipc instruction:
 88	 *  31        12 11 7 6      0
 89	 * | imm[31:12] | rd | opcode |
 90	 *        20       5     7
 91	 */
 92
 93	u32 rd_idx = RV_EXTRACT_RD_REG(opcode);
 94	unsigned long rd_val = addr + (s32)RV_EXTRACT_UTYPE_IMM(opcode);
 95
 96	if (!rv_insn_reg_set_val(regs, rd_idx, rd_val))
 97		return false;
 98
 99	instruction_pointer_set(regs, addr + 4);
100
101	return true;
102}
103
104bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *regs)
105{
106	/*
107	 * branch instructions:
108	 *      31    30       25 24 20 19 15 14    12 11       8    7      6      0
109	 * | imm[12] | imm[10:5] | rs2 | rs1 | funct3 | imm[4:1] | imm[11] | opcode |
110	 *     1           6        5     5      3         4         1         7
111	 *     imm[12|10:5]        rs2   rs1    000       imm[4:1|11]       1100011  BEQ
112	 *     imm[12|10:5]        rs2   rs1    001       imm[4:1|11]       1100011  BNE
113	 *     imm[12|10:5]        rs2   rs1    100       imm[4:1|11]       1100011  BLT
114	 *     imm[12|10:5]        rs2   rs1    101       imm[4:1|11]       1100011  BGE
115	 *     imm[12|10:5]        rs2   rs1    110       imm[4:1|11]       1100011  BLTU
116	 *     imm[12|10:5]        rs2   rs1    111       imm[4:1|11]       1100011  BGEU
117	 */
118
119	s32 offset;
120	s32 offset_tmp;
121	unsigned long rs1_val;
122	unsigned long rs2_val;
123
124	if (!rv_insn_reg_get_val(regs, RV_EXTRACT_RS1_REG(opcode), &rs1_val) ||
125	    !rv_insn_reg_get_val(regs, RV_EXTRACT_RS2_REG(opcode), &rs2_val))
126		return false;
127
128	offset_tmp = RV_EXTRACT_BTYPE_IMM(opcode);
129	switch (RV_EXTRACT_FUNCT3(opcode)) {
130	case RVG_FUNCT3_BEQ:
131		offset = (rs1_val == rs2_val) ? offset_tmp : 4;
132		break;
133	case RVG_FUNCT3_BNE:
134		offset = (rs1_val != rs2_val) ? offset_tmp : 4;
135		break;
136	case RVG_FUNCT3_BLT:
137		offset = ((long)rs1_val < (long)rs2_val) ? offset_tmp : 4;
138		break;
139	case RVG_FUNCT3_BGE:
140		offset = ((long)rs1_val >= (long)rs2_val) ? offset_tmp : 4;
141		break;
142	case RVG_FUNCT3_BLTU:
143		offset = (rs1_val < rs2_val) ? offset_tmp : 4;
144		break;
145	case RVG_FUNCT3_BGEU:
146		offset = (rs1_val >= rs2_val) ? offset_tmp : 4;
147		break;
148	default:
149		return false;
150	}
151
152	instruction_pointer_set(regs, addr + offset);
153
154	return true;
155}
156
157bool __kprobes simulate_c_j(u32 opcode, unsigned long addr, struct pt_regs *regs)
158{
159	s32 offset = RVC_EXTRACT_JTYPE_IMM(opcode);
160
161	instruction_pointer_set(regs, addr + offset);
162
163	return true;
164}
165
166static bool __kprobes simulate_c_jr_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs,
167					 bool is_jalr)
168{
169	/*
170	 *  15    12 11  7 6   2 1  0
171	 * | funct4 | rs1 | rs2 | op |
172	 *     4       5     5    2
173	 */
174
175	unsigned long jump_addr;
176
177	u32 rs1 = RVC_EXTRACT_C2_RS1_REG(opcode);
178
179	if (rs1 == 0) /* C.JR is only valid when rs1 != x0 */
180		return false;
181
182	if (!rv_insn_reg_get_val(regs, rs1, &jump_addr))
183		return false;
184
185	if (is_jalr && !rv_insn_reg_set_val(regs, 1, addr + 2))
186		return false;
187
188	instruction_pointer_set(regs, jump_addr);
189
190	return true;
191}
192
193bool __kprobes simulate_c_jr(u32 opcode, unsigned long addr, struct pt_regs *regs)
194{
195	return simulate_c_jr_jalr(opcode, addr, regs, false);
196}
197
198bool __kprobes simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
199{
200	return simulate_c_jr_jalr(opcode, addr, regs, true);
201}
202
203static bool __kprobes simulate_c_bnez_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs,
204					   bool is_bnez)
205{
206	/*
207	 *  15    13 12           10 9    7 6                 2 1  0
208	 * | funct3 | offset[8|4:3] | rs1' | offset[7:6|2:1|5] | op |
209	 *     3            3          3             5           2
210	 */
211
212	s32 offset;
213	u32 rs1;
214	unsigned long rs1_val;
215
216	rs1 = 0x8 | ((opcode >> 7) & 0x7);
217
218	if (!rv_insn_reg_get_val(regs, rs1, &rs1_val))
219		return false;
220
221	if ((rs1_val != 0 && is_bnez) || (rs1_val == 0 && !is_bnez))
222		offset = RVC_EXTRACT_BTYPE_IMM(opcode);
223	else
224		offset = 2;
225
226	instruction_pointer_set(regs, addr + offset);
227
228	return true;
229}
230
231bool __kprobes simulate_c_bnez(u32 opcode, unsigned long addr, struct pt_regs *regs)
232{
233	return simulate_c_bnez_beqz(opcode, addr, regs, true);
234}
235
236bool __kprobes simulate_c_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs)
237{
238	return simulate_c_bnez_beqz(opcode, addr, regs, false);
239}