Blackfin: add support for kgdb

Signed-off-by: Robin Getz <robin.getz@analog.com>
Signed-off-by: Mike Frysinger <vapier@gentoo.org>

6 files changed, 783 insertions, 7 deletions

diff --git a/lib_blackfin/Makefile b/lib_blackfin/Makefile
index eebb13119..3bdba754d 100644
--- a/lib_blackfin/Makefile
+++ b/lib_blackfin/Makefile
@@ -37,12 +37,14 @@ SOBJS-y	+= memcpy.o
 SOBJS-y	+= memmove.o
 SOBJS-y	+= memset.o
 SOBJS-y	+= outs.o
+SOBJS-$(CONFIG_CMD_KGDB) += __kgdb.o
 
 COBJS-y	+= board.o
 COBJS-y	+= boot.o
 COBJS-y	+= cache.o
 COBJS-y	+= clocks.o
 COBJS-$(CONFIG_CMD_CACHE_DUMP) += cmd_cache_dump.o
+COBJS-$(CONFIG_CMD_KGDB) += kgdb.o
 COBJS-y	+= muldi3.o
 COBJS-$(CONFIG_POST) += post.o tests.o
 COBJS-y	+= string.o
diff --git a/lib_blackfin/__kgdb.S b/lib_blackfin/__kgdb.S
new file mode 100644
index 000000000..cba4179d3
--- /dev/null
+++ b/lib_blackfin/__kgdb.S
@@ -0,0 +1,155 @@
+
+#include <asm/linkage.h>
+
+/* save stack context for non-local goto
+ * int kgdb_setjmp(long *buf)
+ */
+
+ENTRY(_kgdb_setjmp)
+	[--SP] = p0; 	/* Save P0 */
+	p0 = r0;
+	r0 = [SP++];	/* Load P0 into R0 */
+
+	[p0 + 0x00] = r0;       /* GP address registers */
+	[p0 + 0x04] = p1;
+	[p0 + 0x08] = p2;
+	[p0 + 0x0C] = p3;
+	[p0 + 0x10] = p4;
+	[p0 + 0x14] = p5;
+	[p0 + 0x18] = FP;       /* frame pointer */
+	[p0 + 0x1C] = SP;       /* stack pointer */
+
+	[p0 + 0x20] = p0;       /* data regs */
+	[p0 + 0x24] = r1;
+	[p0 + 0x28] = r2;
+	[p0 + 0x2C] = r3;
+	[p0 + 0x30] = r4;
+	[p0 + 0x34] = r5;
+	[p0 + 0x38] = r6;
+	[p0 + 0x3C] = r7;
+
+	r0 = ASTAT;	[p0 + 0x40] = r0;
+
+	/* loop counters */
+	r0 = LC0;	[p0 + 0x44] = r0;
+	r0 = LC1;	[p0 + 0x48] = r0;
+
+	/* Accumulator */
+	r0 = A0.w;	[p0 + 0x4C] = r0;
+	r0.l = A0.x;	[p0 + 0x50] = r0;
+	r0 = A1.w;	[p0 + 0x54] = r0;
+	r0.l = A1.x;	[p0 + 0x58] = r0;
+
+	/* index registers */
+	r0 = i0;	[p0 + 0x5C] = r0;
+	r0 = i1;	[p0 + 0x60] = r0;
+	r0 = i2;	[p0 + 0x64] = r0;
+	r0 = i3;	[p0 + 0x68] = r0;
+
+	/* modifier registers */
+	r0 = m0;	[p0 + 0x6C] = r0;
+	r0 = m1;	[p0 + 0x70] = r0;
+	r0 = m2;	[p0 + 0x74] = r0;
+	r0 = m3;	[p0 + 0x78] = r0;
+
+	/* length registers */
+	r0 = l0;	[p0 + 0x7C] = r0;
+	r0 = l1;	[p0 + 0x80] = r0;
+	r0 = l2;	[p0 + 0x84] = r0;
+	r0 = l3;	[p0 + 0x88] = r0;
+
+	/* base registers */
+	r0 = b0;	[p0 + 0x8C] = r0;
+	r0 = b1;	[p0 + 0x90] = r0;
+	r0 = b2;	[p0 + 0x94] = r0;
+	r0 = b3;	[p0 + 0x98] = r0;
+
+	/* store return address */
+	r0 = RETS;	[p0 + 0x9C] = r0;
+
+	R0 = 0;
+	RTS;
+ENDPROC(_kgdb_setjmp)
+
+/*
+ * non-local jump to a saved stack context
+ * longjmp(long *buf, int val)
+ */
+
+ENTRY(_kgdb_longjmp)
+	p0 = r0;
+	r0 = [p0 + 0x00];
+	[--sp] = r0;
+
+	/* GP address registers - skip p0 for now*/
+	p1 = [p0 + 0x04];
+	p2 = [p0 + 0x08];
+	p3 = [p0 + 0x0C];
+	p4 = [p0 + 0x10];
+	p5 = [p0 + 0x14];
+	/* frame pointer */
+	fp = [p0 + 0x18];
+	/* stack pointer */
+	r0 = [sp++];
+	sp = [p0 + 0x1C];
+	[--sp] = r0;
+	[--sp] = r1;
+
+	/* data regs */
+	r0 = [p0 + 0x20];
+	r1 = [p0 + 0x24];
+	r2 = [p0 + 0x28];
+	r3 = [p0 + 0x2C];
+	r4 = [p0 + 0x30];
+	r5 = [p0 + 0x34];
+	r6 = [p0 + 0x38];
+	r7 = [p0 + 0x3C];
+
+	r0 = [p0 + 0x40];	ASTAT = r0;
+
+	/* loop counters */
+	r0 = [p0 + 0x44];	LC0 = r0;
+	r0 = [p0 + 0x48];	LC1 = r0;
+
+	/* Accumulator */
+	r0 = [p0 + 0x4C];	A0.w = r0;
+	r0 = [p0 + 0x50];	A0.x = r0;
+	r0 = [p0 + 0x54];	A1.w = r0;
+	r0 = [p0 + 0x58];	A1.x = r0;
+
+	/* index registers */
+	r0 = [p0 + 0x5C];	i0 = r0;
+	r0 = [p0 + 0x60];	i1 = r0;
+	r0 = [p0 + 0x64];	i2 = r0;
+	r0 = [p0 + 0x68];	i3 = r0;
+
+	/* modifier registers */
+	r0 = [p0 + 0x6C];	m0 = r0;
+	r0 = [p0 + 0x70];	m1 = r0;
+	r0 = [p0 + 0x74];	m2 = r0;
+	r0 = [p0 + 0x78];	m3 = r0;
+
+	/* length registers */
+	r0 = [p0 + 0x7C];	l0 = r0;
+	r0 = [p0 + 0x80];	l1 = r0;
+	r0 = [p0 + 0x84];	l2 = r0;
+	r0 = [p0 + 0x88];	l3 = r0;
+
+	/* base registers */
+	r0 = [p0 + 0x8C];	b0 = r0;
+	r0 = [p0 + 0x90];	b1 = r0;
+	r0 = [p0 + 0x94];	b2 = r0;
+	r0 = [p0 + 0x98];	b3 = r0;
+
+	/* store return address */
+	r0 = [p0 + 0x9C];	RETS = r0;
+
+	/* fixup R0 & P0 */
+	r0 = [sp++];
+	p0 = [sp++];
+	CC = R0 == 0;
+	IF !CC JUMP .Lfinished;
+	R0 = 1;
+.Lfinished:
+	RTS;
+ENDPROC(_kgdb_longjmp)
diff --git a/lib_blackfin/board.c b/lib_blackfin/board.c
index 90da2b435..21fff334a 100644
--- a/lib_blackfin/board.c
+++ b/lib_blackfin/board.c
@@ -22,6 +22,7 @@
 
 #include <asm/cplb.h>
 #include <asm/mach-common/bits/mpu.h>
