2 files changed, 53 insertions, 13 deletions

diff --git a/arch/blackfin/kernel/kgdb.c b/arch/blackfin/kernel/kgdb.c
index a9c15515bfd7..a1f9641a6425 100644
--- a/arch/blackfin/kernel/kgdb.c
+++ b/arch/blackfin/kernel/kgdb.c
@@ -203,6 +203,8 @@ struct hw_breakpoint {
 
 int kgdb_arch_init(void)
 {
+        debugger_step = 0;
+
         kgdb_remove_all_hw_break();
         return 0;
 }
@@ -368,6 +370,7 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
         char *ptr;
         int newPC;
         int wp_status;
+        int i;
 
         switch (remcom_in_buffer[0]) {
         case 'c':
@@ -392,7 +395,18 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
                 /* set the trace bit if we're stepping */
                 if (remcom_in_buffer[0] == 's') {
                         linux_regs->syscfg |= 0x1;
-                        debugger_step = 1;
+                        debugger_step = linux_regs->ipend;
+                        debugger_step >>= 6;
+                        for (i = 10; i > 0; i--, debugger_step >>= 1)
+                                if (debugger_step & 1)
+                                        break;
+                        /* i indicate event priority of current stopped instruction
+                         * user space instruction is 0, IVG15 is 1, IVTMR is 10.
+                         * debugger_step > 0 means in single step mode
+                         */
+                        debugger_step = i + 1;
+                } else {
+                        debugger_step = 0;
                 }
 
                 wp_status = bfin_read_WPSTAT();
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index 038f70e0be65..eceb484d90f9 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -158,23 +158,45 @@ ENTRY(_ex_single_step)
         cc = r7 == r6;
         if cc jump _bfin_return_from_exception;
 
+        /* Don't do single step in hardware exception handler */
+        p5.l = lo(IPEND);
+        p5.h = hi(IPEND);
+        r6 = [p5];
+        cc = bittst(r6, 5);
+        if cc jump _bfin_return_from_exception;
+
+#ifdef CONFIG_KGDB
+        /* skip single step if current interrupt priority is higher than
+         * that of the first instruction, from which gdb starts single step */
+        r6 >>= 6;
+        r7 = 10;
+.Lfind_priority_start:
+        cc = bittst(r6, 0);
+        if cc jump .Lfind_priority_done;
+        r6 >>= 1;
+        r7 += -1;
+        cc = r7 == 0;
+        if cc jump .Lfind_priority_done;
+        jump.s .Lfind_priority_start;
+.Lfind_priority_done:
+        p4.l = _debugger_step;
+        p4.h = _debugger_step;
+        r6 = [p4];
+        cc = r6 == 0;
+        if cc jump .Ldo_single_step;
+        r6 += -1;
+        cc = r6 < r7;
+        if cc jump _bfin_return_from_exception;
+.Ldo_single_step:
+#endif
+
         /* If we were in user mode, do the single step normally.  */
-        p5.l = lo(IPEND);
-        p5.h = hi(IPEND);
         r6 = [p5];
         r7 = 0xffe0 (z);
         r7 = r7 & r6;
         cc = r7 == 0;
-        if !cc jump 1f;
-
-        /* Single stepping only a single instruction, so clear the trace
-         * bit here.  */
-        r7 = syscfg;
-        bitclr (r7, 0);
-        syscfg = R7;
-        jump _ex_trap_c;
+        if cc jump 1f;
 
-1:
         /*
          * We were in an interrupt handler.  By convention, all of them save
          * SYSCFG with their first instruction, so by checking whether our
@@ -202,11 +224,15 @@ ENTRY(_ex_single_step)
         cc = R7 == R6;
         if !cc jump _bfin_return_from_exception;
 
+1:
+        /* Single stepping only a single instruction, so clear the trace
+         * bit here.  */
         r7 = syscfg;
         bitclr (r7, 0);
         syscfg = R7;
 
-        /* Fall through to _bfin_return_from_exception.  */
+        jump _ex_trap_c;
+
 ENDPROC(_ex_single_step)
 
 ENTRY(_bfin_return_from_exception)