[GDB] Add watch and rwatch hardware watchpoints

3 files changed, 140 insertions, 1 deletions

diff --git a/src/arch/i386/core/gdbidt.S b/src/arch/i386/core/gdbidt.S
index 45d079f6..a4949232 100644
--- a/src/arch/i386/core/gdbidt.S
+++ b/src/arch/i386/core/gdbidt.S
@@ -184,7 +184,7 @@ do_interrupt:
 	/* Call GDB stub exception handler */
 	pushl	%esp
 	pushl	(IH_OFFSET_SIGNO + 4)(%esp)
-	call	gdbstub_handler
+	call	gdbmach_handler
 	addl	$8, %esp
 
 	/* Restore CPU state from GDB register snapshot */
diff --git a/src/arch/i386/core/gdbmach.c b/src/arch/i386/core/gdbmach.c
new file mode 100644
index 00000000..7cffc9e7
--- /dev/null
+++ b/src/arch/i386/core/gdbmach.c
@@ -0,0 +1,126 @@
+#include <stddef.h>
+#include <stdio.h>
+#include <assert.h>
+#include <virtaddr.h>
+#include <gpxe/gdbstub.h>
+#include <gdbmach.h>
+
+enum {
+	DR7_CLEAR = 0x00000400,    /* disable hardware breakpoints */
+	DR6_CLEAR = 0xffff0ff0,    /* clear breakpoint status */
+};
+
+/** Hardware breakpoint, fields stored in x86 bit pattern form */
+struct hwbp {
+	int type;           /* type (1=write watchpoint, 3=access watchpoint) */
+	unsigned long addr; /* linear address */
+	size_t len;         /* length (0=1-byte, 1=2-byte, 3=4-byte) */
+	int enabled;
+};
+
+static struct hwbp hwbps [ 4 ];
+static gdbreg_t dr7 = DR7_CLEAR;
+static gdbreg_t dr6;
+
+static struct hwbp *gdbmach_find_hwbp ( int type, unsigned long addr, size_t len ) {
+	struct hwbp *available = NULL;
+	unsigned int i;
+	for ( i = 0; i < sizeof hwbps / sizeof hwbps [ 0 ]; i++ ) {
+		if ( hwbps [ i ].type == type && hwbps [ i ].addr == addr && hwbps [ i ].len == len ) {
+			return &hwbps [ i ];
+		}
+		if ( !hwbps [ i ].enabled ) {
+			available = &hwbps [ i ];
+		}
+	}
+	return available;
+}
+
+static void gdbmach_commit_hwbp ( struct hwbp *bp ) {
+	int regnum = bp - hwbps;
+
+	/* Set breakpoint address */
+	assert ( regnum >= 0 && regnum < sizeof hwbps / sizeof hwbps [ 0 ] );
+	switch ( regnum ) {
+		case 0:
+			__asm__ __volatile__ ( "movl %0, %%dr0\n" : : "r" ( bp->addr ) );
+			break;
+		case 1:
+			__asm__ __volatile__ ( "movl %0, %%dr1\n" : : "r" ( bp->addr ) );
+			break;
+		case 2:
+			__asm__ __volatile__ ( "movl %0, %%dr2\n" : : "r" ( bp->addr ) );
+			break;
+		case 3:
+			__asm__ __volatile__ ( "movl %0, %%dr3\n" : : "r" ( bp->addr ) );
+			break;
+	}
+
+	/* Set type */
+	dr7 &= ~( 0x3 << ( 16 + 4 * regnum ) );
+	dr7 |= bp->type << ( 16 + 4 * regnum );
+
+	/* Set length */
+	dr7 &= ~( 0x3 << ( 18 + 4 * regnum ) );
+	dr7 |= bp->len << ( 18 + 4 * regnum );
+
+	/* Set/clear local enable bit */
+	dr7 &= ~( 0x3 << 2 * regnum );
+ 	dr7 |= bp->enabled << 2 * regnum;
+}
+
+int gdbmach_set_breakpoint ( int type, unsigned long addr, size_t len, int enable ) {
+	struct hwbp *bp;
+	
+	/* Check and convert breakpoint type to x86 type */
+	switch ( type ) {
+		case GDBMACH_WATCH:
+			type = 0x1;
+			break;
+		case GDBMACH_AWATCH:
+			type = 0x3;
+			break;
+		default:
+			return 0; /* unsupported breakpoint type */
+	}
+
+	/* Only lengths 1, 2, and 4 are supported */
+	if ( len != 2 && len != 4 ) {
+		len = 1;
+	}
+	len--; /* convert to x86 breakpoint length bit pattern */
+
+	/* Calculate linear address by adding segment base */
+	addr += virt_offset;
+
+	/* Set up the breakpoint */
+	bp = gdbmach_find_hwbp ( type, addr, len );
+	if ( !bp ) {
+		return 0; /* ran out of hardware breakpoints */
+	}
+	bp->type = type;
+	bp->addr = addr;
+	bp->len = len;
+	bp->enabled = enable;
+	gdbmach_commit_hwbp ( bp );
+	return 1;
+}
+
+static void gdbmach_disable_hwbps ( void ) {
+	/* Store and clear breakpoint status register */
+	__asm__ __volatile__ ( "movl %%dr6, %0\n" "movl %1, %%dr6\n" : "=r" ( dr6 ) : "r" ( DR6_CLEAR ) );
+
+	/* Store and clear hardware breakpoints */
+	__asm__ __volatile__ ( "movl %0, %%dr7\n" : : "r" ( DR7_CLEAR ) );
+}
+
+static void gdbmach_enable_hwbps ( void ) {
+	/* Restore hardware breakpoints */
+	__asm__ __volatile__ ( "movl %0, %%dr7\n" : : "r" ( dr7 ) );
+}
+
+__cdecl void gdbmach_handler ( int signo, gdbreg_t *regs ) {
+	gdbmach_disable_hwbps();
+	gdbstub_handler ( signo, regs );
+	gdbmach_enable_hwbps();
+}
diff --git a/src/arch/i386/include/gdbmach.h b/src/arch/i386/include/gdbmach.h
index 9f6dc8f0..1a38ccd1 100644
--- a/src/arch/i386/include/gdbmach.h
+++ b/src/arch/i386/include/gdbmach.h
@@ -10,6 +10,8 @@
  *
  */
 
+#include <stdint.h>
+
 typedef uint32_t gdbreg_t;
 
 /* The register snapshot, this must be in sync with interrupt handler and the
@@ -35,6 +37,15 @@ enum {
 	GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof ( gdbreg_t )
 };
 
+/* Breakpoint types */
+enum {
+	GDBMACH_BPMEM,
+	GDBMACH_BPHW,
+	GDBMACH_WATCH,
+	GDBMACH_RWATCH,
+	GDBMACH_AWATCH,
+};
+
 static inline void gdbmach_set_pc ( gdbreg_t *regs, gdbreg_t pc ) {
 	regs [ GDBMACH_EIP ] = pc;
 }
@@ -48,4 +59,6 @@ static inline void gdbmach_breakpoint ( void ) {
 	__asm__ __volatile__ ( "int $3\n" );
 }
 
+extern int gdbmach_set_breakpoint ( int type, unsigned long addr, size_t len, int enable );
+
 #endif /* GDBMACH_H */