+#include <kgdb.h>
 
 #ifdef CONFIG_CMD_NAND
 #include <nand.h>	/* cannot even include nand.h if it isnt configured */
@@ -356,6 +357,11 @@ void board_init_r(gd_t * id, ulong dest_addr)
 	/* Initialize the console (after the relocation and devices init) */
 	console_init_r();
 
+#ifdef CONFIG_CMD_KGDB
+	puts("KGDB:  ");
+	kgdb_init();
+#endif
+
 #ifdef CONFIG_STATUS_LED
 	status_led_set(STATUS_LED_BOOT, STATUS_LED_BLINKING);
 	status_led_set(STATUS_LED_CRASH, STATUS_LED_OFF);
diff --git a/lib_blackfin/kgdb.c b/lib_blackfin/kgdb.c
new file mode 100644
index 000000000..bd62d7105
--- /dev/null
+++ b/lib_blackfin/kgdb.c
@@ -0,0 +1,423 @@
+/*
+ * U-boot - architecture specific kgdb code
+ *
+ * Copyright 2009 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <common.h>
+#include <command.h>
+
+#include <kgdb.h>
+#include <asm/processor.h>
+#include <asm/mach-common/bits/core.h>
+#include "kgdb.h"
+#include <asm/deferred.h>
+#include <asm/traps.h>
+#include <asm/signal.h>
+
+void kgdb_enter(struct pt_regs *regs, kgdb_data *kdp)
+{
+	/* disable interrupts */
+	disable_interrupts();
+
+	/* reply to host that an exception has occurred */
+	kdp->sigval = kgdb_trap(regs);
+
+	/* send the PC and the Stack Pointer */
+	kdp->nregs = 2;
+	kdp->regs[0].num = BFIN_PC;
+	kdp->regs[0].val = regs->pc;
+
+	kdp->regs[1].num = BFIN_SP;
+	kdp->regs[1].val = (unsigned long)regs;
+
+}
+
+void kgdb_exit(struct pt_regs *regs, kgdb_data *kdp)
+{
+	if (kdp->extype & KGDBEXIT_WITHADDR)
+		printf("KGDBEXIT_WITHADDR\n");
+
+	switch (kdp->extype & KGDBEXIT_TYPEMASK) {
+	case KGDBEXIT_KILL:
+		printf("KGDBEXIT_KILL:\n");
+		break;
+	case KGDBEXIT_CONTINUE:
+		/* Make sure the supervisor single step bit is clear */
+		regs->syscfg &= ~1;
+		break;
+	case KGDBEXIT_SINGLE:
+		/* set the supervisor single step bit */
+		regs->syscfg |= 1;
+		break;
+	default:
+		printf("KGDBEXIT : %d\n", kdp->extype);
+	}
+
+	/* enable interrupts */
+	enable_interrupts();
+}
+
+int kgdb_trap(struct pt_regs *regs)
+{
+	/* ipend doesn't get filled in properly */
+	switch (regs->seqstat & EXCAUSE) {
+	case VEC_EXCPT01:
+		return SIGTRAP;
+	case VEC_EXCPT03:
+		return SIGSEGV;
+	case VEC_EXCPT02:
+		return SIGTRAP;
+	case VEC_EXCPT04 ... VEC_EXCPT15:
+		return SIGILL;
+	case VEC_STEP:
+		return SIGTRAP;
+	case VEC_OVFLOW:
+		return SIGTRAP;
+	case VEC_UNDEF_I:
+		return SIGILL;
+	case VEC_ILGAL_I:
+		return SIGILL;
+	case VEC_CPLB_VL:
+		return SIGSEGV;
+	case VEC_MISALI_D:
+		return SIGBUS;
+	case VEC_UNCOV:
+		return SIGILL;
+	case VEC_CPLB_MHIT:
+		return SIGSEGV;
+	case VEC_MISALI_I:
+		return SIGBUS;
+	case VEC_CPLB_I_VL:
+		return SIGBUS;
+	case VEC_CPLB_I_MHIT:
+		return SIGSEGV;
+	default:
+		return SIGBUS;
+	}
+}
+
+/*
+ * getregs - gets the pt_regs, and gives them to kgdb's buffer
+ */
+int kgdb_getregs(struct pt_regs *regs, char *buf, int max)
+{
+	unsigned long *gdb_regs = (unsigned long *)buf;
+
+	if (max < NUMREGBYTES)
+		kgdb_error(KGDBERR_NOSPACE);
+
+	if ((unsigned long)gdb_regs & 3)
+		kgdb_error(KGDBERR_ALIGNFAULT);
+
+	gdb_regs[BFIN_R0] = regs->r0;
+	gdb_regs[BFIN_R1] = regs->r1;
+	gdb_regs[BFIN_R2] = regs->r2;
+	gdb_regs[BFIN_R3] = regs->r3;
+	gdb_regs[BFIN_R4] = regs->r4;
+	gdb_regs[BFIN_R5] = regs->r5;
+	gdb_regs[BFIN_R6] = regs->r6;
+	gdb_regs[BFIN_R7] = regs->r7;
+	gdb_regs[BFIN_P0] = regs->p0;
+	gdb_regs[BFIN_P1] = regs->p1;
+	gdb_regs[BFIN_P2] = regs->p2;
+	gdb_regs[BFIN_P3] = regs->p3;
+	gdb_regs[BFIN_P4] = regs->p4;
+	gdb_regs[BFIN_P5] = regs->p5;
+	gdb_regs[BFIN_SP] = (unsigned long)regs;
+	gdb_regs[BFIN_FP] = regs->fp;
+	gdb_regs[BFIN_I0] = regs->i0;
+	gdb_regs[BFIN_I1] = regs->i1;
+	gdb_regs[BFIN_I2] = regs->i2;
+	gdb_regs[BFIN_I3] = regs->i3;
+	gdb_regs[BFIN_M0] = regs->m0;
+	gdb_regs[BFIN_M1] = regs->m1;
+	gdb_regs[BFIN_M2] = regs->m2;
+	gdb_regs[BFIN_M3] = regs->m3;
+	gdb_regs[BFIN_B0] = regs->b0;
+	gdb_regs[BFIN_B1] = regs->b1;
+	gdb_regs[BFIN_B2] = regs->b2;
+	gdb_regs[BFIN_B3] = regs->b3;
+	gdb_regs[BFIN_L0] = regs->l0;
+	gdb_regs[BFIN_L1] = regs->l1;
+	gdb_regs[BFIN_L2] = regs->l2;
+	gdb_regs[BFIN_L3] = regs->l3;
+	gdb_regs[BFIN_A0_DOT_X] = regs->a0x;
+	gdb_regs[BFIN_A0_DOT_W] = regs->a0w;
+	gdb_regs[BFIN_A1_DOT_X] = regs->a1x;
+	gdb_regs[BFIN_A1_DOT_W] = regs->a1w;
+	gdb_regs[BFIN_ASTAT] = regs->astat;
+	gdb_regs[BFIN_RETS] = regs->rets;
+	gdb_regs[BFIN_LC0] = regs->lc0;
+	gdb_regs[BFIN_LT0] = regs->lt0;
+	gdb_regs[BFIN_LB0] = regs->lb0;
+	gdb_regs[BFIN_LC1] = regs->lc1;
+	gdb_regs[BFIN_LT1] = regs->lt1;
+	gdb_regs[BFIN_LB1] = regs->lb1;
+	gdb_regs[BFIN_CYCLES] = 0;
+	gdb_regs[BFIN_CYCLES2] = 0;
+	gdb_regs[BFIN_USP] = regs->usp;
+	gdb_regs[BFIN_SEQSTAT] = regs->seqstat;
+	gdb_regs[BFIN_SYSCFG] = regs->syscfg;
+	gdb_regs[BFIN_RETI] = regs->pc;
+	gdb_regs[BFIN_RETX] = regs->retx;
+	gdb_regs[BFIN_RETN] = regs->retn;
+	gdb_regs[BFIN_RETE] = regs->rete;
+	gdb_regs[BFIN_PC] = regs->pc;
+	gdb_regs[BFIN_CC] = 0;
+	gdb_regs[BFIN_EXTRA1] = 0;
+	gdb_regs[BFIN_EXTRA2] = 0;
+	gdb_regs[BFIN_EXTRA3] = 0;
+	gdb_regs[BFIN_IPEND] = regs->ipend;
+
+	return NUMREGBYTES;
+}
+
+/*
+ * putreg - put kgdb's reg (regno) into the pt_regs
+ */
+void kgdb_putreg(struct pt_regs *regs, int regno, char *buf, int length)
+{
+	unsigned long *ptr = (unsigned long *)buf;
+
+	if (regno < 0 || regno > BFIN_NUM_REGS)
+		kgdb_error(KGDBERR_BADPARAMS);
+
+	if (length < 4)
+		kgdb_error(KGDBERR_NOSPACE);
+
+	if ((unsigned long)ptr & 3)
+		kgdb_error(KGDBERR_ALIGNFAULT);
+
+	switch (regno) {
+	case BFIN_R0:
+		regs->r0 = *ptr;
+		break;
+	case BFIN_R1:
+		regs->r1 = *ptr;
+		break;
+	case BFIN_R2:
+		regs->r2 = *ptr;
+		break;
+	case BFIN_R3:
+		regs->r3 = *ptr;
+		break;
+	case BFIN_R4:
+		regs->r4 = *ptr;
+		break;
+	case BFIN_R5:
+		regs->r5 = *ptr;
+		break;
+	case BFIN_R6:
+		regs->r6 = *ptr;
+		break;
+	case BFIN_R7:
+		regs->r7 = *ptr;
+		break;
+	case BFIN_P0:
+		regs->p0 = *ptr;
+		break;
+	case BFIN_P1:
+		regs->p1 = *ptr;
+		break;
+	case BFIN_P2:
+		regs->p2 = *ptr;
+		break;
+	case BFIN_P3:
+		regs->p3 = *ptr;
+		break;
+	case BFIN_P4:
+		regs->p4 = *ptr;
+		break;
+	case BFIN_P5:
+		regs->p5 = *ptr;
+		break;
+	case BFIN_SP:
+		regs->reserved = *ptr;
+		break;
+	case BFIN_FP:
+		regs->fp = *ptr;
+		break;
+	case BFIN_I0:
+		regs->i0 = *ptr;
+		break;
+	case BFIN_I1:
+		regs->i1 = *ptr;
+		break;
+	case BFIN_I2:
+		regs->i2 = *ptr;
+		break;
+	case BFIN_I3:
+		regs->i3 = *ptr;
+		break;
+	case BFIN_M0:
+		regs->m0 = *ptr;
+		break;
+	case BFIN_M1:
+		regs->m1 = *ptr;
+		break;
+	case BFIN_M2:
+		regs->m2 = *ptr;
+		break;
+	case BFIN_M3:
+		regs->m3 = *ptr;
+		break;
+	case BFIN_B0:
+		regs->b0 = *ptr;
+		break;
+	case BFIN_B1:
+		regs->b1 = *ptr;
+		break;
+	case BFIN_B2:
+		regs->b2 = *ptr;
+		break;
+	case BFIN_B3:
+		regs->b3 = *ptr;
+		break;
+	case BFIN_L0:
+		regs->l0 = *ptr;
+		break;
+	case BFIN_L1:
+		regs->l1 = *ptr;
+		break;
+	case BFIN_L2:
+		regs->l2 = *ptr;
+		break;
+	case BFIN_L3:
+		regs->l3 = *ptr;
+		break;
+	case BFIN_A0_DOT_X:
+		regs->a0x = *ptr;
+		break;
+	case BFIN_A0_DOT_W:
+		regs->a0w = *ptr;
+		break;
+	case BFIN_A1_DOT_X:
+		regs->a1x = *ptr;
+		break;
+	case BFIN_A1_DOT_W:
+		regs->a1w = *ptr;
+		break;
+	case BFIN_ASTAT:
+		regs->astat = *ptr;
+		break;
+	case BFIN_RETS:
+		regs->rets = *ptr;
+		break;
+	case BFIN_LC0:
+		regs->lc0 = *ptr;
+		break;
+	case BFIN_LT0:
+		regs->lt0 = *ptr;
+		break;
+	case BFIN_LB0:
+		regs->lb0 = *ptr;
+		break;
+	case BFIN_LC1:
+		regs->lc1 = *ptr;
+		break;
+	case BFIN_LT1:
+		regs->lt1 = *ptr;
+		break;
+	case BFIN_LB1:
+		regs->lb1 = *ptr;
+		break;
+/*
+  BFIN_CYCLES,
+  BFIN_CYCLES2,
+  BFIN_USP,
+  BFIN_SEQSTAT,
+  BFIN_SYSCFG,
+*/
+	case BFIN_RETX:
+		regs->retx = *ptr;
+		break;
+	case BFIN_RETN:
+		regs->retn = *ptr;
+		break;
+	case BFIN_RETE:
+		regs->rete = *ptr;
+		break;
+	case BFIN_PC:
+		regs->pc = *ptr;
+		break;
+
+	default:
+		kgdb_error(KGDBERR_BADPARAMS);
+	}
+}
+
+void kgdb_putregs(struct pt_regs *regs, char *buf, int length)
+{
+	unsigned long *gdb_regs = (unsigned long *)buf;
+
+	if (length != BFIN_NUM_REGS)
+		kgdb_error(KGDBERR_NOSPACE);
+
+	if ((unsigned long)gdb_regs & 3)
+		kgdb_error(KGDBERR_ALIGNFAULT);
+
+	regs->r0 = gdb_regs[BFIN_R0];
+	regs->r1 = gdb_regs[BFIN_R1];
+	regs->r2 = gdb_regs[BFIN_R2];
+	regs->r3 = gdb_regs[BFIN_R3];
+	regs->r4 = gdb_regs[BFIN_R4];
+	regs->r5 = gdb_regs[BFIN_R5];
+	regs->r6 = gdb_regs[BFIN_R6];
+	regs->r7 = gdb_regs[BFIN_R7];
+	regs->p0 = gdb_regs[BFIN_P0];
+	regs->p1 = gdb_regs[BFIN_P1];
+	regs->p2 = gdb_regs[BFIN_P2];
+	regs->p3 = gdb_regs[BFIN_P3];
+	regs->p4 = gdb_regs[BFIN_P4];
+	regs->p5 = gdb_regs[BFIN_P5];
+	regs->fp = gdb_regs[BFIN_FP];
+/*	regs->sp = gdb_regs[BFIN_ ]; */
+	regs->i0 = gdb_regs[BFIN_I0];
+	regs->i1 = gdb_regs[BFIN_I1];
+	regs->i2 = gdb_regs[BFIN_I2];
+	regs->i3 = gdb_regs[BFIN_I3];
+	regs->m0 = gdb_regs[BFIN_M0];
+	regs->m1 = gdb_regs[BFIN_M1];
+	regs->m2 = gdb_regs[BFIN_M2];
+	regs->m3 = gdb_regs[BFIN_M3];
+	regs->b0 = gdb_regs[BFIN_B0];
+	regs->b1 = gdb_regs[BFIN_B1];
+	regs->b2 = gdb_regs[BFIN_B2];
+	regs->b3 = gdb_regs[BFIN_B3];
+	regs->l0 = gdb_regs[BFIN_L0];
+	regs->l1 = gdb_regs[BFIN_L1];
+	regs->l2 = gdb_regs[BFIN_L2];
+	regs->l3 = gdb_regs[BFIN_L3];
+	regs->a0x = gdb_regs[BFIN_A0_DOT_X];
+	regs->a0w = gdb_regs[BFIN_A0_DOT_W];
+	regs->a1x = gdb_regs[BFIN_A1_DOT_X];
+	regs->a1w = gdb_regs[BFIN_A1_DOT_W];
+	regs->rets = gdb_regs[BFIN_RETS];
+	regs->lc0 = gdb_regs[BFIN_LC0];
+	regs->lt0 = gdb_regs[BFIN_LT0];
+	regs->lb0 = gdb_regs[BFIN_LB0];
+	regs->lc1 = gdb_regs[BFIN_LC1];
+	regs->lt1 = gdb_regs[BFIN_LT1];
+	regs->lb1 = gdb_regs[BFIN_LB1];
+	regs->usp = gdb_regs[BFIN_USP];
+	regs->syscfg = gdb_regs[BFIN_SYSCFG];
+	regs->retx = gdb_regs[BFIN_PC];
+	regs->retn = gdb_regs[BFIN_RETN];
+	regs->rete = gdb_regs[BFIN_RETE];
+	regs->pc = gdb_regs[BFIN_PC];
+
+#if 0	/* can't change these */
+	regs->astat = gdb_regs[BFIN_ASTAT];
+	regs->seqstat = gdb_regs[BFIN_SEQSTAT];
+	regs->ipend = gdb_regs[BFIN_IPEND];
+#endif
+
+}
+
+void kgdb_breakpoint(int argc, char *argv[])
+{
+	asm volatile ("excpt 0x1\n");
+}
diff --git a/lib_blackfin/kgdb.h b/lib_blackfin/kgdb.h
new file mode 100644
index 000000000..18f1f493c
--- /dev/null
+++ b/lib_blackfin/kgdb.h
@@ -0,0 +1,160 @@
+/* Blackfin KGDB header
+ *
+ * Copyright 2005-2009 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef __ASM_BLACKFIN_KGDB_H__
+#define __ASM_BLACKFIN_KGDB_H__
+
+/* gdb locks */
+#define KGDB_MAX_NO_CPUS 8
+
+/*
+ * BUFMAX defines the maximum number of characters in inbound/outbound buffers.
+ * At least NUMREGBYTES*2 are needed for register packets.
+ * Longer buffer is needed to list all threads.
+ */
+#define BUFMAX 2048
+
+enum regnames {
+  /* Core Registers */
+  BFIN_R0 = 0,
+  BFIN_R1,
+  BFIN_R2,
+  BFIN_R3,
+  BFIN_R4,
+  BFIN_R5,
+  BFIN_R6,
+  BFIN_R7,
+  BFIN_P0,
+  BFIN_P1,
+  BFIN_P2,
+  BFIN_P3,
+  BFIN_P4,
+  BFIN_P5,
+  BFIN_SP,
+  BFIN_FP,
+  BFIN_I0,
+  BFIN_I1,
+  BFIN_I2,
+  BFIN_I3,
+  BFIN_M0,
+  BFIN_M1,
+  BFIN_M2,
+  BFIN_M3,
+  BFIN_B0,
+  BFIN_B1,
+  BFIN_B2,
+  BFIN_B3,
+  BFIN_L0,
+  BFIN_L1,
+  BFIN_L2,
+  BFIN_L3,
+  BFIN_A0_DOT_X,
+  BFIN_A0_DOT_W,
+  BFIN_A1_DOT_X,
+  BFIN_A1_DOT_W,
+  BFIN_ASTAT,
+  BFIN_RETS,
+  BFIN_LC0,
+  BFIN_LT0,
+  BFIN_LB0,
+  BFIN_LC1,
+  BFIN_LT1,
+  BFIN_LB1,
+  BFIN_CYCLES,
+  BFIN_CYCLES2,
+  BFIN_USP,
+  BFIN_SEQSTAT,
+  BFIN_SYSCFG,
+  BFIN_RETI,
+  BFIN_RETX,
+  BFIN_RETN,
+  BFIN_RETE,
+
+  /* Pseudo Registers */
+  BFIN_PC,
+  BFIN_CC,
+  BFIN_EXTRA1,		/* Address of .text section.  */
+  BFIN_EXTRA2,		/* Address of .data section.  */
+  BFIN_EXTRA3,		/* Address of .bss section.  */
+  BFIN_FDPIC_EXEC,
+  BFIN_FDPIC_INTERP,
+
+  /* MMRs */
+  BFIN_IPEND,
+
+  /* LAST ENTRY SHOULD NOT BE CHANGED.  */
+  BFIN_NUM_REGS		/* The number of all registers.  */
+};
+
+/* Number of bytes of registers.  */
+#define NUMREGBYTES (BFIN_NUM_REGS * 4)
+
+static inline void arch_kgdb_breakpoint(void)
+{
+	asm volatile ("EXCPT 2;");
+}
+#define BREAK_INSTR_SIZE	2
+#define CACHE_FLUSH_IS_SAFE	1
+#define GDB_ADJUSTS_BREAK_OFFSET
+#define GDB_SKIP_HW_WATCH_TEST
+#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
+#define WPIREN01	0x2
+#define WPIRINV01	0x4
+#define WPIAEN0		0x8
+#define WPIAEN1		0x10
+#define WPICNTEN0	0x20
+#define WPICNTEN1	0x40
+#define EMUSW0		0x80
+#define EMUSW1		0x100
+#define WPIREN23	0x200
+#define WPIRINV23	0x400
+#define WPIAEN2		0x800
+#define WPIAEN3		0x1000
+#define WPICNTEN2	0x2000
+#define WPICNTEN3	0x4000
+#define EMUSW2		0x8000
+#define EMUSW3		0x10000
+#define WPIREN45	0x20000
+#define WPIRINV45	0x40000
+#define WPIAEN4		0x80000
+#define WPIAEN5		0x100000
+#define WPICNTEN4	0x200000
+#define WPICNTEN5	0x400000
+#define EMUSW4		0x800000
+#define EMUSW5		0x1000000
+#define WPAND		0x2000000
+
+/* Data watchpoint address control register bits mask */
+#define WPDREN01	0x1
+#define WPDRINV01	0x2
+#define WPDAEN0		0x4
+#define WPDAEN1		0x8
+#define WPDCNTEN0	0x10
+#define WPDCNTEN1	0x20
+
+#define WPDSRC0		0xc0
+#define WPDACC0_OFFSET	8
+#define WPDSRC1		0xc00
+#define WPDACC1_OFFSET	12
+
+/* Watchpoint status register bits mask */
+#define STATIA0		0x1
+#define STATIA1		0x2
+#define STATIA2		0x4
+#define STATIA3		0x8
+#define STATIA4		0x10
+#define STATIA5		0x20
+#define STATDA0		0x40
+#define STATDA1		0x80
+
+#endif
diff --git a/lib_blackfin/string.c b/lib_blackfin/string.c
index 12b6d2405..e344d3b94 100644
--- a/lib_blackfin/string.c
+++ b/lib_blackfin/string.c
@@ -230,15 +230,45 @@ void *memcpy(void *dst, const void *src, size_t count)
 	if (!count)
 		return dst;
 
