1/*
  2 * arch/arm/kernel/kprobes-common.c
  3 *
  4 * Copyright (C) 2011 Jon Medhurst <tixy@yxit.co.uk>.
  5 *
  6 * Some contents moved here from arch/arm/include/asm/kprobes-arm.c which is
  7 * Copyright (C) 2006, 2007 Motorola Inc.
  8 *
  9 * This program is free software; you can redistribute it and/or modify
 10 * it under the terms of the GNU General Public License version 2 as
 11 * published by the Free Software Foundation.
 12 */
 13
 14#include <linux/kernel.h>
 15#include <linux/kprobes.h>
 16#include <asm/opcodes.h>
 17
 18#include "kprobes.h"
 19
 20
 21static void __kprobes simulate_ldm1stm1(probes_opcode_t insn,
 22		struct arch_probes_insn *asi,
 23		struct pt_regs *regs)
 24{
 25	int rn = (insn >> 16) & 0xf;
 26	int lbit = insn & (1 << 20);
 27	int wbit = insn & (1 << 21);
 28	int ubit = insn & (1 << 23);
 29	int pbit = insn & (1 << 24);
 30	long *addr = (long *)regs->uregs[rn];
 31	int reg_bit_vector;
 32	int reg_count;
 33
 34	reg_count = 0;
 35	reg_bit_vector = insn & 0xffff;
 36	while (reg_bit_vector) {
 37		reg_bit_vector &= (reg_bit_vector - 1);
 38		++reg_count;
 39	}
 40
 41	if (!ubit)
 42		addr -= reg_count;
 43	addr += (!pbit == !ubit);
 44
 45	reg_bit_vector = insn & 0xffff;
 46	while (reg_bit_vector) {
 47		int reg = __ffs(reg_bit_vector);
 48		reg_bit_vector &= (reg_bit_vector - 1);
 49		if (lbit)
 50			regs->uregs[reg] = *addr++;
 51		else
 52			*addr++ = regs->uregs[reg];
 53	}
 54
 55	if (wbit) {
 56		if (!ubit)
 57			addr -= reg_count;
 58		addr -= (!pbit == !ubit);
 59		regs->uregs[rn] = (long)addr;
 60	}
 61}
 62
 63static void __kprobes simulate_stm1_pc(probes_opcode_t insn,
 64	struct arch_probes_insn *asi,
 65	struct pt_regs *regs)
 66{
 67	unsigned long addr = regs->ARM_pc - 4;
 68
 69	regs->ARM_pc = (long)addr + str_pc_offset;
 70	simulate_ldm1stm1(insn, asi, regs);
 71	regs->ARM_pc = (long)addr + 4;
 72}
 73
 74static void __kprobes simulate_ldm1_pc(probes_opcode_t insn,
 75	struct arch_probes_insn *asi,
 76	struct pt_regs *regs)
 77{
 78	simulate_ldm1stm1(insn, asi, regs);
 79	load_write_pc(regs->ARM_pc, regs);
 80}
 81
 82static void __kprobes
 83emulate_generic_r0_12_noflags(probes_opcode_t insn,
 84	struct arch_probes_insn *asi, struct pt_regs *regs)
 85{
 86	register void *rregs asm("r1") = regs;
 87	register void *rfn asm("lr") = asi->insn_fn;
 88
 89	__asm__ __volatile__ (
 90		"stmdb	sp!, {%[regs], r11}	\n\t"
 91		"ldmia	%[regs], {r0-r12}	\n\t"
 92#if __LINUX_ARM_ARCH__ >= 6
 93		"blx	%[fn]			\n\t"
 94#else
 95		"str	%[fn], [sp, #-4]!	\n\t"
 96		"adr	lr, 1f			\n\t"
 97		"ldr	pc, [sp], #4		\n\t"
 98		"1:				\n\t"
 99#endif
100		"ldr	lr, [sp], #4		\n\t" /* lr = regs */
101		"stmia	lr, {r0-r12}		\n\t"
102		"ldr	r11, [sp], #4		\n\t"
103		: [regs] "=r" (rregs), [fn] "=r" (rfn)
104		: "0" (rregs), "1" (rfn)
105		: "r0", "r2", "r3", "r4", "r5", "r6", "r7",
106		  "r8", "r9", "r10", "r12", "memory", "cc"
107		);
108}
109
110static void __kprobes
111emulate_generic_r2_14_noflags(probes_opcode_t insn,
112	struct arch_probes_insn *asi, struct pt_regs *regs)
113{
114	emulate_generic_r0_12_noflags(insn, asi,
115		(struct pt_regs *)(regs->uregs+2));
116}
117
118static void __kprobes
119emulate_ldm_r3_15(probes_opcode_t insn,
120	struct arch_probes_insn *asi, struct pt_regs *regs)
121{
122	emulate_generic_r0_12_noflags(insn, asi,
123		(struct pt_regs *)(regs->uregs+3));
124	load_write_pc(regs->ARM_pc, regs);
125}
126
127enum probes_insn __kprobes
128kprobe_decode_ldmstm(probes_opcode_t insn, struct arch_probes_insn *asi,
129		const struct decode_header *h)
130{
131	probes_insn_handler_t *handler = 0;
132	unsigned reglist = insn & 0xffff;
133	int is_ldm = insn & 0x100000;
134	int rn = (insn >> 16) & 0xf;
135
136	if (rn <= 12 && (reglist & 0xe000) == 0) {
137		/* Instruction only uses registers in the range R0..R12 */
138		handler = emulate_generic_r0_12_noflags;
139
140	} else if (rn >= 2 && (reglist & 0x8003) == 0) {
141		/* Instruction only uses registers in the range R2..R14 */
142		rn -= 2;
143		reglist >>= 2;
144		handler = emulate_generic_r2_14_noflags;
145
146	} else if (rn >= 3 && (reglist & 0x0007) == 0) {
147		/* Instruction only uses registers in the range R3..R15 */
148		if (is_ldm && (reglist & 0x8000)) {
149			rn -= 3;
150			reglist >>= 3;
151			handler = emulate_ldm_r3_15;
152		}
153	}
154
155	if (handler) {
156		/* We can emulate the instruction in (possibly) modified form */
157		asi->insn[0] = __opcode_to_mem_arm((insn & 0xfff00000) |
158						   (rn << 16) | reglist);
159		asi->insn_handler = handler;
160		return INSN_GOOD;
161	}
162
163	/* Fallback to slower simulation... */
164	if (reglist & 0x8000)
165		handler = is_ldm ? simulate_ldm1_pc : simulate_stm1_pc;
166	else
167		handler = simulate_ldm1stm1;
168	asi->insn_handler = handler;
169	return INSN_GOOD_NO_SLOT;
170}
171