arm64: debug: consolidate software breakpoint handlers

[firefly-linux-kernel-4.4.55.git] / arch / arm64 / kernel / debug-monitors.c

diff --git a/arch/arm64/kernel/debug-monitors.c b/arch/arm64/kernel/debug-monitors.c
index f4726dc054b3bbcdd7c7a5d98d3733b6a893ea3e..08018e3df580c65d259470175fc95319d0589322 100644 (file)
--- a/arch/arm64/kernel/debug-monitors.c
+++ b/arch/arm64/kernel/debug-monitors.c
@@ -24,6 +24,7 @@
 #include <linux/init.h>
 #include <linux/ptrace.h>
 #include <linux/stat.h>
+#include <linux/uaccess.h>
 
 #include <asm/debug-monitors.h>
 #include <asm/local.h>
@@ -226,13 +227,74 @@ static int single_step_handler(unsigned long addr, unsigned int esr,
        return 0;
 }
 
-static int __init single_step_init(void)
+static int brk_handler(unsigned long addr, unsigned int esr,
+                      struct pt_regs *regs)
+{
+       siginfo_t info;
+
+       if (!user_mode(regs))
+               return -EFAULT;
+
+       info = (siginfo_t) {
+               .si_signo = SIGTRAP,
+               .si_errno = 0,
+               .si_code  = TRAP_BRKPT,
+               .si_addr  = (void __user *)instruction_pointer(regs),
+       };
+
+       force_sig_info(SIGTRAP, &info, current);
+       return 0;
+}
+
+int aarch32_break_handler(struct pt_regs *regs)
+{
+       siginfo_t info;
+       unsigned int instr;
+       bool bp = false;
+       void __user *pc = (void __user *)instruction_pointer(regs);
+
+       if (!compat_user_mode(regs))
+               return -EFAULT;
+
+       if (compat_thumb_mode(regs)) {
+               /* get 16-bit Thumb instruction */
+               get_user(instr, (u16 __user *)pc);
+               if (instr == AARCH32_BREAK_THUMB2_LO) {
+                       /* get second half of 32-bit Thumb-2 instruction */
+                       get_user(instr, (u16 __user *)(pc + 2));
+                       bp = instr == AARCH32_BREAK_THUMB2_HI;
+               } else {
+                       bp = instr == AARCH32_BREAK_THUMB;
+               }
+       } else {
+               /* 32-bit ARM instruction */
+               get_user(instr, (u32 __user *)pc);
+               bp = (instr & ~0xf0000000) == AARCH32_BREAK_ARM;
+       }
+
+       if (!bp)
+               return -EFAULT;
+
+       info = (siginfo_t) {
+               .si_signo = SIGTRAP,
+               .si_errno = 0,
+               .si_code  = TRAP_BRKPT,
+               .si_addr  = pc,
+       };
+
+       force_sig_info(SIGTRAP, &info, current);
+       return 0;
+}
+
+static int __init debug_traps_init(void)
 {
        hook_debug_fault_code(DBG_ESR_EVT_HWSS, single_step_handler, SIGTRAP,
                              TRAP_HWBKPT, "single-step handler");
+       hook_debug_fault_code(DBG_ESR_EVT_BRK, brk_handler, SIGTRAP,
+                             TRAP_BRKPT, "ptrace BRK handler");
        return 0;
 }
-arch_initcall(single_step_init);
+arch_initcall(debug_traps_init);
 
 /* Re-enable single step for syscall restarting. */
 void user_rewind_single_step(struct task_struct *task)