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