source: bootcd/isolinux/syslinux-6.03/com32/gdbstub/gdbstub.c @ dd1be7c

/*
 * 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();
}