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