com32/gdbstub/gdbstub.c

   1 /*
   2  * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License as
   6  * published by the Free Software Foundation; either version 2 of the
   7  * License, or any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software
  16  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17  */
  18
  19 /**
  20  * @file
  21  *
  22  * GDB stub for remote debugging
  23  *
  24  */
  25
  26 #include <stdlib.h>
  27 #include <stdint.h>
  28 #include "serial.h"
  29
  30 typedef uint32_t gdbreg_t;
  31
  32 enum {
  33     POSIX_EINVAL = 0x1c,        /* used to report bad arguments to GDB */
  34     SIZEOF_PAYLOAD = 256,       /* buffer size of GDB payload data */
  35     DR7_CLEAR = 0x00000400,     /* disable hardware breakpoints */
  36     DR6_CLEAR = 0xffff0ff0,     /* clear breakpoint status */
  37 };
  38
  39 /* The register snapshot, this must be in sync with interrupt handler and the
  40  * GDB protocol. */
  41 enum {
  42     GDBMACH_EAX,
  43     GDBMACH_ECX,
  44     GDBMACH_EDX,
  45     GDBMACH_EBX,
  46     GDBMACH_ESP,
  47     GDBMACH_EBP,
  48     GDBMACH_ESI,
  49     GDBMACH_EDI,
  50     GDBMACH_EIP,
  51     GDBMACH_EFLAGS,
  52     GDBMACH_CS,
  53     GDBMACH_SS,
  54     GDBMACH_DS,
  55     GDBMACH_ES,
  56     GDBMACH_FS,
  57     GDBMACH_GS,
  58     GDBMACH_NREGS,
  59     GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof(gdbreg_t)
  60 };
  61
  62 /* Breakpoint types */
  63 enum {
  64     GDBMACH_BPMEM,
  65     GDBMACH_BPHW,
  66     GDBMACH_WATCH,
  67     GDBMACH_RWATCH,
  68     GDBMACH_AWATCH,
  69 };
  70
  71 struct gdbstub {
  72     int exit_handler;           /* leave interrupt handler */
  73
  74     int signo;
  75     gdbreg_t *regs;
  76
  77     void (*parse) (struct gdbstub * stub, char ch);
  78     uint8_t cksum1;
  79
  80     /* Buffer for payload data when parsing a packet.  Once the
  81      * packet has been received, this buffer is used to hold
  82      * the reply payload. */
  83     char buf[SIZEOF_PAYLOAD + 4];       /* $...PAYLOAD...#XX */
  84     char *payload;              /* start of payload */
  85     int len;                    /* length of payload */
  86 };
  87
  88 /** Hardware breakpoint, fields stored in x86 bit pattern form */
  89 struct hwbp {
  90     int type;                   /* type (1=write watchpoint, 3=access watchpoint) */
  91     unsigned long addr;         /* linear address */
  92     size_t len;                 /* length (0=1-byte, 1=2-byte, 3=4-byte) */
  93     int enabled;
  94 };
  95
  96 static struct hwbp hwbps[4];
  97 static gdbreg_t dr7 = DR7_CLEAR;
  98
  99 static inline void gdbmach_set_pc(gdbreg_t * regs, gdbreg_t pc)
 100 {
 101     regs[GDBMACH_EIP] = pc;
 102 }
 103
 104 static inline void gdbmach_set_single_step(gdbreg_t * regs, int step)
 105 {
 106     regs[GDBMACH_EFLAGS] &= ~(1 << 8);  /* Trace Flag (TF) */
 107     regs[GDBMACH_EFLAGS] |= (step << 8);
 108 }
 109
 110 static inline void gdbmach_breakpoint(void)
 111 {
 112     __asm__ __volatile__("int $3\n");
 113 }
 114
 115 static struct hwbp *gdbmach_find_hwbp(int type, unsigned long addr, size_t len)
 116 {
 117     struct hwbp *available = NULL;
 118     unsigned int i;
 119     for (i = 0; i < sizeof hwbps / sizeof hwbps[0]; i++) {
 120         if (hwbps[i].type == type && hwbps[i].addr == addr
 121             && hwbps[i].len == len) {
 122             return &hwbps[i];
 123         }
 124         if (!hwbps[i].enabled) {
 125             available = &hwbps[i];
 126         }
 127     }
 128     return available;
 129 }
 130
 131 static void gdbmach_commit_hwbp(struct hwbp *bp)
 132 {
 133     int regnum = bp - hwbps;
 134
 135     /* Set breakpoint address */
 136     switch (regnum) {
 137     case 0:
 138 __asm__ __volatile__("movl %0, %%dr0\n": :"r"(bp->addr));
 139         break;
 140     case 1:
 141 __asm__ __volatile__("movl %0, %%dr1\n": :"r"(bp->addr));
 142         break;
 143     case 2:
 144 __asm__ __volatile__("movl %0, %%dr2\n": :"r"(bp->addr));
 145         break;
 146     case 3:
 147 __asm__ __volatile__("movl %0, %%dr3\n": :"r"(bp->addr));
 148         break;
 149     }
 150
 151     /* Set type */
 152     dr7 &= ~(0x3 << (16 + 4 * regnum));
 153     dr7 |= bp->type << (16 + 4 * regnum);
 154
 155     /* Set length */
 156     dr7 &= ~(0x3 << (18 + 4 * regnum));
 157     dr7 |= bp->len << (18 + 4 * regnum);
 158
 159     /* Set/clear local enable bit */
 160     dr7 &= ~(0x3 << 2 * regnum);
 161     dr7 |= bp->enabled << 2 * regnum;
 162 }
 163
 164 int gdbmach_set_breakpoint(int type, unsigned long addr, size_t len, int enable)
 165 {
 166     struct hwbp *bp;
 167
 168     /* Check and convert breakpoint type to x86 type */
 169     switch (type) {
 170     case GDBMACH_WATCH:
 171         type = 0x1;
 172         break;
 173     case GDBMACH_AWATCH:
 174         type = 0x3;
 175         break;
 176     default:
 177         return 0;               /* unsupported breakpoint type */
 178     }
 179
 180     /* Only lengths 1, 2, and 4 are supported */
 181     if (len != 2 && len != 4) {
 182         len = 1;
 183     }
 184     len--;                      /* convert to x86 breakpoint length bit pattern */
 185
 186     /* Set up the breakpoint */
 187     bp = gdbmach_find_hwbp(type, addr, len);
 188     if (!bp) {
 189         return 0;               /* ran out of hardware breakpoints */
 190     }
 191     bp->type = type;
 192     bp->addr = addr;
 193     bp->len = len;
 194     bp->enabled = enable;
 195     gdbmach_commit_hwbp(bp);
 196     return 1;
 197 }
 198
 199 static void gdbmach_disable_hwbps(void)
 200 {
 201     /* Store and clear hardware breakpoints */
 202     __asm__ __volatile__("movl %0, %%dr7\n"::"r"(DR7_CLEAR));
 203 }
 204
 205 static void gdbmach_enable_hwbps(void)
 206 {
 207     /* Clear breakpoint status register */
 208     __asm__ __volatile__("movl %0, %%dr6\n"::"r"(DR6_CLEAR));
 209
 210     /* Restore hardware breakpoints */
 211     __asm__ __volatile__("movl %0, %%dr7\n"::"r"(dr7));
 212 }
 213
 214 /* Packet parser states */
 215 static void gdbstub_state_new(struct gdbstub *stub, char ch);
 216 static void gdbstub_state_data(struct gdbstub *stub, char ch);
 217 static void gdbstub_state_cksum1(struct gdbstub *stub, char ch);
 218 static void gdbstub_state_cksum2(struct gdbstub *stub, char ch);
 219 static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch);
 220
 221 static void serial_write(void *buf, size_t len)
 222 {
 223     char *p = buf;
 224     while (len-- > 0)
 225         serial_putc(*p++);
 226 }
 227
 228 static uint8_t gdbstub_from_hex_digit(char ch)
 229 {
 230     if (ch >= '0' && ch <= '9')
 231         return ch - '0';
 232     else if (ch >= 'A' && ch <= 'F')
 233         return ch - 'A' + 0xa;
 234     else
 235         return (ch - 'a' + 0xa) & 0xf;
 236 }
 237
 238 static uint8_t gdbstub_to_hex_digit(uint8_t b)
 239 {
 240     b &= 0xf;
 241     return (b < 0xa ? '0' : 'a' - 0xa) + b;
 242 }
 243
 244 /*
 245  * To make reading/writing device memory atomic, we check for
 246  * 2- or 4-byte aligned operations and handle them specially.
 247  */
 248
 249 static void gdbstub_from_hex_buf(char *dst, char *src, int lenbytes)
 250 {
 251     if (lenbytes == 2 && ((unsigned long)dst & 0x1) == 0) {
 252         uint16_t i = gdbstub_from_hex_digit(src[2]) << 12 |
 253             gdbstub_from_hex_digit(src[3]) << 8 |
 254             gdbstub_from_hex_digit(src[0]) << 4 |
 255             gdbstub_from_hex_digit(src[1]);
 256         *(uint16_t *) dst = i;
 257     } else if (lenbytes == 4 && ((unsigned long)dst & 0x3) == 0) {
 258         uint32_t i = gdbstub_from_hex_digit(src[6]) << 28 |
 259             gdbstub_from_hex_digit(src[7]) << 24 |
 260             gdbstub_from_hex_digit(src[4]) << 20 |
 261             gdbstub_from_hex_digit(src[5]) << 16 |
 262             gdbstub_from_hex_digit(src[2]) << 12 |
 263             gdbstub_from_hex_digit(src[3]) << 8 |
 264             gdbstub_from_hex_digit(src[0]) << 4 |
 265             gdbstub_from_hex_digit(src[1]);
 266         *(uint32_t *) dst = i;
 267     } else {
 268         while (lenbytes-- > 0) {
 269             *dst++ = gdbstub_from_hex_digit(src[0]) << 4 |
 270                 gdbstub_from_hex_digit(src[1]);
 271             src += 2;
 272         }
 273     }
 274 }
 275
 276 static void gdbstub_to_hex_buf(char *dst, char *src, int lenbytes)
 277 {
 278     if (lenbytes == 2 && ((unsigned long)src & 0x1) == 0) {
 279         uint16_t i = *(uint16_t *) src;
 280         dst[0] = gdbstub_to_hex_digit(i >> 4);
 281         dst[1] = gdbstub_to_hex_digit(i);
 282         dst[2] = gdbstub_to_hex_digit(i >> 12);
 283         dst[3] = gdbstub_to_hex_digit(i >> 8);
 284     } else if (lenbytes == 4 && ((unsigned long)src & 0x3) == 0) {
 285         uint32_t i = *(uint32_t *) src;
 286         dst[0] = gdbstub_to_hex_digit(i >> 4);
 287         dst[1] = gdbstub_to_hex_digit(i);
 288         dst[2] = gdbstub_to_hex_digit(i >> 12);
 289         dst[3] = gdbstub_to_hex_digit(i >> 8);
 290         dst[4] = gdbstub_to_hex_digit(i >> 20);
 291         dst[5] = gdbstub_to_hex_digit(i >> 16);
 292         dst[6] = gdbstub_to_hex_digit(i >> 28);
 293         dst[7] = gdbstub_to_hex_digit(i >> 24);
 294     } else {
 295         while (lenbytes-- > 0) {
 296             *dst++ = gdbstub_to_hex_digit(*src >> 4);
 297             *dst++ = gdbstub_to_hex_digit(*src);
 298             src++;
 299         }
 300     }
 301 }
 302
 303 static uint8_t gdbstub_cksum(char *data, int len)
 304 {
 305     uint8_t cksum = 0;
 306     while (len-- > 0) {
 307         cksum += (uint8_t) * data++;
 308     }
 309     return cksum;
 310 }
 311
 312 static void gdbstub_tx_packet(struct gdbstub *stub)
 313 {
 314     uint8_t cksum = gdbstub_cksum(stub->payload, stub->len);
 315     stub->buf[0] = '$';
 316     stub->buf[stub->len + 1] = '#';
 317     stub->buf[stub->len + 2] = gdbstub_to_hex_digit(cksum >> 4);
 318     stub->buf[stub->len + 3] = gdbstub_to_hex_digit(cksum);
 319     serial_write(stub->buf, stub->len + 4);
 320     stub->parse = gdbstub_state_wait_ack;
 321 }
 322
 323 /* GDB commands */
 324 static void gdbstub_send_ok(struct gdbstub *stub)
 325 {
 326     stub->payload[0] = 'O';
 327     stub->payload[1] = 'K';
 328     stub->len = 2;
 329     gdbstub_tx_packet(stub);
 330 }
 331
 332 static void gdbstub_send_num_packet(struct gdbstub *stub, char reply, int num)
 333 {
 334     stub->payload[0] = reply;
 335     stub->payload[1] = gdbstub_to_hex_digit((char)num >> 4);
 336     stub->payload[2] = gdbstub_to_hex_digit((char)num);
 337     stub->len = 3;
 338     gdbstub_tx_packet(stub);
 339 }
 340
 341 /* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
 342 static int gdbstub_get_packet_args(struct gdbstub *stub, unsigned long *args,
 343                                    int nargs, int *stop_idx)
 344 {
 345     int i;
 346     char ch = 0;
 347     int argc = 0;
 348     unsigned long val = 0;
 349     for (i = 1; i < stub->len && argc < nargs; i++) {
 350         ch = stub->payload[i];
 351         if (ch == ':') {
 352             break;
 353         } else if (ch == ',') {
 354             args[argc++] = val;
 355             val = 0;
 356         } else {
 357             val = (val << 4) | gdbstub_from_hex_digit(ch);
 358         }
 359     }
 360     if (stop_idx) {
 361         *stop_idx = i;
 362     }
 363     if (argc < nargs) {
 364         args[argc++] = val;
 365     }
 366     return ((i == stub->len || ch == ':') && argc == nargs);
 367 }
 368
 369 static void gdbstub_send_errno(struct gdbstub *stub, int errno)
 370 {
 371     gdbstub_send_num_packet(stub, 'E', errno);
 372 }
 373
 374 static void gdbstub_report_signal(struct gdbstub *stub)
 375 {
 376     gdbstub_send_num_packet(stub, 'S', stub->signo);
 377 }
 378
 379 static void gdbstub_read_regs(struct gdbstub *stub)
 380 {
 381     gdbstub_to_hex_buf(stub->payload, (char *)stub->regs, GDBMACH_SIZEOF_REGS);
 382     stub->len = GDBMACH_SIZEOF_REGS * 2;
 383     gdbstub_tx_packet(stub);
 384 }
 385
 386 static void gdbstub_write_regs(struct gdbstub *stub)
 387 {
 388     if (stub->len != 1 + GDBMACH_SIZEOF_REGS * 2) {
 389         gdbstub_send_errno(stub, POSIX_EINVAL);
 390         return;
 391     }
 392     gdbstub_from_hex_buf((char *)stub->regs, &stub->payload[1],
 393                          GDBMACH_SIZEOF_REGS);
 394     gdbstub_send_ok(stub);
 395 }
 396
 397 static void gdbstub_read_mem(struct gdbstub *stub)
 398 {
 399     unsigned long args[2];
 400     if (!gdbstub_get_packet_args
 401         (stub, args, sizeof args / sizeof args[0], NULL)) {
 402         gdbstub_send_errno(stub, POSIX_EINVAL);
 403         return;
 404     }
 405     args[1] = (args[1] < SIZEOF_PAYLOAD / 2) ? args[1] : SIZEOF_PAYLOAD / 2;
 406     gdbstub_to_hex_buf(stub->payload, (char *)args[0], args[1]);
 407     stub->len = args[1] * 2;
 408     gdbstub_tx_packet(stub);
 409 }
 410
 411 static void gdbstub_write_mem(struct gdbstub *stub)
 412 {
 413     unsigned long args[2];
 414     int colon;
 415     if (!gdbstub_get_packet_args
 416         (stub, args, sizeof args / sizeof args[0], &colon) || colon >= stub->len
 417         || stub->payload[colon] != ':' || (stub->len - colon - 1) % 2 != 0) {
 418         gdbstub_send_errno(stub, POSIX_EINVAL);
 419         return;
 420     }
 421     gdbstub_from_hex_buf((char *)args[0], &stub->payload[colon + 1],
 422                          (stub->len - colon - 1) / 2);
 423     gdbstub_send_ok(stub);
 424 }
 425
 426 static void gdbstub_continue(struct gdbstub *stub, int single_step)
 427 {
 428     gdbreg_t pc;
 429     if (stub->len > 1
 430         && gdbstub_get_packet_args(stub, (unsigned long *)&pc, 1, NULL)) {
 431         gdbmach_set_pc(stub->regs, pc);
 432     }
 433     gdbmach_set_single_step(stub->regs, single_step);
 434     stub->exit_handler = 1;
 435     /* Reply will be sent when we hit the next breakpoint or interrupt */
 436 }
 437
 438 static void gdbstub_breakpoint(struct gdbstub *stub)
 439 {
 440     unsigned long args[3];
 441     int enable = stub->payload[0] == 'Z' ? 1 : 0;
 442     if (!gdbstub_get_packet_args
 443         (stub, args, sizeof args / sizeof args[0], NULL)) {
 444         gdbstub_send_errno(stub, POSIX_EINVAL);
 445         return;
 446     }
 447     if (gdbmach_set_breakpoint(args[0], args[1], args[2], enable)) {
 448         gdbstub_send_ok(stub);
 449     } else {
 450         /* Not supported */
 451         stub->len = 0;
 452         gdbstub_tx_packet(stub);
 453     }
 454 }
 455
 456 static void gdbstub_rx_packet(struct gdbstub *stub)
 457 {
 458     switch (stub->payload[0]) {
 459     case '?':
 460         gdbstub_report_signal(stub);
 461         break;
 462     case 'g':
 463         gdbstub_read_regs(stub);
 464         break;
 465     case 'G':
 466         gdbstub_write_regs(stub);
 467         break;
 468     case 'm':
 469         gdbstub_read_mem(stub);
 470         break;
 471     case 'M':
 472         gdbstub_write_mem(stub);
 473         break;
 474     case 'c':                   /* Continue */
 475     case 'k':                   /* Kill */
 476     case 's':                   /* Step */
 477     case 'D':                   /* Detach */
 478         gdbstub_continue(stub, stub->payload[0] == 's');
 479         if (stub->payload[0] == 'D') {
 480             gdbstub_send_ok(stub);
 481         }
 482         break;
 483     case 'Z':                   /* Insert breakpoint */
 484     case 'z':                   /* Remove breakpoint */
 485         gdbstub_breakpoint(stub);
 486         break;
 487     default:
 488         stub->len = 0;
 489         gdbstub_tx_packet(stub);
 490         break;
 491     }
 492 }
 493
 494 /* GDB packet parser */
 495 static void gdbstub_state_new(struct gdbstub *stub, char ch)
 496 {
 497     if (ch == '$') {
 498         stub->len = 0;
 499         stub->parse = gdbstub_state_data;
 500     }
 501 }
 502
 503 static void gdbstub_state_data(struct gdbstub *stub, char ch)
 504 {
 505     if (ch == '#') {
 506         stub->parse = gdbstub_state_cksum1;
 507     } else if (ch == '$') {
 508         stub->len = 0;          /* retry new packet */
 509     } else {
 510         /* If the length exceeds our buffer, let the checksum fail */
 511         if (stub->len < SIZEOF_PAYLOAD) {
 512             stub->payload[stub->len++] = ch;
 513         }
 514     }
 515 }
 516
 517 static void gdbstub_state_cksum1(struct gdbstub *stub, char ch)
 518 {
 519     stub->cksum1 = gdbstub_from_hex_digit(ch) << 4;
 520     stub->parse = gdbstub_state_cksum2;
 521 }
 522
 523 static void gdbstub_state_cksum2(struct gdbstub *stub, char ch)
 524 {
 525     uint8_t their_cksum;
 526     uint8_t our_cksum;
 527
 528     stub->parse = gdbstub_state_new;
 529     their_cksum = stub->cksum1 + gdbstub_from_hex_digit(ch);
 530     our_cksum = gdbstub_cksum(stub->payload, stub->len);
 531
 532     if (their_cksum == our_cksum) {
 533         serial_write("+", 1);
 534         if (stub->len > 0) {
 535             gdbstub_rx_packet(stub);
 536         }
 537     } else {
 538         serial_write("-", 1);
 539     }
 540 }
 541
 542 static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch)
 543 {
 544     if (ch == '+') {
 545         stub->parse = gdbstub_state_new;
 546     } else {
 547         /* This retransmit is very aggressive but necessary to keep
 548          * in sync with GDB. */
 549         gdbstub_tx_packet(stub);
 550     }
 551 }
 552
 553 void gdbstub_handler(int signo, gdbreg_t * regs)
 554 {
 555     struct gdbstub stub;
 556
 557     gdbmach_disable_hwbps();
 558
 559     stub.parse = gdbstub_state_new;
 560     stub.payload = &stub.buf[1];
 561     stub.signo = signo;
 562     stub.regs = regs;
 563     stub.exit_handler = 0;
 564     gdbstub_report_signal(&stub);
 565     while (!stub.exit_handler)
 566         stub.parse(&stub, serial_getc());
 567
 568     gdbmach_enable_hwbps();
 569 }