1 files changed, 201 insertions, 21 deletions

diff --git a/arch/arm/kernel/kprobes.c b/arch/arm/kernel/kprobes.c
index 1656c87501c0..129c1163248b 100644
--- a/arch/arm/kernel/kprobes.c
+++ b/arch/arm/kernel/kprobes.c
@@ -28,14 +28,16 @@
 #include <asm/traps.h>
 #include <asm/cacheflush.h>
 
+#include "kprobes.h"
+
 #define MIN_STACK_SIZE(addr)                            \
         min((unsigned long)MAX_STACK_SIZE,              \
             (unsigned long)current_thread_info() + THREAD_START_SP - (addr))
 
-#define flush_insns(addr, cnt)                          \
+#define flush_insns(addr, size)                         \
         flush_icache_range((unsigned long)(addr),       \
                            (unsigned long)(addr) +      \
-                           sizeof(kprobe_opcode_t) * (cnt))
+                           (size))
 
 /* Used as a marker in ARM_pc to note when we're in a jprobe. */
 #define JPROBE_MAGIC_ADDR               0xffffffff
@@ -49,16 +51,35 @@ int __kprobes arch_prepare_kprobe(struct kprobe *p)
         kprobe_opcode_t insn;
         kprobe_opcode_t tmp_insn[MAX_INSN_SIZE];
         unsigned long addr = (unsigned long)p->addr;
+        bool thumb;
+        kprobe_decode_insn_t *decode_insn;
         int is;
 
-        if (addr & 0x3 || in_exception_text(addr))
+        if (in_exception_text(addr))
                 return -EINVAL;
 
+#ifdef CONFIG_THUMB2_KERNEL
+        thumb = true;
+        addr &= ~1; /* Bit 0 would normally be set to indicate Thumb code */
+        insn = ((u16 *)addr)[0];
+        if (is_wide_instruction(insn)) {
+                insn <<= 16;
+                insn |= ((u16 *)addr)[1];
+                decode_insn = thumb32_kprobe_decode_insn;
+        } else
+                decode_insn = thumb16_kprobe_decode_insn;
+#else /* !CONFIG_THUMB2_KERNEL */
+        thumb = false;
+        if (addr & 0x3)
+                return -EINVAL;
         insn = *p->addr;
+        decode_insn = arm_kprobe_decode_insn;
+#endif
+
         p->opcode = insn;
         p->ainsn.insn = tmp_insn;
 
