Add GDB stub

6 files changed, 874 insertions, 16 deletions

diff --git a/com32/gdbstub/Makefile b/com32/gdbstub/Makefile
index d9e55c8c..c5b79ead 100644
--- a/com32/gdbstub/Makefile
+++ b/com32/gdbstub/Makefile
@@ -25,7 +25,7 @@ LNXLIBS	   = ../libutil/libutil_lnx.a
 MODULES	  = gdbstub.c32
 TESTFILES =
 
-OBJS = main.o
+OBJS = main.o int.o serial.o gdbstub.o
 
 all: $(MODULES) $(TESTFILES)
 
diff --git a/com32/gdbstub/gdbstub.c b/com32/gdbstub/gdbstub.c
new file mode 100644
index 00000000..77c9dcb9
--- /dev/null
+++ b/com32/gdbstub/gdbstub.c
@@ -0,0 +1,531 @@
+/*
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/**
+ * @file
+ *
+ * GDB stub for remote debugging
+ *
+ */
+
+#include <stdlib.h>
+#include <stdint.h>
+#include "serial.h"
+
+typedef uint32_t gdbreg_t;
+
+enum {
+	POSIX_EINVAL = 0x1c,       /* used to report bad arguments to GDB */
+	SIZEOF_PAYLOAD = 256,      /* buffer size of GDB payload data */
+	DR7_CLEAR = 0x00000400,    /* disable hardware breakpoints */
+	DR6_CLEAR = 0xffff0ff0,    /* clear breakpoint status */
+};
+
+/* The register snapshot, this must be in sync with interrupt handler and the
+ * GDB protocol. */
+enum {
+	GDBMACH_EAX,
+	GDBMACH_ECX,
+	GDBMACH_EDX,
+	GDBMACH_EBX,
+	GDBMACH_ESP,
+	GDBMACH_EBP,
+	GDBMACH_ESI,
+	GDBMACH_EDI,
+	GDBMACH_EIP,
+	GDBMACH_EFLAGS,
+	GDBMACH_CS,
+	GDBMACH_SS,
+	GDBMACH_DS,
+	GDBMACH_ES,
+	GDBMACH_FS,
+	GDBMACH_GS,
+	GDBMACH_NREGS,
+	GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof ( gdbreg_t )
+};
+
+/* Breakpoint types */
+enum {
+	GDBMACH_BPMEM,
+	GDBMACH_BPHW,
+	GDBMACH_WATCH,
+	GDBMACH_RWATCH,
+	GDBMACH_AWATCH,
+};
+
+struct gdbstub {
+	int exit_handler; /* leave interrupt handler */
+
+	int signo;
+	gdbreg_t *regs;
+
+	void ( * parse ) ( struct gdbstub *stub, char ch );
+	uint8_t cksum1;
+
+	/* Buffer for payload data when parsing a packet.  Once the
+	 * packet has been received, this buffer is used to hold
+	 * the reply payload. */
+	char buf [ SIZEOF_PAYLOAD + 4 ]; /* $...PAYLOAD...#XX */
+	char *payload;                   /* start of payload */
+	int len;                         /* length of payload */
+};
+
+/** 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 inline void gdbmach_set_pc ( gdbreg_t *regs, gdbreg_t pc ) {
+	regs [ GDBMACH_EIP ] = 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 );
+}
+
+static inline void gdbmach_breakpoint ( void ) {
+	__asm__ __volatile__ ( "int $3\n" );
+}
+
+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 */
+	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 */
+
+	/* 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 ) );
+}
+
+/* Packet parser states */
+static void gdbstub_state_new ( struct gdbstub *stub, char ch );
+static void gdbstub_state_data ( struct gdbstub *stub, char ch );
+static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch );
+static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch );
+static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch );
+
+static void serial_write ( void *buf, size_t len ) {
+    char *p = buf;
+    while ( len-- > 0 )
+        serial_putc ( *p++ );
+}
+
+static uint8_t gdbstub_from_hex_digit ( char ch ) {
+    if ( ch >= '0' && ch <= '9' )
+        return ch - '0';
+    else if ( ch >= 'A' && ch <= 'F' )
+        return ch - 'A' + 0xa;
+    else
+        return ( ch - 'a' + 0xa ) & 0xf;
+}
+
+static uint8_t gdbstub_to_hex_digit ( uint8_t b ) {
+	b &= 0xf;
+	return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
+}
+
+/*
+ * To make reading/writing device memory atomic, we check for
+ * 2- or 4-byte aligned operations and handle them specially.
+ */
+
+static void gdbstub_from_hex_buf ( char *dst, char *src, int lenbytes ) {
+	if ( lenbytes == 2 && ( ( unsigned long ) dst & 0x1 ) == 0 ) {
+		uint16_t i = gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
+			gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
+			gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+			gdbstub_from_hex_digit ( src [ 1 ] );
+		* ( uint16_t * ) dst = i;
+	} else if ( lenbytes == 4 && ( ( unsigned long ) dst & 0x3 ) == 0 ) {
+		uint32_t i = gdbstub_from_hex_digit ( src [ 6 ] ) << 28 |
+			gdbstub_from_hex_digit ( src [ 7 ] ) << 24 |
+			gdbstub_from_hex_digit ( src [ 4 ] ) << 20 |
+			gdbstub_from_hex_digit ( src [ 5 ] ) << 16 |
+			gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
+			gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
+			gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+			gdbstub_from_hex_digit ( src [ 1 ] );
+		* ( uint32_t * ) dst = i;
+	} else {
+		while ( lenbytes-- > 0 ) {
+			*dst++ = gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
+				gdbstub_from_hex_digit ( src [ 1 ] );
+			src += 2;
+		}
+	}
+}
+
+static void gdbstub_to_hex_buf ( char *dst, char *src, int lenbytes ) {
+	if ( lenbytes == 2 && ( ( unsigned long ) src & 0x1 ) == 0 ) {
+		uint16_t i = * ( uint16_t * ) src;
+		dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
+		dst [ 1 ] = gdbstub_to_hex_digit ( i );
+		dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
+		dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
+	} else if ( lenbytes == 4 && ( ( unsigned long ) src & 0x3 ) == 0 ) {
+		uint32_t i = * ( uint32_t * ) src;
+		dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
+		dst [ 1 ] = gdbstub_to_hex_digit ( i );
+		dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
+		dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
+		dst [ 4 ] = gdbstub_to_hex_digit ( i >> 20 );
+		dst [ 5 ] = gdbstub_to_hex_digit ( i >> 16);
+		dst [ 6 ] = gdbstub_to_hex_digit ( i >> 28 );
+		dst [ 7 ] = gdbstub_to_hex_digit ( i >> 24 );
+	} else {
+		while ( lenbytes-- > 0 ) {
+			*dst++ = gdbstub_to_hex_digit ( *src >> 4 );
+			*dst++ = gdbstub_to_hex_digit ( *src );
+			src++;
+		}
+	}
+}
+
+static uint8_t gdbstub_cksum ( char *data, int len ) {
+	uint8_t cksum = 0;
+	while ( len-- > 0 ) {
+		cksum += ( uint8_t ) *data++;
+	}
+	return cksum;
+}
+
+static void gdbstub_tx_packet ( struct gdbstub *stub ) {
+	uint8_t cksum = gdbstub_cksum ( stub->payload, stub->len );
+	stub->buf [ 0 ] = '$';
+	stub->buf [ stub->len + 1 ] = '#';
+	stub->buf [ stub->len + 2 ] = gdbstub_to_hex_digit ( cksum >> 4 );
+	stub->buf [ stub->len + 3 ] = gdbstub_to_hex_digit ( cksum );
+	serial_write ( stub->buf, stub->len + 4 );
+	stub->parse = gdbstub_state_wait_ack;
+}
+
+/* GDB commands */
+static void gdbstub_send_ok ( struct gdbstub *stub ) {
+	stub->payload [ 0 ] = 'O';
+	stub->payload [ 1 ] = 'K';
+	stub->len = 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_send_num_packet ( struct gdbstub *stub, char reply, int num ) {
+	stub->payload [ 0 ] = reply;
+	stub->payload [ 1 ] = gdbstub_to_hex_digit ( ( char ) num >> 4 );
+	stub->payload [ 2 ] = gdbstub_to_hex_digit ( ( char ) num );
+	stub->len = 3;
+	gdbstub_tx_packet ( stub );
+}
+
+/* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
+static int gdbstub_get_packet_args ( struct gdbstub *stub, unsigned long *args, int nargs, int *stop_idx ) {
+	int i;
+	char ch = 0;
+	int argc = 0;
+	unsigned long val = 0;
+	for ( i = 1; i < stub->len && argc < nargs; i++ ) {
+		ch = stub->payload [ i ];
+		if ( ch == ':' ) {
+			break;
+		} else if ( ch == ',' ) {
+			args [ argc++ ] = val;
+			val = 0;
+		} else {
+			val = ( val << 4 ) | gdbstub_from_hex_digit ( ch );
+		}
+	}
+	if ( stop_idx ) {
+		*stop_idx = i;
+	}
+	if ( argc < nargs ) {
+		args [ argc++ ] = val;
+	}
+	return ( ( i == stub->len || ch == ':' ) && argc == nargs );
+}
+
+static void gdbstub_send_errno ( struct gdbstub *stub, int errno ) {
+	gdbstub_send_num_packet ( stub, 'E', errno );
+}
+
+static void gdbstub_report_signal ( struct gdbstub *stub ) {
+	gdbstub_send_num_packet ( stub, 'S', stub->signo );
+}
+
+static void gdbstub_read_regs ( struct gdbstub *stub ) {
+	gdbstub_to_hex_buf ( stub->payload, ( char * ) stub->regs, GDBMACH_SIZEOF_REGS );
+	stub->len = GDBMACH_SIZEOF_REGS * 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_write_regs ( struct gdbstub *stub ) {
+	if ( stub->len != 1 + GDBMACH_SIZEOF_REGS * 2 ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], GDBMACH_SIZEOF_REGS );
+	gdbstub_send_ok ( stub );
+}
+
+static void gdbstub_read_mem ( struct gdbstub *stub ) {
+	unsigned long args [ 2 ];
+	if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	args [ 1 ] = ( args [ 1 ] < SIZEOF_PAYLOAD / 2 ) ? args [ 1 ] : SIZEOF_PAYLOAD / 2;
+	gdbstub_to_hex_buf ( stub->payload, ( char * ) args [ 0 ], args [ 1 ] );
+	stub->len = args [ 1 ] * 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_write_mem ( struct gdbstub *stub ) {
+	unsigned long args [ 2 ];
+	int colon;
+	if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], &colon ) ||
+			colon >= stub->len || stub->payload [ colon ] != ':' ||
+			( stub->len - colon - 1 ) % 2 != 0 ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], ( stub->len - colon - 1 ) / 2 );
+	gdbstub_send_ok ( stub );
+}
+
+static void gdbstub_continue ( struct gdbstub *stub, int single_step ) {
+	gdbreg_t pc;
+	if ( stub->len > 1 && gdbstub_get_packet_args ( stub, (unsigned long *)&pc, 1, NULL ) ) {
+		gdbmach_set_pc ( stub->regs, pc );
+	}
+	gdbmach_set_single_step ( stub->regs, single_step );
+	stub->exit_handler = 1;
+	/* 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 '?':
+			gdbstub_report_signal ( stub );
+			break;
+		case 'g':
+			gdbstub_read_regs ( stub );
+			break;
+		case 'G':
+			gdbstub_write_regs ( stub );
+			break;
+		case 'm':
+			gdbstub_read_mem ( stub );
+			break;
+		case 'M':
+			gdbstub_write_mem ( stub );
+			break;
+		case 'c': /* Continue */
+		case 'k': /* Kill */
+		case 's': /* Step */
+		case 'D': /* Detach */
+			gdbstub_continue ( stub, stub->payload [ 0 ] == 's' );
+			if ( stub->payload [ 0 ] == 'D' ) {
+				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 );
+			break;
+	}
+}
+
+/* GDB packet parser */
+static void gdbstub_state_new ( struct gdbstub *stub, char ch ) {
+	if ( ch == '$' ) {
+		stub->len = 0;
+		stub->parse = gdbstub_state_data;
+	}
+}
+
+static void gdbstub_state_data ( struct gdbstub *stub, char ch ) {
+	if ( ch == '#' ) {
+		stub->parse = gdbstub_state_cksum1;
+	} else if ( ch == '$' ) {
+		stub->len = 0; /* retry new packet */
+	} else {
+		/* If the length exceeds our buffer, let the checksum fail */
+		if ( stub->len < SIZEOF_PAYLOAD ) {
+			stub->payload [ stub->len++ ] = ch;
+		}
+	}
+}
+
+static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch ) {
+	stub->cksum1 = gdbstub_from_hex_digit ( ch ) << 4;
+	stub->parse = gdbstub_state_cksum2;
+}
+
+static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch ) {
+	uint8_t their_cksum;
+	uint8_t our_cksum;
+
+	stub->parse = gdbstub_state_new;
+	their_cksum = stub->cksum1 + gdbstub_from_hex_digit ( ch );
+	our_cksum = gdbstub_cksum ( stub->payload, stub->len );
+
+	if ( their_cksum == our_cksum ) {
+		serial_write ( "+", 1 );
+		if ( stub->len > 0 ) {
+			gdbstub_rx_packet ( stub );
+		}
+	} else {
+		serial_write ( "-", 1 );
+	}
+}
+
+static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch ) {
+	if ( ch == '+' ) {
+		stub->parse = gdbstub_state_new;
+	} else if ( ch == '-' ) {
+		gdbstub_tx_packet ( stub ); /* retransmit */
+	} else if ( ch == '$' ) {
+		/* GDB is reconnecting, drop our packet and listen to GDB */
+		serial_write ( "-", 1 );
+		stub->parse = gdbstub_state_new;
+	}
+}
+
+void gdbstub_handler ( int signo, gdbreg_t *regs ) {
+    struct gdbstub stub;
+
+    gdbmach_disable_hwbps();
+
+    stub.parse = gdbstub_state_new;
+    stub.payload = &stub.buf [ 1 ];
+	stub.signo = signo;
+	stub.regs = regs;
+	stub.exit_handler = 0;
+	gdbstub_report_signal ( &stub );
+	while ( !stub.exit_handler )
+        stub.parse ( &stub, serial_getc() );
+
+    gdbmach_enable_hwbps();
+}
diff --git a/com32/gdbstub/int.S b/com32/gdbstub/int.S
new file mode 100644
index 00000000..2a1ff9dd
--- /dev/null
+++ b/com32/gdbstub/int.S
@@ -0,0 +1,77 @@
+    .section ".text","ax"
+
+#define SIGTRAP 5
+
+#define SIZEOF_I386_REGS    32
+#define SIZEOF_I386_FLAGS   4
+
+/* 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 )
+#define IH_OFFSET_GDB_EFLAGS ( IH_OFFSET_GDB_EIP + 4 )
+#define IH_OFFSET_GDB_SEG_REGS ( IH_OFFSET_GDB_EFLAGS + SIZEOF_I386_FLAGS )
+#define IH_OFFSET_GDB_END ( IH_OFFSET_GDB_SEG_REGS + 6 * 4 )
+#define IH_OFFSET_OLD_EIP ( IH_OFFSET_GDB_END )
+#define IH_OFFSET_OLD_CS ( IH_OFFSET_OLD_EIP + 4 )
+#define IH_OFFSET_OLD_EFLAGS ( IH_OFFSET_OLD_CS + 4 )
+#define IH_OFFSET_END ( IH_OFFSET_OLD_EFLAGS + 4 )
+
+/* We also access the stack whilst still storing or restoring
+ * the register snapshot.  Since ESP is in flux, we need
+ * special offsets.
+ */
+#define IH_OFFSET_FLUX_OLD_CS ( IH_OFFSET_OLD_CS - 44 )
+#define IH_OFFSET_FLUX_OLD_EFLAGS ( IH_OFFSET_OLD_EFLAGS - 40 )
+#define IH_OFFSET_FLUX_OLD_EIP ( IH_OFFSET_OLD_EIP - 36 )
+#define IH_OFFSET_FLUX_END ( IH_OFFSET_END - 20 )
+
+    .global int_handler
+int_handler:
+	/* Store CPU state in GDB register snapshot */
+	pushw	$0
+	pushw	%gs
+	pushw	$0
+	pushw	%fs
+	pushw	$0
+	pushw	%es
+	pushw	$0
+	pushw	%ds
+	pushw	$0
+	pushw	%ss
+	pushw	$0
+	pushw	IH_OFFSET_FLUX_OLD_CS + 2(%esp)
+	pushl	IH_OFFSET_FLUX_OLD_EFLAGS(%esp)
+	pushl	IH_OFFSET_FLUX_OLD_EIP(%esp)
+	pushl	%edi
+	pushl	%esi
+	pushl	%ebp
+	leal	IH_OFFSET_FLUX_END(%esp), %edi
+	pushl	%edi /* old ESP */
+	pushl	%ebx
+	pushl	%edx
+	pushl	%ecx
+	pushl	%eax
+
+	/* Call GDB stub exception handler */
+    movl    $SIGTRAP, %eax
+	movl	%esp, %edx
+	call	gdbstub_handler
+
+	/* Restore CPU state from GDB register snapshot */
+	popl	%eax
+	popl	%ecx
+	popl	%edx
+	popl	%ebx
+	addl	$4, %esp /* Changing ESP currently not supported */
+	popl	%ebp
+	popl	%esi
+	popl	%edi
+	popl	IH_OFFSET_FLUX_OLD_EIP(%esp)
+	popl	IH_OFFSET_FLUX_OLD_EFLAGS(%esp)
+	popl	IH_OFFSET_FLUX_OLD_CS(%esp)
+	popl	%ss
+	popl	%ds
+	popl	%es
+	popl	%fs
+	popl	%gs
+	iret
diff --git a/com32/gdbstub/main.c b/com32/gdbstub/main.c
index 11c4e08b..8bb1dc5e 100644
--- a/com32/gdbstub/main.c
+++ b/com32/gdbstub/main.c
@@ -4,15 +4,44 @@
 #include <console.h>
 #include <com32.h>
 #include <syslinux/loadfile.h>
