riscv/gdbserver.cc

   1 #include <arpa/inet.h>
   2 #include <errno.h>
   3 #include <fcntl.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include <sys/socket.h>
   7 #include <sys/types.h>
   8 #include <unistd.h>
   9
  10 #include <algorithm>
  11 #include <cassert>
  12 #include <cstdio>
  13 #include <vector>
  14
  15 #include "disasm.h"
  16 #include "sim.h"
  17 #include "gdbserver.h"
  18 #include "mmu.h"
  19
  20 #define C_EBREAK        0x9002
  21 #define EBREAK          0x00100073
  22
  23 template <typename T>
  24 unsigned int circular_buffer_t<T>::size() const
  25 {
  26   if (end >= start)
  27     return end - start;
  28   else
  29     return end + capacity - start;
  30 }
  31
  32 template <typename T>
  33 void circular_buffer_t<T>::consume(unsigned int bytes)
  34 {
  35   start = (start + bytes) % capacity;
  36 }
  37
  38 template <typename T>
  39 unsigned int circular_buffer_t<T>::contiguous_empty_size() const
  40 {
  41   if (end >= start)
  42     if (start == 0)
  43       return capacity - end - 1;
  44     else
  45       return capacity - end;
  46   else
  47     return start - end - 1;
  48 }
  49
  50 template <typename T>
  51 unsigned int circular_buffer_t<T>::contiguous_data_size() const
  52 {
  53   if (end >= start)
  54     return end - start;
  55   else
  56     return capacity - start;
  57 }
  58
  59 template <typename T>
  60 void circular_buffer_t<T>::data_added(unsigned int bytes)
  61 {
  62   end += bytes;
  63   assert(end <= capacity);
  64   if (end == capacity)
  65     end = 0;
  66 }
  67
  68 template <typename T>
  69 void circular_buffer_t<T>::reset()
  70 {
  71   start = 0;
  72   end = 0;
  73 }
  74
  75 template <typename T>
  76 void circular_buffer_t<T>::append(const T *src, unsigned int count)
  77 {
  78   unsigned int copy = std::min(count, contiguous_empty_size());
  79   memcpy(contiguous_empty(), src, copy * sizeof(T));
  80   data_added(copy);
  81   count -= copy;
  82   if (count > 0) {
  83     assert(count < contiguous_empty_size());
  84     memcpy(contiguous_empty(), src, count * sizeof(T));
  85     data_added(count);
  86   }
  87 }
  88
  89 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
  90   sim(sim),
  91   client_fd(0),
  92   recv_buf(64 * 1024), send_buf(64 * 1024)
  93 {
  94   // TODO: listen on socket
  95   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
  96   if (socket_fd == -1) {
  97     fprintf(stderr, "failed to make socket: %s (%d)\n", strerror(errno), errno);
  98     abort();
  99   }
 100
 101   fcntl(socket_fd, F_SETFL, O_NONBLOCK);
 102   int reuseaddr = 1;
 103   if (setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,
 104         sizeof(int)) == -1) {
 105     fprintf(stderr, "failed setsockopt: %s (%d)\n", strerror(errno), errno);
 106     abort();
 107   }
 108
 109   struct sockaddr_in addr;
 110   memset(&addr, 0, sizeof(addr));
 111   addr.sin_family = AF_INET;
 112   addr.sin_addr.s_addr = INADDR_ANY;
 113   addr.sin_port = htons(port);
 114
 115   if (bind(socket_fd, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
 116     fprintf(stderr, "failed to bind socket: %s (%d)\n", strerror(errno), errno);
 117     abort();
 118   }
 119
 120   if (listen(socket_fd, 1) == -1) {
 121     fprintf(stderr, "failed to listen on socket: %s (%d)\n", strerror(errno), errno);
 122     abort();
 123   }
 124 }
 125
 126 void gdbserver_t::accept()
 127 {
 128   client_fd = ::accept(socket_fd, NULL, NULL);
 129   if (client_fd == -1) {
 130     if (errno == EAGAIN) {
 131       // No client waiting to connect right now.
 132     } else {
 133       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno),
 134           errno);
 135       abort();
 136     }
 137   } else {
 138     fcntl(client_fd, F_SETFL, O_NONBLOCK);
 139
 140     expect_ack = false;
 141     extended_mode = false;
 142
 143     // gdb wants the core to be halted when it attaches.
 144     processor_t *p = sim->get_core(0);
 145     p->set_halted(true, HR_ATTACHED);
 146   }
 147 }
 148
 149 void gdbserver_t::read()
 150 {
 151   // Reading from a non-blocking socket still blocks if there is no data
 152   // available.
 153
 154   size_t count = recv_buf.contiguous_empty_size();
 155   assert(count > 0);
 156   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
 157   if (bytes == -1) {
 158     if (errno == EAGAIN) {
 159       // We'll try again the next call.
 160     } else {
 161       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
 162       abort();
 163     }
 164   } else if (bytes == 0) {
 165     // The remote disconnected.
 166     client_fd = 0;
 167     processor_t *p = sim->get_core(0);
 168     p->set_halted(false, HR_NONE);
 169     recv_buf.reset();
 170     send_buf.reset();
 171   } else {
 172     recv_buf.data_added(bytes);
 173   }
 174 }
 175
 176 void gdbserver_t::write()
 177 {
 178   if (send_buf.empty())
 179     return;
 180
 181   while (!send_buf.empty()) {
 182     unsigned int count = send_buf.contiguous_data_size();
 183     assert(count > 0);
 184     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
 185     if (bytes == -1) {
 186       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
 187       abort();
 188     } else if (bytes == 0) {
 189       // Client can't take any more data right now.
 190       break;
 191     } else {
 192       fprintf(stderr, "wrote %ld bytes: ", bytes);
 193       for (unsigned int i = 0; i < bytes; i++) {
 194         fprintf(stderr, "%c", send_buf[i]);
 195       }
 196       fprintf(stderr, "\n");
 197       send_buf.consume(bytes);
 198     }
 199   }
 200 }
 201
 202 void print_packet(const std::vector<uint8_t> &packet)
 203 {
 204   for (uint8_t c : packet) {
 205     if (c >= ' ' and c <= '~')
 206       fprintf(stderr, "%c", c);
 207     else
 208       fprintf(stderr, "\\x%x", c);
 209   }
 210   fprintf(stderr, "\n");
 211 }
 212
 213 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
 214 {
 215   uint8_t checksum = 0;
 216   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
 217     checksum += *i;
 218   }
 219   return checksum;
 220 }
 221
 222 uint8_t character_hex_value(uint8_t character)
 223 {
 224   if (character >= '0' && character <= '9')
 225     return character - '0';
 226   if (character >= 'a' && character <= 'f')
 227     return 10 + character - 'a';
 228   if (character >= 'A' && character <= 'F')
 229     return 10 + character - 'A';
 230   return 0xff;
 231 }
 232
 233 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
 234 {
 235   return character_hex_value(*(packet.end() - 1)) +
 236     16 * character_hex_value(*(packet.end() - 2));
 237 }
 238
 239 void gdbserver_t::process_requests()
 240 {
 241   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
 242
 243   while (!recv_buf.empty()) {
 244     std::vector<uint8_t> packet;
 245     for (unsigned int i = 0; i < recv_buf.size(); i++) {
 246       uint8_t b = recv_buf[i];
 247
 248       if (packet.empty() && expect_ack && b == '+') {
 249         recv_buf.consume(1);
 250         break;
 251       }
 252
 253       if (packet.empty() && b == 3) {
 254         fprintf(stderr, "Received interrupt\n");
 255         recv_buf.consume(1);
 256         handle_interrupt();
 257         break;
 258       }
 259
 260       if (b == '$') {
 261         // Start of new packet.
 262         if (!packet.empty()) {
 263           fprintf(stderr, "Received malformed %ld-byte packet from debug client: ",
 264               packet.size());
 265           print_packet(packet);
 266           recv_buf.consume(i);
 267           break;
 268         }
 269       }
 270
 271       packet.push_back(b);
 272
 273       // Packets consist of $<packet-data>#<checksum>
 274       // where <checksum> is
 275       if (packet.size() >= 4 &&
 276           packet[packet.size()-3] == '#') {
 277         handle_packet(packet);
 278         recv_buf.consume(i+1);
 279         break;
 280       }
 281     }
 282     // There's a partial packet in the buffer. Wait until we get more data to
 283     // process it.
 284     if (packet.size()) {
 285       break;
 286     }
 287   }
 288 }
 289
 290 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
 291 {
 292   send_packet("S00");
 293 }
 294
 295 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
 296 {
 297   // Register order that gdb expects is:
 298   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 299   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 300   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 301   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 302
 303   // Each byte of register data is described by two hex digits. The bytes with
 304   // the register are transmitted in target byte order. The size of each
 305   // register and their position within the ‘g’ packet are determined by the
 306   // gdb internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and
 307   // gdbarch_register_name.
 308
 309   send("$");
 310   running_checksum = 0;
 311   processor_t *p = sim->get_core(0);
 312   for (int r = 0; r < 32; r++) {
 313     send(p->state.XPR[r]);
 314   }
 315   send_running_checksum();
 316   expect_ack = true;
 317 }
 318
 319 // First byte is the most-significant one.
 320 // Eg. "08675309" becomes 0x08675309.
 321 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
 322     std::vector<uint8_t>::const_iterator end)
 323 {
 324   uint64_t value = 0;
 325
 326   while (iter != end) {
 327     uint8_t c = *iter;
 328     uint64_t c_value = character_hex_value(c);
 329     if (c_value > 15)
 330       break;
 331     iter++;
 332     value <<= 4;
 333     value += c_value;
 334   }
 335   return value;
 336 }
 337
 338 // First byte is the least-significant one.
 339 // Eg. "08675309" becomes 0x09536708
 340 uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
 341     std::vector<uint8_t>::const_iterator end)
 342 {
 343   uint64_t value = 0;
 344   unsigned int shift = 4;
 345
 346   while (iter != end) {
 347     uint8_t c = *iter;
 348     uint64_t c_value = character_hex_value(c);
 349     if (c_value > 15)
 350       break;
 351     iter++;
 352     value |= c_value << shift;
 353     if ((shift % 8) == 0)
 354       shift += 12;
 355     else
 356       shift -= 4;
 357   }
 358   return value;
 359 }
 360
 361 void consume_string(std::string &str, std::vector<uint8_t>::const_iterator &iter,
 362     std::vector<uint8_t>::const_iterator end, uint8_t separator)
 363 {
 364   while (iter != end && *iter != separator) {
 365     str.append(1, (char) *iter);
 366     iter++;
 367   }
 368 }
 369
 370 typedef enum {
 371   RC_XPR,
 372   RC_PC,
 373   RC_FPR,
 374   RC_CSR
 375 } register_class_t;
 376
 377 typedef struct {
 378   register_class_t clss;
 379   int index;
 380 } register_access_t;
 381
 382 // gdb's register list is defined in riscv_gdb_reg_names gdb/riscv-tdep.c in
 383 // its source tree. The definition here must match that one.
 384 const register_access_t register_access[] = {
 385   { RC_XPR, 0 },
 386   { RC_XPR, 1 },
 387   { RC_XPR, 2 },
 388   { RC_XPR, 3 },
 389   { RC_XPR, 4 },
 390   { RC_XPR, 5 },
 391   { RC_XPR, 6 },
 392   { RC_XPR, 7 },
 393   { RC_XPR, 8 },
 394   { RC_XPR, 9 },
 395   { RC_XPR, 10 },
 396   { RC_XPR, 11 },
 397   { RC_XPR, 12 },
 398   { RC_XPR, 13 },
 399   { RC_XPR, 14 },
 400   { RC_XPR, 15 },
 401   { RC_XPR, 16 },
 402   { RC_XPR, 17 },
 403   { RC_XPR, 18 },
 404   { RC_XPR, 19 },
 405   { RC_XPR, 20 },
 406   { RC_XPR, 21 },
 407   { RC_XPR, 22 },
 408   { RC_XPR, 23 },
 409   { RC_XPR, 24 },
 410   { RC_XPR, 25 },
 411   { RC_XPR, 26 },
 412   { RC_XPR, 27 },
 413   { RC_XPR, 28 },
 414   { RC_XPR, 29 },
 415   { RC_XPR, 30 },
 416   { RC_XPR, 31 },
 417   { RC_PC, 0 },
 418   { RC_FPR, 0 },
 419   { RC_FPR, 1 },
 420   { RC_FPR, 2 },
 421   { RC_FPR, 3 },
 422   { RC_FPR, 4 },
 423   { RC_FPR, 5 },
 424   { RC_FPR, 6 },
 425   { RC_FPR, 7 },
 426   { RC_FPR, 8 },
 427   { RC_FPR, 9 },
 428   { RC_FPR, 10 },
 429   { RC_FPR, 11 },
 430   { RC_FPR, 12 },
 431   { RC_FPR, 13 },
 432   { RC_FPR, 14 },
 433   { RC_FPR, 15 },
 434   { RC_FPR, 16 },
 435   { RC_FPR, 17 },
 436   { RC_FPR, 18 },
 437   { RC_FPR, 19 },
 438   { RC_FPR, 20 },
 439   { RC_FPR, 21 },
 440   { RC_FPR, 22 },
 441   { RC_FPR, 23 },
 442   { RC_FPR, 24 },
 443   { RC_FPR, 25 },
 444   { RC_FPR, 26 },
 445   { RC_FPR, 27 },
 446   { RC_FPR, 28 },
 447   { RC_FPR, 29 },
 448   { RC_FPR, 30 },
 449   { RC_FPR, 31 },
 450
 451 #define DECLARE_CSR(name, num) { RC_CSR, num },
 452 #include "encoding.h"
 453 #undef DECLARE_CSR
 454 };
 455
 456 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
 457 {
 458   // p n
 459
 460   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 461   unsigned int n = consume_hex_number(iter, packet.end());
 462   if (*iter != '#')
 463     return send_packet("E01");
 464
 465   if (n >= sizeof(register_access) / sizeof(*register_access))
 466     return send_packet("E02");
 467
 468   processor_t *p = sim->get_core(0);
 469   send("$");
 470   running_checksum = 0;
 471
 472   register_access_t access = register_access[n];
 473   switch (access.clss) {
 474     case RC_XPR:
 475       send(p->state.XPR[access.index]);
 476       break;
 477     case RC_PC:
 478       send(p->state.pc);
 479       break;
 480     case RC_FPR:
 481       send(p->state.FPR[access.index]);
 482       break;
 483     case RC_CSR:
 484       try {
 485         send(p->get_csr(access.index));
 486       } catch(trap_t& t) {
 487         send((reg_t) 0);
 488       }
 489       break;
 490   }
 491
 492   send_running_checksum();
 493   expect_ack = true;
 494 }
 495
 496 void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
 497 {
 498   // P n...=r...
 499
 500   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 501   unsigned int n = consume_hex_number(iter, packet.end());
 502   if (*iter != '=')
 503     return send_packet("E05");
 504   iter++;
 505
 506   if (n >= sizeof(register_access) / sizeof(*register_access))
 507     return send_packet("E07");
 508
 509   reg_t value = consume_hex_number_le(iter, packet.end());
 510   if (*iter != '#')
 511     return send_packet("E06");
 512
 513   processor_t *p = sim->get_core(0);
 514
 515   register_access_t access = register_access[n];
 516   switch (access.clss) {
 517     case RC_XPR:
 518       p->state.XPR.write(access.index, value);
 519       break;
 520     case RC_PC:
 521       p->state.pc = value;
 522       break;
 523     case RC_FPR:
 524       p->state.FPR.write(access.index, value);
 525       break;
 526     case RC_CSR:
 527       try {
 528         p->set_csr(access.index, value);
 529       } catch(trap_t& t) {
 530         return send_packet("EFF");
 531       }
 532       break;
 533   }
 534
 535   return send_packet("OK");
 536 }
 537
 538 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
 539 {
 540   // m addr,length
 541   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 542   reg_t address = consume_hex_number(iter, packet.end());
 543   if (*iter != ',')
 544     return send_packet("E10");
 545   iter++;
 546   reg_t length = consume_hex_number(iter, packet.end());
 547   if (*iter != '#')
 548     return send_packet("E11");
 549
 550   send("$");
 551   running_checksum = 0;
 552   char buffer[3];
 553   processor_t *p = sim->get_core(0);
 554   mmu_t* mmu = sim->debug_mmu;
 555
 556   for (reg_t i = 0; i < length; i++) {
 557     sprintf(buffer, "%02x", mmu->load_uint8(address + i));
 558     send(buffer);
 559   }
 560   send_running_checksum();
 561 }
 562
 563 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
 564 {
 565   // X addr,length:XX...
 566   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 567   reg_t address = consume_hex_number(iter, packet.end());
 568   if (*iter != ',')
 569     return send_packet("E20");
 570   iter++;
 571   reg_t length = consume_hex_number(iter, packet.end());
 572   if (*iter != ':')
 573     return send_packet("E21");
 574   iter++;
 575
 576   processor_t *p = sim->get_core(0);
 577   mmu_t* mmu = sim->debug_mmu;
 578   for (unsigned int i = 0; i < length; i++) {
 579     if (iter == packet.end()) {
 580       return send_packet("E22");
 581     }
 582     mmu->store_uint8(address + i, *iter);
 583     iter++;
 584   }
 585   if (*iter != '#')
 586     return send_packet("E4b"); // EOVERFLOW
 587
 588   send_packet("OK");
 589 }
 590
 591 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
 592 {
 593   // c [addr]
 594   processor_t *p = sim->get_core(0);
 595   if (packet[2] != '#') {
 596     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 597     p->state.pc = consume_hex_number(iter, packet.end());
 598     if (*iter != '#')
 599       return send_packet("E30");
 600   }
 601
 602   p->set_halted(false, HR_NONE);
 603   running = true;
 604 }
 605
 606 void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
 607 {
 608   // s [addr]
 609   processor_t *p = sim->get_core(0);
 610   if (packet[2] != '#') {
 611     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 612     p->state.pc = consume_hex_number(iter, packet.end());
 613     if (*iter != '#')
 614       return send_packet("E40");
 615   }
 616
 617   p->set_single_step(true);
 618   running = true;
 619 }
 620
 621 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
 622 {
 623   // k
 624   // The exact effect of this packet is not specified.
 625   // Looks like OpenOCD disconnects?
 626   // TODO
 627 }
 628
 629 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
 630 {
 631   // Enable extended mode. In extended mode, the remote server is made
 632   // persistent. The ‘R’ packet is used to restart the program being debugged.
 633   send_packet("OK");
 634   extended_mode = true;
 635 }
 636
 637 void software_breakpoint_t::insert(mmu_t* mmu)
 638 {
 639   if (size == 2) {
 640     instruction = mmu->load_uint16(address);
 641     mmu->store_uint16(address, C_EBREAK);
 642   } else {
 643     instruction = mmu->load_uint32(address);
 644     mmu->store_uint32(address, EBREAK);
 645   }
 646   fprintf(stderr, ">>> Read %x from %lx\n", instruction, address);
 647 }
 648
 649 void software_breakpoint_t::remove(mmu_t* mmu)
 650 {
 651   fprintf(stderr, ">>> write %x to %lx\n", instruction, address);
 652   if (size == 2) {
 653     mmu->store_uint16(address, instruction);
 654   } else {
 655     mmu->store_uint32(address, instruction);
 656   }
 657 }
 658
 659 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
 660 {
 661   // insert: Z type,addr,kind
 662   // remove: z type,addr,kind
 663
 664   software_breakpoint_t bp;
 665   bool insert = (packet[1] == 'Z');
 666   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 667   int type = consume_hex_number(iter, packet.end());
 668   if (*iter != ',')
 669     return send_packet("E50");
 670   iter++;
 671   bp.address = consume_hex_number(iter, packet.end());
 672   if (*iter != ',')
 673     return send_packet("E51");
 674   iter++;
 675   bp.size = consume_hex_number(iter, packet.end());
 676   // There may be more options after a ; here, but we don't support that.
 677   if (*iter != '#')
 678     return send_packet("E52");
 679
 680   if (bp.size != 2 && bp.size != 4) {
 681     return send_packet("E53");
 682   }
 683
 684   processor_t *p = sim->get_core(0);
 685   mmu_t* mmu = p->mmu;
 686   if (insert) {
 687     bp.insert(mmu);
 688     breakpoints[bp.address] = bp;
 689
 690   } else {
 691     bp = breakpoints[bp.address];
 692     bp.remove(mmu);
 693     breakpoints.erase(bp.address);
 694   }
 695   mmu->flush_icache();
 696   sim->debug_mmu->flush_icache();
 697   return send_packet("OK");
 698 }
 699
 700 void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
 701 {
 702   std::string name;
 703   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 704
 705   consume_string(name, iter, packet.end(), ':');
 706   if (iter != packet.end())
 707     iter++;
 708   if (name == "Supported") {
 709     send("$");
 710     running_checksum = 0;
 711     while (iter != packet.end()) {
 712       std::string feature;
 713       consume_string(feature, iter, packet.end(), ';');
 714       if (iter != packet.end())
 715         iter++;
 716       if (feature == "swbreak+") {
 717         send("swbreak+;");
 718       }
 719     }
 720     return send_running_checksum();
 721   }
 722
 723   fprintf(stderr, "Unsupported query %s\n", name.c_str());
 724   return send_packet("");
 725 }
 726
 727 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 728 {
 729   if (compute_checksum(packet) != extract_checksum(packet)) {
 730     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
 731     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
 732     print_packet(packet);
 733     send("-");
 734     return;
 735   }
 736
 737   fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size());
 738   print_packet(packet);
 739   send("+");
 740
 741   switch (packet[1]) {
 742     case '!':
 743       return handle_extended(packet);
 744     case '?':
 745       return handle_halt_reason(packet);
 746     case 'g':
 747       return handle_general_registers_read(packet);
 748     case 'k':
 749       return handle_kill(packet);
 750     case 'm':
 751       return handle_memory_read(packet);
 752 //    case 'M':
 753 //      return handle_memory_write(packet);
 754     case 'X':
 755       return handle_memory_binary_write(packet);
 756     case 'p':
 757       return handle_register_read(packet);
 758     case 'P':
 759       return handle_register_write(packet);
 760     case 'c':
 761       return handle_continue(packet);
 762     case 's':
 763       return handle_step(packet);
 764     case 'z':
 765     case 'Z':
 766       return handle_breakpoint(packet);
 767     case 'q':
 768     case 'Q':
 769       return handle_query(packet);
 770   }
 771
 772   // Not supported.
 773   fprintf(stderr, "** Unsupported packet: ");
 774   print_packet(packet);
 775   send_packet("");
 776 }
 777
 778 void gdbserver_t::handle_interrupt()
 779 {
 780   processor_t *p = sim->get_core(0);
 781   p->set_halted(true, HR_INTERRUPT);
 782   send_packet("S02");   // Pretend program received SIGINT.
 783   running = false;
 784 }
 785
 786 void gdbserver_t::handle()
 787 {
 788   processor_t *p = sim->get_core(0);
 789   if (running && p->halted) {
 790     // The core was running, but now it's halted. Better tell gdb.
 791     switch (p->halt_reason) {
 792       case HR_NONE:
 793         fprintf(stderr, "Internal error. Processor halted without reason.\n");
 794         abort();
 795       case HR_STEPPED:
 796       case HR_INTERRUPT:
 797       case HR_CMDLINE:
 798       case HR_ATTACHED:
 799         // There's no gdb code for this.
 800         send_packet("T05");
 801         break;
 802       case HR_SWBP:
 803         send_packet("T05swbreak:;");
 804         break;
 805     }
 806     send_packet("T00");
 807     // TODO: Actually include register values here
 808     running = false;
 809   }
 810
 811   if (client_fd > 0) {
 812     this->read();
 813     this->write();
 814
 815   } else {
 816     this->accept();
 817   }
 818
 819   this->process_requests();
 820 }
 821
 822 void gdbserver_t::send(const char* msg)
 823 {
 824   unsigned int length = strlen(msg);
 825   for (const char *c = msg; *c; c++)
 826     running_checksum += *c;
 827   send_buf.append((const uint8_t *) msg, length);
 828 }
 829
 830 void gdbserver_t::send(uint64_t value)
 831 {
 832   char buffer[3];
 833   for (unsigned int i = 0; i < 8; i++) {
 834     sprintf(buffer, "%02x", (int) (value & 0xff));
 835     send(buffer);
 836     value >>= 8;
 837   }
 838 }
 839
 840 void gdbserver_t::send(uint32_t value)
 841 {
 842   char buffer[3];
 843   for (unsigned int i = 0; i < 4; i++) {
 844     sprintf(buffer, "%02x", (int) (value & 0xff));
 845     send(buffer);
 846     value >>= 8;
 847   }
 848 }
 849
 850 void gdbserver_t::send_packet(const char* data)
 851 {
 852   send("$");
 853   running_checksum = 0;
 854   send(data);
 855   send_running_checksum();
 856   expect_ack = true;
 857 }
 858
 859 void gdbserver_t::send_running_checksum()
 860 {
 861   char checksum_string[4];
 862   sprintf(checksum_string, "#%02x", running_checksum);
 863   send(checksum_string);
 864 }