MN10300: Emulate single stepping in KGDB on MN10300

Emulate single stepping in KGDB on MN10300 by way of temporary breakpoint
insertion.  These breakpoints are never actually seen by KGDB, and will overlay
KGDB's own breakpoints.

The breakpoints are removed by switch_to() and reinstalled on switching back so
that if preemption occurs, the preempting task doesn't hit them (though it will
still hit KGDB's regular breakpoints).  If KGDB is reentered for any reason,
then the single step breakpoint is completely erased and must be set again by
the debugger.

We take advantage of the fact that KGDB will effectively halt all other CPUs
whilst this CPU is single-stepping to avoid SMP problems.

If the single-stepping task is preempted and killed without KGDB being
reinvoked, then the breakpoint(s) will be cleared and KGDB will be jumped back
into.

Signed-off-by: David Howells <dhowells@redhat.com>

2 files changed, 421 insertions, 12 deletions

diff --git a/arch/mn10300/kernel/kgdb.c b/arch/mn10300/kernel/kgdb.c
index 7d575f553099..f6c981db2a36 100644
--- a/arch/mn10300/kernel/kgdb.c
+++ b/arch/mn10300/kernel/kgdb.c
@@ -9,6 +9,7 @@
  * 2 of the Licence, or (at your option) any later version.
  */
 
+#include <linux/slab.h>
 #include <linux/ptrace.h>
 #include <linux/kgdb.h>
 #include <linux/uaccess.h>
@@ -19,6 +20,13 @@
 #include "internal.h"
 
 /*
+ * Software single-stepping breakpoint save (used by __switch_to())
+ */
+static struct thread_info *kgdb_sstep_thread;
+u8 *kgdb_sstep_bp_addr[2];
+u8 kgdb_sstep_bp[2];
+
+/*
  * Copy kernel exception frame registers to the GDB register file
  */
 void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
@@ -118,8 +126,293 @@ struct kgdb_arch arch_kgdb_ops = {
         .flags          = KGDB_HW_BREAKPOINT,
 };
 
