[gdb] Add support for x86_64

Signed-off-by: Michael Brown <mcb30@ipxe.org>

7 files changed, 470 insertions, 221 deletions

diff --git a/src/arch/i386/core/gdbidt.S b/src/arch/i386/core/gdbidt.S
index a1e309d7..666ecce3 100644
--- a/src/arch/i386/core/gdbidt.S
+++ b/src/arch/i386/core/gdbidt.S
@@ -15,41 +15,29 @@
 /* POSIX signal numbers for reporting traps to GDB */
 #define SIGILL 4
 #define SIGTRAP 5
-#define SIGBUS 7
 #define SIGFPE 8
-#define SIGSEGV 11
 #define SIGSTKFLT 16
 
-	.globl	gdbmach_nocode_sigfpe
-gdbmach_nocode_sigfpe:
+	.globl	gdbmach_sigfpe
+gdbmach_sigfpe:
 	pushl	$SIGFPE
 	jmp	gdbmach_interrupt
 
-	.globl	gdbmach_nocode_sigtrap
-gdbmach_nocode_sigtrap:
+	.globl	gdbmach_sigtrap
+gdbmach_sigtrap:
 	pushl	$SIGTRAP
 	jmp	gdbmach_interrupt
 
-	.globl	gdbmach_nocode_sigstkflt
-gdbmach_nocode_sigstkflt:
+	.globl	gdbmach_sigstkflt
+gdbmach_sigstkflt:
 	pushl	$SIGSTKFLT
 	jmp	gdbmach_interrupt
 
-	.globl	gdbmach_nocode_sigill
-gdbmach_nocode_sigill:
+	.globl	gdbmach_sigill
+gdbmach_sigill:
 	pushl	$SIGILL
 	jmp	gdbmach_interrupt
 
-	.globl	gdbmach_withcode_sigbus
-gdbmach_withcode_sigbus:
-	movl	$SIGBUS, (%esp)
-	jmp	gdbmach_interrupt
-
-	.globl	gdbmach_withcode_sigsegv
-gdbmach_withcode_sigsegv:
-	movl	$SIGSEGV, (%esp)
-	jmp	gdbmach_interrupt
-
 /* When invoked, the stack contains: eflags, cs, eip, signo. */
 #define IH_OFFSET_GDB_REGS ( 0 )
 #define IH_OFFSET_GDB_EIP ( IH_OFFSET_GDB_REGS + SIZEOF_I386_REGS )
