Blackfin arch: add supporting for kgdb

Signed-off-by: Sonic Zhang <sonic.zhang@analog.com>
Signed-off-by: Bryan Wu <cooloney@kernel.org>

7 files changed, 606 insertions, 195 deletions

diff --git a/arch/blackfin/Kconfig.debug b/arch/blackfin/Kconfig.debug
index 0afa89818722..2cb0a3080d79 100644
--- a/arch/blackfin/Kconfig.debug
+++ b/arch/blackfin/Kconfig.debug
@@ -2,6 +2,9 @@ menu "Kernel hacking"
 
 source "lib/Kconfig.debug"
 
+config HAVE_ARCH_KGDB
+       def_bool y
+
 config DEBUG_MMRS
         bool "Generate Blackfin MMR tree"
         select DEBUG_FS
diff --git a/arch/blackfin/include/asm/kgdb.h b/arch/blackfin/include/asm/kgdb.h
index 0f73847fd6bc..26ebac6646d8 100644
--- a/arch/blackfin/include/asm/kgdb.h
+++ b/arch/blackfin/include/asm/kgdb.h
@@ -124,9 +124,16 @@ enum regnames {
 /* Number of bytes of registers.  */
 #define NUMREGBYTES BFIN_NUM_REGS*4
 
-#define BREAKPOINT() asm("   EXCPT 2;");
-#define BREAK_INSTR_SIZE       2
-#define HW_BREAKPOINT_NUM               6
+static inline void arch_kgdb_breakpoint(void)
+{
+        asm("   EXCPT 2;");
+}
+#define BREAK_INSTR_SIZE        2
+#define CACHE_FLUSH_IS_SAFE     1
+#define HW_INST_WATCHPOINT_NUM  6
+#define HW_WATCHPOINT_NUM       8
+#define TYPE_INST_WATCHPOINT    0
+#define TYPE_DATA_WATCHPOINT    1
 
 /* Instruction watchpoint address control register bits mask */
 #define WPPWR           0x1
@@ -163,10 +170,11 @@ enum regnames {
 #define WPDAEN1         0x8
 #define WPDCNTEN0       0x10
 #define WPDCNTEN1       0x20
+
 #define WPDSRC0         0xc0
-#define WPDACC0         0x300
+#define WPDACC0_OFFSET  8
 #define WPDSRC1         0xc00
-#define WPDACC1         0x3000
+#define WPDACC1_OFFSET  12
 
 /* Watchpoint status register bits mask */
 #define STATIA0         0x1
@@ -178,7 +186,4 @@ enum regnames {
 #define STATDA0         0x40
 #define STATDA1         0x80
 
-extern void kgdb_print(const char *fmt, ...);
-extern void init_kgdb_uart(void);
-
 #endif
diff --git a/arch/blackfin/kernel/kgdb.c b/arch/blackfin/kernel/kgdb.c
index a1f9641a6425..b795a207742c 100644
--- a/arch/blackfin/kernel/kgdb.c
+++ b/arch/blackfin/kernel/kgdb.c
@@ -1,32 +1,9 @@
 /*
- * File:         arch/blackfin/kernel/kgdb.c
- * Based on:
- * Author:       Sonic Zhang
+ * arch/blackfin/kernel/kgdb.c - Blackfin kgdb pieces
  *
- * Created:
- * Description:
+ * Copyright 2005-2008 Analog Devices Inc.
  *
- * Rev:          $Id: kgdb_bfin_linux-2.6.x.patch 4934 2007-02-13 09:32:11Z sonicz $
- *
- * Modified:
- *               Copyright 2005-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/string.h>
@@ -39,24 +16,29 @@
 #include <linux/kgdb.h>
 #include <linux/console.h>
 #include <linux/init.h>
-#include <linux/debugger.h>
 #include <linux/errno.h>
 #include <linux/irq.h>
+#include <linux/uaccess.h>
 #include <asm/system.h>
 #include <asm/traps.h>
 #include <asm/blackfin.h>
+#include <asm/dma.h>
 
 /* Put the error code here just in case the user cares.  */
-int gdb_bf533errcode;
+int gdb_bfin_errcode;
 /* Likewise, the vector number here (since GDB only gets the signal
    number through the usual means, and that's not very specific).  */
-int gdb_bf533vector = -1;
+int gdb_bfin_vector = -1;
 
 #if KGDB_MAX_NO_CPUS != 8
 #error change the definition of slavecpulocks
 #endif
 
-void regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
+#ifdef CONFIG_BFIN_WDT
+# error "Please unselect blackfin watchdog driver before build KGDB."
+#endif
+
+void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
 {
         gdb_regs[BFIN_R0] = regs->r0;
         gdb_regs[BFIN_R1] = regs->r1;
@@ -133,7 +115,7 @@ void sleeping_thread_to_gdb_regs(unsigned long *gdb_regs, struct task_struct *p)
         gdb_regs[BFIN_SEQSTAT] = p->thread.seqstat;
 }
 
-void gdb_regs_to_regs(unsigned long *gdb_regs, struct pt_regs *regs)
+void gdb_regs_to_pt_regs(unsigned long *gdb_regs, struct pt_regs *regs)
 {
         regs->r0 = gdb_regs[BFIN_R0];
         regs->r1 = gdb_regs[BFIN_R1];
@@ -199,171 +181,208 @@ struct hw_breakpoint {
         unsigned int dataacc:2;
         unsigned short count;
         unsigned int addr;
-} breakinfo[HW_BREAKPOINT_NUM];
+} breakinfo[HW_WATCHPOINT_NUM];
 
-int kgdb_arch_init(void)
-{
-        debugger_step = 0;
-
-        kgdb_remove_all_hw_break();
-        return 0;
-}
-
-int kgdb_set_hw_break(unsigned long addr)
+int bfin_set_hw_break(unsigned long addr, int len, enum kgdb_bptype type)
 {
         int breakno;
-        for (breakno = 0; breakno < HW_BREAKPOINT_NUM; breakno++)
-                if (!breakinfo[breakno].occupied) {
+        int bfin_type;
+        int dataacc = 0;
+
+        switch (type) {
+        case BP_HARDWARE_BREAKPOINT:
+                bfin_type = TYPE_INST_WATCHPOINT;
+                break;
+        case BP_WRITE_WATCHPOINT:
+                dataacc = 1;
+                bfin_type = TYPE_DATA_WATCHPOINT;
+                break;
+        case BP_READ_WATCHPOINT:
+                dataacc = 2;
+                bfin_type = TYPE_DATA_WATCHPOINT;
+                break;
+        case BP_ACCESS_WATCHPOINT:
+                dataacc = 3;
+                bfin_type = TYPE_DATA_WATCHPOINT;
+                break;
+        default:
+                return -ENOSPC;
+        }
+
+        /* Becasue hardware data watchpoint impelemented in current
+         * Blackfin can not trigger an exception event as the hardware
+         * instrction watchpoint does, we ignaore all data watch point here.
+         * They can be turned on easily after future blackfin design
+         * supports this feature.
+         */
+        for (breakno = 0; breakno < HW_INST_WATCHPOINT_NUM; breakno++)
+                if (bfin_type == breakinfo[breakno].type
+                        && !breakinfo[breakno].occupied) {
                         breakinfo[breakno].occupied = 1;
                         breakinfo[breakno].enabled = 1;
-                        breakinfo[breakno].type = 1;
                         breakinfo[breakno].addr = addr;
+                        breakinfo[breakno].dataacc = dataacc;
+                        breakinfo[breakno].count = 0;
                         return 0;
                 }
 
         return -ENOSPC;
 }
 
-int kgdb_remove_hw_break(unsigned long addr)
+int bfin_remove_hw_break(unsigned long addr, int len, enum kgdb_bptype type)
 {
         int breakno;
-        for (breakno = 0; breakno < HW_BREAKPOINT_NUM; breakno++)
-                if (breakinfo[breakno].addr == addr)
-                        memset(&(breakinfo[breakno]), 0, sizeof(struct hw_breakpoint));
+        int bfin_type;
+
+        switch (type) {
+        case BP_HARDWARE_BREAKPOINT:
+                bfin_type = TYPE_INST_WATCHPOINT;
+                break;
+        case BP_WRITE_WATCHPOINT:
+        case BP_READ_WATCHPOINT:
+        case BP_ACCESS_WATCHPOINT:
+                bfin_type = TYPE_DATA_WATCHPOINT;
+                break;
+        default:
+                return 0;
+        }
+        for (breakno = 0; breakno < HW_WATCHPOINT_NUM; breakno++)
+                if (bfin_type == breakinfo[breakno].type
+                        && breakinfo[breakno].occupied
+                        && breakinfo[breakno].addr == addr) {
+                        breakinfo[breakno].occupied = 0;
+                        breakinfo[breakno].enabled = 0;
+                }
 
         return 0;
 }
 
-void kgdb_remove_all_hw_break(void)
+void bfin_remove_all_hw_break(void)
 {
-        memset(breakinfo, 0, sizeof(struct hw_breakpoint)*8);
-}
+        int breakno;
 
-/*
-void kgdb_show_info(void)
-{
-        printk(KERN_DEBUG "hwd: wpia0=0x%x, wpiacnt0=%d, wpiactl=0x%x, wpstat=0x%x\n",
-                bfin_read_WPIA0(), bfin_read_WPIACNT0(),
-                bfin_read_WPIACTL(), bfin_read_WPSTAT());
+        memset(breakinfo, 0, sizeof(struct hw_breakpoint)*HW_WATCHPOINT_NUM);
+
+        for (breakno = 0; breakno < HW_INST_WATCHPOINT_NUM; breakno++)
+                breakinfo[breakno].type = TYPE_INST_WATCHPOINT;
+        for (; breakno < HW_WATCHPOINT_NUM; breakno++)
+                breakinfo[breakno].type = TYPE_DATA_WATCHPOINT;
 }
-*/
 
-void kgdb_correct_hw_break(void)
+void bfin_correct_hw_break(void)
 {
         int breakno;
-        int correctit;
-        uint32_t wpdactl = bfin_read_WPDACTL();
+        unsigned int wpiactl = 0;
+        unsigned int wpdactl = 0;
+        int enable_wp = 0;
+
+        for (breakno = 0; breakno < HW_WATCHPOINT_NUM; breakno++)
+                if (breakinfo[breakno].enabled) {
+                        enable_wp = 1;
 
-        correctit = 0;
-        for (breakno = 0; breakno < HW_BREAKPOINT_NUM; breakno++) {
-                if (breakinfo[breakno].type == 1) {
                         switch (breakno) {
                         case 0:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN0)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN01|EMUSW0);
-                                        wpdactl |= WPIAEN0|WPICNTEN0;
-                                        bfin_write_WPIA0(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT0(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN0)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN0;
-                                }
+                                wpiactl |= WPIAEN0|WPICNTEN0;
+                                bfin_write_WPIA0(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT0(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
-
                         case 1:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN1)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN01|EMUSW1);
-                                        wpdactl |= WPIAEN1|WPICNTEN1;
-                                        bfin_write_WPIA1(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT1(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN1)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN1;
-                                }
+                                wpiactl |= WPIAEN1|WPICNTEN1;
+                                bfin_write_WPIA1(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT1(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
-
                         case 2:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN2)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN23|EMUSW2);
-                                        wpdactl |= WPIAEN2|WPICNTEN2;
-                                        bfin_write_WPIA2(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT2(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN2)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN2;
-                                }
+                                wpiactl |= WPIAEN2|WPICNTEN2;
+                                bfin_write_WPIA2(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT2(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
-
                         case 3:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN3)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN23|EMUSW3);
-                                        wpdactl |= WPIAEN3|WPICNTEN3;
-                                        bfin_write_WPIA3(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT3(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN3)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN3;
-                                }
+                                wpiactl |= WPIAEN3|WPICNTEN3;
+                                bfin_write_WPIA3(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT3(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
                         case 4:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN4)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN45|EMUSW4);
-                                        wpdactl |= WPIAEN4|WPICNTEN4;
-                                        bfin_write_WPIA4(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT4(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN4)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN4;
-                                }
+                                wpiactl |= WPIAEN4|WPICNTEN4;
+                                bfin_write_WPIA4(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT4(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
                         case 5:
-                                if (breakinfo[breakno].enabled && !(wpdactl & WPIAEN5)) {
-                                        correctit = 1;
-                                        wpdactl &= ~(WPIREN45|EMUSW5);
-                                        wpdactl |= WPIAEN5|WPICNTEN5;
-                                        bfin_write_WPIA5(breakinfo[breakno].addr);
-                                        bfin_write_WPIACNT5(breakinfo[breakno].skip);
-                                } else if (!breakinfo[breakno].enabled && (wpdactl & WPIAEN5)) {
-                                        correctit = 1;
-                                        wpdactl &= ~WPIAEN5;
-                                }
+                                wpiactl |= WPIAEN5|WPICNTEN5;
+                                bfin_write_WPIA5(breakinfo[breakno].addr);
+                                bfin_write_WPIACNT5(breakinfo[breakno].count
+                                        + breakinfo->skip);
+                                break;
+                        case 6:
+                                wpdactl |= WPDAEN0|WPDCNTEN0|WPDSRC0;
+                                wpdactl |= breakinfo[breakno].dataacc
+                                        << WPDACC0_OFFSET;
+                                bfin_write_WPDA0(breakinfo[breakno].addr);
+                                bfin_write_WPDACNT0(breakinfo[breakno].count
+                                        + breakinfo->skip);
+                                break;
+                        case 7:
+                                wpdactl |= WPDAEN1|WPDCNTEN1|WPDSRC1;
+                                wpdactl |= breakinfo[breakno].dataacc
+                                        << WPDACC1_OFFSET;
+                                bfin_write_WPDA1(breakinfo[breakno].addr);
+                                bfin_write_WPDACNT1(breakinfo[breakno].count
+                                        + breakinfo->skip);
                                 break;
                         }
                 }
-        }
-        if (correctit) {
-                wpdactl &= ~WPAND;
-                wpdactl |= WPPWR;
-                /*printk("correct_hw_break: wpdactl=0x%x\n", wpdactl);*/
+
+        /* Should enable WPPWR bit first before set any other
+         * WPIACTL and WPDACTL bits */
+        if (enable_wp) {
+                bfin_write_WPIACTL(WPPWR);
+                CSYNC();
+                bfin_write_WPIACTL(wpiactl|WPPWR);
                 bfin_write_WPDACTL(wpdactl);
                 CSYNC();
-                /*kgdb_show_info();*/
         }
 }
 
 void kgdb_disable_hw_debug(struct pt_regs *regs)
 {
         /* Disable hardware debugging while we are in kgdb */
-        bfin_write_WPIACTL(bfin_read_WPIACTL() & ~0x1);
+        bfin_write_WPIACTL(0);
+        bfin_write_WPDACTL(0);
         CSYNC();
 }
 
-void kgdb_post_master_code(struct pt_regs *regs, int eVector, int err_code)
+#ifdef CONFIG_SMP
+void kgdb_passive_cpu_callback(void *info)
+{
+        kgdb_nmicallback(raw_smp_processor_id(), get_irq_regs());
+}
+
+void kgdb_roundup_cpus(unsigned long flags)
+{
+        smp_call_function(kgdb_passive_cpu_callback, NULL, 0, 0);
+}
+
+void kgdb_roundup_cpu(int cpu, unsigned long flags)
+{
+        smp_call_function_single(cpu, kgdb_passive_cpu_callback, NULL, 0, 0);
+}
+#endif
+
+void kgdb_post_primary_code(struct pt_regs *regs, int eVector, int err_code)
 {
         /* Master processor is completely in the debugger */
-        gdb_bf533vector = eVector;
-        gdb_bf533errcode = err_code;
+        gdb_bfin_vector = eVector;
+        gdb_bfin_errcode = err_code;
 }
 
-int kgdb_arch_handle_exception(int exceptionVector, int signo,
+int kgdb_arch_handle_exception(int vector, int signo,
                                int err_code, char *remcom_in_buffer,
                                char *remcom_out_buffer,
-                               struct pt_regs *linux_regs)
+                               struct pt_regs *regs)
 {
         long addr;
         long breakno;
@@ -385,44 +404,40 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
                 /* try to read optional parameter, pc unchanged if no parm */
                 ptr = &remcom_in_buffer[1];
                 if (kgdb_hex2long(&ptr, &addr)) {
-                        linux_regs->retx = addr;
+                        regs->retx = addr;
                 }
-                newPC = linux_regs->retx;
+                newPC = regs->retx;
 
                 /* clear the trace bit */
-                linux_regs->syscfg &= 0xfffffffe;
+                regs->syscfg &= 0xfffffffe;
 
                 /* set the trace bit if we're stepping */
                 if (remcom_in_buffer[0] == 's') {
-                        linux_regs->syscfg |= 0x1;
-                        debugger_step = linux_regs->ipend;
-                        debugger_step >>= 6;
-                        for (i = 10; i > 0; i--, debugger_step >>= 1)
-                                if (debugger_step & 1)
+                        regs->syscfg |= 0x1;
+                        kgdb_single_step = regs->ipend;
+                        kgdb_single_step >>= 6;
+                        for (i = 10; i > 0; i--, kgdb_single_step >>= 1)
+                                if (kgdb_single_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
+                         * kgdb_single_step > 0 means in single step mode
                          */
-                        debugger_step = i + 1;
-                } else {
-                        debugger_step = 0;
+                        kgdb_single_step = i + 1;
                 }
 
-                wp_status = bfin_read_WPSTAT();
-                CSYNC();
-
-                if (exceptionVector == VEC_WATCH) {
-                        for (breakno = 0; breakno < 6; ++breakno) {
+                if (vector == VEC_WATCH) {
+                        wp_status = bfin_read_WPSTAT();
+                        for (breakno = 0; breakno < HW_WATCHPOINT_NUM; breakno++) {
                                 if (wp_status & (1 << breakno)) {
                                         breakinfo->skip = 1;
                                         break;
                                 }
                         }
+                        bfin_write_WPSTAT(0);
                 }
-                kgdb_correct_hw_break();
 
-                bfin_write_WPSTAT(0);
+                bfin_correct_hw_break();
 
                 return 0;
         }                       /* switch */
@@ -431,5 +446,385 @@ int kgdb_arch_handle_exception(int exceptionVector, int signo,
 
 struct kgdb_arch arch_kgdb_ops = {
         .gdb_bpt_instr = {0xa1},
+#ifdef CONFIG_SMP
+        .flags = KGDB_HW_BREAKPOINT|KGDB_THR_PROC_SWAP,
+#else
         .flags = KGDB_HW_BREAKPOINT,
+#endif
+        .set_hw_breakpoint = bfin_set_hw_break,
+        .remove_hw_breakpoint = bfin_remove_hw_break,
+        .remove_all_hw_break = bfin_remove_all_hw_break,
+        .correct_hw_break = bfin_correct_hw_break,
 };
+
+static int hex(char ch)
+{
+        if ((ch >= 'a') && (ch <= 'f'))
+                return ch - 'a' + 10;
+        if ((ch >= '0') && (ch <= '9'))
+                return ch - '0';
+        if ((ch >= 'A') && (ch <= 'F'))
+                return ch - 'A' + 10;
+        return -1;
+}
+
+static int validate_memory_access_address(unsigned long addr, int size)
+{
+        int cpu = raw_smp_processor_id();
+
+        if (size < 0)
+                return EFAULT;
+        if (addr >= 0x1000 && (addr + size) <= physical_mem_end)
+                return 0;
+        if (addr >= SYSMMR_BASE)
+                return 0;
+        if (addr >= ASYNC_BANK0_BASE
+           && addr + size <= ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE)
+                return 0;
+        if (cpu == 0) {
+                if (addr >= L1_SCRATCH_START
+                   && (addr + size <= L1_SCRATCH_START + L1_SCRATCH_LENGTH))
+                        return 0;
+#if L1_CODE_LENGTH != 0
+                if (addr >= L1_CODE_START
+                   && (addr + size <= L1_CODE_START + L1_CODE_LENGTH))
+                        return 0;
+#endif
+#if L1_DATA_A_LENGTH != 0
+                if (addr >= L1_DATA_A_START
+                   && (addr + size <= L1_DATA_A_START + L1_DATA_A_LENGTH))
+                        return 0;
+#endif
+#if L1_DATA_B_LENGTH != 0
+                if (addr >= L1_DATA_B_START
+                   && (addr + size <= L1_DATA_B_START + L1_DATA_B_LENGTH))
+                        return 0;
+#endif
+#ifdef CONFIG_SMP
+        } else if (cpu == 1) {
+                if (addr >= COREB_L1_SCRATCH_START
+                   && (addr + size <= COREB_L1_SCRATCH_START
+                   + L1_SCRATCH_LENGTH))
+                        return 0;
+# if L1_CODE_LENGTH != 0
+                if (addr >= COREB_L1_CODE_START
+                   && (addr + size <= COREB_L1_CODE_START + L1_CODE_LENGTH))
+                        return 0;
+# endif
+# if L1_DATA_A_LENGTH != 0
+                if (addr >= COREB_L1_DATA_A_START
+                   && (addr + size <= COREB_L1_DATA_A_START + L1_DATA_A_LENGTH))
+                        return 0;
+# endif
+# if L1_DATA_B_LENGTH != 0
+                if (addr >= COREB_L1_DATA_B_START
+                   && (addr + size <= COREB_L1_DATA_B_START + L1_DATA_B_LENGTH))
+                        return 0;
+# endif
+#endif
+        }
+
+#if L2_LENGTH != 0
+        if (addr >= L2_START
+           && addr + size <= L2_START + L2_LENGTH)
+                return 0;
+#endif
+
+        return EFAULT;
+}
+
+/*
+ * Convert the memory pointed to by mem into hex, placing result in buf.
+ * Return a pointer to the last char put in buf (null). May return an error.
+ */
+int kgdb_mem2hex(char *mem, char *buf, int count)
+{
+        char *tmp;
+        int err = 0;
+        unsigned char *pch;
+        unsigned short mmr16;
+        unsigned long mmr32;
+        int cpu = raw_smp_processor_id();
+
+        if (validate_memory_access_address((unsigned long)mem, count))
+                return EFAULT;
+
+        /*
+         * We use the upper half of buf as an intermediate buffer for the
+         * raw memory copy.  Hex conversion will work against this one.
+         */
+        tmp = buf + count;
+
+        if ((unsigned int)mem >= SYSMMR_BASE) { /*access MMR registers*/
+                switch (count) {
+                case 2:
+                        if ((unsigned int)mem % 2 == 0) {
+                                mmr16 = *(unsigned short *)mem;
+                                pch = (unsigned char *)&mmr16;
+                                *tmp++ = *pch++;
+                                *tmp++ = *pch++;
+                                tmp -= 2;
+                        } else
+                                err = EFAULT;
+                        break;
+                case 4:
+                        if ((unsigned int)mem % 4 == 0) {
+                                mmr32 = *(unsigned long *)mem;
+                                pch = (unsigned char *)&mmr32;
+                                *tmp++ = *pch++;
+                                *tmp++ = *pch++;
+                                *tmp++ = *pch++;
+                                *tmp++ = *pch++;
+                                tmp -= 4;
+                        } else
+                                err = EFAULT;
+                        break;
+                default:
+                        err = EFAULT;
+                }
+        } else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
+                (unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+#ifdef CONFIG_SMP
+                || cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
+                (unsigned int)(mem + count) <=
+                COREB_L1_CODE_START + L1_CODE_LENGTH
+#endif
+                ) {
+                /* access L1 instruction SRAM*/
+                if (dma_memcpy(tmp, mem, count) == NULL)
+                        err = EFAULT;
+        } else
+                err = probe_kernel_read(tmp, mem, count);
+
+        if (!err) {
+                while (count > 0) {
+                        buf = pack_hex_byte(buf, *tmp);
+                        tmp++;
+                        count--;
+                }
+
+                *buf = 0;
+        }
+
+        return err;
+}
+
+/*
+ * Copy the binary array pointed to by buf into mem.  Fix $, #, and
+ * 0x7d escaped with 0x7d.  Return a pointer to the character after
+ * the last byte written.
+ */
+int kgdb_ebin2mem(char *buf, char *mem, int count)
+{
+        char *tmp_old;
+        char *tmp_new;
+        unsigned short *mmr16;
+        unsigned long *mmr32;
+        int err = 0;
+        int size = 0;
+        int cpu = raw_smp_processor_id();
+
+        tmp_old = tmp_new = buf;
+
+        while (count-- > 0) {
+                if (*tmp_old == 0x7d)
+                        *tmp_new = *(++tmp_old) ^ 0x20;
+                else
+                        *tmp_new = *tmp_old;
+                tmp_new++;
+                tmp_old++;
+                size++;
+        }
+
+        if (validate_memory_access_address((unsigned long)mem, size))
+                return EFAULT;
+
+        if ((unsigned int)mem >= SYSMMR_BASE) { /*access MMR registers*/
+                switch (size) {
+                case 2:
+                        if ((unsigned int)mem % 2 == 0) {
+                                mmr16 = (unsigned short *)buf;
+                                *(unsigned short *)mem = *mmr16;
+                        } else
+                                return EFAULT;
+                        break;
+                case 4:
+                        if ((unsigned int)mem % 4 == 0) {
+                                mmr32 = (unsigned long *)buf;
+                                *(unsigned long *)mem = *mmr32;
+                        } else
+                                return EFAULT;
+                        break;
+                default:
+                        return EFAULT;
+                }
+        } else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
+                (unsigned int)(mem + count) < L1_CODE_START + L1_CODE_LENGTH
+#ifdef CONFIG_SMP
+                || cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
+                (unsigned int)(mem + count) <=
+                COREB_L1_CODE_START + L1_CODE_LENGTH
+#endif
+                ) {
+                /* access L1 instruction SRAM */
+                if (dma_memcpy(mem, buf, size) == NULL)
+                        err = EFAULT;
+        } else
+                err = probe_kernel_write(mem, buf, size);
+
+        return err;
+}
+
+/*
+ * Convert the hex array pointed to by buf into binary to be placed in mem.
+ * Return a pointer to the character AFTER the last byte written.
+ * May return an error.
+ */
+int kgdb_hex2mem(char *buf, char *mem, int count)
+{
+        char *tmp_raw;
+        char *tmp_hex;
+        unsigned short *mmr16;
+        unsigned long *mmr32;
+        int cpu = raw_smp_processor_id();
+
+        if (validate_memory_access_address((unsigned long)mem, count))
+                return EFAULT;
+
+        /*
+         * We use the upper half of buf as an intermediate buffer for the
+         * raw memory that is converted from hex.
+         */
+        tmp_raw = buf + count * 2;
+
+        tmp_hex = tmp_raw - 1;
+        while (tmp_hex >= buf) {
+                tmp_raw--;
+                *tmp_raw = hex(*tmp_hex--);
+                *tmp_raw |= hex(*tmp_hex--) << 4;
+        }
+
+        if ((unsigned int)mem >= SYSMMR_BASE) { /*access MMR registers*/
+                switch (count) {
+                case 2:
+                        if ((unsigned int)mem % 2 == 0) {
+                                mmr16 = (unsigned short *)tmp_raw;
+                                *(unsigned short *)mem = *mmr16;
+                        } else
+                                return EFAULT;
+                        break;
+                case 4:
+                        if ((unsigned int)mem % 4 == 0) {
+                                mmr32 = (unsigned long *)tmp_raw;
+                                *(unsigned long *)mem = *mmr32;
+                        } else
+                                return EFAULT;
+                        break;
+                default:
+                        return EFAULT;
+                }
+        } else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
+                (unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+#ifdef CONFIG_SMP
+                || cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
+                (unsigned int)(mem + count) <=
+                COREB_L1_CODE_START + L1_CODE_LENGTH
+#endif
+                ) {
+                /* access L1 instruction SRAM */
+                if (dma_memcpy(mem, tmp_raw, count) == NULL)
+                        return EFAULT;
+        } else
+                return probe_kernel_write(mem, tmp_raw, count);
+        return 0;
+}
+
+int kgdb_validate_break_address(unsigned long addr)
+{
+        int cpu = raw_smp_processor_id();
+
+        if (addr >= 0x1000 && (addr + BREAK_INSTR_SIZE) <= physical_mem_end)
+                return 0;
+        if (addr >= ASYNC_BANK0_BASE
+           && addr + BREAK_INSTR_SIZE <= ASYNC_BANK3_BASE + ASYNC_BANK3_BASE)
+                return 0;
+#if L1_CODE_LENGTH != 0
+        if (cpu == 0 && addr >= L1_CODE_START
+           && addr + BREAK_INSTR_SIZE <= L1_CODE_START + L1_CODE_LENGTH)
+                return 0;
+# ifdef CONFIG_SMP
+        else if (cpu == 1 && addr >= COREB_L1_CODE_START
+           && addr + BREAK_INSTR_SIZE <= COREB_L1_CODE_START + L1_CODE_LENGTH)
+                return 0;
+# endif
+#endif
+#if L2_LENGTH != 0
+        if (addr >= L2_START
+           && addr + BREAK_INSTR_SIZE <= L2_START + L2_LENGTH)
+                return 0;
+#endif
+
+        return EFAULT;
+}
+
+int kgdb_arch_set_breakpoint(unsigned long addr, char *saved_instr)
+{
+        int err;
+        int cpu = raw_smp_processor_id();
+
+        if ((cpu == 0 && (unsigned int)addr >= L1_CODE_START
+                && (unsigned int)(addr + BREAK_INSTR_SIZE)
+                < L1_CODE_START + L1_CODE_LENGTH)
+#ifdef CONFIG_SMP
+                || (cpu == 1 && (unsigned int)addr >= COREB_L1_CODE_START
+                && (unsigned int)(addr + BREAK_INSTR_SIZE)
+                < COREB_L1_CODE_START + L1_CODE_LENGTH)
+#endif
+                ) {
+                /* access L1 instruction SRAM */
+                if (dma_memcpy(saved_instr, (void *)addr, BREAK_INSTR_SIZE)
+                        == NULL)
+                        return -EFAULT;
+
+                if (dma_memcpy((void *)addr, arch_kgdb_ops.gdb_bpt_instr,
+                        BREAK_INSTR_SIZE) == NULL)
+                        return -EFAULT;
+
+                return 0;
+        } else {
+                err = probe_kernel_read(saved_instr, (char *)addr,
+                        BREAK_INSTR_SIZE);
+                if (err)
+                        return err;
+
+                return probe_kernel_write((char *)addr,
+                        arch_kgdb_ops.gdb_bpt_instr, BREAK_INSTR_SIZE);
+        }
+}
+
+int kgdb_arch_remove_breakpoint(unsigned long addr, char *bundle)
+{
+        if ((unsigned int)addr >= L1_CODE_START &&
+                (unsigned int)(addr + BREAK_INSTR_SIZE) <
+                        L1_CODE_START + L1_CODE_LENGTH) {
+                /* access L1 instruction SRAM */
+                if (dma_memcpy((void *)addr, bundle, BREAK_INSTR_SIZE) == NULL)
+                        return -EFAULT;
+
+                return 0;
+        } else
+                return probe_kernel_write((char *)addr,
+                                (char *)bundle, BREAK_INSTR_SIZE);
+}
+
+int kgdb_arch_init(void)
+{
+        kgdb_single_step = 0;
+
+        bfin_remove_all_hw_break();
+        return 0;
+}
+
+void kgdb_arch_exit(void)
+{
+}
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index bd41fca315dd..8d561baef896 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -43,12 +43,11 @@
 #include <asm/dma.h>
 
 #ifdef CONFIG_KGDB
-# include <linux/debugger.h>
 # include <linux/kgdb.h>
 
 # define CHK_DEBUGGER_TRAP() \
         do { \
-                CHK_DEBUGGER(trapnr, sig, info.si_code, fp, ); \
+                kgdb_handle_exception(trapnr, sig, info.si_code, fp); \
         } while (0)
 # define CHK_DEBUGGER_TRAP_MAYBE() \
         do { \
@@ -300,7 +299,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = SEGV_STACKFLOW;
                 sig = SIGSEGV;
                 printk(KERN_NOTICE EXC_0x03(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x04 - User Defined, Caught by default */
         /* 0x05 - User Defined, Caught by default */
@@ -329,7 +328,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = TRAP_TRACEFLOW;
                 sig = SIGTRAP;
                 printk(KERN_NOTICE EXC_0x11(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x12 - Reserved, Caught by default */
         /* 0x13 - Reserved, Caught by default */
@@ -351,35 +350,35 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = ILL_ILLOPC;
                 sig = SIGILL;
                 printk(KERN_NOTICE EXC_0x21(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x22 - Illegal Instruction Combination, handled here */
         case VEC_ILGAL_I:
                 info.si_code = ILL_ILLPARAOP;
                 sig = SIGILL;
                 printk(KERN_NOTICE EXC_0x22(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x23 - Data CPLB protection violation, handled here */
         case VEC_CPLB_VL:
                 info.si_code = ILL_CPLB_VI;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x23(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x24 - Data access misaligned, handled here */
         case VEC_MISALI_D:
                 info.si_code = BUS_ADRALN;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x24(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x25 - Unrecoverable Event, handled here */
         case VEC_UNCOV:
                 info.si_code = ILL_ILLEXCPT;
                 sig = SIGILL;
                 printk(KERN_NOTICE EXC_0x25(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x26 - Data CPLB Miss, normal case is handled in _cplb_hdr,
                 error case is handled here */
@@ -387,7 +386,6 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = BUS_ADRALN;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x26(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
                 break;
         /* 0x27 - Data CPLB Multiple Hits - Linux Trap Zero, handled here */
         case VEC_CPLB_MHIT:
@@ -399,7 +397,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 else
 #endif
                         printk(KERN_NOTICE EXC_0x27(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x28 - Emulation Watchpoint, handled here */
         case VEC_WATCH:
@@ -418,7 +416,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = BUS_OPFETCH;
                 sig = SIGBUS;
                 printk(KERN_NOTICE "BF535: VEC_ISTRU_VL\n");
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
 #else
         /* 0x29 - Reserved, Caught by default */
@@ -428,21 +426,20 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 info.si_code = BUS_ADRALN;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x2A(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x2B - Instruction CPLB protection violation, handled here */
         case VEC_CPLB_I_VL:
                 info.si_code = ILL_CPLB_VI;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x2B(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x2C - Instruction CPLB miss, handled in _cplb_hdr */
         case VEC_CPLB_I_M:
                 info.si_code = ILL_CPLB_MISS;
                 sig = SIGBUS;
                 printk(KERN_NOTICE EXC_0x2C(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
                 break;
         /* 0x2D - Instruction CPLB Multiple Hits, handled here */
         case VEC_CPLB_I_MHIT:
@@ -454,14 +451,14 @@ asmlinkage void trap_c(struct pt_regs *fp)
                 else
 #endif
                         printk(KERN_NOTICE EXC_0x2D(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x2E - Illegal use of Supervisor Resource, handled here */
         case VEC_ILL_RES:
                 info.si_code = ILL_PRVOPC;
                 sig = SIGILL;
                 printk(KERN_NOTICE EXC_0x2E(KERN_NOTICE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         /* 0x2F - Reserved, Caught by default */
         /* 0x30 - Reserved, Caught by default */
@@ -508,14 +505,14 @@ asmlinkage void trap_c(struct pt_regs *fp)
                         printk(KERN_NOTICE HWC_default(KERN_NOTICE));
                         break;
                 }
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         default:
                 info.si_code = TRAP_ILLTRAP;
                 sig = SIGTRAP;
                 printk(KERN_EMERG "Caught Unhandled Exception, code = %08lx\n",
                         (fp->seqstat & SEQSTAT_EXCAUSE));
-                CHK_DEBUGGER_TRAP();
+                CHK_DEBUGGER_TRAP_MAYBE();
                 break;
         }
 
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_map.h b/arch/blackfin/mach-bf561/include/mach/mem_map.h
index c26d8486cc4b..9d6674ad1e5e 100644
--- a/arch/blackfin/mach-bf561/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf561/include/mach/mem_map.h
@@ -35,9 +35,16 @@
 /* Memory Map for ADSP-BF561 processors */
 
 #ifdef CONFIG_BF561
-#define L1_CODE_START     0xFFA00000
-#define L1_DATA_A_START     0xFF800000
-#define L1_DATA_B_START     0xFF900000
+#define COREA_L1_CODE_START     0xFFA00000
+#define COREA_L1_DATA_A_START     0xFF800000
+#define COREA_L1_DATA_B_START     0xFF900000
+#define COREB_L1_CODE_START     0xFF600000
+#define COREB_L1_DATA_A_START     0xFF500000
+#define COREB_L1_DATA_B_START     0xFF400000
+
+#define L1_CODE_START     COREA_L1_CODE_START
+#define L1_DATA_A_START     COREA_L1_DATA_A_START
+#define L1_DATA_B_START     COREA_L1_DATA_B_START
 
 #define L1_CODE_LENGTH      0x4000
 
@@ -72,7 +79,10 @@
 
 /* Scratch Pad Memory */
 
-#define L1_SCRATCH_START        0xFFB00000
+#define COREA_L1_SCRATCH_START  0xFFB00000
+#define COREB_L1_SCRATCH_START  0xFF700000
+
+#define L1_SCRATCH_START        COREA_L1_SCRATCH_START
 #define L1_SCRATCH_LENGTH       0x1000
 
 #endif                          /* _MEM_MAP_533_H_ */
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index 90c7397036ed..5a219b228de3 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -190,8 +190,8 @@ ENTRY(_ex_single_step)
         if cc jump .Lfind_priority_done;
         jump.s .Lfind_priority_start;
 .Lfind_priority_done:
-        p4.l = _debugger_step;
-        p4.h = _debugger_step;
+        p4.l = _kgdb_single_step;
+        p4.h = _kgdb_single_step;
         r6 = [p4];
         cc = r6 == 0;
         if cc jump .Ldo_single_step;
@@ -1071,7 +1071,12 @@ ENTRY(_ex_table)
          */
         .long _ex_syscall       /* 0x00 - User Defined - Linux Syscall */
         .long _ex_soft_bp       /* 0x01 - User Defined - Software breakpoint */
+#ifdef  CONFIG_KGDB
+        .long _ex_trap_c        /* 0x02 - User Defined - KGDB initial connection
+                                                         and break signal trap */
+#else
         .long _ex_replaceable   /* 0x02 - User Defined */
+#endif
         .long _ex_trap_c        /* 0x03 - User Defined - userspace stack overflow */
         .long _ex_trap_c        /* 0x04 - User Defined - dump trace buffer */
         .long _ex_replaceable   /* 0x05 - User Defined */
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
index 5fa536727c61..ff4d5c2879a5 100644
--- a/arch/blackfin/mach-common/ints-priority.c
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -1136,8 +1136,4 @@ void do_irq(int vec, struct pt_regs *fp)
                 vec = ivg->irqno;
         }
         asm_do_IRQ(vec, fp);
-
-#ifdef CONFIG_KGDB
-        kgdb_process_breakpoint();
-#endif
 }