+#include "serial.h"
 
+#define X86_INT_DB      1
+#define X86_INT_BP      3
+#define COM32_IDT       ((void*)0x100000)
 #define COM32_LOAD_ADDR ((void*)0x101000)
-#define STACK_RED_ZONE  0x1000
+#define STACK_SIZE      0x1000
+
+extern char _start[], _end[];
+
+struct reloc_info {
+    void *data;
+    size_t len;
+    uint32_t old_esp;
+    uint32_t reloc_base;
+};
 
 static inline void error(const char *msg)
 {
     fputs(msg, stderr);
 }
 
+static inline uint32_t reloc_ptr(struct reloc_info *ri, void *ptr)
+{
+    return ri->reloc_base + (uint32_t)((char*)ptr - _start);
+}
+
+static void hijack_interrupt(int intn, uint32_t handler)
+{
+    struct {
+        uint32_t lo;
+        uint32_t hi;
+    } *idt = COM32_IDT;
+
+    idt[intn].lo = (idt[intn].lo & 0xffff0000) | (handler & 0x0000ffff);
+    idt[intn].hi = (idt[intn].hi & 0x0000ffff) | (handler & 0xffff0000);
+}
+
 static void shift_cmdline(struct com32_sys_args *com32)
 {
     char *p;
@@ -32,52 +61,67 @@ static void shift_cmdline(struct com32_sys_args *com32)
     com32->cs_cmdline = p;
 }
 
