[PATCH 3/4] ARM: kprobes: Make ARM instruction emulation work on Thumb-2 kernels

[Tixy]

From: Jon Medhurst <tixy at yxit.co.uk>

On Thumb-2 kernels there may still be some kernel code using the ARM
instruction set, so kprobes will need to support this. This patch
fixes the ARM emulation code to built and work correctly when built as
Thumb.

Signed-off-by: Jon Medhurst <tixy at yxit.co.uk>
---
 arch/arm/kernel/kprobes-decode.c |   61 +++++++++++++++++++++----------------
 1 files changed, 35 insertions(+), 26 deletions(-)

diff --git a/arch/arm/kernel/kprobes-decode.c b/arch/arm/kernel/kprobes-decode.c
index 8f6ed43..0532ba3 100644
--- a/arch/arm/kernel/kprobes-decode.c
+++ b/arch/arm/kernel/kprobes-decode.c
@@ -94,6 +94,14 @@ union reg_pair {
 #endif
 };
 
+
+#ifdef CONFIG_THUMB2_KERNEL
+#define BLX(reg)	"blx	"reg"		\n\t"
+#else
+#define BLX(reg)	"mov	lr, pc		\n\t"	\
+			"mov	pc, "reg"	\n\t"
+#endif
+
 /*
  * For STR and STM instructions, an ARM core may choose to use either
  * a +8 or a +12 displacement from the current instruction's address.
@@ -108,8 +116,22 @@ static void __init find_str_pc_offset(void)
 	int addr, scratch, ret;
 
 	__asm__ (
+#ifdef CONFIG_THUMB2_KERNEL
+		/* Switch to ARM mode */
+		"adr	%[scr], 1f		\n\t"
+		"bx	%[scr]			\n\t"
+		".arm				\n\t"
+		"1:				\n\t"
+#endif
 		"sub	%[ret], pc, #4		\n\t"
 		"str	pc, %[addr]		\n\t"
+#ifdef CONFIG_THUMB2_KERNEL
+		/* Switch back to Thumb mode */
+		"adr	%[scr], 2f + 1		\n\t"
+		"bx	%[scr]			\n\t"
+		".thumb				\n\t"
+		"2:				\n\t"
+#endif
 		"ldr	%[scr], %[addr]		\n\t"
 		"sub	%[ret], %[scr], %[ret]	\n\t"
 		: [ret] "=r" (ret), [scr] "=r" (scratch), [addr] "+m" (addr));
@@ -131,8 +153,7 @@ insnslot_0arg_rflags(long cpsr, insn_0arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret)
 		: [cpsr] "r" (cpsr), [fn] "r" (fn)
 		: "lr", "cc"
@@ -149,8 +170,7 @@ insnslot_llret_0arg_rflags(long cpsr, insn_llret_0arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret0), "=r" (ret1)
 		: [cpsr] "r" (cpsr), [fn] "r" (fn)
 		: "lr", "cc"
@@ -168,8 +188,7 @@ insnslot_1arg_rflags(long r0, long cpsr, insn_1arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret)
 		: "0" (rr0), [cpsr] "r" (cpsr), [fn] "r" (fn)
 		: "lr", "cc"
@@ -186,8 +205,7 @@ insnslot_2arg_rflags(long r0, long r1, long cpsr, insn_2arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret)
 		: "0" (rr0), "r" (rr1),
 		  [cpsr] "r" (cpsr), [fn] "r" (fn)
@@ -206,8 +224,7 @@ insnslot_3arg_rflags(long r0, long r1, long r2, long cpsr, insn_3arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret)
 		: "0" (rr0), "r" (rr1), "r" (rr2),
 		  [cpsr] "r" (cpsr), [fn] "r" (fn)
@@ -229,8 +246,7 @@ insnslot_llret_3arg_rflags(long r0, long r1, long r2, long cpsr,
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret0), "=r" (ret1)
 		: "0" (rr0), "r" (rr1), "r" (rr2),
 		  [cpsr] "r" (cpsr), [fn] "r" (fn)
@@ -253,8 +269,7 @@ insnslot_4arg_rflags(long r0, long r1, long r2, long r3, long cpsr,
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[cpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		: "=r" (ret)
 		: "0" (rr0), "r" (rr1), "r" (rr2), "r" (rr3),
 		  [cpsr] "r" (cpsr), [fn] "r" (fn)
@@ -273,8 +288,7 @@ insnslot_1arg_rwflags(long r0, long *cpsr, insn_1arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[oldcpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		"mrs	%[newcpsr], cpsr	\n\t"
 		: "=r" (ret), [newcpsr] "=r" (newcpsr)
 		: "0" (rr0), [oldcpsr] "r" (oldcpsr), [fn] "r" (fn)
@@ -295,8 +309,7 @@ insnslot_2arg_rwflags(long r0, long r1, long *cpsr, insn_2arg_fn_t *fn)
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[oldcpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		"mrs	%[newcpsr], cpsr	\n\t"
 		: "=r" (ret), [newcpsr] "=r" (newcpsr)
 		: "0" (rr0), "r" (rr1), [oldcpsr] "r" (oldcpsr), [fn] "r" (fn)
@@ -319,8 +332,7 @@ insnslot_3arg_rwflags(long r0, long r1, long r2, long *cpsr,
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[oldcpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		"mrs	%[newcpsr], cpsr	\n\t"
 		: "=r" (ret), [newcpsr] "=r" (newcpsr)
 		: "0" (rr0), "r" (rr1), "r" (rr2),
@@ -345,8 +357,7 @@ insnslot_4arg_rwflags(long r0, long r1, long r2, long r3, long *cpsr,
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[oldcpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		"mrs	%[newcpsr], cpsr	\n\t"
 		: "=r" (ret), [newcpsr] "=r" (newcpsr)
 		: "0" (rr0), "r" (rr1), "r" (rr2), "r" (rr3),
@@ -373,8 +384,7 @@ insnslot_llret_4arg_rwflags(long r0, long r1, long r2, long r3, long *cpsr,
 
 	__asm__ __volatile__ (
 		"msr	cpsr_fs, %[oldcpsr]	\n\t"
-		"mov	lr, pc			\n\t"
-		"mov	pc, %[fn]		\n\t"
+		BLX("%[fn]")
 		"mrs	%[newcpsr], cpsr	\n\t"
 		: "=r" (ret0), "=r" (ret1), [newcpsr] "=r" (newcpsr)
 		: "0" (rr0), "r" (rr1), "r" (rr2), "r" (rr3),
@@ -549,8 +559,7 @@ static void __kprobes emulate_ldrd(struct kprobe *p, struct pt_regs *regs)
 		"ldr	r0, %[rn]	\n\t"
 		"ldr	r1, %[rm]	\n\t"
 		"msr	cpsr_fs, %[cpsr]\n\t"
-		"mov	lr, pc		\n\t"
-		"mov	pc, %[i_fn]	\n\t"
+		BLX("%[i_fn]")
 		"str	r0, %[rn]	\n\t"	/* in case of writeback */
 		"str	r2, %[rd0]	\n\t"
 		"str	r3, %[rd1]	\n\t"
-- 
1.7.2.5