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