-        switch (arm_kprobe_decode_insn(insn, &p->ainsn)) {
+        switch ((*decode_insn)(insn, &p->ainsn)) {
         case INSN_REJECTED:     /* not supported */
                 return -EINVAL;
 
@@ -68,7 +89,10 @@ int __kprobes arch_prepare_kprobe(struct kprobe *p)
                         return -ENOMEM;
                 for (is = 0; is < MAX_INSN_SIZE; ++is)
                         p->ainsn.insn[is] = tmp_insn[is];
-                flush_insns(p->ainsn.insn, MAX_INSN_SIZE);
+                flush_insns(p->ainsn.insn,
+                                sizeof(p->ainsn.insn[0]) * MAX_INSN_SIZE);
+                p->ainsn.insn_fn = (kprobe_insn_fn_t *)
+                                        ((uintptr_t)p->ainsn.insn | thumb);
                 break;
 
         case INSN_GOOD_NO_SLOT: /* instruction doesn't need insn slot */
@@ -79,24 +103,88 @@ int __kprobes arch_prepare_kprobe(struct kprobe *p)
         return 0;
 }
 
+#ifdef CONFIG_THUMB2_KERNEL
+
+/*
+ * For a 32-bit Thumb breakpoint spanning two memory words we need to take
+ * special precautions to insert the breakpoint atomically, especially on SMP
+ * systems. This is achieved by calling this arming function using stop_machine.
+ */
+static int __kprobes set_t32_breakpoint(void *addr)
+{
+        ((u16 *)addr)[0] = KPROBE_THUMB32_BREAKPOINT_INSTRUCTION >> 16;
+        ((u16 *)addr)[1] = KPROBE_THUMB32_BREAKPOINT_INSTRUCTION & 0xffff;
+        flush_insns(addr, 2*sizeof(u16));
+        return 0;
+}
+
+void __kprobes arch_arm_kprobe(struct kprobe *p)
+{
+        uintptr_t addr = (uintptr_t)p->addr & ~1; /* Remove any Thumb flag */
+
+        if (!is_wide_instruction(p->opcode)) {
+                *(u16 *)addr = KPROBE_THUMB16_BREAKPOINT_INSTRUCTION;
+                flush_insns(addr, sizeof(u16));
+        } else if (addr & 2) {
+                /* A 32-bit instruction spanning two words needs special care */
+                stop_machine(set_t32_breakpoint, (void *)addr, &cpu_online_map);
+        } else {
+                /* Word aligned 32-bit instruction can be written atomically */
+                u32 bkp = KPROBE_THUMB32_BREAKPOINT_INSTRUCTION;
+#ifndef __ARMEB__ /* Swap halfwords for little-endian */
+                bkp = (bkp >> 16) | (bkp << 16);
+#endif
+                *(u32 *)addr = bkp;
+                flush_insns(addr, sizeof(u32));
+        }
+}
+
+#else /* !CONFIG_THUMB2_KERNEL */
+
 void __kprobes arch_arm_kprobe(struct kprobe *p)
 {
-        *p->addr = KPROBE_BREAKPOINT_INSTRUCTION;
-        flush_insns(p->addr, 1);
+        kprobe_opcode_t insn = p->opcode;
+        kprobe_opcode_t brkp = KPROBE_ARM_BREAKPOINT_INSTRUCTION;
+        if (insn >= 0xe0000000)
+                brkp |= 0xe0000000;  /* Unconditional instruction */
+        else
+                brkp |= insn & 0xf0000000;  /* Copy condition from insn */
+        *p->addr = brkp;
+        flush_insns(p->addr, sizeof(p->addr[0]));
 }
 
+#endif /* !CONFIG_THUMB2_KERNEL */
+
 /*
  * The actual disarming is done here on each CPU and synchronized using
  * stop_machine. This synchronization is necessary on SMP to avoid removing
  * a probe between the moment the 'Undefined Instruction' exception is raised
  * and the moment the exception handler reads the faulting instruction from
- * memory.
+ * memory. It is also needed to atomically set the two half-words of a 32-bit
+ * Thumb breakpoint.
  */
 int __kprobes __arch_disarm_kprobe(void *p)
 {
         struct kprobe *kp = p;
+#ifdef CONFIG_THUMB2_KERNEL
+        u16 *addr = (u16 *)((uintptr_t)kp->addr & ~1);
+        kprobe_opcode_t insn = kp->opcode;
+        unsigned int len;
+
+        if (is_wide_instruction(insn)) {
+                ((u16 *)addr)[0] = insn>>16;
+                ((u16 *)addr)[1] = insn;
+                len = 2*sizeof(u16);
+        } else {
+                ((u16 *)addr)[0] = insn;
+                len = sizeof(u16);
+        }
+        flush_insns(addr, len);
+
+#else /* !CONFIG_THUMB2_KERNEL */
         *kp->addr = kp->opcode;
-        flush_insns(kp->addr, 1);
+        flush_insns(kp->addr, sizeof(kp->addr[0]));
+#endif
         return 0;
 }
 
@@ -130,12 +218,24 @@ static void __kprobes set_current_kprobe(struct kprobe *p)
         __get_cpu_var(current_kprobe) = p;
 }
 
