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
 31static inline void set_x_reg(struct pt_regs *regs, int reg, u64 val)
 32{
 33	pt_regs_write_reg(regs, reg, val);
 34}
 35
 36static 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
 41static inline u64 get_x_reg(struct pt_regs *regs, int reg)
 42{
 43	return pt_regs_read_reg(regs, reg);
 44}
 45
 46static 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
 51static 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
 59static 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
 67static 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
 75static 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 */
 86void __kprobes
 87simulate_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
104void __kprobes
105simulate_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
116void __kprobes
117simulate_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
127void __kprobes
128simulate_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
140void __kprobes
141simulate_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
155void __kprobes
156simulate_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
170void __kprobes
171simulate_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
188void __kprobes
189simulate_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}