3 files changed, 425 insertions, 12 deletions

diff --git a/arch/mn10300/include/asm/thread_info.h b/arch/mn10300/include/asm/thread_info.h
index aa07a4a5d794..c139fa17e2e4 100644
--- a/arch/mn10300/include/asm/thread_info.h
+++ b/arch/mn10300/include/asm/thread_info.h
@@ -129,7 +129,11 @@ static inline unsigned long current_stack_pointer(void)
 #define alloc_thread_info(tsk) kmalloc(THREAD_SIZE, GFP_KERNEL)
 #endif
 
+#ifndef CONFIG_KGDB
 #define free_thread_info(ti)    kfree((ti))
+#else
+extern void free_thread_info(struct thread_info *);
+#endif
 #define get_thread_info(ti)     get_task_struct((ti)->task)
 #define put_thread_info(ti)     put_task_struct((ti)->task)
 
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 */