-static void __kprobes singlestep(struct kprobe *p, struct pt_regs *regs,
-                                 struct kprobe_ctlblk *kcb)
+static void __kprobes
+singlestep_skip(struct kprobe *p, struct pt_regs *regs)
 {
+#ifdef CONFIG_THUMB2_KERNEL
+        regs->ARM_cpsr = it_advance(regs->ARM_cpsr);
+        if (is_wide_instruction(p->opcode))
+                regs->ARM_pc += 4;
+        else
+                regs->ARM_pc += 2;
+#else
         regs->ARM_pc += 4;
-        if (p->ainsn.insn_check_cc(regs->ARM_cpsr))
-                p->ainsn.insn_handler(p, regs);
+#endif
+}
+
+static inline void __kprobes
+singlestep(struct kprobe *p, struct pt_regs *regs, struct kprobe_ctlblk *kcb)
+{
+        p->ainsn.insn_singlestep(p, regs);
 }
 
 /*
@@ -149,11 +249,23 @@ void __kprobes kprobe_handler(struct pt_regs *regs)
 {
         struct kprobe *p, *cur;
         struct kprobe_ctlblk *kcb;
-        kprobe_opcode_t *addr = (kprobe_opcode_t *)regs->ARM_pc;
 
         kcb = get_kprobe_ctlblk();
         cur = kprobe_running();
-        p = get_kprobe(addr);
+
+#ifdef CONFIG_THUMB2_KERNEL
+        /*
+         * First look for a probe which was registered using an address with
+         * bit 0 set, this is the usual situation for pointers to Thumb code.
+         * If not found, fallback to looking for one with bit 0 clear.
+         */
+        p = get_kprobe((kprobe_opcode_t *)(regs->ARM_pc | 1));
+        if (!p)
+                p = get_kprobe((kprobe_opcode_t *)regs->ARM_pc);
+
+#else /* ! CONFIG_THUMB2_KERNEL */
+        p = get_kprobe((kprobe_opcode_t *)regs->ARM_pc);
+#endif
 
         if (p) {
                 if (cur) {
@@ -173,7 +285,8 @@ void __kprobes kprobe_handler(struct pt_regs *regs)
                                 /* impossible cases */
                                 BUG();
                         }
-                } else {
+                } else if (p->ainsn.insn_check_cc(regs->ARM_cpsr)) {
+                        /* Probe hit and conditional execution check ok. */
                         set_current_kprobe(p);
                         kcb->kprobe_status = KPROBE_HIT_ACTIVE;
 
@@ -193,6 +306,13 @@ void __kprobes kprobe_handler(struct pt_regs *regs)
                                 }
                                 reset_current_kprobe();
                         }
+                } else {
+                        /*
+                         * Probe hit but conditional execution check failed,
+                         * so just skip the instruction and continue as if
+                         * nothing had happened.
+                         */
+                        singlestep_skip(p, regs);
                 }
         } else if (cur) {
                 /* We probably hit a jprobe.  Call its break handler. */
@@ -300,7 +420,11 @@ void __naked __kprobes kretprobe_trampoline(void)
                 "bl     trampoline_handler      \n\t"
                 "mov    lr, r0                  \n\t"
                 "ldmia  sp!, {r0 - r11}         \n\t"
+#ifdef CONFIG_THUMB2_KERNEL
+                "bx     lr                      \n\t"
+#else
                 "mov    pc, lr                  \n\t"
+#endif
                 : : : "memory");
 }
 
@@ -378,11 +502,22 @@ int __kprobes setjmp_pre_handler(struct kprobe *p, struct pt_regs *regs)
         struct jprobe *jp = container_of(p, struct jprobe, kp);
         struct kprobe_ctlblk *kcb = get_kprobe_ctlblk();
         long sp_addr = regs->ARM_sp;
+        long cpsr;
 
         kcb->jprobe_saved_regs = *regs;
         memcpy(kcb->jprobes_stack, (void *)sp_addr, MIN_STACK_SIZE(sp_addr));
         regs->ARM_pc = (long)jp->entry;
-        regs->ARM_cpsr |= PSR_I_BIT;
+
+        cpsr = regs->ARM_cpsr | PSR_I_BIT;
+#ifdef CONFIG_THUMB2_KERNEL
+        /* Set correct Thumb state in cpsr */
+        if (regs->ARM_pc & 1)
+                cpsr |= PSR_T_BIT;
+        else
+                cpsr &= ~PSR_T_BIT;
+#endif
+        regs->ARM_cpsr = cpsr;
+
         preempt_disable();
         return 1;
 }
@@ -404,7 +539,12 @@ void __kprobes jprobe_return(void)
                  * This is to prevent any simulated instruction from writing
                  * over the regs when they are accessing the stack.
                  */