+static const unsigned char mn10300_kgdb_insn_sizes[256] =
+{
+        /* 1  2  3  4  5  6  7  8  9  a  b  c  d  e  f */
+        1, 3, 3, 3, 1, 3, 3, 3, 1, 3, 3, 3, 1, 3, 3, 3, /* 0 */
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 1 */
+        2, 2, 2, 2, 3, 3, 3, 3, 2, 2, 2, 2, 3, 3, 3, 3, /* 2 */
+        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 1, 1, 1, 1, /* 3 */
+        1, 1, 2, 2, 1, 1, 2, 2, 1, 1, 2, 2, 1, 1, 2, 2, /* 4 */
+        1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, /* 5 */
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 6 */
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 7 */
+        2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, /* 8 */
+        2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, /* 9 */
+        2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, /* a */
+        2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1, 2, /* b */
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 2, 2, /* c */
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, /* d */
+        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* e */
+        0, 2, 2, 2, 2, 2, 2, 4, 0, 3, 0, 4, 0, 6, 7, 1  /* f */
+};
+
 /*
- * Handle unknown packets and [Ccs] packets
+ * Attempt to emulate single stepping by means of breakpoint instructions.
+ * Although there is a single-step trace flag in EPSW, its use is not
+ * sufficiently documented and is only intended for use with the JTAG debugger.
+ */
+static int kgdb_arch_do_singlestep(struct pt_regs *regs)
+{
+        unsigned long arg;
+        unsigned size;
+        u8 *pc = (u8 *)regs->pc, *sp = (u8 *)(regs + 1), cur;
+        u8 *x = NULL, *y = NULL;
+        int ret;
+
+        ret = probe_kernel_read(&cur, pc, 1);
+        if (ret < 0)
+                return ret;
+
+        size = mn10300_kgdb_insn_sizes[cur];
+        if (size > 0) {
+                x = pc + size;
+                goto set_x;
+        }
+
+        switch (cur) {
+                /* Bxx (d8,PC) */
+        case 0xc0 ... 0xca:
+                ret = probe_kernel_read(&arg, pc + 1, 1);
+                if (ret < 0)
+                        return ret;
+                x = pc + 2;
+                if (arg >= 0 && arg <= 2)
+                        goto set_x;
+                y = pc + (s8)arg;
+                goto set_x_and_y;
+
+                /* LXX (d8,PC) */
+        case 0xd0 ... 0xda:
+                x = pc + 1;
+                if (regs->pc == regs->lar)
+                        goto set_x;
+                y = (u8 *)regs->lar;
+                goto set_x_and_y;
+
+                /* SETLB - loads the next four bytes into the LIR register
+                 * (which mustn't include a breakpoint instruction) */
+        case 0xdb:
+                x = pc + 5;
+                goto set_x;
+
+                /* JMP (d16,PC) or CALL (d16,PC) */
+        case 0xcc:
+        case 0xcd:
+                ret = probe_kernel_read(&arg, pc + 1, 2);
+                if (ret < 0)
+                        return ret;
+                x = pc + (s16)arg;
+                goto set_x;
+
+                /* JMP (d32,PC) or CALL (d32,PC) */
+        case 0xdc:
+        case 0xdd:
+                ret = probe_kernel_read(&arg, pc + 1, 4);
+                if (ret < 0)
+                        return ret;
+                x = pc + (s32)arg;
+                goto set_x;
+
+                /* RETF */
+        case 0xde:
+                x = (u8 *)regs->mdr;
+                goto set_x;
+
+                /* RET */
+        case 0xdf:
+                ret = probe_kernel_read(&arg, pc + 2, 1);
+                if (ret < 0)
+                        return ret;
+                ret = probe_kernel_read(&x, sp + (s8)arg, 4);
+                if (ret < 0)
+                        return ret;
+                goto set_x;
+
+        case 0xf0:
+                ret = probe_kernel_read(&cur, pc + 1, 1);
+                if (ret < 0)
+                        return ret;
+
+                if (cur >= 0xf0 && cur <= 0xf7) {
+                        /* JMP (An) / CALLS (An) */
+                        switch (cur & 3) {
+                        case 0: x = (u8 *)regs->a0; break;
+                        case 1: x = (u8 *)regs->a1; break;
+                        case 2: x = (u8 *)regs->a2; break;
+                        case 3: x = (u8 *)regs->a3; break;
+                        }
+                        goto set_x;
+                } else if (cur == 0xfc) {
+                        /* RETS */
+                        ret = probe_kernel_read(&x, sp, 4);
+                        if (ret < 0)
+                                return ret;
+                        goto set_x;
+                } else if (cur == 0xfd) {
+                        /* RTI */
+                        ret = probe_kernel_read(&x, sp + 4, 4);
+                        if (ret < 0)
+                                return ret;
+                        goto set_x;
+                } else {
+                        x = pc + 2;
+                        goto set_x;
+                }
+                break;
+
+                /* potential 3-byte conditional branches */
+        case 0xf8:
+                ret = probe_kernel_read(&cur, pc + 1, 1);
+                if (ret < 0)
+                        return ret;
+                x = pc + 3;
+
+                if (cur >= 0xe8 && cur <= 0xeb) {
+                        ret = probe_kernel_read(&arg, pc + 2, 1);
+                        if (ret < 0)
+                                return ret;
+                        if (arg >= 0 && arg <= 3)
+                                goto set_x;
+                        y = pc + (s8)arg;
+                        goto set_x_and_y;
+                }
+                goto set_x;
+
+        case 0xfa:
+                ret = probe_kernel_read(&cur, pc + 1, 1);
+                if (ret < 0)
+                        return ret;
+
+                if (cur == 0xff) {
+                        /* CALLS (d16,PC) */
+                        ret = probe_kernel_read(&arg, pc + 2, 2);
+                        if (ret < 0)
+                                return ret;
+                        x = pc + (s16)arg;
+                        goto set_x;
+                }
+
+                x = pc + 4;
+                goto set_x;
+
+        case 0xfc:
+                ret = probe_kernel_read(&cur, pc + 1, 1);
+                if (ret < 0)
+                        return ret;
+
+                if (cur == 0xff) {
+                        /* CALLS (d32,PC) */
+                        ret = probe_kernel_read(&arg, pc + 2, 4);
+                        if (ret < 0)
+                                return ret;
+                        x = pc + (s32)arg;
+                        goto set_x;
+                }
+
+                x = pc + 6;
+                goto set_x;
+        }
+
+        return 0;
+
+set_x:
+        kgdb_sstep_bp_addr[0] = x;
+        kgdb_sstep_bp_addr[1] = NULL;
+        ret = probe_kernel_read(&kgdb_sstep_bp[0], x, 1);
+        if (ret < 0)
+                return ret;
+        ret = probe_kernel_write(x, &arch_kgdb_ops.gdb_bpt_instr, 1);
+        if (ret < 0)
+                return ret;
+        kgdb_sstep_thread = current_thread_info();
+        debugger_local_cache_flushinv_one(x);
+        return ret;
+
+set_x_and_y:
+        kgdb_sstep_bp_addr[0] = x;
+        kgdb_sstep_bp_addr[1] = y;
+        ret = probe_kernel_read(&kgdb_sstep_bp[0], x, 1);
+        if (ret < 0)
+                return ret;
+        ret = probe_kernel_read(&kgdb_sstep_bp[1], y, 1);
+        if (ret < 0)
+                return ret;
+        ret = probe_kernel_write(x, &arch_kgdb_ops.gdb_bpt_instr, 1);
+        if (ret < 0)
+                return ret;
+        ret = probe_kernel_write(y, &arch_kgdb_ops.gdb_bpt_instr, 1);
+        if (ret < 0) {
+                probe_kernel_write(kgdb_sstep_bp_addr[0],
+                                   &kgdb_sstep_bp[0], 1);
+        } else {
+                kgdb_sstep_thread = current_thread_info();
+        }
+        debugger_local_cache_flushinv_one(x);
+        debugger_local_cache_flushinv_one(y);
+        return ret;
+}
+
+/*
+ * Remove emplaced single-step breakpoints, returning true if we hit one of
+ * them.
+ */
+static bool kgdb_arch_undo_singlestep(struct pt_regs *regs)
+{
+        bool hit = false;
+        u8 *x = kgdb_sstep_bp_addr[0], *y = kgdb_sstep_bp_addr[1];
+        u8 opcode;
+
+        if (kgdb_sstep_thread == current_thread_info()) {
+                if (x) {
+                        if (x == (u8 *)regs->pc)
+                                hit = true;
+                        if (probe_kernel_read(&opcode, x,
+                                              1) < 0 ||
+                            opcode != 0xff)
+                                BUG();
+                        probe_kernel_write(x, &kgdb_sstep_bp[0], 1);
+                        debugger_local_cache_flushinv_one(x);
+                }
+                if (y) {
+                        if (y == (u8 *)regs->pc)
+                                hit = true;
+                        if (probe_kernel_read(&opcode, y,
+                                              1) < 0 ||
+                            opcode != 0xff)
+                                BUG();
+                        probe_kernel_write(y, &kgdb_sstep_bp[1], 1);
+                        debugger_local_cache_flushinv_one(y);
+                }
+        }
+
+        kgdb_sstep_bp_addr[0] = NULL;
+        kgdb_sstep_bp_addr[1] = NULL;
+        kgdb_sstep_thread = NULL;
+        return hit;
+}
+
+/*
+ * Catch a single-step-pending thread being deleted and make sure the global
+ * single-step state is cleared.  At this point the breakpoints should have
+ * been removed by __switch_to().
+ */
+void free_thread_info(struct thread_info *ti)
+{
+        if (kgdb_sstep_thread == ti) {
+                kgdb_sstep_thread = NULL;
+
+                /* However, we may now be running in degraded mode, with most
+                 * of the CPUs disabled until such a time as KGDB is reentered,
+                 * so force immediate reentry */
+                kgdb_breakpoint();
+        }
+        kfree(ti);
+}
+
+/*
+ * Handle unknown packets and [CcsDk] packets
+ * - at this point breakpoints have been installed
  */
 int kgdb_arch_handle_exception(int vector, int signo, int err_code,
                                char *remcom_in_buffer, char *remcom_out_buffer,
@@ -130,21 +423,22 @@ int kgdb_arch_handle_exception(int vector, int signo, int err_code,
 
         switch (remcom_in_buffer[0]) {
         case 'c':
-                if (kgdb_contthread && kgdb_contthread != current) {
-                        strcpy(remcom_out_buffer, "E00");
-                        break;
-                }
-
-                kgdb_contthread = NULL;
-
+        case 's':
                 /* try to read optional parameter, pc unchanged if no parm */
                 ptr = &remcom_in_buffer[1];
                 if (kgdb_hex2long(&ptr, &addr))
                         regs->pc = addr;
-                return 0;
+        case 'D':
+        case 'k':
+                atomic_set(&kgdb_cpu_doing_single_step, -1);
 
-        case 's':
-                break; /* we don't do hardware single stepping */
+                if (remcom_in_buffer[0] == 's') {
+                        kgdb_arch_do_singlestep(regs);
+                        kgdb_single_step = 1;
+                        atomic_set(&kgdb_cpu_doing_single_step,
+                                   raw_smp_processor_id());
+                }
+                return 0;
         }
         return -1; /* this means that we do not want to exit from the handler */
 }
@@ -158,6 +452,12 @@ int debugger_intercept(enum exception_code excep, int signo, int si_code,
 {
         int ret;
 
+        if (kgdb_arch_undo_singlestep(regs)) {
+                excep = EXCEP_TRAP;
+                signo = SIGTRAP;
+                si_code = TRAP_TRACE;
+        }
+
         ret = kgdb_handle_exception(excep, signo, si_code, regs);
 
         debugger_local_cache_flushinv();
diff --git a/arch/mn10300/kernel/switch_to.S b/arch/mn10300/kernel/switch_to.S
index 9074d0fb8788..de3e74fc9ea0 100644
--- a/arch/mn10300/kernel/switch_to.S
+++ b/arch/mn10300/kernel/switch_to.S
@@ -39,11 +39,17 @@ ENTRY(__switch_to)
 
         # save prev context
         mov     __switch_back,d0
-        mov     d0,(THREAD_PC,a0)
         mov     sp,a2
         mov     a2,(THREAD_SP,a0)
         mov     a3,(THREAD_A3,a0)
 
+#ifdef CONFIG_KGDB
+        btst    0xff,(kgdb_single_step)
+        bne     __switch_to__lift_sstep_bp
+__switch_to__continue:
+#endif
+        mov     d0,(THREAD_PC,a0)
+
         mov     (THREAD_A3,a1),a3
         mov     (THREAD_SP,a1),a2
 
@@ -68,3 +74,106 @@ ENTRY(__switch_to)
 __switch_back:
         and     ~EPSW_NMID,epsw
         ret     [d2,d3,a2,a3,exreg1],32
+
+#ifdef CONFIG_KGDB
+###############################################################################
+#
+# Lift the single-step breakpoints when the task being traced is switched out
+# A0 = prev
+# A1 = next
+#
+###############################################################################
+__switch_to__lift_sstep_bp:
+        add     -12,sp
+        mov     a0,e4
+        mov     a1,e5
+
+        # Clear the single-step flag to prevent us coming this way until we get
+        # switched back in
+        bclr    0xff,(kgdb_single_step)
+
+        # Remove first breakpoint
+        mov     (kgdb_sstep_bp_addr),a2
+        cmp     0,a2
+        beq     1f
+        movbu   (kgdb_sstep_bp),d0
+        movbu   d0,(a2)
+#if defined(CONFIG_MN10300_CACHE_FLUSH_ICACHE) || defined(CONFIG_MN10300_CACHE_INV_ICACHE)
+        mov     a2,d0
+        mov     a2,d1
+        add     1,d1
+        calls   flush_icache_range
+#endif
+1:
+
+        # Remove second breakpoint
+        mov     (kgdb_sstep_bp_addr+4),a2
+        cmp     0,a2
+        beq     2f
+        movbu   (kgdb_sstep_bp+1),d0
+        movbu   d0,(a2)
+#if defined(CONFIG_MN10300_CACHE_FLUSH_ICACHE) || defined(CONFIG_MN10300_CACHE_INV_ICACHE)
+        mov     a2,d0
+        mov     a2,d1
+        add     1,d1
+        calls   flush_icache_range
+#endif
+2:
+
+        # Change the resumption address and return
+        mov     __switch_back__reinstall_sstep_bp,d0
+        mov     e4,a0
+        mov     e5,a1
+        add     12,sp
+        bra     __switch_to__continue
+
+###############################################################################
+#
+# Reinstall the single-step breakpoints when the task being traced is switched
+# back in (A1 points to the new thread_struct).
+#
+###############################################################################
+__switch_back__reinstall_sstep_bp:
+        add     -12,sp
+        mov     a0,e4                   # save the return value
+        mov     0xff,d3
+
+        # Reinstall first breakpoint
+        mov     (kgdb_sstep_bp_addr),a2
+        cmp     0,a2
+        beq     1f
+        movbu   (a2),d0
+        movbu   d0,(kgdb_sstep_bp)
+        movbu   d3,(a2)
+#if defined(CONFIG_MN10300_CACHE_FLUSH_ICACHE) || defined(CONFIG_MN10300_CACHE_INV_ICACHE)
+        mov     a2,d0
+        mov     a2,d1
+        add     1,d1
+        calls   flush_icache_range
+#endif
+1:
+
+        # Reinstall second breakpoint
+        mov     (kgdb_sstep_bp_addr+4),a2
+        cmp     0,a2
+        beq     2f
+        movbu   (a2),d0
+        movbu   d0,(kgdb_sstep_bp+1)
+        movbu   d3,(a2)
+#if defined(CONFIG_MN10300_CACHE_FLUSH_ICACHE) || defined(CONFIG_MN10300_CACHE_INV_ICACHE)
+        mov     a2,d0
+        mov     a2,d1
+        add     1,d1
+        calls   flush_icache_range
+#endif
+2:
+
+        mov     d3,(kgdb_single_step)
+
+        # Restore the return value (the previous thread_struct pointer)
+        mov     e4,a0
+        mov     a0,d0
+        add     12,sp
+        bra     __switch_back
+
+#endif /* CONFIG_KGDB */