[U-Boot] [PATCH] KGDB set / remove breakpoints

[Tonny Tzeng]

This patch extends the current KGDB logic to handle 'Z' and 'z'
GDB packets for setting or removing breakpoints.

Two weak functions have been added to the kgdb_stub.c:
arch_kgdb_set_sw_break() and arch_kgdb_remove_sw_break() could be
overrode by the arch implementations.

Please note, after applying this patch, those architectures, which
already enabled KGDB support, have to create a new asm/kgdb.h and
define the length of the break instruction (BREAK_INSTR_SIZE) in that
file.

Signed-off-by: Tonny Tzeng <tonny.tzeng at gmail.com>
---
 common/kgdb.c       |   85 +++++++++++++++++++++++++++++++++++++++++++++++++++
 common/kgdb_stubs.c |   12 +++++++
 include/kgdb.h      |   31 ++++++++++++++++++
 3 files changed, 128 insertions(+), 0 deletions(-)

diff --git a/common/kgdb.c b/common/kgdb.c
index 0531452..66378e5 100644
--- a/common/kgdb.c
+++ b/common/kgdb.c
@@ -220,6 +220,85 @@ hexToInt(char **ptr, int *intValue)
 	return (numChars);
 }
 
+/*
+ * Holds information about breakpoints in a kernel. These breakpoints are
+ * added and removed by gdb.
+ */
+static struct kgdb_bkpt	kgdb_break[KGDB_MAX_BREAKPOINTS];
+
+static int kgdb_set_sw_break(int addr)
+{
+	int i, breakno = -1;
+	struct kgdb_bkpt *bkpt;
+
+	for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+		if ((kgdb_break[i].state == BP_SET) &&
+		    (kgdb_break[i].bpt_addr == addr))
+			return -KGDBERR_BPEXIST;
+		if ((kgdb_break[i].state == BP_REMOVED) &&
+		    (kgdb_break[i].bpt_addr == addr)) {
+			breakno = i;
+			break;
+		}
+	}
+	if (breakno == -1)
+		for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+			if (kgdb_break[i].state == BP_UNDEFINED) {
+				breakno = i;
+				break;
+			}
+		}
+	if (breakno == -1)
+		return -KGDBERR_BPNOENT;
+
+	bkpt = kgdb_break + breakno;
+	bkpt->state = BP_SET;
+	bkpt->type = BP_BREAKPOINT;
+	bkpt->bpt_addr = addr;
+	arch_kgdb_set_sw_break(bkpt);
+
+	return 0;
+}
+
+static int kgdb_remove_sw_break(int addr)
+{
+	int i;
+	struct kgdb_bkpt *bkpt;
+
+	for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+		bkpt = kgdb_break + i;
+		if ((bkpt->state == BP_SET) && (bkpt->bpt_addr == addr)) {
+			bkpt->state = BP_REMOVED;
+			arch_kgdb_remove_sw_break(bkpt);
+			return 0;
+		}
+	}
+	return -KGDBERR_BPNOENT;
+}
+
+/* Handle the 'z' or 'Z' breakpoint remove or set packets */
+static void gdb_cmd_break(kgdb_data *kdp)
+{
+	/*
+	 * Since GDB-5.3, it's been drafted that '0' is a software
+	 * breakpoint, '1' is a hardware breakpoint, so let's do that.
+	 */
+	char *bpt_type = &remcomInBuffer[1];
+	char *ptr = &remcomInBuffer[2];
+	int addr, length;
+
+	if (*bpt_type != '0')
+		return;		/* Unsupported. */
+	if (*ptr++ != ',' || !hexToInt(&ptr, &addr) ||
+	    *ptr++ != ',' || !hexToInt(&ptr, &length)) {
+		kgdb_error(KGDBERR_BADPARAMS);
+	}
+
+	if ((remcomInBuffer[0] == 'Z' && kgdb_set_sw_break(addr) == 0) ||
+	    (remcomInBuffer[0] == 'z' && kgdb_remove_sw_break(addr) == 0))
+		strcpy(remcomOutBuffer, "OK");
+}
+
 /* scan for the sequence $<data>#<checksum>     */
 static void
 getpacket(char *buffer)