+#ifdef CONFIG_THUMB2_KERNEL
+                "sub    r0, %0, %1              \n\t"
+                "mov    sp, r0                  \n\t"
+#else
                 "sub    sp, %0, %1              \n\t"
+#endif
                 "ldr    r0, ="__stringify(JPROBE_MAGIC_ADDR)"\n\t"
                 "str    %0, [sp, %2]            \n\t"
                 "str    r0, [sp, %3]            \n\t"
@@ -415,15 +555,28 @@ void __kprobes jprobe_return(void)
                  * Return to the context saved by setjmp_pre_handler
                  * and restored by longjmp_break_handler.
                  */
+#ifdef CONFIG_THUMB2_KERNEL
+                "ldr    lr, [sp, %2]            \n\t" /* lr = saved sp */
+                "ldrd   r0, r1, [sp, %5]        \n\t" /* r0,r1 = saved lr,pc */
+                "ldr    r2, [sp, %4]            \n\t" /* r2 = saved psr */
+                "stmdb  lr!, {r0, r1, r2}       \n\t" /* push saved lr and */
+                                                      /* rfe context */
+                "ldmia  sp, {r0 - r12}          \n\t"
+                "mov    sp, lr                  \n\t"
+                "ldr    lr, [sp], #4            \n\t"
+                "rfeia  sp!                     \n\t"
+#else
                 "ldr    r0, [sp, %4]            \n\t"
                 "msr    cpsr_cxsf, r0           \n\t"
                 "ldmia  sp, {r0 - pc}           \n\t"
+#endif
                 :
                 : "r" (kcb->jprobe_saved_regs.ARM_sp),
                   "I" (sizeof(struct pt_regs) * 2),
                   "J" (offsetof(struct pt_regs, ARM_sp)),
                   "J" (offsetof(struct pt_regs, ARM_pc)),
-                  "J" (offsetof(struct pt_regs, ARM_cpsr))
+                  "J" (offsetof(struct pt_regs, ARM_cpsr)),
+                  "J" (offsetof(struct pt_regs, ARM_lr))
                 : "memory", "cc");
 }
 
@@ -460,17 +613,44 @@ int __kprobes arch_trampoline_kprobe(struct kprobe *p)
         return 0;
 }
 
-static struct undef_hook kprobes_break_hook = {
+#ifdef CONFIG_THUMB2_KERNEL
+
+static struct undef_hook kprobes_thumb16_break_hook = {
+        .instr_mask     = 0xffff,
+        .instr_val      = KPROBE_THUMB16_BREAKPOINT_INSTRUCTION,
+        .cpsr_mask      = MODE_MASK,
+        .cpsr_val       = SVC_MODE,
+        .fn             = kprobe_trap_handler,
+};
+
+static struct undef_hook kprobes_thumb32_break_hook = {
         .instr_mask     = 0xffffffff,
-        .instr_val      = KPROBE_BREAKPOINT_INSTRUCTION,
+        .instr_val      = KPROBE_THUMB32_BREAKPOINT_INSTRUCTION,
         .cpsr_mask      = MODE_MASK,
         .cpsr_val       = SVC_MODE,
         .fn             = kprobe_trap_handler,
 };
 
+#else  /* !CONFIG_THUMB2_KERNEL */
+
+static struct undef_hook kprobes_arm_break_hook = {
+        .instr_mask     = 0x0fffffff,
+        .instr_val      = KPROBE_ARM_BREAKPOINT_INSTRUCTION,
+        .cpsr_mask      = MODE_MASK,
+        .cpsr_val       = SVC_MODE,
+        .fn             = kprobe_trap_handler,
+};
+
+#endif /* !CONFIG_THUMB2_KERNEL */
+
 int __init arch_init_kprobes()
 {
         arm_kprobe_decode_init();
-        register_undef_hook(&kprobes_break_hook);
+#ifdef CONFIG_THUMB2_KERNEL
+        register_undef_hook(&kprobes_thumb16_break_hook);
+        register_undef_hook(&kprobes_thumb32_break_hook);
+#else
+        register_undef_hook(&kprobes_arm_break_hook);
+#endif
         return 0;
 }