[GDB] Add watch and rwatch hardware watchpoints

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 */

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();
+}

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 */

src/core/gdbstub.c

@@ -249,6 +249,22 @@ static void gdbstub_continue ( struct gdbstub *stub, int single_step ) {
 	/* Reply will be sent when we hit the next breakpoint or interrupt */
 }
 
+static void gdbstub_breakpoint ( struct gdbstub *stub ) {
+	unsigned long args [ 3 ];
+	int enable = stub->payload [ 0 ] == 'Z' ? 1 : 0;
+	if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	if ( gdbmach_set_breakpoint ( args [ 0 ], args [ 1 ], args [ 2 ], enable ) ) {
+		gdbstub_send_ok ( stub );
+	} else {
+		/* Not supported */
+		stub->len = 0;
+		gdbstub_tx_packet ( stub );
+	}
+}
+
 static void gdbstub_rx_packet ( struct gdbstub *stub ) {
 	switch ( stub->payload [ 0 ] ) {
 		case '?':
@@ -275,6 +291,10 @@ static void gdbstub_rx_packet ( struct gdbstub *stub ) {
 				gdbstub_send_ok ( stub );
 			}
 			break;
+		case 'Z': /* Insert breakpoint */
+		case 'z': /* Remove breakpoint */
+			gdbstub_breakpoint ( stub );
+			break;
 		default:
 			stub->len = 0;
 			gdbstub_tx_packet ( stub );
@@ -341,7 +361,7 @@ static struct gdbstub stub = {
 	.parse = gdbstub_state_new
 };
 
-__cdecl void gdbstub_handler ( int signo, gdbreg_t *regs ) {
+void gdbstub_handler ( int signo, gdbreg_t *regs ) {
 	char packet [ SIZEOF_PAYLOAD + 4 ];
 	size_t len, i;
 

src/include/gpxe/gdbstub.h

@@ -9,6 +9,7 @@
 
 #include <stdint.h>
 #include <gpxe/tables.h>
+#include <gdbmach.h>
 
 /**
  * A transport mechanism for the GDB protocol
@@ -61,4 +62,12 @@ extern struct gdb_transport *find_gdb_transport ( const char *name );
  */
 extern void gdbstub_start ( struct gdb_transport *trans );
 
+/**
+ * Interrupt handler
+ *
+ * @signo POSIX signal number
+ * @regs CPU register snapshot
+ **/
+extern void gdbstub_handler ( int signo, gdbreg_t *regs );
+
 #endif /* _GPXE_GDBSTUB_H */

src/tests/gdbstub_test.S

@@ -1,4 +1,9 @@
 	.arch i386
+
+	.section ".data"
+watch_me:
+	.long 0xfeedbeef
+
 	.section ".text"
 	.code32
 gdbstub_test:
@@ -29,5 +34,21 @@ gdbstub_test:
 	int	$3
 	nop
 
+	/* 6. Access watch test */
+	movl	$0x600d0000, %ecx
+	movl	watch_me, %eax
+	movl	$0xbad00000, %ecx
+	int	$3
+	movl	$0x600d0001, %ecx
+	movl	%eax, watch_me
+	movl	$0xbad00001, %ecx
+	int	$3
+
+	/* 7. Write watch test */
+	movl	$0x600d0002, %ecx
+	movl	%eax, watch_me
+	movl	$0xbad00002, %ecx
+	int	$3
+
 1:
 	jmp	1b

src/tests/gdbstub_test.gdb

@@ -77,6 +77,34 @@ define gpxe_test_step
 	gpxe_assert ({char}($eip-1)) (char)0x90 "gpxe_test_step" # nop = 0x90
 end
 
+define gpxe_test_awatch
+	awatch watch_me
+
+	c
+	gpxe_assert $ecx 0x600d0000 "gpxe_test_awatch"
+	if $ecx == 0x600d0000
+		c
+	end
+
+	c
+	gpxe_assert $ecx 0x600d0001 "gpxe_test_awatch"
+	if $ecx == 0x600d0001
+		c
+	end
+
+	delete
+end
+
+define gpxe_test_watch
+	watch watch_me
+	c
+	gpxe_assert $ecx 0x600d0002 "gpxe_test_watch"
+	if $ecx == 0x600d0002
+		c
+	end
+	delete
+end
+
 gpxe_load_symbols
 gpxe_start_tests
 gpxe_test_regs_read
@@ -84,3 +112,5 @@ gpxe_test_regs_write
 gpxe_test_mem_read
 gpxe_test_mem_write
 gpxe_test_step
+gpxe_test_awatch
+gpxe_test_watch