1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * arch/arm64/kernel/probes/simulate-insn.c
4  *
5  * Copyright (C) 2013 Linaro Limited.
6  */
7 
8 #include <linux/bitops.h>
9 #include <linux/kernel.h>
10 #include <linux/kprobes.h>
11 
12 #include <asm/ptrace.h>
13 
14 #include "simulate-insn.h"
15 
16 #define bbl_displacement(insn)		\
17 	sign_extend32(((insn) & 0x3ffffff) << 2, 27)
18 
19 #define bcond_displacement(insn)	\
20 	sign_extend32(((insn >> 5) & 0x7ffff) << 2, 20)
21 
22 #define cbz_displacement(insn)	\
23 	sign_extend32(((insn >> 5) & 0x7ffff) << 2, 20)
24 
25 #define tbz_displacement(insn)	\
26 	sign_extend32(((insn >> 5) & 0x3fff) << 2, 15)
27 
28 #define ldr_displacement(insn)	\
29 	sign_extend32(((insn >> 5) & 0x7ffff) << 2, 20)
30 
31 static inline void set_x_reg(struct pt_regs *regs, int reg, u64 val)
32 {
33 	pt_regs_write_reg(regs, reg, val);
34 }
35 
36 static inline void set_w_reg(struct pt_regs *regs, int reg, u64 val)
37 {
38 	pt_regs_write_reg(regs, reg, lower_32_bits(val));
39 }
40 
41 static inline u64 get_x_reg(struct pt_regs *regs, int reg)
42 {
43 	return pt_regs_read_reg(regs, reg);
44 }
45 
46 static inline u32 get_w_reg(struct pt_regs *regs, int reg)
47 {
48 	return lower_32_bits(pt_regs_read_reg(regs, reg));
49 }
50 
51 static bool __kprobes check_cbz(u32 opcode, struct pt_regs *regs)
52 {
53 	int xn = opcode & 0x1f;
54 
55 	return (opcode & (1 << 31)) ?
56 	    (get_x_reg(regs, xn) == 0) : (get_w_reg(regs, xn) == 0);
57 }
58 
59 static bool __kprobes check_cbnz(u32 opcode, struct pt_regs *regs)
60 {
61 	int xn = opcode & 0x1f;
62 
63 	return (opcode & (1 << 31)) ?
64 	    (get_x_reg(regs, xn) != 0) : (get_w_reg(regs, xn) != 0);
65 }
66 
67 static bool __kprobes check_tbz(u32 opcode, struct pt_regs *regs)
68 {
69 	int xn = opcode & 0x1f;
70 	int bit_pos = ((opcode & (1 << 31)) >> 26) | ((opcode >> 19) & 0x1f);
71 
72 	return ((get_x_reg(regs, xn) >> bit_pos) & 0x1) == 0;
73 }
74 
75 static bool __kprobes check_tbnz(u32 opcode, struct pt_regs *regs)
76 {
77 	int xn = opcode & 0x1f;
78 	int bit_pos = ((opcode & (1 << 31)) >> 26) | ((opcode >> 19) & 0x1f);
79 
80 	return ((get_x_reg(regs, xn) >> bit_pos) & 0x1) != 0;
81 }
82 
83 /*
84  * instruction simulation functions
85  */
86 void __kprobes
87 simulate_adr_adrp(u32 opcode, long addr, struct pt_regs *regs)
88 {
89 	long imm, xn, val;
90 
91 	xn = opcode & 0x1f;
92 	imm = ((opcode >> 3) & 0x1ffffc) | ((opcode >> 29) & 0x3);
93 	imm = sign_extend64(imm, 20);
94 	if (opcode & 0x80000000)
95 		val = (imm<<12) + (addr & 0xfffffffffffff000);
96 	else
97 		val = imm + addr;
98 
99 	set_x_reg(regs, xn, val);
100 
101 	instruction_pointer_set(regs, instruction_pointer(regs) + 4);
102 }
103 
104 void __kprobes
105 simulate_b_bl(u32 opcode, long addr, struct pt_regs *regs)
106 {
107 	int disp = bbl_displacement(opcode);
108 
109 	/* Link register is x30 */
110 	if (opcode & (1 << 31))
111 		set_x_reg(regs, 30, addr + 4);
112 
113 	instruction_pointer_set(regs, addr + disp);
114 }
115 
116 void __kprobes
117 simulate_b_cond(u32 opcode, long addr, struct pt_regs *regs)
118 {
119 	int disp = 4;
120 
121 	if (aarch32_opcode_cond_checks[opcode & 0xf](regs->pstate & 0xffffffff))
122 		disp = bcond_displacement(opcode);
123 
124 	instruction_pointer_set(regs, addr + disp);
125 }
126 
127 void __kprobes
128 simulate_br_blr_ret(u32 opcode, long addr, struct pt_regs *regs)
129 {
130 	int xn = (opcode >> 5) & 0x1f;
131 
132 	/* update pc first in case we're doing a "blr lr" */
133 	instruction_pointer_set(regs, get_x_reg(regs, xn));
134 
135 	/* Link register is x30 */
136 	if (((opcode >> 21) & 0x3) == 1)
137 		set_x_reg(regs, 30, addr + 4);
138 }
139 
140 void __kprobes
141 simulate_cbz_cbnz(u32 opcode, long addr, struct pt_regs *regs)
142 {
143 	int disp = 4;
144 
145 	if (opcode & (1 << 24)) {
146 		if (check_cbnz(opcode, regs))
147 			disp = cbz_displacement(opcode);
148 	} else {
149 		if (check_cbz(opcode, regs))
150 			disp = cbz_displacement(opcode);
151 	}
152 	instruction_pointer_set(regs, addr + disp);
153 }
154 
155 void __kprobes
156 simulate_tbz_tbnz(u32 opcode, long addr, struct pt_regs *regs)
157 {
158 	int disp = 4;
159 
160 	if (opcode & (1 << 24)) {
161 		if (check_tbnz(opcode, regs))
162 			disp = tbz_displacement(opcode);
163 	} else {
164 		if (check_tbz(opcode, regs))
165 			disp = tbz_displacement(opcode);
166 	}
167 	instruction_pointer_set(regs, addr + disp);
168 }
169 
170 void __kprobes
171 simulate_ldr_literal(u32 opcode, long addr, struct pt_regs *regs)
172 {
173 	u64 *load_addr;
174 	int xn = opcode & 0x1f;
175 	int disp;
176 
177 	disp = ldr_displacement(opcode);
178 	load_addr = (u64 *) (addr + disp);
179 
180 	if (opcode & (1 << 30))	/* x0-x30 */
181 		set_x_reg(regs, xn, *load_addr);
182 	else			/* w0-w30 */
183 		set_w_reg(regs, xn, *load_addr);
184 
185 	instruction_pointer_set(regs, instruction_pointer(regs) + 4);
186 }
187 
188 void __kprobes
189 simulate_ldrsw_literal(u32 opcode, long addr, struct pt_regs *regs)
190 {
191 	s32 *load_addr;
192 	int xn = opcode & 0x1f;
193 	int disp;
194 
195 	disp = ldr_displacement(opcode);
196 	load_addr = (s32 *) (addr + disp);
197 
198 	set_x_reg(regs, xn, *load_addr);
199 
200 	instruction_pointer_set(regs, instruction_pointer(regs) + 4);
201 }
202