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