1 files changed, 110 insertions, 1 deletions

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 */