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