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