[GDB] Add GDB stub for remote debugging

See http://etherboot.org/wiki/dev/gdbstub for documentation.

6 files changed, 626 insertions, 11 deletions

diff --git a/src/arch/i386/core/gdbidt.S b/src/arch/i386/core/gdbidt.S
new file mode 100644
index 00000000..45d079f6
--- /dev/null
+++ b/src/arch/i386/core/gdbidt.S
@@ -0,0 +1,209 @@
+/*
+ * Interrupt Descriptor Table (IDT) setup and interrupt handlers for GDB stub.
+ */
+
+#include <virtaddr.h>
+
+#define SIZEOF_I386_REGS	32
+#define SIZEOF_I386_FLAGS	4
+
+/****************************************************************************
+ * Interrupt Descriptor Table
+ ****************************************************************************
+ */
+	.section ".data16"
+	.globl idtr
+idtr:
+idt_limit:
+	.word	idt_length - 1
+idt_base:
+	.long	0
+
+/* IDT entries have the following format:
+ * offset_lo, segment selector, flags, offset_hi
+ *
+ * Since it is not possible to specify relocations in arbitrary
+ * expressions like (int_overflow & 0xffff), we initialise the
+ * IDT with entries in an incorrect format.
+ *
+ * The entries are shuffled into the correct format in init_librm().
+ */
+#define IDT_ENTRY_EMPTY(name) .word 0, 0, 0, 0
+#define IDT_ENTRY_PRESENT(name) \
+	.long	int_##name; \
+	.word	0x8e00, VIRTUAL_CS
+
+.align 16
+idt:
+	IDT_ENTRY_PRESENT(divide_error)
+	IDT_ENTRY_PRESENT(debug_trap)
+	IDT_ENTRY_EMPTY(non_maskable_interrupt)
+	IDT_ENTRY_PRESENT(breakpoint)
+	IDT_ENTRY_PRESENT(overflow)
+	IDT_ENTRY_PRESENT(bound_range_exceeded)
+	IDT_ENTRY_PRESENT(invalid_opcode)
+	IDT_ENTRY_EMPTY(device_not_available)
+	IDT_ENTRY_PRESENT(double_fault)
+	IDT_ENTRY_EMPTY(coprocessor_segment_overrun)
+	IDT_ENTRY_PRESENT(invalid_tss)
+	IDT_ENTRY_PRESENT(segment_not_present)
+	IDT_ENTRY_PRESENT(stack_segment_fault)
+	IDT_ENTRY_PRESENT(general_protection)
+	IDT_ENTRY_PRESENT(page_fault)
+idt_end:
+	.equ	idt_length, idt_end - idt
+
+/* The IDT entries are fixed up (once) in init_librm() */
+idt_fixed:
+	.byte	0
+
+/****************************************************************************
+ * idt_init (real-mode near call, 16-bit real-mode near return address)
+ *
+ * Initialise the IDT, called from init_librm.
+ *
+ * Parameters:
+ *   %eax : IDT base address
+ *
+ * Destroys %ax, %bx, and %di.
+ ****************************************************************************
+ */
+	.section ".text16"
+	.code16
+	.globl idt_init
+idt_init:
+	movl	%eax, idt_base
+	addl	$idt, idt_base
+
+	/* IDT entries are only fixed up once */
+	movb	idt_fixed, %al
+	orb	%al, %al
+	jnz	2f
+	movb	$1, idt_fixed
+
+	/* Shuffle IDT entries into the correct format */
+	movb	$(idt_length / 8), %al
+	movw	$idt, %bx
+	or	%al, %al
+	jz	2f
+1:
+	movw	2(%bx), %di
+	xchg	%di, 6(%bx)
+	movw	%di, 2(%bx)
+	addw	$8, %bx
+	dec	%al
+	jnz	1b
+2:
+	ret
+
+/****************************************************************************
+ * Interrupt handlers
+ ****************************************************************************
+ */
+	.section ".text"
+	.code32
+
+/* 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
+
+int_divide_error:
+	pushl	$SIGFPE
+	jmp	do_interrupt
+
+int_debug_trap:
+int_breakpoint:
+	pushl	$SIGTRAP
+	jmp	do_interrupt
+
+int_overflow:
+int_bound_range_exceeded:
+	pushl	$SIGSTKFLT
+	jmp	do_interrupt
+
+int_invalid_opcode:
+	pushl	$SIGILL
+	jmp	do_interrupt
+
+int_double_fault:
+	movl	$SIGBUS, (%esp)
+	jmp	do_interrupt
+
+int_invalid_tss:
+int_segment_not_present:
+int_stack_segment_fault:
+int_general_protection:
+int_page_fault:
+	movl	$SIGSEGV, (%esp)
+	jmp	do_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 )
+#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_SIGNO ( IH_OFFSET_GDB_END )
+#define IH_OFFSET_OLD_EIP ( IH_OFFSET_SIGNO + 4 )
+#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 )
+do_interrupt:
+	/* Store CPU state in GDB register snapshot */
+	pushl	%gs
+	pushl	%fs
+	pushl	%es
+	pushl	%ds
+	pushl	%ss
+	pushl	IH_OFFSET_FLUX_OLD_CS(%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 */
+	pushl	%esp
+	pushl	(IH_OFFSET_SIGNO + 4)(%esp)
+	call	gdbstub_handler
+	addl	$8, %esp
+
+	/* 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
+
+	addl	$4, %esp /* drop signo */
+	iret
diff --git a/src/arch/i386/include/gdbmach.h b/src/arch/i386/include/gdbmach.h
new file mode 100644
index 00000000..9f6dc8f0
--- /dev/null
+++ b/src/arch/i386/include/gdbmach.h
@@ -0,0 +1,51 @@
+#ifndef GDBMACH_H
+#define GDBMACH_H
+
+/** @file
+ *
+ * GDB architecture specifics
+ *
+ * This file declares functions for manipulating the machine state and
+ * debugging context.
+ *
+ */
+
+typedef uint32_t gdbreg_t;
+
+/* 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 )
+};
+
+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" );
+}
+
+#endif /* GDBMACH_H */
diff --git a/src/arch/i386/transitions/librm.S b/src/arch/i386/transitions/librm.S
index b1f9dd59..45e0d0ff 100644
--- a/src/arch/i386/transitions/librm.S
+++ b/src/arch/i386/transitions/librm.S
@@ -50,6 +50,7 @@
 	.section ".data16"
 	.align 16
 gdt:
+gdtr:		/* The first GDT entry is unused, the GDTR can fit here. */
 gdt_limit:		.word gdt_length - 1
 gdt_base:		.long 0
 			.word 0 /* padding */
@@ -127,7 +128,7 @@ init_librm:
 	addr32 leal	(%eax, %edi), %ebx
 	movl	%ebx, _text16
 
-	/* Store rm_ds and _data16, set up real_ds segment and set GDT base */
+	/* Store rm_ds and _data16, set up real_ds segment */
 	xorl	%eax, %eax
 	movw	%ds, %ax
 	movw	%ax, %cs:rm_ds
@@ -136,9 +137,12 @@ init_librm:
 	call	set_seg_base
 	addr32 leal	(%eax, %edi), %ebx
 	movl	%ebx, _data16
-	addl	$gdt, %eax
+
+	/* Set GDT and IDT base */
 	movl	%eax, gdt_base
-		
+	addl	$gdt, gdt_base
+	call	idt_init
+
 	/* Restore registers */
 	negl	%edi
 	popl	%ebx
@@ -147,14 +151,16 @@ init_librm:
 
 	.section ".text16"
 	.code16
+	.weak idt_init
 set_seg_base:
 1:	movw	%ax, 2(%bx)
 	rorl	$16, %eax
 	movb	%al, 4(%bx)
 	movb	%ah, 7(%bx)
 	roll	$16, %eax
+idt_init: /* Reuse the return opcode here */
 	ret
-	
+
 /****************************************************************************
  * real_to_prot (real-mode near call, 32-bit virtual return address)
  *
@@ -197,7 +203,8 @@ real_to_prot:
 
 	/* Switch to protected mode */
 	cli
-	data32 lgdt	gdt
+	data32 lgdt	gdtr
+	data32 lidt	idtr
 	movl	%cr0, %eax
 	orb	$CR0_PE, %al
 	movl	%eax, %cr0
@@ -232,6 +239,14 @@ real_to_prot:
 	/* Return to virtual address */
 	ret
 
+	/* Default IDTR with no interrupts */
+	.section ".data16"
+	.weak idtr
+idtr:
+rm_idtr:
+	.word 0xffff /* limit */
+	.long 0 /* base */
+
 /****************************************************************************
  * prot_to_real (protected-mode near call, 32-bit real-mode return address)
  *
@@ -300,6 +315,9 @@ p2r_jump_target:
 	movw	%bp, %ss
 	movl	%edx, %esp
 
+	/* Reset IDTR to the real-mode defaults */
+	lidt	rm_idtr
+
 	/* Return to real-mode address */
 	data32 ret
 
@@ -318,7 +336,7 @@ rm_cs:	.word 0
 	.globl rm_ds
 	.section ".text16.data"
 rm_ds:	.word 0
-	
+
 /****************************************************************************
  * prot_call (real-mode far call, 16-bit real-mode far return address)
  *
@@ -354,7 +372,8 @@ rm_ds:	.word 0
  */
 
 #define PC_OFFSET_GDT ( 0 )
-#define PC_OFFSET_IX86 ( PC_OFFSET_GDT + 8 /* pad to 8 to keep alignment */ )
+#define PC_OFFSET_IDT ( PC_OFFSET_GDT + 8 /* pad to 8 to keep alignment */ )
+#define PC_OFFSET_IX86 ( PC_OFFSET_IDT + 8 /* pad to 8 to keep alignment */ )
 #define PC_OFFSET_RETADDR ( PC_OFFSET_IX86 + SIZEOF_I386_ALL_REGS )
 #define PC_OFFSET_FUNCTION ( PC_OFFSET_RETADDR + 4 )
 #define PC_OFFSET_END ( PC_OFFSET_FUNCTION + 4 )
@@ -372,8 +391,9 @@ prot_call:
 	pushw	%ds
 	pushw	%ss
 	pushw	%cs
-	subw	$8, %sp
+	subw	$16, %sp
 	movw	%sp, %bp
+	sidt	8(%bp)
 	sgdt	(%bp)
 
 	/* For sanity's sake, clear the direction flag as soon as possible */
@@ -402,10 +422,11 @@ prot_call:
 	.section ".text16"
 	.code16
 1:	
-	/* Reload GDT, restore registers and flags and return */
+	/* Reload GDT and IDT, restore registers and flags and return */
 	movw	%sp, %bp
 	lgdt	(%bp)
-	addw	$12, %sp /* also skip %cs and %ss */
+	lidt	8(%bp)
+	addw	$20, %sp /* also skip %cs and %ss */
 	popw	%ds
 	popw	%es
 	popw	%fs
@@ -495,7 +516,7 @@ real_call:
 	 */
 	.section ".data16"
 rc_function:	.word 0, 0
-	
+
 /****************************************************************************
  * Stored real-mode and protected-mode stack pointers
  *
diff --git a/src/config.h b/src/config.h
index cec832c3..2d0980b0 100644
--- a/src/config.h
+++ b/src/config.h
@@ -164,6 +164,7 @@
 #undef	BUILD_ID		/* Include a custom build ID string,
 				 * e.g "test-foo" */
 #undef	NULL_TRAP		/* Attempt to catch NULL function calls */
+#undef	GDBSTUB			/* Remote GDB debugging */
 
 /* @END general.h */
 
diff --git a/src/core/config.c b/src/core/config.c
index efb82809..01f709c6 100644
--- a/src/core/config.c
+++ b/src/core/config.c
@@ -198,3 +198,6 @@ REQUIRE_OBJECT ( sanboot_cmd );
 #ifdef NULL_TRAP
 REQUIRE_OBJECT ( nulltrap );
 #endif
+#ifdef GDBSTUB
+REQUIRE_OBJECT ( gdbidt );
+#endif
diff --git a/src/core/gdbstub.c b/src/core/gdbstub.c
new file mode 100644
index 00000000..213887b5
--- /dev/null
+++ b/src/core/gdbstub.c
@@ -0,0 +1,330 @@
+/*
+ * 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 <stddef.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <assert.h>
+#include <gpxe/process.h>
+#include <gpxe/serial.h>
+#include "gdbmach.h"
+
+enum {
+	POSIX_EINVAL = 0x1c /* used to report bad arguments to GDB */
+};
+
+struct gdbstub {
+	int signo;
+	gdbreg_t *regs;
+	int exit_handler; /* leave interrupt handler */
+
+	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 payload [ 256 ];
+	int len;
+};
+
+/* 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 uint8_t gdbstub_from_hex_digit ( char ch ) {
+	return ( isdigit ( ch ) ? ch - '0' : tolower ( ch ) - 'a' + 0xa ) & 0xf;
+}
+
+static uint8_t gdbstub_to_hex_digit ( uint8_t b ) {
+	b &= 0xf;
+	return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
+}
+
+static void gdbstub_from_hex_buf ( char *dst, char *src, int len ) {
+	while ( len-- > 0 ) {
+		*dst = gdbstub_from_hex_digit ( *src++ );
+		if ( len-- > 0 ) {
+			*dst = (*dst << 4) | gdbstub_from_hex_digit ( *src++ );
+		}
+		dst++;
+	}
+}
+
+static void gdbstub_to_hex_buf ( char *dst, char *src, int len ) {
+	while ( len-- > 0 ) {
+		*dst++ = gdbstub_to_hex_digit ( *src >> 4 );
+		*dst++ = gdbstub_to_hex_digit ( *src++ );
+	}
+}
+
+static uint8_t gdbstub_cksum ( char *data, int len ) {
+	uint8_t cksum = 0;
+	while ( len-- > 0 ) {
+		cksum += ( uint8_t ) *data++;
+	}
+	return cksum;
+}
+
+static int gdbstub_getchar ( struct gdbstub *stub ) {
+	if ( stub->exit_handler ) {
+		return -1;
+	}
+	return serial_getc();
+}
+
+static void gdbstub_putchar ( struct gdbstub * stub __unused, char ch ) {
+	serial_putc ( ch );
+}
+
+static void gdbstub_tx_packet ( struct gdbstub *stub ) {
+	uint8_t cksum = gdbstub_cksum ( stub->payload, stub->len );
+	int i;
+
+	gdbstub_putchar ( stub, '$' );
+	for ( i = 0; i < stub->len; i++ ) {
+		gdbstub_putchar ( stub, stub->payload [ i ] );
+	}
+	gdbstub_putchar ( stub, '#' );
+	gdbstub_putchar ( stub, gdbstub_to_hex_digit ( cksum >> 4 ) );
+	gdbstub_putchar ( stub, gdbstub_to_hex_digit ( cksum ) );
+
+	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 ], stub->len );
+	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 stub->payload / 2 ) ? args [ 1 ] : sizeof stub->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 );
+	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, &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_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':
+			gdbstub_continue ( stub, 0 );
+			break;
+		case 's':
+			gdbstub_continue ( stub, 1 );
+			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 < ( int ) sizeof stub->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 ) {
+		gdbstub_putchar ( stub, '+' );
+		if ( stub->len > 0 ) {
+			gdbstub_rx_packet ( stub );
+		}
+	} else {
+		gdbstub_putchar ( stub, '-' );
+	}
+}
+
+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 */
+	}
+}
+
+static void gdbstub_parse ( struct gdbstub *stub, char ch ) {
+	stub->parse ( stub, ch );
+}
+
+static struct gdbstub stub = {
+	.parse = gdbstub_state_new
+};
+
+__cdecl void gdbstub_handler ( int signo, gdbreg_t *regs ) {
+	int ch;
+	stub.signo = signo;
+	stub.regs = regs;
+	stub.exit_handler = 0;
+	gdbstub_report_signal ( &stub );
+	while ( ( ch = gdbstub_getchar( &stub ) ) != -1 ) {
+		gdbstub_parse ( &stub, ch );
+	}
+}
+
+/* Activity monitor to detect packets from GDB when we are not active */
+static void gdbstub_activity_step ( struct process *process __unused ) {
+	if ( serial_ischar() ) {
+		gdbmach_breakpoint();
+	}
+}
+
+struct process gdbstub_activity_process __permanent_process = {
+	.step = gdbstub_activity_step,
+};