@@ -341,7 +420,9 @@ handle_exception (struct pt_regs *regs)
 
 	kgdb_interruptible(0);
 
+#ifdef KGDB_DEBUG
 	printf("kgdb: handle_exception; trap [0x%x]\n", kgdb_trap(regs));
+#endif
 
 	if (kgdb_setjmp(error_jmp_buf) != 0)
 		panic("kgdb: error or fault in entry init!\n");
@@ -516,6 +597,10 @@ handle_exception (struct pt_regs *regs)
 				kgdb_error(KGDBERR_BADPARAMS);
 			}
 			break;
+		case 'Z': /* [Z|z]N,AA..AA,LLLL Set/Remove breakpoint type N */
+		case 'z': /*      LLLL bytes at address AA.AA return OK      */
+			gdb_cmd_break(&kd);
+			break;
 		}			/* switch */
 
 		if (errnum != 0)
diff --git a/common/kgdb_stubs.c b/common/kgdb_stubs.c
index 19b0c18..2b3f424 100644
--- a/common/kgdb_stubs.c
+++ b/common/kgdb_stubs.c
@@ -45,6 +45,18 @@ void kgdb_interruptible(int yes)
 }
 
 __attribute__((weak))
+void arch_kgdb_set_sw_break(struct kgdb_bkpt *bkpt)
+{
+	return;
+}
+
+__attribute__((weak))
+void arch_kgdb_remove_sw_break(struct kgdb_bkpt *bkpt)
+{
+	return;
+}
+
+__attribute__((weak))
 void kgdb_flush_cache_range(void *from, void *to)
 {
 	flush_cache((unsigned long)from, (unsigned long)(to - from));
diff --git a/include/kgdb.h b/include/kgdb.h
index f543cd6..82ae8ab 100644
--- a/include/kgdb.h
+++ b/include/kgdb.h
@@ -2,12 +2,41 @@
 #define __KGDB_H__
 
 #include <asm/ptrace.h>
+#include <asm/kgdb.h>
 
 #define KGDBERR_BADPARAMS	1
 #define KGDBERR_NOTHEXDIG	2
 #define KGDBERR_MEMFAULT	3
 #define KGDBERR_NOSPACE		4
 #define KGDBERR_ALIGNFAULT	5
+#define KGDBERR_BPEXIST		6
+#define KGDBERR_BPNOENT		7
+
+#ifndef KGDB_MAX_BREAKPOINTS
+#define KGDB_MAX_BREAKPOINTS	1000
+#endif
+
+enum kgdb_bptype {
+	BP_BREAKPOINT = 0,
+	BP_HARDWARE_BREAKPOINT,
+	BP_WRITE_WATCHPOINT,
+	BP_READ_WATCHPOINT,
+	BP_ACCESS_WATCHPOINT
+};
+
+enum kgdb_bpstate {
+	BP_UNDEFINED = 0,
+	BP_REMOVED,
+	BP_SET,
+	BP_ACTIVE
+};
+
+struct kgdb_bkpt {
+	unsigned long		bpt_addr;
+	unsigned char		saved_instr[BREAK_INSTR_SIZE];
+	enum kgdb_bptype	type;
+	enum kgdb_bpstate	state;
+};
 
 #define KGDBDATA_MAXREGS	8
 #define KGDBDATA_MAXPRIV	8
@@ -56,6 +85,8 @@ extern void kgdb_putreg(struct pt_regs *, int, char *, int);
 extern void kgdb_putregs(struct pt_regs *, char *, int);
 extern int kgdb_trap(struct pt_regs *);
 extern void kgdb_breakpoint(int argc, char *argv[]);
+extern void arch_kgdb_set_sw_break(struct kgdb_bkpt *);
+extern void arch_kgdb_remove_sw_break(struct kgdb_bkpt *);
 
 /* these functions are provided by the platform serial driver */
 extern void kgdb_serial_init(void);
-- 
1.6.0.6