diff --git a/src/arch/i386/core/gdbmach.c b/src/arch/i386/core/gdbmach.c
deleted file mode 100644
index d92a4ac0..00000000
--- a/src/arch/i386/core/gdbmach.c
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
- *
- * 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 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, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- * You can also choose to distribute this program under the terms of
- * the Unmodified Binary Distribution Licence (as given in the file
- * COPYING.UBDL), provided that you have satisfied its requirements.
- */
-
-FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
-
-#include <stddef.h>
-#include <stdio.h>
-#include <assert.h>
-#include <ipxe/uaccess.h>
-#include <ipxe/gdbstub.h>
-#include <librm.h>
-#include <gdbmach.h>
-
-/** @file
- *
- * GDB architecture-specific bits for i386
- *
- */
-
-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 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 ) {
-	unsigned int regnum = bp - hwbps;
-
-	/* Set breakpoint address */
-	assert ( 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 hardware breakpoints */
-	__asm__ __volatile__ ( "movl %0, %%dr7\n" : : "r" ( DR7_CLEAR ) );
-}
-
-static void gdbmach_enable_hwbps ( void ) {
-	/* Clear breakpoint status register */
-	__asm__ __volatile__ ( "movl %0, %%dr6\n" : : "r" ( DR6_CLEAR ) );
-
-	/* Restore hardware breakpoints */
-	__asm__ __volatile__ ( "movl %0, %%dr7\n" : : "r" ( dr7 ) );
-}
-
-__asmcall void gdbmach_handler ( int signo, gdbreg_t *regs ) {
-	gdbmach_disable_hwbps();
-	gdbstub_handler ( signo, regs );
-	gdbmach_enable_hwbps();
-}
-
-static void * gdbmach_interrupt_vectors[] = {
-	gdbmach_nocode_sigfpe,		/* Divide by zero */
-	gdbmach_nocode_sigtrap,		/* Debug trap */
-	NULL,				/* Non-maskable interrupt */
-	gdbmach_nocode_sigtrap,		/* Breakpoint */
-	gdbmach_nocode_sigstkflt,	/* Overflow */
-	gdbmach_nocode_sigstkflt,	/* Bound range exceeded */
-	gdbmach_nocode_sigill,		/* Invalid opcode */
-	NULL,				/* Device not available */
-	gdbmach_withcode_sigbus,	/* Double fault */
-	NULL,				/* Coprocessor segment overrun */
-	gdbmach_withcode_sigsegv,	/* Invalid TSS */
-	gdbmach_withcode_sigsegv,	/* Segment not present */
-	gdbmach_withcode_sigsegv,	/* Stack segment fault */
-	gdbmach_withcode_sigsegv,	/* General protection fault */
-	gdbmach_withcode_sigsegv,	/* Page fault */
-};
-
-void gdbmach_init ( void ) {
-	unsigned int i;
-
-	for ( i = 0 ; i < ( sizeof ( gdbmach_interrupt_vectors ) /
-			    sizeof ( gdbmach_interrupt_vectors[0] ) ) ; i++ ) {
-		set_interrupt_vector ( i, gdbmach_interrupt_vectors[i] );
-	}
-}
diff --git a/src/arch/i386/include/gdbmach.h b/src/arch/i386/include/gdbmach.h
index 416ae341..52cce783 100644
--- a/src/arch/i386/include/gdbmach.h
+++ b/src/arch/i386/include/gdbmach.h
@@ -47,12 +47,10 @@ enum {
 };
 
 /* Interrupt vectors */
-extern void gdbmach_nocode_sigfpe ( void );
-extern void gdbmach_nocode_sigtrap ( void );
-extern void gdbmach_nocode_sigstkflt ( void );
-extern void gdbmach_nocode_sigill ( void );
-extern void gdbmach_withcode_sigbus ( void );
-extern void gdbmach_withcode_sigsegv ( void );
+extern void gdbmach_sigfpe ( void );
+extern void gdbmach_sigtrap ( void );
+extern void gdbmach_sigstkflt ( void );
+extern void gdbmach_sigill ( void );
 
 static inline void gdbmach_set_pc ( gdbreg_t *regs, gdbreg_t pc ) {
 	regs [ GDBMACH_EIP ] = pc;
diff --git a/src/arch/x86/core/gdbmach.c b/src/arch/x86/core/gdbmach.c
new file mode 100644
index 00000000..af6abfed
--- /dev/null
+++ b/src/arch/x86/core/gdbmach.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+ * Copyright (C) 2016 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * 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 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stddef.h>
+#include <stdio.h>
+#include <errno.h>
+#include <assert.h>
+#include <ipxe/uaccess.h>
+#include <ipxe/gdbstub.h>
+#include <librm.h>
+#include <gdbmach.h>
+
+/** @file
+ *
+ * GDB architecture-specific bits for x86
+ *
+ */
+
+/** Number of hardware breakpoints */
+#define NUM_HWBP 4
+
+/** Debug register 7: Global breakpoint enable */
+#define DR7_G( bp ) ( 2 << ( 2 * (bp) ) )
+
+/** Debug register 7: Global exact breakpoint enable */
+#define DR7_GE ( 1 << 9 )
+
+/** Debug register 7: Break on data writes */
+#define DR7_RWLEN_WRITE 0x11110000
+
+/** Debug register 7: Break on data access */
+#define DR7_RWLEN_ACCESS 0x33330000
+
+/** Debug register 7: One-byte length */
+#define DR7_RWLEN_1 0x00000000
+
+/** Debug register 7: Two-byte length */
+#define DR7_RWLEN_2 0x44440000
+
+/** Debug register 7: Four-byte length */
+#define DR7_RWLEN_4 0xcccc0000
+
+/** Debug register 7: Eight-byte length */
+#define DR7_RWLEN_8 0x88880000
+
+/** Debug register 7: Breakpoint R/W and length mask */
+#define DR7_RWLEN_MASK( bp ) ( 0xf0000 << ( 4 * (bp) ) )
+
+/** Hardware breakpoint addresses (debug registers 0-3) */
+static unsigned long dr[NUM_HWBP];
+
+/** Active value of debug register 7 */
+static unsigned long dr7 = DR7_GE;
+
+/**
+ * Update debug registers
+ *
+ */
+static void gdbmach_update ( void ) {
+
+	/* Set debug registers */
+	__asm__ __volatile__ ( "mov %0, %%dr0" : : "r" ( dr[0] ) );
+	__asm__ __volatile__ ( "mov %0, %%dr1" : : "r" ( dr[1] ) );
+	__asm__ __volatile__ ( "mov %0, %%dr2" : : "r" ( dr[2] ) );
+	__asm__ __volatile__ ( "mov %0, %%dr3" : : "r" ( dr[3] ) );
+	__asm__ __volatile__ ( "mov %0, %%dr7" : : "r" ( dr7 ) );
+}
+
+/**
+ * Find reusable or available hardware breakpoint
+ *
+ * @v addr		Linear address
+ * @v rwlen		Control bits
+ * @ret bp		Hardware breakpoint, or negative error
+ */
+static int gdbmach_find ( unsigned long addr, unsigned int rwlen ) {
+	unsigned int i;
+	int bp = -ENOENT;
+
+	/* Look for a reusable or available breakpoint */
+	for ( i = 0 ; i < NUM_HWBP ; i++ ) {
+
+		/* If breakpoint is not enabled, then it is available */
+		if ( ! ( dr7 & DR7_G ( i ) ) ) {
+			bp = i;
+			continue;
+		}
+
+		/* If breakpoint is enabled and has the same address
+		 * and control bits, then reuse it.
+		 */
+		if ( ( dr[i] == addr ) &&
+		     ( ( ( dr7 ^ rwlen ) & DR7_RWLEN_MASK ( i ) ) == 0 ) ) {
+			bp = i;
+			break;
+		}
+	}
+
+	return bp;
+}
+
+/**
+ * Set hardware breakpoint
+ *
+ * @v type		GDB breakpoint type
+ * @v addr		Virtual address
+ * @v len		Length
+ * @v enable		Enable (not disable) breakpoint
+ * @ret rc		Return status code
+ */
+int gdbmach_set_breakpoint ( int type, unsigned long addr, size_t len,
+			     int enable ) {
+	unsigned int rwlen;
+	unsigned long mask;
+	int bp;
+
+	/* Parse breakpoint type */
+	switch ( type ) {
+	case GDBMACH_WATCH:
+		rwlen = DR7_RWLEN_WRITE;
+		break;
+	case GDBMACH_AWATCH:
+		rwlen = DR7_RWLEN_ACCESS;
+		break;
+	default:
+		return -ENOTSUP;
+	}
+
+	/* Parse breakpoint length */
+	switch ( len ) {
+	case 1:
+		rwlen |= DR7_RWLEN_1;
+		break;
+	case 2:
+		rwlen |= DR7_RWLEN_2;
+		break;
+	case 4:
+		rwlen |= DR7_RWLEN_4;
+		break;
+	case 8:
+		rwlen |= DR7_RWLEN_8;
+		break;
+	default:
+		return -ENOTSUP;
+	}
+
+	/* Convert to linear address */
+	if ( sizeof ( physaddr_t ) <= sizeof ( uint32_t ) )
+		addr = virt_to_phys ( ( void * ) addr );
+
+	/* Find reusable or available hardware breakpoint */
+	bp = gdbmach_find ( addr, rwlen );
+	if ( bp < 0 )
+		return ( enable ? -ENOBUFS : 0 );
+
+	/* Configure this breakpoint */
+	DBGC ( &dr[0], "GDB bp %d at %p+%zx type %d (%sabled)\n",
+	       bp, ( ( void * ) addr ), len, type, ( enable ? "en" : "dis" ) );
+	dr[bp] = addr;
+	mask = DR7_RWLEN_MASK ( bp );
+	dr7 = ( ( dr7 & ~mask ) | ( rwlen & mask ) );
+	mask = DR7_G ( bp );
+	dr7 &= ~mask;
+	if ( enable )
+		dr7 |= mask;
+
+	/* Update debug registers */
+	gdbmach_update();
+
+	return 0;
+}
+
+/**
+ * Handle exception
+ *
+ * @v signo		GDB signal number
+ * @v regs		Register dump
+ */
+__asmcall void gdbmach_handler ( int signo, gdbreg_t *regs ) {
+	unsigned long dr7_disabled = DR7_GE;
+	unsigned long dr6_clear = 0;
+
+	/* Temporarily disable breakpoints */
+	__asm__ __volatile__ ( "mov %0, %%dr7\n" : : "r" ( dr7_disabled ) );
+
+	/* Handle exception */
+	DBGC ( &dr[0], "GDB signal %d\n", signo );
+	DBGC2_HDA ( &dr[0], 0, regs, ( GDBMACH_NREGS * sizeof ( *regs ) ) );
+	gdbstub_handler ( signo, regs );
+	DBGC ( &dr[0], "GDB signal %d returning\n", signo );
+	DBGC2_HDA ( &dr[0], 0, regs, ( GDBMACH_NREGS * sizeof ( *regs ) ) );
+
+	/* Clear breakpoint status register */
+	__asm__ __volatile__ ( "mov %0, %%dr6\n" : : "r" ( dr6_clear ) );
+
+	/* Re-enable breakpoints */
+	__asm__ __volatile__ ( "mov %0, %%dr7\n" : : "r" ( dr7 ) );
+}
+
+/**
+ * CPU exception vectors
+ *
+ * Note that we cannot intercept anything from INT8 (double fault)
+ * upwards, since these overlap by default with IRQ0-7.
+ */
+static void * gdbmach_vectors[] = {
+	gdbmach_sigfpe,		/* Divide by zero */
+	gdbmach_sigtrap,	/* Debug trap */
+	NULL,			/* Non-maskable interrupt */
+	gdbmach_sigtrap,	/* Breakpoint */
+	gdbmach_sigstkflt,	/* Overflow */
+	gdbmach_sigstkflt,	/* Bound range exceeded */
+	gdbmach_sigill,		/* Invalid opcode */
+};
+
+/**
+ * Initialise GDB
+ */
+void gdbmach_init ( void ) {
+	unsigned int i;
+
+	/* Hook CPU exception vectors */
+	for ( i = 0 ; i < ( sizeof ( gdbmach_vectors ) /
+			    sizeof ( gdbmach_vectors[0] ) ) ; i++ ) {
+		if ( gdbmach_vectors[i] )
+			set_interrupt_vector ( i, gdbmach_vectors[i] );
+	}
+}
diff --git a/src/arch/x86/include/bits/errfile.h b/src/arch/x86/include/bits/errfile.h
index 0d1617d2..9eb4b548 100644
--- a/src/arch/x86/include/bits/errfile.h
+++ b/src/arch/x86/include/bits/errfile.h
@@ -22,6 +22,7 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #define ERRFILE_apm		( ERRFILE_ARCH | ERRFILE_CORE | 0x000b0000 )
 #define ERRFILE_vesafb		( ERRFILE_ARCH | ERRFILE_CORE | 0x000c0000 )
 #define ERRFILE_int13con	( ERRFILE_ARCH | ERRFILE_CORE | 0x000d0000 )
+#define ERRFILE_gdbmach		( ERRFILE_ARCH | ERRFILE_CORE | 0x000e0000 )
 
 #define ERRFILE_bootsector     ( ERRFILE_ARCH | ERRFILE_IMAGE | 0x00000000 )
 #define ERRFILE_bzimage	       ( ERRFILE_ARCH | ERRFILE_IMAGE | 0x00010000 )
diff --git a/src/arch/x86_64/core/gdbidt.S b/src/arch/x86_64/core/gdbidt.S
new file mode 100644
index 00000000..89280bf8
--- /dev/null
+++ b/src/arch/x86_64/core/gdbidt.S
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) 2016 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * 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 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+/** @file
+ *
+ * GDB exception handlers
+ *
+ */
+
+/* Size of a register */
+#define SIZEOF_REG 8
+
+/* POSIX signal numbers for reporting traps to GDB */
+#define SIGILL 4
+#define SIGTRAP 5
+#define SIGFPE 8
+#define SIGSTKFLT 16
+
+	.section ".text.gdbmach_interrupt", "ax", @progbits
+	.code64
+
+	.struct 0
+/* Register dump created for GDB stub */
+regs:
+regs_rax:	.space	SIZEOF_REG
+regs_rbx:	.space	SIZEOF_REG
+regs_rcx:	.space	SIZEOF_REG
+regs_rdx:	.space	SIZEOF_REG
+regs_rsi:	.space	SIZEOF_REG
+regs_rdi:	.space	SIZEOF_REG
+regs_rbp:	.space	SIZEOF_REG
+regs_rsp:	.space	SIZEOF_REG
+regs_r8:	.space	SIZEOF_REG
+regs_r9:	.space	SIZEOF_REG
+regs_r10:	.space	SIZEOF_REG
+regs_r11:	.space	SIZEOF_REG
+regs_r12:	.space	SIZEOF_REG
+regs_r13:	.space	SIZEOF_REG
+regs_r14:	.space	SIZEOF_REG
+regs_r15:	.space	SIZEOF_REG
+regs_rip:	.space	SIZEOF_REG
+regs_rflags:	.space	SIZEOF_REG
+regs_cs:	.space	SIZEOF_REG
+regs_ss:	.space	SIZEOF_REG
+regs_ds:	.space	SIZEOF_REG
+regs_es:	.space	SIZEOF_REG
+regs_fs:	.space	SIZEOF_REG
+regs_gs:	.space	SIZEOF_REG
+regs_end:
+/* GDB signal code */
+gdb:
+gdb_code:	.space	SIZEOF_REG
+gdb_end:
+/* Long-mode exception frame */
+frame:
+frame_rip:	.space	SIZEOF_REG
+frame_cs:	.space	SIZEOF_REG
+frame_rflags:	.space	SIZEOF_REG
+frame_rsp:	.space	SIZEOF_REG
+frame_ss:	.space	SIZEOF_REG
+frame_end:
+	.previous
+
+	.globl	gdbmach_sigfpe
+gdbmach_sigfpe:
+	push	$SIGFPE
+	jmp	gdbmach_interrupt
+
+	.globl	gdbmach_sigtrap
+gdbmach_sigtrap:
+	push	$SIGTRAP
+	jmp	gdbmach_interrupt
+
+	.globl	gdbmach_sigstkflt
+gdbmach_sigstkflt:
+	push	$SIGSTKFLT
+	jmp	gdbmach_interrupt
+
+	.globl	gdbmach_sigill
+gdbmach_sigill:
+	push	$SIGILL
+	jmp	gdbmach_interrupt
+
+gdbmach_interrupt:
+
+	/* Create register dump */
+	pushq	%gs
+	pushq	%fs
+	pushq	$0		/* %es unused in long mode */
+	pushq	$0		/* %ds unused in long mode */
+	pushq	( frame_ss	- regs_ss	- SIZEOF_REG )(%rsp)
+	pushq	( frame_cs	- regs_cs	- SIZEOF_REG )(%rsp)
+	pushq	( frame_rflags	- regs_rflags	- SIZEOF_REG )(%rsp)
+	pushq	( frame_rip	- regs_rip	- SIZEOF_REG )(%rsp)
+	pushq	%r15
+	pushq	%r14
+	pushq	%r13
+	pushq	%r12
+	pushq	%r11
+	pushq	%r10
+	pushq	%r9
+	pushq	%r8
+	pushq	( frame_rsp	- regs_rsp	- SIZEOF_REG )(%rsp)
+	pushq	%rbp
+	pushq	%rdi
+	pushq	%rsi
+	pushq	%rdx
+	pushq	%rcx
+	pushq	%rbx
+	pushq	%rax
+
+	/* Call GDB stub exception handler */
+	movq	gdb_code(%rsp), %rdi
+	movq	%rsp, %rsi
+	call	gdbmach_handler
+
+	/* Restore from register dump */
+	popq	%rax
+	popq	%rbx
+	popq	%rcx
+	popq	%rdx
+	popq	%rsi
+	popq	%rdi
+	popq	%rbp
+	popq	( frame_rsp	- regs_rsp	- SIZEOF_REG )(%rsp)
+	popq	%r8
+	popq	%r9
+	popq	%r10
+	popq	%r11
+	popq	%r12
+	popq	%r13
+	popq	%r14
+	popq	%r15
+	popq	( frame_rip	- regs_rip	- SIZEOF_REG )(%rsp)
+	popq	( frame_rflags	- regs_rflags	- SIZEOF_REG )(%rsp)
+	popq	( frame_cs	- regs_cs	- SIZEOF_REG )(%rsp)
+	popq	( frame_ss	- regs_ss	- SIZEOF_REG )(%rsp)
+	addq	$( regs_fs - regs_ds ), %rsp	/* skip %ds, %es */
+	popq	%fs
+	popq	%gs
+
+	/* Skip code */
+	addq	$( gdb_end - gdb_code ), %rsp	/* skip code */
+
+	/* Return */
+	iretq
diff --git a/src/arch/x86_64/include/gdbmach.h b/src/arch/x86_64/include/gdbmach.h
index 6dadbbdd..367405fd 100644
--- a/src/arch/x86_64/include/gdbmach.h
+++ b/src/arch/x86_64/include/gdbmach.h
@@ -14,16 +14,37 @@
 
 typedef unsigned long gdbreg_t;
 
-/* The register snapshot, this must be in sync with interrupt handler and the
- * GDB protocol. */
+/* Register snapshot */
 enum {
-	// STUB: don't expect this to work!
-	GDBMACH_EIP,
-	GDBMACH_EFLAGS,
+	GDBMACH_RAX,
+	GDBMACH_RBX,
+	GDBMACH_RCX,
+	GDBMACH_RDX,
+	GDBMACH_RSI,
+	GDBMACH_RDI,
+	GDBMACH_RBP,
+	GDBMACH_RSP,
+	GDBMACH_R8,
+	GDBMACH_R9,
+	GDBMACH_R10,
+	GDBMACH_R11,
+	GDBMACH_R12,
+	GDBMACH_R13,
+	GDBMACH_R14,
+	GDBMACH_R15,
+	GDBMACH_RIP,
+	GDBMACH_RFLAGS,
+	GDBMACH_CS,
+	GDBMACH_SS,
+	GDBMACH_DS,
+	GDBMACH_ES,
+	GDBMACH_FS,
+	GDBMACH_GS,
 	GDBMACH_NREGS,
-	GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof ( gdbreg_t )
 };
 
+#define GDBMACH_SIZEOF_REGS ( GDBMACH_NREGS * sizeof ( gdbreg_t ) )
+
 /* Breakpoint types */
 enum {
 	GDBMACH_BPMEM,
@@ -33,21 +54,27 @@ enum {
 	GDBMACH_AWATCH,
 };
 
+/* Exception vectors */
+extern void gdbmach_sigfpe ( void );
+extern void gdbmach_sigtrap ( void );
+extern void gdbmach_sigstkflt ( void );
+extern void gdbmach_sigill ( void );
+
 static inline void gdbmach_set_pc ( gdbreg_t *regs, gdbreg_t pc ) {
-	regs [ GDBMACH_EIP ] = pc;
+	regs[GDBMACH_RIP] = pc;
 }
 
 static inline void gdbmach_set_single_step ( gdbreg_t *regs, int step ) {
-	regs [ GDBMACH_EFLAGS ] &= ~( 1 << 8 ); /* Trace Flag (TF) */
-	regs [ GDBMACH_EFLAGS ] |= ( step << 8 );
+	regs[GDBMACH_RFLAGS] &= ~( 1 << 8 ); /* Trace Flag (TF) */
+	regs[GDBMACH_RFLAGS] |= ( step << 8 );
 }
 
 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 );
-
+extern int gdbmach_set_breakpoint ( int type, unsigned long addr, size_t len,
+				    int enable );
 extern void gdbmach_init ( void );
 
 #endif /* GDBMACH_H */