diff options
Diffstat (limited to 'com32/gdbstub/gdbstub.c')
| -rw-r--r-- | com32/gdbstub/gdbstub.c | 814 |
1 files changed, 427 insertions, 387 deletions
diff --git a/com32/gdbstub/gdbstub.c b/com32/gdbstub/gdbstub.c index f235a349..bd19addc 100644 --- a/com32/gdbstub/gdbstub.c +++ b/com32/gdbstub/gdbstub.c @@ -30,203 +30,215 @@ 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 */ + 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 ) + 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, + GDBMACH_BPMEM, + GDBMACH_BPHW, + GDBMACH_WATCH, + GDBMACH_RWATCH, + GDBMACH_AWATCH, }; struct gdbstub { - int exit_handler; /* leave interrupt handler */ + int exit_handler; /* leave interrupt handler */ - int signo; - gdbreg_t *regs; + int signo; + gdbreg_t *regs; - void ( * parse ) ( struct gdbstub *stub, char ch ); - uint8_t cksum1; + 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 */ + /* 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; + 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 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_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_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 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 ]; - } +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]; } - 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; + 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 */ - } + /* 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 */ + /* 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; + /* 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_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 ) ); +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 ) ); + /* 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 ) { +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++ ); + 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; +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; + return (ch - 'a' + 0xa) & 0xf; } -static uint8_t gdbstub_to_hex_digit ( uint8_t b ) { - b &= 0xf; - return ( b < 0xa ? '0' : 'a' - 0xa ) + b; +static uint8_t gdbstub_to_hex_digit(uint8_t b) +{ + b &= 0xf; + return (b < 0xa ? '0' : 'a' - 0xa) + b; } /* @@ -234,296 +246,324 @@ static uint8_t gdbstub_to_hex_digit ( uint8_t b ) { * 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_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 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 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; +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_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 ); +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; +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); } - 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 ); + } + if (stop_idx) { + *stop_idx = i; + } + if (argc < nargs) { + args[argc++] = val; + } + return ((i == stub->len || ch == ':') && argc == nargs); } -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_send_errno(struct gdbstub *stub, int errno) +{ + gdbstub_send_num_packet(stub, 'E', errno); } -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_report_signal(struct gdbstub *stub) +{ + gdbstub_send_num_packet(stub, 'S', stub->signo); } -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_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_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_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_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_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; +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_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_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_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; +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 ); + 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 { - /* This retransmit is very aggressive but necessary to keep - * in sync with GDB. */ - gdbstub_tx_packet ( stub ); +static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch) +{ + if (ch == '+') { + stub->parse = gdbstub_state_new; + } else { + /* This retransmit is very aggressive but necessary to keep + * in sync with GDB. */ + gdbstub_tx_packet(stub); } } -void gdbstub_handler ( int signo, gdbreg_t *regs ) { +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() ); + 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(); } |