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