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