-static __noreturn reloc_entry(void *ptr, size_t len, uintptr_t entry_esp, uintptr_t module_esp)
+static __noreturn reloc_entry(struct reloc_info *ri)
 {
+    extern char int_handler[];
     size_t stack_frame_size = sizeof(struct com32_sys_args) + 4;
     struct com32_sys_args *com32;
+    uint32_t module_esp;
+
+    hijack_interrupt(X86_INT_DB, reloc_ptr(ri, int_handler));
+    hijack_interrupt(X86_INT_BP, reloc_ptr(ri, int_handler));
 
     /* Copy module to load address */
-    memcpy(COM32_LOAD_ADDR, ptr, len);
+    memcpy(COM32_LOAD_ADDR, ri->data, ri->len);
 
     /* Copy stack frame onto module stack */
-    module_esp = (module_esp - stack_frame_size) & ~15;
-    memcpy((void*)module_esp, (void*)entry_esp, stack_frame_size);
+    module_esp = (ri->reloc_base - stack_frame_size) & ~15;
+    memcpy((void*)module_esp, (void*)ri->old_esp, stack_frame_size);
 
     /* Fix up command line */
-    com32 = (struct com32_sys_args*)((char*)module_esp + 4);
+    com32 = (struct com32_sys_args*)(module_esp + 4);
     shift_cmdline(com32);
 
-    /* Invoke module with stack set up */
+    /* Set up CPU state to run module and enter GDB */
     asm volatile (
             "movl %0, %%esp\n\t"
-            "jmp *%%ecx"
-            : : "r"(module_esp), "c"(COM32_LOAD_ADDR)
+            "pushf\n\t"
+            "pushl %%cs\n\t"
+            "pushl %1\n\t"
+            "jmp *%2\n\t"
+            : : "r"(module_esp),
+                "c"(COM32_LOAD_ADDR),
+                "r"(reloc_ptr(ri, int_handler))
     );
     for(;;); /* shut the compiler up */
 }
 
 static inline __noreturn reloc(void *ptr, size_t len)
 {
-    extern uintptr_t __entry_esp;
-    extern char _start[], _end[];
+    extern uint32_t __entry_esp;
     size_t total_size = _end - _start;
-    __noreturn (*success_fn)(void*, size_t, uintptr_t, uintptr_t);
+    __noreturn (*entry_fn)(struct reloc_info*);
+    struct reloc_info ri;
     uint32_t esp;
     char *dest;
 
     /* Calculate relocation address, preserve current stack */
     asm volatile ("movl %%esp, %0\n\t" : "=m"(esp));
-    dest = (char*)((esp - STACK_RED_ZONE - total_size) & ~3);
+    dest = (char*)((esp - STACK_SIZE - total_size) & ~3);
 
     /* Calculate entry point in relocated code */
-    success_fn = (void*)(dest + ((char*)reloc_entry - _start));
+    entry_fn = (void*)(dest + ((char*)reloc_entry - _start));
 
     /* Copy all sections to relocation address */
     printf("Relocating %d bytes from %p to %p\n", total_size, _start, dest);
     memcpy(dest, _start, total_size);
 
-    success_fn(ptr, len, __entry_esp, (uintptr_t)dest);
+    /* Call into relocated code */
+    ri.data = ptr;
+    ri.len = len;
+    ri.old_esp = __entry_esp;
+    ri.reloc_base = (uint32_t)dest;
+    entry_fn(&ri);
 }
 
 int main(int argc, char *argv[])
