1c22b0bcbSGuo Ren // SPDX-License-Identifier: GPL-2.0+
2c22b0bcbSGuo Ren
3c22b0bcbSGuo Ren #include <linux/bitops.h>
4c22b0bcbSGuo Ren #include <linux/kernel.h>
5c22b0bcbSGuo Ren #include <linux/kprobes.h>
6c22b0bcbSGuo Ren
7c22b0bcbSGuo Ren #include "decode-insn.h"
8c22b0bcbSGuo Ren #include "simulate-insn.h"
9c22b0bcbSGuo Ren
rv_insn_reg_get_val(struct pt_regs * regs,u32 index,unsigned long * ptr)10c22b0bcbSGuo Ren static inline bool rv_insn_reg_get_val(struct pt_regs *regs, u32 index,
11c22b0bcbSGuo Ren unsigned long *ptr)
12c22b0bcbSGuo Ren {
13c22b0bcbSGuo Ren if (index == 0)
14c22b0bcbSGuo Ren *ptr = 0;
15c22b0bcbSGuo Ren else if (index <= 31)
16c22b0bcbSGuo Ren *ptr = *((unsigned long *)regs + index);
17c22b0bcbSGuo Ren else
18c22b0bcbSGuo Ren return false;
19c22b0bcbSGuo Ren
20c22b0bcbSGuo Ren return true;
21c22b0bcbSGuo Ren }
22c22b0bcbSGuo Ren
rv_insn_reg_set_val(struct pt_regs * regs,u32 index,unsigned long val)23c22b0bcbSGuo Ren static inline bool rv_insn_reg_set_val(struct pt_regs *regs, u32 index,
24c22b0bcbSGuo Ren unsigned long val)
25c22b0bcbSGuo Ren {
26c22b0bcbSGuo Ren if (index == 0)
27*b59f2129SNam Cao return true;
28c22b0bcbSGuo Ren else if (index <= 31)
29c22b0bcbSGuo Ren *((unsigned long *)regs + index) = val;
30c22b0bcbSGuo Ren else
31c22b0bcbSGuo Ren return false;
32c22b0bcbSGuo Ren
33c22b0bcbSGuo Ren return true;
34c22b0bcbSGuo Ren }
35c22b0bcbSGuo Ren
simulate_jal(u32 opcode,unsigned long addr,struct pt_regs * regs)36c22b0bcbSGuo Ren bool __kprobes simulate_jal(u32 opcode, unsigned long addr, struct pt_regs *regs)
37c22b0bcbSGuo Ren {
38c22b0bcbSGuo Ren /*
39c22b0bcbSGuo Ren * 31 30 21 20 19 12 11 7 6 0
40c22b0bcbSGuo Ren * imm [20] | imm[10:1] | imm[11] | imm[19:12] | rd | opcode
41c22b0bcbSGuo Ren * 1 10 1 8 5 JAL/J
42c22b0bcbSGuo Ren */
43c22b0bcbSGuo Ren bool ret;
44c22b0bcbSGuo Ren u32 imm;
45c22b0bcbSGuo Ren u32 index = (opcode >> 7) & 0x1f;
46c22b0bcbSGuo Ren
47c22b0bcbSGuo Ren ret = rv_insn_reg_set_val(regs, index, addr + 4);
48c22b0bcbSGuo Ren if (!ret)
49c22b0bcbSGuo Ren return ret;
50c22b0bcbSGuo Ren
51c22b0bcbSGuo Ren imm = ((opcode >> 21) & 0x3ff) << 1;
52c22b0bcbSGuo Ren imm |= ((opcode >> 20) & 0x1) << 11;
53c22b0bcbSGuo Ren imm |= ((opcode >> 12) & 0xff) << 12;
54c22b0bcbSGuo Ren imm |= ((opcode >> 31) & 0x1) << 20;
55c22b0bcbSGuo Ren
56c22b0bcbSGuo Ren instruction_pointer_set(regs, addr + sign_extend32((imm), 20));
57c22b0bcbSGuo Ren
58c22b0bcbSGuo Ren return ret;
59c22b0bcbSGuo Ren }
60c22b0bcbSGuo Ren
simulate_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs)61c22b0bcbSGuo Ren bool __kprobes simulate_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
62c22b0bcbSGuo Ren {
63c22b0bcbSGuo Ren /*
64c22b0bcbSGuo Ren * 31 20 19 15 14 12 11 7 6 0
65c22b0bcbSGuo Ren * offset[11:0] | rs1 | 010 | rd | opcode
66c22b0bcbSGuo Ren * 12 5 3 5 JALR/JR
67c22b0bcbSGuo Ren */
68c22b0bcbSGuo Ren bool ret;
69c22b0bcbSGuo Ren unsigned long base_addr;
70c22b0bcbSGuo Ren u32 imm = (opcode >> 20) & 0xfff;
71c22b0bcbSGuo Ren u32 rd_index = (opcode >> 7) & 0x1f;
72c22b0bcbSGuo Ren u32 rs1_index = (opcode >> 15) & 0x1f;
73c22b0bcbSGuo Ren
74ca025499SLiao Chang ret = rv_insn_reg_get_val(regs, rs1_index, &base_addr);
75c22b0bcbSGuo Ren if (!ret)
76c22b0bcbSGuo Ren return ret;
77c22b0bcbSGuo Ren
78ca025499SLiao Chang ret = rv_insn_reg_set_val(regs, rd_index, addr + 4);
79c22b0bcbSGuo Ren if (!ret)
80c22b0bcbSGuo Ren return ret;
81c22b0bcbSGuo Ren
82c22b0bcbSGuo Ren instruction_pointer_set(regs, (base_addr + sign_extend32((imm), 11))&~1);
83c22b0bcbSGuo Ren
84c22b0bcbSGuo Ren return ret;
85c22b0bcbSGuo Ren }
86b7d2be48SChen Lifu
87b7d2be48SChen Lifu #define auipc_rd_idx(opcode) \
88b7d2be48SChen Lifu ((opcode >> 7) & 0x1f)
89b7d2be48SChen Lifu
90b7d2be48SChen Lifu #define auipc_imm(opcode) \
91b7d2be48SChen Lifu ((((opcode) >> 12) & 0xfffff) << 12)
92b7d2be48SChen Lifu
93b7d2be48SChen Lifu #if __riscv_xlen == 64
94b7d2be48SChen Lifu #define auipc_offset(opcode) sign_extend64(auipc_imm(opcode), 31)
95b7d2be48SChen Lifu #elif __riscv_xlen == 32
96b7d2be48SChen Lifu #define auipc_offset(opcode) auipc_imm(opcode)
97b7d2be48SChen Lifu #else
98b7d2be48SChen Lifu #error "Unexpected __riscv_xlen"
99b7d2be48SChen Lifu #endif
100b7d2be48SChen Lifu
simulate_auipc(u32 opcode,unsigned long addr,struct pt_regs * regs)101b7d2be48SChen Lifu bool __kprobes simulate_auipc(u32 opcode, unsigned long addr, struct pt_regs *regs)
102b7d2be48SChen Lifu {
103b7d2be48SChen Lifu /*
104b7d2be48SChen Lifu * auipc instruction:
105b7d2be48SChen Lifu * 31 12 11 7 6 0
106b7d2be48SChen Lifu * | imm[31:12] | rd | opcode |
107b7d2be48SChen Lifu * 20 5 7
108b7d2be48SChen Lifu */
109b7d2be48SChen Lifu
110b7d2be48SChen Lifu u32 rd_idx = auipc_rd_idx(opcode);
111b7d2be48SChen Lifu unsigned long rd_val = addr + auipc_offset(opcode);
112b7d2be48SChen Lifu
113b7d2be48SChen Lifu if (!rv_insn_reg_set_val(regs, rd_idx, rd_val))
114b7d2be48SChen Lifu return false;
115b7d2be48SChen Lifu
116b7d2be48SChen Lifu instruction_pointer_set(regs, addr + 4);
117b7d2be48SChen Lifu
118b7d2be48SChen Lifu return true;
119b7d2be48SChen Lifu }
12067979e92SChen Lifu
12167979e92SChen Lifu #define branch_rs1_idx(opcode) \
12267979e92SChen Lifu (((opcode) >> 15) & 0x1f)
12367979e92SChen Lifu
12467979e92SChen Lifu #define branch_rs2_idx(opcode) \
12567979e92SChen Lifu (((opcode) >> 20) & 0x1f)
12667979e92SChen Lifu
12767979e92SChen Lifu #define branch_funct3(opcode) \
12867979e92SChen Lifu (((opcode) >> 12) & 0x7)
12967979e92SChen Lifu
13067979e92SChen Lifu #define branch_imm(opcode) \
13167979e92SChen Lifu (((((opcode) >> 8) & 0xf ) << 1) | \
13267979e92SChen Lifu ((((opcode) >> 25) & 0x3f) << 5) | \
13367979e92SChen Lifu ((((opcode) >> 7) & 0x1 ) << 11) | \
13467979e92SChen Lifu ((((opcode) >> 31) & 0x1 ) << 12))
13567979e92SChen Lifu
13667979e92SChen Lifu #define branch_offset(opcode) \
13767979e92SChen Lifu sign_extend32((branch_imm(opcode)), 12)
13867979e92SChen Lifu
simulate_branch(u32 opcode,unsigned long addr,struct pt_regs * regs)13967979e92SChen Lifu bool __kprobes simulate_branch(u32 opcode, unsigned long addr, struct pt_regs *regs)
14067979e92SChen Lifu {
14167979e92SChen Lifu /*
14267979e92SChen Lifu * branch instructions:
14367979e92SChen Lifu * 31 30 25 24 20 19 15 14 12 11 8 7 6 0
14467979e92SChen Lifu * | imm[12] | imm[10:5] | rs2 | rs1 | funct3 | imm[4:1] | imm[11] | opcode |
14567979e92SChen Lifu * 1 6 5 5 3 4 1 7
14667979e92SChen Lifu * imm[12|10:5] rs2 rs1 000 imm[4:1|11] 1100011 BEQ
14767979e92SChen Lifu * imm[12|10:5] rs2 rs1 001 imm[4:1|11] 1100011 BNE
14867979e92SChen Lifu * imm[12|10:5] rs2 rs1 100 imm[4:1|11] 1100011 BLT
14967979e92SChen Lifu * imm[12|10:5] rs2 rs1 101 imm[4:1|11] 1100011 BGE
15067979e92SChen Lifu * imm[12|10:5] rs2 rs1 110 imm[4:1|11] 1100011 BLTU
15167979e92SChen Lifu * imm[12|10:5] rs2 rs1 111 imm[4:1|11] 1100011 BGEU
15267979e92SChen Lifu */
15367979e92SChen Lifu
15467979e92SChen Lifu s32 offset;
15567979e92SChen Lifu s32 offset_tmp;
15667979e92SChen Lifu unsigned long rs1_val;
15767979e92SChen Lifu unsigned long rs2_val;
15867979e92SChen Lifu
15967979e92SChen Lifu if (!rv_insn_reg_get_val(regs, branch_rs1_idx(opcode), &rs1_val) ||
16067979e92SChen Lifu !rv_insn_reg_get_val(regs, branch_rs2_idx(opcode), &rs2_val))
16167979e92SChen Lifu return false;
16267979e92SChen Lifu
16367979e92SChen Lifu offset_tmp = branch_offset(opcode);
16467979e92SChen Lifu switch (branch_funct3(opcode)) {
165debe28d0SHeiko Stuebner case RVG_FUNCT3_BEQ:
16667979e92SChen Lifu offset = (rs1_val == rs2_val) ? offset_tmp : 4;
16767979e92SChen Lifu break;
168debe28d0SHeiko Stuebner case RVG_FUNCT3_BNE:
16967979e92SChen Lifu offset = (rs1_val != rs2_val) ? offset_tmp : 4;
17067979e92SChen Lifu break;
171debe28d0SHeiko Stuebner case RVG_FUNCT3_BLT:
17267979e92SChen Lifu offset = ((long)rs1_val < (long)rs2_val) ? offset_tmp : 4;
17367979e92SChen Lifu break;
174debe28d0SHeiko Stuebner case RVG_FUNCT3_BGE:
17567979e92SChen Lifu offset = ((long)rs1_val >= (long)rs2_val) ? offset_tmp : 4;
17667979e92SChen Lifu break;
177debe28d0SHeiko Stuebner case RVG_FUNCT3_BLTU:
17867979e92SChen Lifu offset = (rs1_val < rs2_val) ? offset_tmp : 4;
17967979e92SChen Lifu break;
180debe28d0SHeiko Stuebner case RVG_FUNCT3_BGEU:
18167979e92SChen Lifu offset = (rs1_val >= rs2_val) ? offset_tmp : 4;
18267979e92SChen Lifu break;
18367979e92SChen Lifu default:
18467979e92SChen Lifu return false;
18567979e92SChen Lifu }
18667979e92SChen Lifu
18767979e92SChen Lifu instruction_pointer_set(regs, addr + offset);
18867979e92SChen Lifu
18967979e92SChen Lifu return true;
19067979e92SChen Lifu }
191a9389297SNam Cao
simulate_c_j(u32 opcode,unsigned long addr,struct pt_regs * regs)192a9389297SNam Cao bool __kprobes simulate_c_j(u32 opcode, unsigned long addr, struct pt_regs *regs)
193a9389297SNam Cao {
194a9389297SNam Cao /*
195a9389297SNam Cao * 15 13 12 2 1 0
196a9389297SNam Cao * | funct3 | offset[11|4|9:8|10|6|7|3:1|5] | opcode |
197a9389297SNam Cao * 3 11 2
198a9389297SNam Cao */
199a9389297SNam Cao
200a9389297SNam Cao s32 offset;
201a9389297SNam Cao
202a9389297SNam Cao offset = ((opcode >> 3) & 0x7) << 1;
203a9389297SNam Cao offset |= ((opcode >> 11) & 0x1) << 4;
204a9389297SNam Cao offset |= ((opcode >> 2) & 0x1) << 5;
205a9389297SNam Cao offset |= ((opcode >> 7) & 0x1) << 6;
206a9389297SNam Cao offset |= ((opcode >> 6) & 0x1) << 7;
207a9389297SNam Cao offset |= ((opcode >> 9) & 0x3) << 8;
208a9389297SNam Cao offset |= ((opcode >> 8) & 0x1) << 10;
209a9389297SNam Cao offset |= ((opcode >> 12) & 0x1) << 11;
210a9389297SNam Cao
211a9389297SNam Cao instruction_pointer_set(regs, addr + sign_extend32(offset, 11));
212a9389297SNam Cao
213a9389297SNam Cao return true;
214a9389297SNam Cao }
215b18256d9SNam Cao
simulate_c_jr_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs,bool is_jalr)216b18256d9SNam Cao static bool __kprobes simulate_c_jr_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs,
217b18256d9SNam Cao bool is_jalr)
218b18256d9SNam Cao {
219b18256d9SNam Cao /*
220b18256d9SNam Cao * 15 12 11 7 6 2 1 0
221b18256d9SNam Cao * | funct4 | rs1 | rs2 | op |
222b18256d9SNam Cao * 4 5 5 2
223b18256d9SNam Cao */
224b18256d9SNam Cao
225b18256d9SNam Cao unsigned long jump_addr;
226b18256d9SNam Cao
227b18256d9SNam Cao u32 rs1 = (opcode >> 7) & 0x1f;
228b18256d9SNam Cao
229b18256d9SNam Cao if (rs1 == 0) /* C.JR is only valid when rs1 != x0 */
230b18256d9SNam Cao return false;
231b18256d9SNam Cao
232b18256d9SNam Cao if (!rv_insn_reg_get_val(regs, rs1, &jump_addr))
233b18256d9SNam Cao return false;
234b18256d9SNam Cao
235b18256d9SNam Cao if (is_jalr && !rv_insn_reg_set_val(regs, 1, addr + 2))
236b18256d9SNam Cao return false;
237b18256d9SNam Cao
238b18256d9SNam Cao instruction_pointer_set(regs, jump_addr);
239b18256d9SNam Cao
240b18256d9SNam Cao return true;
241b18256d9SNam Cao }
242b18256d9SNam Cao
simulate_c_jr(u32 opcode,unsigned long addr,struct pt_regs * regs)243b18256d9SNam Cao bool __kprobes simulate_c_jr(u32 opcode, unsigned long addr, struct pt_regs *regs)
244b18256d9SNam Cao {
245b18256d9SNam Cao return simulate_c_jr_jalr(opcode, addr, regs, false);
246b18256d9SNam Cao }
247b18256d9SNam Cao
simulate_c_jalr(u32 opcode,unsigned long addr,struct pt_regs * regs)248b18256d9SNam Cao bool __kprobes simulate_c_jalr(u32 opcode, unsigned long addr, struct pt_regs *regs)
249b18256d9SNam Cao {
250b18256d9SNam Cao return simulate_c_jr_jalr(opcode, addr, regs, true);
251b18256d9SNam Cao }
252d943705fSNam Cao
simulate_c_bnez_beqz(u32 opcode,unsigned long addr,struct pt_regs * regs,bool is_bnez)253d943705fSNam Cao static bool __kprobes simulate_c_bnez_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs,
254d943705fSNam Cao bool is_bnez)
255d943705fSNam Cao {
256d943705fSNam Cao /*
257d943705fSNam Cao * 15 13 12 10 9 7 6 2 1 0
258d943705fSNam Cao * | funct3 | offset[8|4:3] | rs1' | offset[7:6|2:1|5] | op |
259d943705fSNam Cao * 3 3 3 5 2
260d943705fSNam Cao */
261d943705fSNam Cao
262d943705fSNam Cao s32 offset;
263d943705fSNam Cao u32 rs1;
264d943705fSNam Cao unsigned long rs1_val;
265d943705fSNam Cao
266d943705fSNam Cao rs1 = 0x8 | ((opcode >> 7) & 0x7);
267d943705fSNam Cao
268d943705fSNam Cao if (!rv_insn_reg_get_val(regs, rs1, &rs1_val))
269d943705fSNam Cao return false;
270d943705fSNam Cao
271d943705fSNam Cao if ((rs1_val != 0 && is_bnez) || (rs1_val == 0 && !is_bnez)) {
272d943705fSNam Cao offset = ((opcode >> 3) & 0x3) << 1;
273d943705fSNam Cao offset |= ((opcode >> 10) & 0x3) << 3;
274d943705fSNam Cao offset |= ((opcode >> 2) & 0x1) << 5;
275d943705fSNam Cao offset |= ((opcode >> 5) & 0x3) << 6;
276d943705fSNam Cao offset |= ((opcode >> 12) & 0x1) << 8;
277d943705fSNam Cao offset = sign_extend32(offset, 8);
278d943705fSNam Cao } else {
279d943705fSNam Cao offset = 2;
280d943705fSNam Cao }
281d943705fSNam Cao
282d943705fSNam Cao instruction_pointer_set(regs, addr + offset);
283d943705fSNam Cao
284d943705fSNam Cao return true;
285d943705fSNam Cao }
286d943705fSNam Cao
simulate_c_bnez(u32 opcode,unsigned long addr,struct pt_regs * regs)287d943705fSNam Cao bool __kprobes simulate_c_bnez(u32 opcode, unsigned long addr, struct pt_regs *regs)
288d943705fSNam Cao {
289d943705fSNam Cao return simulate_c_bnez_beqz(opcode, addr, regs, true);
290d943705fSNam Cao }
291d943705fSNam Cao
simulate_c_beqz(u32 opcode,unsigned long addr,struct pt_regs * regs)292d943705fSNam Cao bool __kprobes simulate_c_beqz(u32 opcode, unsigned long addr, struct pt_regs *regs)
293d943705fSNam Cao {
294d943705fSNam Cao return simulate_c_bnez_beqz(opcode, addr, regs, false);
295d943705fSNam Cao }
296