-	if (addr_bfin_on_chip_mem(dst)) {
-		/* L1 is the destination */
-		return dma_memcpy(dst, src, count);
+#ifdef CONFIG_CMD_KGDB
+	if (src >= (void *)SYSMMR_BASE) {
+		if (count == 2 && (unsigned long)src % 2 == 0) {
+			u16 mmr = bfin_read16(src);
+			memcpy(dst, &mmr, sizeof(mmr));
+			return dst;
+		}
+		if (count == 4 && (unsigned long)src % 4 == 0) {
+			u32 mmr = bfin_read32(src);
+			memcpy(dst, &mmr, sizeof(mmr));
+			return dst;
+		}
+		/* Failed for some reason */
+		memset(dst, 0xad, count);
+		return dst;
+	}
+	if (dst >= (void *)SYSMMR_BASE) {
+		if (count == 2 && (unsigned long)dst % 2 == 0) {
+			u16 mmr;
+			memcpy(&mmr, src, sizeof(mmr));
+			bfin_write16(dst, mmr);
+			return dst;
+		}
+		if (count == 4 && (unsigned long)dst % 4 == 0) {
+			u32 mmr;
+			memcpy(&mmr, src, sizeof(mmr));
+			bfin_write32(dst, mmr);
+			return dst;
+		}
+		/* Failed for some reason */
+		memset(dst, 0xad, count);
+		return dst;
+	}
+#endif
 
-	} else if (addr_bfin_on_chip_mem(src)) {
-		/* L1 is the source */
+	/* if L1 is the source or dst, use DMA */
+	if (addr_bfin_on_chip_mem(dst) || addr_bfin_on_chip_mem(src))
 		return dma_memcpy(dst, src, count);
-
-	} else
+	else
 		/* No L1 is involved, so just call regular memcpy */
 		return memcpy_ASM(dst, src, count);
 }