@@ -97,6 +141,8 @@ int main(int argc, char *argv[])
         return 1;
     }
 
+    serial_init();
+
     /* No more lib calls after this point */
     reloc(data, data_len);
 }
diff --git a/com32/gdbstub/serial.c b/com32/gdbstub/serial.c
new file mode 100644
index 00000000..1c4d4b6f
--- /dev/null
+++ b/com32/gdbstub/serial.c
@@ -0,0 +1,190 @@
+/*
+ * The serial port interface routines implement a simple polled i/o
+ * interface to a standard serial port.  Due to the space restrictions
+ * for the boot blocks, no BIOS support is used (since BIOS requires
+ * expensive real/protected mode switches), instead the rudimentary
+ * BIOS support is duplicated here.
+ *
+ * The base address and speed for the i/o port are passed from the
+ * Makefile in the COMCONSOLE and CONSPEED preprocessor macros.  The
+ * line control parameters are currently hard-coded to 8 bits, no
+ * parity, 1 stop bit (8N1).  This can be changed in init_serial().
+ */
+
+#include <stddef.h>
+#include <sys/io.h>
+#include "serial.h"
+
+/* Set default values if none specified */
+
+#ifndef COMCONSOLE
+#define COMCONSOLE	0x3f8
+#endif
+
+#ifndef COMSPEED
+#define COMSPEED	9600
+#endif
+
+#ifndef COMDATA
+#define COMDATA		8
+#endif
+
+#ifndef COMPARITY
+#define COMPARITY	0
+#endif
+
+#ifndef COMSTOP
+#define COMSTOP		1
+#endif
+
+#undef UART_BASE
+#define UART_BASE ( COMCONSOLE )
+
+#undef UART_BAUD
+#define UART_BAUD ( COMSPEED )
+
+#if ((115200%UART_BAUD) != 0)
+#error Bad ttys0 baud rate
+#endif
+
+#define COMBRD (115200/UART_BAUD)
+
+/* Line Control Settings */
+#define UART_LCS ( ( ( (COMDATA) - 5 )	<< 0 ) | \
+		   ( ( (COMPARITY) )	<< 3 ) | \
+		   ( ( (COMSTOP) - 1 )	<< 2 ) )
+
+/* Data */
+#define UART_RBR 0x00
+#define UART_TBR 0x00
+
+/* Control */
+#define UART_IER 0x01
+#define UART_IIR 0x02
+#define UART_FCR 0x02
+#define UART_LCR 0x03
+#define UART_MCR 0x04
+#define UART_DLL 0x00
+#define UART_DLM 0x01
+
+/* Status */
+#define UART_LSR 0x05
+#define  UART_LSR_TEMPT 0x40	/* Transmitter empty */
+#define  UART_LSR_THRE  0x20	/* Transmit-hold-register empty */
+#define  UART_LSR_BI	0x10	/* Break interrupt indicator */
+#define  UART_LSR_FE	0x08	/* Frame error indicator */
+#define  UART_LSR_PE	0x04	/* Parity error indicator */
+#define  UART_LSR_OE	0x02	/* Overrun error indicator */
+#define  UART_LSR_DR	0x01	/* Receiver data ready */
+
+#define UART_MSR 0x06
+#define UART_SCR 0x07
+
+#define uart_readb(addr) inb(addr)
+#define uart_writeb(val,addr) outb((val),(addr))
+
+/*
+ * void serial_putc(int ch);
+ *	Write character `ch' to port UART_BASE.
+ */
+void serial_putc ( int ch ) {
+	int status;
+	for (;;) {
+		status = uart_readb(UART_BASE + UART_LSR);
+		if (status & UART_LSR_THRE) { 
+			/* TX buffer emtpy */
+			uart_writeb(ch, UART_BASE + UART_TBR);
+			break;
+		}
+	}
+}
+
+/*
+ * int serial_getc(void);
+ *	Read a character from port UART_BASE.
+ */
+int serial_getc ( void ) {
+	int status;
+	int ch;
+	do {
+		status = uart_readb(UART_BASE + UART_LSR);
+	} while((status & 1) == 0);
+	ch = uart_readb(UART_BASE + UART_RBR);	/* fetch (first) character */
+	ch &= 0x7f;				/* remove any parity bits we get */
+	if (ch == 0x7f) {			/* Make DEL... look like BS */
+		ch = 0x08;
+	}
+	return ch;
+}
+
+/*
+ * int serial_init(void);
+ *	Initialize port UART_BASE to speed COMSPEED, line settings 8N1.
+ */
+void serial_init ( void ) {
+	int status;
+	int divisor, lcs;
+
+	divisor = COMBRD;
+	lcs = UART_LCS;
+
+
+#ifdef COMPRESERVE
+	lcs = uart_readb(UART_BASE + UART_LCR) & 0x7f;
+	uart_writeb(0x80 | lcs, UART_BASE + UART_LCR);
+	divisor = (uart_readb(UART_BASE + UART_DLM) << 8) | uart_readb(UART_BASE + UART_DLL);
+	uart_writeb(lcs, UART_BASE + UART_LCR);
+#endif
+
+	/* Set Baud Rate Divisor to COMSPEED, and test to see if the
+	 * serial port appears to be present.
+	 */
+	uart_writeb(0x80 | lcs, UART_BASE + UART_LCR);
+	uart_writeb(0xaa, UART_BASE + UART_DLL);
+	if (uart_readb(UART_BASE + UART_DLL) != 0xaa) {
+		goto out;
+	}
+	uart_writeb(0x55, UART_BASE + UART_DLL);
+	if (uart_readb(UART_BASE + UART_DLL) != 0x55) {
+		goto out;
+	}
+	uart_writeb(divisor & 0xff, UART_BASE + UART_DLL);
+	if (uart_readb(UART_BASE + UART_DLL) != (divisor & 0xff)) {
+		goto out;
+	}
+	uart_writeb(0xaa, UART_BASE + UART_DLM);
+	if (uart_readb(UART_BASE + UART_DLM) != 0xaa) {
+		goto out;
+	}
+	uart_writeb(0x55, UART_BASE + UART_DLM);
+	if (uart_readb(UART_BASE + UART_DLM) != 0x55) {
+		goto out;
+	}
+	uart_writeb((divisor >> 8) & 0xff, UART_BASE + UART_DLM);
+	if (uart_readb(UART_BASE + UART_DLM) != ((divisor >> 8) & 0xff)) {
+		goto out;
+	}
+	uart_writeb(lcs, UART_BASE + UART_LCR);
+	
+	/* disable interrupts */
+	uart_writeb(0x0, UART_BASE + UART_IER);
+
+	/* disable fifo's */
+	uart_writeb(0x00, UART_BASE + UART_FCR);
+
+	/* Set clear to send, so flow control works... */
+	uart_writeb((1<<1), UART_BASE + UART_MCR);
+
+
+	/* Flush the input buffer. */
+	do {
+		/* rx buffer reg
+		 * throw away (unconditionally the first time)
+		 */
+	        (void) uart_readb(UART_BASE + UART_RBR);
+		/* line status reg */
+		status = uart_readb(UART_BASE + UART_LSR);
+	} while(status & UART_LSR_DR);
+ out:
+	return;
+}
diff --git a/com32/gdbstub/serial.h b/com32/gdbstub/serial.h
new file mode 100644
index 00000000..c78a5757
--- /dev/null
+++ b/com32/gdbstub/serial.h
@@ -0,0 +1,14 @@
+#ifndef _GPXE_SERIAL_H
+#define _GPXE_SERIAL_H
+
+/** @file
+ *
+ * Serial driver functions
+ *
+ */
+
+extern void serial_putc ( int ch );
+extern int serial_getc ( void );
+extern void serial_init ( void );
+
+#endif /* _GPXE_SERIAL_H */