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}