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