lunaix-os/kernel/debug/gdbstub.c

   1 /**
   2  * @file gdbstub.h
   3  * @author Lunaixsky ()
   4  * @brief GDB Stub implementation (https://github.com/mborgerson/gdbstub).
   5  *          Ported for LunaixOS
   6  * @version 0.1
   7  * @date 2022-10-18
   8  *
   9  * @copyright Copyright (c) 2022 Lunaixsky & (See license below)
  10  *
  11  */
  12
  13 /*
  14  * Copyright (c) 2016-2022 Matt Borgerson
  15  *
  16  * Permission is hereby granted, free of charge, to any person obtaining a copy
  17  * of this software and associated documentation files (the "Software"), to deal
  18  * in the Software without restriction, including without limitation the rights
  19  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  20  * copies of the Software, and to permit persons to whom the Software is
  21  * furnished to do so, subject to the following conditions:
  22  *
  23  * The above copyright notice and this permission notice shall be included in
  24  * all copies or substantial portions of the Software.
  25  *
  26  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  27  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  28  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  29  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  30  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  31  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  32  * SOFTWARE.
  33  */
  34
  35 #include <hal/serial.h>
  36 #include <klibc/string.h>
  37 #include <sdbg/gdbstub.h>
  38 #include <sys/port_io.h>
  39
  40 /*****************************************************************************
  41  * Types
  42  ****************************************************************************/
  43
  44 #ifndef GDBSTUB_DONT_DEFINE_STDINT_TYPES
  45 typedef unsigned char u8_t;
  46 typedef unsigned short u16_t;
  47 typedef unsigned long uint32_t;
  48 #endif
  49
  50 typedef unsigned int address;
  51 typedef unsigned int reg;
  52
  53 enum GDB_REGISTER
  54 {
  55     GDB_CPU_I386_REG_EAX = 0,
  56     GDB_CPU_I386_REG_ECX = 1,
  57     GDB_CPU_I386_REG_EDX = 2,
  58     GDB_CPU_I386_REG_EBX = 3,
  59     GDB_CPU_I386_REG_ESP = 4,
  60     GDB_CPU_I386_REG_EBP = 5,
  61     GDB_CPU_I386_REG_ESI = 6,
  62     GDB_CPU_I386_REG_EDI = 7,
  63     GDB_CPU_I386_REG_PC = 8,
  64     GDB_CPU_I386_REG_PS = 9,
  65     GDB_CPU_I386_REG_CS = 10,
  66     GDB_CPU_I386_REG_SS = 11,
  67     GDB_CPU_I386_REG_DS = 12,
  68     GDB_CPU_I386_REG_ES = 13,
  69     GDB_CPU_I386_REG_FS = 14,
  70     GDB_CPU_I386_REG_GS = 15,
  71     GDB_CPU_NUM_REGISTERS = 16
  72 };
  73
  74 struct gdb_state
  75 {
  76     int signum;
  77     struct serial_dev* sdev;
  78     reg registers[GDB_CPU_NUM_REGISTERS];
  79 };
  80
  81 /*****************************************************************************
  82  *
  83  *  GDB Remote Serial Protocol
  84  *
  85  ****************************************************************************/
  86
  87 /*****************************************************************************
  88  * Macros
  89  ****************************************************************************/
  90
  91 #define GDB_PRINT(...)
  92
  93 #define COM_PORT SERIAL_COM1
  94
  95 #define GDB_EOF (-1)
  96
  97 #ifndef NULL
  98 #define NULL ((void*)0)
  99 #endif
 100
 101 #ifndef GDB_ASSERT
 102 #define GDB_ASSERT(x)                                                          \
 103     do {                                                                       \
 104     } while (0)
 105 #endif
 106
 107 /*****************************************************************************
 108  * Prototypes
 109  ****************************************************************************/
 110
 111 int
 112 gdb_main(struct gdb_state* state);
 113
 114 /* System functions, supported by all stubs */
 115 void
 116 gdb_sys_init(void);
 117 int
 118 gdb_sys_getc(struct gdb_state* state);
 119 int
 120 gdb_sys_putchar(struct gdb_state* state, int ch);
 121 int
 122 gdb_sys_mem_readb(struct gdb_state* state, address addr, char* val);
 123 int
 124 gdb_sys_mem_writeb(struct gdb_state* state, address addr, char val);
 125 int
 126 gdb_sys_continue(struct gdb_state* state);
 127 int
 128 gdb_sys_step(struct gdb_state* state);
 129
 130 /*****************************************************************************
 131  * Types
 132  ****************************************************************************/
 133
 134 typedef int (*gdb_enc_func)(char* buf,
 135                             unsigned int buf_len,
 136                             const char* data,
 137                             unsigned int data_len);
 138 typedef int (*gdb_dec_func)(const char* buf,
 139                             unsigned int buf_len,
 140                             char* data,
 141                             unsigned int data_len);
 142
 143 /*****************************************************************************
 144  * Const Data
 145  ****************************************************************************/
 146
 147 static const char digits[] = "0123456789abcdef";
 148
 149 /*****************************************************************************
 150  * Prototypes
 151  ****************************************************************************/
 152
 153 /* Communication functions */
 154 static int
 155 gdb_write(struct gdb_state* state, const char* buf, unsigned int len);
 156 static int
 157 gdb_read(struct gdb_state* state,
 158          char* buf,
 159          unsigned int buf_len,
 160          unsigned int len);
 161
 162 /* String processing helper functions */
 163 static int
 164 gdb_strlen(const char* ch);
 165 static char
 166 gdb_get_digit(int val);
 167 static int
 168 gdb_get_val(char digit, int base);
 169 static int
 170 gdb_strtol(const char* str, unsigned int len, int base, const char** endptr);
 171
 172 /* Packet functions */
 173 static int
 174 gdb_send_packet(struct gdb_state* state, const char* pkt, unsigned int pkt_len);
 175 static int
 176 gdb_recv_packet(struct gdb_state* state,
 177                 char* pkt_buf,
 178                 unsigned int pkt_buf_len,
 179                 unsigned int* pkt_len);
 180 static int
 181 gdb_checksum(const char* buf, unsigned int len);
 182 static int
 183 gdb_recv_ack(struct gdb_state* state);
 184
 185 /* Data encoding/decoding */
 186 static int
 187 gdb_enc_hex(char* buf,
 188             unsigned int buf_len,
 189             const char* data,
 190             unsigned int data_len);
 191 static int
 192 gdb_dec_hex(const char* buf,
 193             unsigned int buf_len,
 194             char* data,
 195             unsigned int data_len);
 196 static int
 197 gdb_enc_bin(char* buf,
 198             unsigned int buf_len,
 199             const char* data,
 200             unsigned int data_len);
 201 static int
 202 gdb_dec_bin(const char* buf,
 203             unsigned int buf_len,
 204             char* data,
 205             unsigned int data_len);
 206
 207 /* Packet creation helpers */
 208 static int
 209 gdb_send_ok_packet(struct gdb_state* state, char* buf, unsigned int buf_len);
 210 static int
 211 gdb_send_conmsg_packet(struct gdb_state* state,
 212                        char* buf,
 213                        unsigned int buf_len,
 214                        const char* msg);
 215 static int
 216 gdb_send_signal_packet(struct gdb_state* state,
 217                        char* buf,
 218                        unsigned int buf_len,
 219                        char signal);
 220 static int
 221 gdb_send_error_packet(struct gdb_state* state,
 222                       char* buf,
 223                       unsigned int buf_len,
 224                       char error);
 225
 226 /* Command functions */
 227 static int
 228 gdb_mem_read(struct gdb_state* state,
 229              char* buf,
 230              unsigned int buf_len,
 231              address addr,
 232              unsigned int len,
 233              gdb_enc_func enc);
 234 static int
 235 gdb_mem_write(struct gdb_state* state,
 236               const char* buf,
 237               unsigned int buf_len,
 238               address addr,
 239               unsigned int len,
 240               gdb_dec_func dec);
 241 static int
 242 gdb_continue(struct gdb_state* state);
 243 static int
 244 gdb_step(struct gdb_state* state);
 245
 246 /*****************************************************************************
 247  * String Processing Helper Functions
 248  ****************************************************************************/
 249
 250 /*
 251  * Get null-terminated string length.
 252  */
 253 static int
 254 gdb_strlen(const char* ch)
 255 {
 256     int len;
 257
 258     len = 0;
 259     while (*ch++) {
 260         len += 1;
 261     }
 262
 263     return len;
 264 }
 265
 266 /*
 267  * Get integer value for a string representation.
 268  *
 269  * If the string starts with + or -, it will be signed accordingly.
 270  *
 271  * If base == 0, the base will be determined:
 272  *   base 16 if the string starts with 0x or 0X,
 273  *   base 10 otherwise
 274  *
 275  * If endptr is specified, it will point to the last non-digit in the
 276  * string. If there are no digits in the string, it will be set to NULL.
 277  */
 278 static int
 279 gdb_strtol(const char* str, unsigned int len, int base, const char** endptr)
 280 {
 281     unsigned int pos;
 282     int sign, tmp, value, valid;
 283
 284     value = 0;
 285     pos = 0;
 286     sign = 1;
 287     valid = 0;
 288
 289     if (endptr) {
 290         *(endptr) = 0;
 291     }
 292
 293     if (len < 1) {
 294         return 0;
 295     }
 296
 297     /* Detect negative numbers */
 298     if (str[pos] == '-') {
 299         sign = -1;
 300         pos += 1;
 301     } else if (str[pos] == '+') {
 302         sign = 1;
 303         pos += 1;
 304     }
 305
 306     /* Detect '0x' hex prefix */
 307     if ((pos + 2 < len) && (str[pos] == '0') &&
 308         ((str[pos + 1] == 'x') || (str[pos + 1] == 'X'))) {
 309         base = 16;
 310         pos += 2;
 311     }
 312
 313     if (base == 0) {
 314         base = 10;
 315     }
 316
 317     for (; (pos < len) && (str[pos] != '\x00'); pos++) {
 318         tmp = gdb_get_val(str[pos], base);
 319         if (tmp == GDB_EOF) {
 320             break;
 321         }
 322
 323         value = value * base + tmp;
 324         valid = 1; /* At least one digit is valid */
 325     }
 326
 327     if (!valid) {
 328         return 0;
 329     }
 330
 331     if (endptr) {
 332         *endptr = str + pos;
 333     }
 334
 335     value *= sign;
 336
 337     return value;
 338 }
 339
 340 /*
 341  * Get the corresponding ASCII hex digit character for a value.
 342  */
 343 static char
 344 gdb_get_digit(int val)
 345 {
 346     if ((val >= 0) && (val <= 0xf)) {
 347         return digits[val];
 348     } else {
 349         return GDB_EOF;
 350     }
 351 }
 352
 353 /*
 354  * Get the corresponding value for a ASCII digit character.
 355  *
 356  * Supports bases 2-16.
 357  */
 358 static int
 359 gdb_get_val(char digit, int base)
 360 {
 361     int value;
 362
 363     if ((digit >= '0') && (digit <= '9')) {
 364         value = digit - '0';
 365     } else if ((digit >= 'a') && (digit <= 'f')) {
 366         value = digit - 'a' + 0xa;
 367     } else if ((digit >= 'A') && (digit <= 'F')) {
 368         value = digit - 'A' + 0xa;
 369     } else {
 370         return GDB_EOF;
 371     }
 372
 373     return (value < base) ? value : GDB_EOF;
 374 }
 375
 376 /*****************************************************************************
 377  * Packet Functions
 378  ****************************************************************************/
 379
 380 /*
 381  * Receive a packet acknowledgment
 382  *
 383  * Returns:
 384  *    0   if an ACK (+) was received
 385  *    1   if a NACK (-) was received
 386  *    GDB_EOF otherwise
 387  */
 388 static int
 389 gdb_recv_ack(struct gdb_state* state)
 390 {
 391     int response;
 392
 393     /* Wait for packet ack */
 394     switch (response = gdb_sys_getc(state)) {
 395         case '+':
 396             /* Packet acknowledged */
 397             return 0;
 398         case '-':
 399             /* Packet negative acknowledged */
 400             return 1;
 401         default:
 402             /* Bad response! */
 403             GDB_PRINT("received bad packet response: 0x%2x\n", response);
 404             return GDB_EOF;
 405     }
 406 }
 407
 408 /*
 409  * Calculate 8-bit checksum of a buffer.
 410  *
 411  * Returns:
 412  *    8-bit checksum.
 413  */
 414 static int
 415 gdb_checksum(const char* buf, unsigned int len)
 416 {
 417     unsigned char csum;
 418
 419     csum = 0;
 420
 421     while (len--) {
 422         csum += *buf++;
 423     }
 424
 425     return csum;
 426 }
 427
 428 /*
 429  * Transmits a packet of data.
 430  * Packets are of the form: $<packet-data>#<checksum>
 431  *
 432  * Returns:
 433  *    0   if the packet was transmitted and acknowledged
 434  *    1   if the packet was transmitted but not acknowledged
 435  *    GDB_EOF otherwise
 436  */
 437 static int
 438 gdb_send_packet(struct gdb_state* state,
 439                 const char* pkt_data,
 440                 unsigned int pkt_len)
 441 {
 442     char buf[3];
 443     char csum;
 444
 445     /* Send packet start */
 446     if (gdb_sys_putchar(state, '$') == GDB_EOF) {
 447         return GDB_EOF;
 448     }
 449
 450     /* Send packet data */
 451     if (gdb_write(state, pkt_data, pkt_len) == GDB_EOF) {
 452         return GDB_EOF;
 453     }
 454
 455     /* Send the checksum */
 456     buf[0] = '#';
 457     csum = gdb_checksum(pkt_data, pkt_len);
 458     if ((gdb_enc_hex(buf + 1, sizeof(buf) - 1, &csum, 1) == GDB_EOF) ||
 459         (gdb_write(state, buf, sizeof(buf)) == GDB_EOF)) {
 460         return GDB_EOF;
 461     }
 462
 463     return gdb_recv_ack(state);
 464 }
 465
 466 /*
 467  * Receives a packet of data, assuming a 7-bit clean connection.
 468  *
 469  * Returns:
 470  *    0   if the packet was received
 471  *    GDB_EOF otherwise
 472  */
 473 static int
 474 gdb_recv_packet(struct gdb_state* state,
 475                 char* pkt_buf,
 476                 unsigned int pkt_buf_len,
 477                 unsigned int* pkt_len)
 478 {
 479     int data;
 480     char expected_csum, actual_csum;
 481     char buf[2];
 482
 483     /* Wait for packet start */
 484     actual_csum = 0;
 485
 486     while (1) {
 487         data = gdb_sys_getc(state);
 488         if (data == GDB_EOF) {
 489             return GDB_EOF;
 490         } else if (data == '$') {
 491             /* Detected start of packet. */
 492             break;
 493         }
 494     }
 495
 496     /* Read until checksum */
 497     *pkt_len = 0;
 498     while (1) {
 499         data = gdb_sys_getc(state);
 500
 501         if (data == GDB_EOF) {
 502             /* Error receiving character */
 503             return GDB_EOF;
 504         } else if (data == '#') {
 505             /* End of packet */
 506             break;
 507         } else {
 508             /* Check for space */
 509             if (*pkt_len >= pkt_buf_len) {
 510                 GDB_PRINT("packet buffer overflow\n");
 511                 return GDB_EOF;
 512             }
 513
 514             /* Store character and update checksum */
 515             pkt_buf[(*pkt_len)++] = (char)data;
 516         }
 517     }
 518
 519     /* Receive the checksum */
 520     if ((gdb_read(state, buf, sizeof(buf), 2) == GDB_EOF) ||
 521         (gdb_dec_hex(buf, 2, &expected_csum, 1) == GDB_EOF)) {
 522         return GDB_EOF;
 523     }
 524
 525     /* Verify checksum */
 526     actual_csum = gdb_checksum(pkt_buf, *pkt_len);
 527     if (actual_csum != expected_csum) {
 528         /* Send packet nack */
 529         GDB_PRINT("received packet with bad checksum\n");
 530         gdb_sys_putchar(state, '-');
 531         return GDB_EOF;
 532     }
 533
 534     /* Send packet ack */
 535     gdb_sys_putchar(state, '+');
 536     return 0;
 537 }
 538
 539 /*****************************************************************************
 540  * Data Encoding/Decoding
 541  ****************************************************************************/
 542
 543 /*
 544  * Encode data to its hex-value representation in a buffer.
 545  *
 546  * Returns:
 547  *    0+  number of bytes written to buf
 548  *    GDB_EOF if the buffer is too small
 549  */
 550 static int
 551 gdb_enc_hex(char* buf,
 552             unsigned int buf_len,
 553             const char* data,
 554             unsigned int data_len)
 555 {
 556     unsigned int pos;
 557
 558     if (buf_len < data_len * 2) {
 559         /* Buffer too small */
 560         return GDB_EOF;
 561     }
 562
 563     for (pos = 0; pos < data_len; pos++) {
 564         *buf++ = gdb_get_digit((data[pos] >> 4) & 0xf);
 565         *buf++ = gdb_get_digit((data[pos]) & 0xf);
 566     }
 567
 568     return data_len * 2;
 569 }
 570
 571 /*
 572  * Decode data from its hex-value representation to a buffer.
 573  *
 574  * Returns:
 575  *    0   if successful
 576  *    GDB_EOF if the buffer is too small
 577  */
 578 static int
 579 gdb_dec_hex(const char* buf,
 580             unsigned int buf_len,
 581             char* data,
 582             unsigned int data_len)
 583 {
 584     unsigned int pos;
 585     int tmp;
 586
 587     if (buf_len != data_len * 2) {
 588         /* Buffer too small */
 589         return GDB_EOF;
 590     }
 591
 592     for (pos = 0; pos < data_len; pos++) {
 593         /* Decode high nibble */
 594         tmp = gdb_get_val(*buf++, 16);
 595         if (tmp == GDB_EOF) {
 596             /* Buffer contained junk. */
 597             GDB_ASSERT(0);
 598             return GDB_EOF;
 599         }
 600
 601         data[pos] = tmp << 4;
 602
 603         /* Decode low nibble */
 604         tmp = gdb_get_val(*buf++, 16);
 605         if (tmp == GDB_EOF) {
 606             /* Buffer contained junk. */
 607             GDB_ASSERT(0);
 608             return GDB_EOF;
 609         }
 610         data[pos] |= tmp;
 611     }
 612
 613     return 0;
 614 }
 615
 616 /*
 617  * Encode data to its binary representation in a buffer.
 618  *
 619  * Returns:
 620  *    0+  number of bytes written to buf
 621  *    GDB_EOF if the buffer is too small
 622  */
 623 static int
 624 gdb_enc_bin(char* buf,
 625             unsigned int buf_len,
 626             const char* data,
 627             unsigned int data_len)
 628 {
 629     unsigned int buf_pos, data_pos;
 630
 631     for (buf_pos = 0, data_pos = 0; data_pos < data_len; data_pos++) {
 632         if (data[data_pos] == '$' || data[data_pos] == '#' ||
 633             data[data_pos] == '}' || data[data_pos] == '*') {
 634             if (buf_pos + 1 >= buf_len) {
 635                 GDB_ASSERT(0);
 636                 return GDB_EOF;
 637             }
 638             buf[buf_pos++] = '}';
 639             buf[buf_pos++] = data[data_pos] ^ 0x20;
 640         } else {
 641             if (buf_pos >= buf_len) {
 642                 GDB_ASSERT(0);
 643                 return GDB_EOF;
 644             }
 645             buf[buf_pos++] = data[data_pos];
 646         }
 647     }
 648
 649     return buf_pos;
 650 }
 651
 652 /*
 653  * Decode data from its bin-value representation to a buffer.
 654  *
 655  * Returns:
 656  *    0+  if successful, number of bytes decoded
 657  *    GDB_EOF if the buffer is too small
 658  */
 659 static int
 660 gdb_dec_bin(const char* buf,
 661             unsigned int buf_len,
 662             char* data,
 663             unsigned int data_len)
 664 {
 665     unsigned int buf_pos, data_pos;
 666
 667     for (buf_pos = 0, data_pos = 0; buf_pos < buf_len; buf_pos++) {
 668         if (data_pos >= data_len) {
 669             /* Output buffer overflow */
 670             GDB_ASSERT(0);
 671             return GDB_EOF;
 672         }
 673         if (buf[buf_pos] == '}') {
 674             /* The next byte is escaped! */
 675             if (buf_pos + 1 >= buf_len) {
 676                 /* There's an escape character, but no escaped character
 677                  * following the escape character. */
 678                 GDB_ASSERT(0);
 679                 return GDB_EOF;
 680             }
 681             buf_pos += 1;
 682             data[data_pos++] = buf[buf_pos] ^ 0x20;
 683         } else {
 684             data[data_pos++] = buf[buf_pos];
 685         }
 686     }
 687
 688     return data_pos;
 689 }
 690
 691 /*****************************************************************************
 692  * Command Functions
 693  ****************************************************************************/
 694
 695 /*
 696  * Read from memory and encode into buf.
 697  *
 698  * Returns:
 699  *    0+  number of bytes written to buf
 700  *    GDB_EOF if the buffer is too small
 701  */
 702 static int
 703 gdb_mem_read(struct gdb_state* state,
 704              char* buf,
 705              unsigned int buf_len,
 706              address addr,
 707              unsigned int len,
 708              gdb_enc_func enc)
 709 {
 710     char data[64];
 711     unsigned int pos;
 712
 713     if (len > sizeof(data)) {
 714         return GDB_EOF;
 715     }
 716
 717     /* Read from system memory */
 718     for (pos = 0; pos < len; pos++) {
 719         if (gdb_sys_mem_readb(state, addr + pos, &data[pos])) {
 720             /* Failed to read */
 721             return GDB_EOF;
 722         }
 723     }
 724
 725     /* Encode data */
 726     return enc(buf, buf_len, data, len);
 727 }
 728
 729 /*
 730  * Write to memory from encoded buf.
 731  */
 732 static int
 733 gdb_mem_write(struct gdb_state* state,
 734               const char* buf,
 735               unsigned int buf_len,
 736               address addr,
 737               unsigned int len,
 738               gdb_dec_func dec)
 739 {
 740     char data[64];
 741     unsigned int pos;
 742
 743     if (len > sizeof(data)) {
 744         return GDB_EOF;
 745     }
 746
 747     /* Decode data */
 748     if (dec(buf, buf_len, data, len) == GDB_EOF) {
 749         return GDB_EOF;
 750     }
 751
 752     /* Write to system memory */
 753     for (pos = 0; pos < len; pos++) {
 754         if (gdb_sys_mem_writeb(state, addr + pos, data[pos])) {
 755             /* Failed to write */
 756             return GDB_EOF;
 757         }
 758     }
 759
 760     return 0;
 761 }
 762
 763 /*
 764  * Continue program execution at PC.
 765  */
 766 int
 767 gdb_continue(struct gdb_state* state)
 768 {
 769     gdb_sys_continue(state);
 770     return 0;
 771 }
 772
 773 /*
 774  * Step one instruction.
 775  */
 776 int
 777 gdb_step(struct gdb_state* state)
 778 {
 779     gdb_sys_step(state);
 780     return 0;
 781 }
 782
 783 /*****************************************************************************
 784  * Packet Creation Helpers
 785  ****************************************************************************/
 786
 787 /*
 788  * Send OK packet
 789  */
 790 static int
 791 gdb_send_ok_packet(struct gdb_state* state, char* buf, unsigned int buf_len)
 792 {
 793     return gdb_send_packet(state, "OK", 2);
 794 }
 795
 796 /*
 797  * Send a message to the debugging console (via O XX... packet)
 798  */
 799 static int
 800 gdb_send_conmsg_packet(struct gdb_state* state,
 801                        char* buf,
 802                        unsigned int buf_len,
 803                        const char* msg)
 804 {
 805     unsigned int size;
 806     int status;
 807
 808     if (buf_len < 2) {
 809         /* Buffer too small */
 810         return GDB_EOF;
 811     }
 812
 813     buf[0] = 'O';
 814     status = gdb_enc_hex(&buf[1], buf_len - 1, msg, gdb_strlen(msg));
 815     if (status == GDB_EOF) {
 816         return GDB_EOF;
 817     }
 818     size = 1 + status;
 819     return gdb_send_packet(state, buf, size);
 820 }
 821
 822 /*
 823  * Send a signal packet (S AA).
 824  */
 825 static int
 826 gdb_send_signal_packet(struct gdb_state* state,
 827                        char* buf,
 828                        unsigned int buf_len,
 829                        char signal)
 830 {
 831     unsigned int size;
 832     int status;
 833
 834     if (buf_len < 4) {
 835         /* Buffer too small */
 836         return GDB_EOF;
 837     }
 838
 839     buf[0] = 'S';
 840     status = gdb_enc_hex(&buf[1], buf_len - 1, &signal, 1);
 841     if (status == GDB_EOF) {
 842         return GDB_EOF;
 843     }
 844     size = 1 + status;
 845     return gdb_send_packet(state, buf, size);
 846 }
 847
 848 /*
 849  * Send a error packet (E AA).
 850  */
 851 static int
 852 gdb_send_error_packet(struct gdb_state* state,
 853                       char* buf,
 854                       unsigned int buf_len,
 855                       char error)
 856 {
 857     unsigned int size;
 858     int status;
 859
 860     if (buf_len < 4) {
 861         /* Buffer too small */
 862         return GDB_EOF;
 863     }
 864
 865     buf[0] = 'E';
 866     status = gdb_enc_hex(&buf[1], buf_len - 1, &error, 1);
 867     if (status == GDB_EOF) {
 868         return GDB_EOF;
 869     }
 870     size = 1 + status;
 871     return gdb_send_packet(state, buf, size);
 872 }
 873
 874 /*****************************************************************************
 875  * Communication Functions
 876  ****************************************************************************/
 877
 878 /*
 879  * Write a sequence of bytes.
 880  *
 881  * Returns:
 882  *    0   if successful
 883  *    GDB_EOF if failed to write all bytes
 884  */
 885 static int
 886 gdb_write(struct gdb_state* state, const char* buf, unsigned int len)
 887 {
 888     int err = serial_rwbuf_sync(state->sdev, buf, len, SERIAL_RW_TX);
 889
 890     return err < 0 ? GDB_EOF : 0;
 891 }
 892
 893 /*
 894  * Read a sequence of bytes.
 895  *
 896  * Returns:
 897  *    0   if successfully read len bytes
 898  *    GDB_EOF if failed to read all bytes
 899  */
 900 static int
 901 gdb_read(struct gdb_state* state,
 902          char* buf,
 903          unsigned int buf_len,
 904          unsigned int len)
 905 {
 906     char c;
 907
 908     if (buf_len < len) {
 909         /* Buffer too small */
 910         return GDB_EOF;
 911     }
 912
 913     int err = serial_rwbuf_sync(state->sdev, buf, buf_len, SERIAL_RW_RX);
 914
 915     return err < 0 ? GDB_EOF : 0;
 916 }
 917
 918 /*****************************************************************************
 919  * Main Loop
 920  ****************************************************************************/
 921
 922 /*
 923  * Main debug loop. Handles commands.
 924  */
 925 int
 926 gdb_main(struct gdb_state* state)
 927 {
 928     address addr;
 929     char pkt_buf[256];
 930     int status;
 931     unsigned int length;
 932     unsigned int pkt_len;
 933     const char* ptr_next;
 934
 935     gdb_send_signal_packet(state, pkt_buf, sizeof(pkt_buf), state->signum);
 936
 937     while (1) {
 938         /* Receive the next packet */
 939         status = gdb_recv_packet(state, pkt_buf, sizeof(pkt_buf), &pkt_len);
 940         if (status == GDB_EOF) {
 941             break;
 942         }
 943
 944         if (pkt_len == 0) {
 945             /* Received empty packet.. */
 946             continue;
 947         }
 948
 949         ptr_next = pkt_buf;
 950
 951         /*
 952          * Handle one letter commands
 953          */
 954         switch (pkt_buf[0]) {
 955
 956 /* Calculate remaining space in packet from ptr_next position. */
 957 #define token_remaining_buf (pkt_len - (ptr_next - pkt_buf))
 958
 959 /* Expecting a seperator. If not present, go to error */
 960 #define token_expect_seperator(c)                                              \
 961     {                                                                          \
 962         if (!ptr_next || *ptr_next != c) {                                     \
 963             goto error;                                                        \
 964         } else {                                                               \
 965             ptr_next += 1;                                                     \
 966         }                                                                      \
 967     }
 968
 969 /* Expecting an integer argument. If not present, go to error */
 970 #define token_expect_integer_arg(arg)                                          \
 971     {                                                                          \
 972         arg = gdb_strtol(ptr_next, token_remaining_buf, 16, &ptr_next);        \
 973         if (!ptr_next) {                                                       \
 974             goto error;                                                        \
 975         }                                                                      \
 976     }
 977
 978             /*
 979              * Read Registers
 980              * Command Format: g
 981              */
 982             case 'g':
 983                 /* Encode registers */
 984                 status = gdb_enc_hex(pkt_buf,
 985                                      sizeof(pkt_buf),
 986                                      (char*)&(state->registers),
 987                                      sizeof(state->registers));
 988                 if (status == GDB_EOF) {
 989                     goto error;
 990                 }
 991                 pkt_len = status;
 992                 gdb_send_packet(state, pkt_buf, pkt_len);
 993                 break;
 994
 995             /*
 996              * Write Registers
 997              * Command Format: G XX...
 998              */
 999             case 'G':
1000                 status = gdb_dec_hex(pkt_buf + 1,
1001                                      pkt_len - 1,
1002                                      (char*)&(state->registers),
1003                                      sizeof(state->registers));
1004                 if (status == GDB_EOF) {
1005                     goto error;
1006                 }
1007                 gdb_send_ok_packet(state, pkt_buf, sizeof(pkt_buf));
1008                 break;
1009
1010             /*
1011              * Read a Register
1012              * Command Format: p n
1013              */
1014             case 'p':
1015                 ptr_next += 1;
1016                 token_expect_integer_arg(addr);
1017
1018                 if (addr >= GDB_CPU_NUM_REGISTERS) {
1019                     goto error;
1020                 }
1021
1022                 /* Read Register */
1023                 status = gdb_enc_hex(pkt_buf,
1024                                      sizeof(pkt_buf),
1025                                      (char*)&(state->registers[addr]),
1026                                      sizeof(state->registers[addr]));
1027                 if (status == GDB_EOF) {
1028                     goto error;
1029                 }
1030                 gdb_send_packet(state, pkt_buf, status);
1031                 break;
1032
1033             /*
1034              * Write a Register
1035              * Command Format: P n...=r...
1036              */
1037             case 'P':
1038                 ptr_next += 1;
1039                 token_expect_integer_arg(addr);
1040                 token_expect_seperator('=');
1041
1042                 if (addr < GDB_CPU_NUM_REGISTERS) {
1043                     status = gdb_dec_hex(ptr_next,
1044                                          token_remaining_buf,
1045                                          (char*)&(state->registers[addr]),
1046                                          sizeof(state->registers[addr]));
1047                     if (status == GDB_EOF) {
1048                         goto error;
1049                     }
1050                 }
1051                 gdb_send_ok_packet(state, pkt_buf, sizeof(pkt_buf));
1052                 break;
1053
1054             /*
1055              * Read Memory
1056              * Command Format: m addr,length
1057              */
1058             case 'm':
1059                 ptr_next += 1;
1060                 token_expect_integer_arg(addr);
1061                 token_expect_seperator(',');
1062                 token_expect_integer_arg(length);
1063
1064                 /* Read Memory */
1065                 status = gdb_mem_read(
1066                   state, pkt_buf, sizeof(pkt_buf), addr, length, gdb_enc_hex);
1067                 if (status == GDB_EOF) {
1068                     goto error;
1069                 }
1070                 gdb_send_packet(state, pkt_buf, status);
1071                 break;
1072
1073             /*
1074              * Write Memory
1075              * Command Format: M addr,length:XX..
1076              */
1077             case 'M':
1078                 ptr_next += 1;
1079                 token_expect_integer_arg(addr);
1080                 token_expect_seperator(',');
1081                 token_expect_integer_arg(length);
1082                 token_expect_seperator(':');
1083
1084                 /* Write Memory */
1085                 status = gdb_mem_write(state,
1086                                        ptr_next,
1087                                        token_remaining_buf,
1088                                        addr,
1089                                        length,
1090                                        gdb_dec_hex);
1091                 if (status == GDB_EOF) {
1092                     goto error;
1093                 }
1094                 gdb_send_ok_packet(state, pkt_buf, sizeof(pkt_buf));
1095                 break;
1096
1097             /*
1098              * Write Memory (Binary)
1099              * Command Format: X addr,length:XX..
1100              */
1101             case 'X':
1102                 ptr_next += 1;
1103                 token_expect_integer_arg(addr);
1104                 token_expect_seperator(',');
1105                 token_expect_integer_arg(length);
1106                 token_expect_seperator(':');
1107
1108                 /* Write Memory */
1109                 status = gdb_mem_write(state,
1110                                        ptr_next,
1111                                        token_remaining_buf,
1112                                        addr,
1113                                        length,
1114                                        gdb_dec_bin);
1115                 if (status == GDB_EOF) {
1116                     goto error;
1117                 }
1118                 gdb_send_ok_packet(state, pkt_buf, sizeof(pkt_buf));
1119                 break;
1120
1121             /*
1122              * Continue, Kill (also treated as continue!)
1123              * Command Format: c [addr]
1124              */
1125             case 'c':
1126             case 'C':
1127             case 'k':
1128                 gdb_continue(state);
1129                 return 0;
1130
1131             /*
1132              * Single-step
1133              * Command Format: s [addr]
1134              */
1135             case 's':
1136             case 'S':
1137                 gdb_step(state);
1138                 return 0;
1139
1140             case '?':
1141                 gdb_send_signal_packet(
1142                   state, pkt_buf, sizeof(pkt_buf), state->signum);
1143                 break;
1144
1145             /*
1146              * Unsupported Command
1147              */
1148             default:
1149                 gdb_send_packet(state, 0, 0);
1150         }
1151
1152         continue;
1153
1154     error:
1155         gdb_send_error_packet(state, pkt_buf, sizeof(pkt_buf), 0x00);
1156
1157 #undef token_remaining_buf
1158 #undef token_expect_seperator
1159 #undef token_expect_integer_arg
1160     }
1161
1162     return 0;
1163 }
1164
1165 /*****************************************************************************
1166  * Types
1167  ****************************************************************************/
1168
1169 struct gdb_idtr
1170 {
1171     u16_t len;
1172     uint32_t offset;
1173 } __attribute__((packed));
1174
1175 struct gdb_idt_gate
1176 {
1177     u16_t offset_low;
1178     u16_t segment;
1179     u16_t flags;
1180     u16_t offset_high;
1181 } __attribute__((packed));
1182
1183 /*****************************************************************************
1184  * Prototypes
1185  ****************************************************************************/
1186 #define gdb_x86_io_write_8(port, val) port_wrbyte(port, val)
1187 #define gdb_x86_io_read_8(port) port_rdbyte(port)
1188
1189 #define gdb_x86_serial_getc() serial_rx_byte(COM_PORT)
1190 #define gdb_x86_serial_putchar(ch) serial_tx_byte(COM_PORT, ch)
1191
1192 #ifdef __STRICT_ANSI__
1193 #define asm __asm__
1194 #endif
1195
1196 static struct gdb_state gdb_state;
1197 static volatile int start_debugging = 0;
1198
1199 /*
1200  * Debug interrupt handler.
1201  */
1202 void
1203 gdbstub_loop(isr_param* param)
1204 {
1205     /* Translate vector to signal */
1206     switch (param->execp->vector) {
1207         case 1:
1208             gdb_state.signum = 5;
1209             break;
1210         case 3:
1211             gdb_state.signum = 5;
1212             break;
1213         default:
1214             gdb_state.signum = 7;
1215     }
1216
1217     /* Load Registers */
1218     gdb_state.registers[GDB_CPU_I386_REG_EAX] = param->registers.eax;
1219     gdb_state.registers[GDB_CPU_I386_REG_ECX] = param->registers.ecx;
1220     gdb_state.registers[GDB_CPU_I386_REG_EDX] = param->registers.edx;
1221     gdb_state.registers[GDB_CPU_I386_REG_EBX] = param->registers.ebx;
1222     gdb_state.registers[GDB_CPU_I386_REG_ESP] = param->esp;
1223     gdb_state.registers[GDB_CPU_I386_REG_EBP] = param->registers.ebp;
1224     gdb_state.registers[GDB_CPU_I386_REG_ESI] = param->registers.esi;
1225     gdb_state.registers[GDB_CPU_I386_REG_EDI] = param->registers.edi;
1226     gdb_state.registers[GDB_CPU_I386_REG_PC] = param->execp->eip;
1227     gdb_state.registers[GDB_CPU_I386_REG_CS] = param->execp->cs;
1228     gdb_state.registers[GDB_CPU_I386_REG_PS] = param->execp->eflags;
1229     gdb_state.registers[GDB_CPU_I386_REG_SS] = param->execp->ss;
1230     gdb_state.registers[GDB_CPU_I386_REG_DS] = param->registers.ds;
1231     gdb_state.registers[GDB_CPU_I386_REG_ES] = param->registers.es;
1232     gdb_state.registers[GDB_CPU_I386_REG_FS] = param->registers.fs;
1233     gdb_state.registers[GDB_CPU_I386_REG_GS] = param->registers.gs;
1234
1235     gdb_main(&gdb_state);
1236
1237     /* Restore Registers */
1238     param->registers.eax = gdb_state.registers[GDB_CPU_I386_REG_EAX];
1239     param->registers.ecx = gdb_state.registers[GDB_CPU_I386_REG_ECX];
1240     param->registers.edx = gdb_state.registers[GDB_CPU_I386_REG_EDX];
1241     param->registers.ebx = gdb_state.registers[GDB_CPU_I386_REG_EBX];
1242     param->esp = gdb_state.registers[GDB_CPU_I386_REG_ESP];
1243     param->registers.ebp = gdb_state.registers[GDB_CPU_I386_REG_EBP];
1244     param->registers.esi = gdb_state.registers[GDB_CPU_I386_REG_ESI];
1245     param->registers.edi = gdb_state.registers[GDB_CPU_I386_REG_EDI];
1246     param->execp->eip = gdb_state.registers[GDB_CPU_I386_REG_PC];
1247     param->execp->cs = gdb_state.registers[GDB_CPU_I386_REG_CS];
1248     param->execp->eflags = gdb_state.registers[GDB_CPU_I386_REG_PS];
1249     param->execp->ss = gdb_state.registers[GDB_CPU_I386_REG_SS];
1250     param->registers.ds = gdb_state.registers[GDB_CPU_I386_REG_DS];
1251     param->registers.es = gdb_state.registers[GDB_CPU_I386_REG_ES];
1252     param->registers.fs = gdb_state.registers[GDB_CPU_I386_REG_FS];
1253     param->registers.gs = gdb_state.registers[GDB_CPU_I386_REG_GS];
1254 }
1255
1256 /*****************************************************************************
1257  * Debugging System Functions
1258  ****************************************************************************/
1259
1260 /*
1261  * Write one character to the debugging stream.
1262  */
1263 int
1264 gdb_sys_putchar(struct gdb_state* state, int ch)
1265 {
1266     serial_rwbuf_sync(state->sdev, &ch, 1, SERIAL_RW_TX);
1267     return ch;
1268 }
1269
1270 /*
1271  * Read one character from the debugging stream.
1272  */
1273 int
1274 gdb_sys_getc(struct gdb_state* state)
1275 {
1276     char ch;
1277     serial_rwbuf_sync(state->sdev, &ch, 1, SERIAL_RW_RX);
1278     return ch & 0xff;
1279 }
1280
1281 /*
1282  * Read one byte from memory.
1283  */
1284 int
1285 gdb_sys_mem_readb(struct gdb_state* state, address addr, char* val)
1286 {
1287     *val = *(volatile char*)addr;
1288     return 0;
1289 }
1290
1291 /*
1292  * Write one byte to memory.
1293  */
1294 int
1295 gdb_sys_mem_writeb(struct gdb_state* state, address addr, char val)
1296 {
1297     *(volatile char*)addr = val;
1298     return 0;
1299 }
1300
1301 /*
1302  * Continue program execution.
1303  */
1304 int
1305 gdb_sys_continue(struct gdb_state* state)
1306 {
1307     gdb_state.registers[GDB_CPU_I386_REG_PS] &= ~(1 << 8);
1308     return 0;
1309 }
1310
1311 /*
1312  * Single step the next instruction.
1313  */
1314 int
1315 gdb_sys_step(struct gdb_state* state)
1316 {
1317     gdb_state.registers[GDB_CPU_I386_REG_PS] |= 1 << 8;
1318     return 0;
1319 }