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