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