[GDB] Add GDB stub for remote debugging
authorStefan Hajnoczi <stefanha@gmail.com>
Wed, 4 Jun 2008 20:09:59 +0000 (21:09 +0100)
committerMichael Brown <mcb30@etherboot.org>
Wed, 4 Jun 2008 23:45:50 +0000 (00:45 +0100)
See http://etherboot.org/wiki/dev/gdbstub for documentation.

src/arch/i386/core/gdbidt.S [new file with mode: 0644]
src/arch/i386/include/gdbmach.h [new file with mode: 0644]
src/arch/i386/transitions/librm.S
src/config.h
src/core/config.c
src/core/gdbstub.c [new file with mode: 0644]

diff --git a/src/arch/i386/core/gdbidt.S b/src/arch/i386/core/gdbidt.S
new file mode 100644 (file)
index 0000000..45d079f
--- /dev/null
@@ -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 (file)
index 0000000..9f6dc8f
--- /dev/null
@@ -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 */
index b1f9dd5..45e0d0f 100644 (file)
@@ -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
  *
index cec832c..2d0980b 100644 (file)
 #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 */
 
index efb8280..01f709c 100644 (file)
@@ -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 (file)
index 0000000..213887b
--- /dev/null
@@ -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,
+};