diff options
Diffstat (limited to 'contrib/syslinux-4.02/com32/gdbstub/gdbstub.c')
-rw-r--r-- | contrib/syslinux-4.02/com32/gdbstub/gdbstub.c | 569 |
1 files changed, 569 insertions, 0 deletions
diff --git a/contrib/syslinux-4.02/com32/gdbstub/gdbstub.c b/contrib/syslinux-4.02/com32/gdbstub/gdbstub.c new file mode 100644 index 0000000..bd19add --- /dev/null +++ b/contrib/syslinux-4.02/com32/gdbstub/gdbstub.c @@ -0,0 +1,569 @@ +/* + * 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 { + /* 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) +{ + 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(); +} |