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 #undef DEBUG
  26 #ifdef DEBUG
  27 #  define D(x) x
  28 #else
  29 #  define D(x)
  30 #endif // DEBUG
  31
  32 const int debug_gdbserver = 0;
  33
  34 void die(const char* msg)
  35 {
  36   fprintf(stderr, "gdbserver code died: %s\n", msg);
  37   abort();
  38 }
  39
  40 // gdb's register list is defined in riscv_gdb_reg_names gdb/riscv-tdep.c in
  41 // its source tree. We must interpret the numbers the same here.
  42 enum {
  43   REG_XPR0 = 0,
  44   REG_XPR31 = 31,
  45   REG_PC = 32,
  46   REG_FPR0 = 33,
  47   REG_FPR31 = 64,
  48   REG_CSR0 = 65,
  49   REG_CSR4095 = 4160,
  50   REG_END = 4161
  51 };
  52
  53 //////////////////////////////////////// Functions to generate RISC-V opcodes.
  54
  55 // TODO: Does this already exist somewhere?
  56
  57 #define ZERO    0
  58 // Using regnames.cc as source. The RVG Calling Convention of the 2.0 RISC-V
  59 // spec says it should be 2 and 3.
  60 #define S0      8
  61 #define S1      9
  62 static uint32_t bits(uint32_t value, unsigned int hi, unsigned int lo) {
  63   return (value >> lo) & ((1 << (hi+1-lo)) - 1);
  64 }
  65
  66 static uint32_t bit(uint32_t value, unsigned int b) {
  67   return (value >> b) & 1;
  68 }
  69
  70 static uint32_t jal(unsigned int rd, uint32_t imm) {
  71   return (bit(imm, 20) << 31) |
  72     (bits(imm, 10, 1) << 21) |
  73     (bit(imm, 11) << 20) |
  74     (bits(imm, 19, 12) << 12) |
  75     (rd << 7) |
  76     MATCH_JAL;
  77 }
  78
  79 static uint32_t csrsi(unsigned int csr, uint16_t imm) {
  80   return (csr << 20) |
  81     (bits(imm, 4, 0) << 15) |
  82     MATCH_CSRRSI;
  83 }
  84
  85 static uint32_t csrci(unsigned int csr, uint16_t imm) {
  86   return (csr << 20) |
  87     (bits(imm, 4, 0) << 15) |
  88     MATCH_CSRRCI;
  89 }
  90
  91 static uint32_t csrr(unsigned int rd, unsigned int csr) {
  92   return (csr << 20) | (rd << 7) | MATCH_CSRRS;
  93 }
  94
  95 static uint32_t csrw(unsigned int source, unsigned int csr) {
  96   return (csr << 20) | (source << 15) | MATCH_CSRRW;
  97 }
  98
  99 static uint32_t fence_i()
 100 {
 101   return MATCH_FENCE_I;
 102 }
 103
 104 static uint32_t sb(unsigned int src, unsigned int base, uint16_t offset)
 105 {
 106   return (bits(offset, 11, 5) << 25) |
 107     (src << 20) |
 108     (base << 15) |
 109     (bits(offset, 4, 0) << 7) |
 110     MATCH_SB;
 111 }
 112
 113 static uint32_t sh(unsigned int src, unsigned int base, uint16_t offset)
 114 {
 115   return (bits(offset, 11, 5) << 25) |
 116     (src << 20) |
 117     (base << 15) |
 118     (bits(offset, 4, 0) << 7) |
 119     MATCH_SH;
 120 }
 121
 122 static uint32_t sw(unsigned int src, unsigned int base, uint16_t offset)
 123 {
 124   return (bits(offset, 11, 5) << 25) |
 125     (src << 20) |
 126     (base << 15) |
 127     (bits(offset, 4, 0) << 7) |
 128     MATCH_SW;
 129 }
 130
 131 static uint32_t sd(unsigned int src, unsigned int base, uint16_t offset)
 132 {
 133   return (bits(offset, 11, 5) << 25) |
 134     (bits(src, 4, 0) << 20) |
 135     (base << 15) |
 136     (bits(offset, 4, 0) << 7) |
 137     MATCH_SD;
 138 }
 139
 140 static uint32_t sq(unsigned int src, unsigned int base, uint16_t offset)
 141 {
 142 #if 0
 143   return (bits(offset, 11, 5) << 25) |
 144     (bits(src, 4, 0) << 20) |
 145     (base << 15) |
 146     (bits(offset, 4, 0) << 7) |
 147     MATCH_SQ;
 148 #else
 149   abort();
 150 #endif
 151 }
 152
 153 static uint32_t lq(unsigned int rd, unsigned int base, uint16_t offset)
 154 {
 155 #if 0
 156   return (bits(offset, 11, 0) << 20) |
 157     (base << 15) |
 158     (bits(rd, 4, 0) << 7) |
 159     MATCH_LQ;
 160 #else
 161   abort();
 162 #endif
 163 }
 164
 165 static uint32_t ld(unsigned int rd, unsigned int base, uint16_t offset)
 166 {
 167   return (bits(offset, 11, 0) << 20) |
 168     (base << 15) |
 169     (bits(rd, 4, 0) << 7) |
 170     MATCH_LD;
 171 }
 172
 173 static uint32_t lw(unsigned int rd, unsigned int base, uint16_t offset)
 174 {
 175   return (bits(offset, 11, 0) << 20) |
 176     (base << 15) |
 177     (bits(rd, 4, 0) << 7) |
 178     MATCH_LW;
 179 }
 180
 181 static uint32_t lh(unsigned int rd, unsigned int base, uint16_t offset)
 182 {
 183   return (bits(offset, 11, 0) << 20) |
 184     (base << 15) |
 185     (bits(rd, 4, 0) << 7) |
 186     MATCH_LH;
 187 }
 188
 189 static uint32_t lb(unsigned int rd, unsigned int base, uint16_t offset)
 190 {
 191   return (bits(offset, 11, 0) << 20) |
 192     (base << 15) |
 193     (bits(rd, 4, 0) << 7) |
 194     MATCH_LB;
 195 }
 196
 197 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset)
 198 {
 199   return (bits(offset, 11, 5) << 25) |
 200     (bits(src, 4, 0) << 20) |
 201     (base << 15) |
 202     (bits(offset, 4, 0) << 7) |
 203     MATCH_FSW;
 204 }
 205
 206 static uint32_t fsd(unsigned int src, unsigned int base, uint16_t offset)
 207 {
 208   return (bits(offset, 11, 5) << 25) |
 209     (bits(src, 4, 0) << 20) |
 210     (base << 15) |
 211     (bits(offset, 4, 0) << 7) |
 212     MATCH_FSD;
 213 }
 214
 215 static uint32_t flw(unsigned int src, unsigned int base, uint16_t offset)
 216 {
 217   return (bits(offset, 11, 5) << 25) |
 218     (bits(src, 4, 0) << 20) |
 219     (base << 15) |
 220     (bits(offset, 4, 0) << 7) |
 221     MATCH_FLW;
 222 }
 223
 224 static uint32_t fld(unsigned int src, unsigned int base, uint16_t offset)
 225 {
 226   return (bits(offset, 11, 5) << 25) |
 227     (bits(src, 4, 0) << 20) |
 228     (base << 15) |
 229     (bits(offset, 4, 0) << 7) |
 230     MATCH_FLD;
 231 }
 232
 233 static uint32_t addi(unsigned int dest, unsigned int src, uint16_t imm)
 234 {
 235   return (bits(imm, 11, 0) << 20) |
 236     (src << 15) |
 237     (dest << 7) |
 238     MATCH_ADDI;
 239 }
 240
 241 static uint32_t ori(unsigned int dest, unsigned int src, uint16_t imm)
 242 {
 243   return (bits(imm, 11, 0) << 20) |
 244     (src << 15) |
 245     (dest << 7) |
 246     MATCH_ORI;
 247 }
 248
 249 static uint32_t xori(unsigned int dest, unsigned int src, uint16_t imm)
 250 {
 251   return (bits(imm, 11, 0) << 20) |
 252     (src << 15) |
 253     (dest << 7) |
 254     MATCH_XORI;
 255 }
 256
 257 static uint32_t srli(unsigned int dest, unsigned int src, uint8_t shamt)
 258 {
 259   return (bits(shamt, 4, 0) << 20) |
 260     (src << 15) |
 261     (dest << 7) |
 262     MATCH_SRLI;
 263 }
 264
 265
 266 static uint32_t nop()
 267 {
 268   return addi(0, 0, 0);
 269 }
 270
 271 template <typename T>
 272 unsigned int circular_buffer_t<T>::size() const
 273 {
 274   if (end >= start)
 275     return end - start;
 276   else
 277     return end + capacity - start;
 278 }
 279
 280 template <typename T>
 281 void circular_buffer_t<T>::consume(unsigned int bytes)
 282 {
 283   start = (start + bytes) % capacity;
 284 }
 285
 286 template <typename T>
 287 unsigned int circular_buffer_t<T>::contiguous_empty_size() const
 288 {
 289   if (end >= start)
 290     if (start == 0)
 291       return capacity - end - 1;
 292     else
 293       return capacity - end;
 294   else
 295     return start - end - 1;
 296 }
 297
 298 template <typename T>
 299 unsigned int circular_buffer_t<T>::contiguous_data_size() const
 300 {
 301   if (end >= start)
 302     return end - start;
 303   else
 304     return capacity - start;
 305 }
 306
 307 template <typename T>
 308 void circular_buffer_t<T>::data_added(unsigned int bytes)
 309 {
 310   end += bytes;
 311   assert(end <= capacity);
 312   if (end == capacity)
 313     end = 0;
 314 }
 315
 316 template <typename T>
 317 void circular_buffer_t<T>::reset()
 318 {
 319   start = 0;
 320   end = 0;
 321 }
 322
 323 template <typename T>
 324 void circular_buffer_t<T>::append(const T *src, unsigned int count)
 325 {
 326   unsigned int copy = std::min(count, contiguous_empty_size());
 327   memcpy(contiguous_empty(), src, copy * sizeof(T));
 328   data_added(copy);
 329   count -= copy;
 330   if (count > 0) {
 331     assert(count < contiguous_empty_size());
 332     memcpy(contiguous_empty(), src, count * sizeof(T));
 333     data_added(count);
 334   }
 335 }
 336
 337 ////////////////////////////// Debug Operations
 338
 339 class halt_op_t : public operation_t
 340 {
 341   public:
 342     halt_op_t(gdbserver_t& gdbserver, bool send_status=false) :
 343       operation_t(gdbserver), send_status(send_status),
 344       state(ST_ENTER) {};
 345
 346     void write_dpc_program() {
 347       gs.dr_write32(0, csrsi(CSR_DCSR, DCSR_HALT));
 348       gs.dr_write32(1, csrr(S0, CSR_DPC));
 349       gs.dr_write_store(2, S0, SLOT_DATA0);
 350       gs.dr_write_jump(3);
 351       gs.set_interrupt(0);
 352     }
 353
 354     bool perform_step(unsigned int step) {
 355       switch (state) {
 356         case ST_ENTER:
 357           if (gs.xlen == 0) {
 358             gs.dr_write32(0, xori(S1, ZERO, -1));
 359             gs.dr_write32(1, srli(S1, S1, 31));
 360             // 0x00000001  0x00000001:ffffffff  0x00000001:ffffffff:ffffffff:ffffffff
 361             gs.dr_write32(2, sw(S1, ZERO, DEBUG_RAM_START));
 362             gs.dr_write32(3, srli(S1, S1, 31));
 363             // 0x00000000  0x00000000:00000003  0x00000000:00000003:ffffffff:ffffffff
 364             gs.dr_write32(4, sw(S1, ZERO, DEBUG_RAM_START + 4));
 365             gs.dr_write_jump(5);
 366             gs.set_interrupt(0);
 367             state = ST_XLEN;
 368
 369           } else {
 370             write_dpc_program();
 371             state = ST_DPC;
 372           }
 373           return false;
 374
 375         case ST_XLEN:
 376           {
 377             uint32_t word0 = gs.dr_read32(0);
 378             uint32_t word1 = gs.dr_read32(1);
 379
 380             if (word0 == 1 && word1 == 0) {
 381               gs.xlen = 32;
 382             } else if (word0 == 0xffffffff && word1 == 3) {
 383               gs.xlen = 64;
 384             } else if (word0 == 0xffffffff && word1 == 0xffffffff) {
 385               gs.xlen = 128;
 386             }
 387
 388             write_dpc_program();
 389             state = ST_DPC;
 390             return false;
 391           }
 392
 393         case ST_DPC:
 394           gs.dpc = gs.dr_read(SLOT_DATA0);
 395           fprintf(stderr, "dpc=0x%lx\n", gs.dpc);
 396           gs.dr_write32(0, csrr(S0, CSR_MSTATUS));
 397           gs.dr_write_store(1, S0, SLOT_DATA0);
 398           gs.dr_write_jump(2);
 399           gs.set_interrupt(0);
 400           state = ST_MSTATUS;
 401           return false;
 402
 403         case ST_MSTATUS:
 404           gs.mstatus = gs.dr_read(SLOT_DATA0);
 405           gs.dr_write32(0, csrr(S0, CSR_DCSR));
 406           gs.dr_write32(1, sw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 407           gs.dr_write_jump(2);
 408           gs.set_interrupt(0);
 409           state = ST_DCSR;
 410           return false;
 411
 412         case ST_DCSR:
 413           gs.dcsr = gs.dr_read32(4);
 414
 415           gs.sptbr_valid = false;
 416           gs.pte_cache.clear();
 417
 418           if (send_status) {
 419             switch (get_field(gs.dcsr, DCSR_CAUSE)) {
 420               case DCSR_CAUSE_NONE:
 421                 fprintf(stderr, "Internal error. Processor halted without reason.\n");
 422                 abort();
 423
 424               case DCSR_CAUSE_DEBUGINT:
 425                 gs.send_packet("S02");   // Pretend program received SIGINT.
 426                 break;
 427
 428               case DCSR_CAUSE_HWBP:
 429               case DCSR_CAUSE_STEP:
 430               case DCSR_CAUSE_HALT:
 431                 // There's no gdb code for this.
 432                 gs.send_packet("T05");
 433                 break;
 434               case DCSR_CAUSE_SWBP:
 435                 gs.send_packet("T05swbreak:;");
 436                 break;
 437             }
 438           }
 439           return true;
 440
 441         default:
 442           assert(0);
 443       }
 444     }
 445
 446   private:
 447     bool send_status;
 448     enum {
 449       ST_ENTER,
 450       ST_XLEN,
 451       ST_DPC,
 452       ST_MSTATUS,
 453       ST_DCSR
 454     } state;
 455 };
 456
 457 class continue_op_t : public operation_t
 458 {
 459   public:
 460     continue_op_t(gdbserver_t& gdbserver, bool single_step) :
 461       operation_t(gdbserver), single_step(single_step) {};
 462
 463     bool perform_step(unsigned int step) {
 464       switch (step) {
 465         case 0:
 466           gs.dr_write_load(0, S0, SLOT_DATA0);
 467           gs.dr_write32(1, csrw(S0, CSR_DPC));
 468           // TODO: Isn't there a fence.i in Debug ROM already?
 469           if (gs.fence_i_required) {
 470             gs.dr_write32(2, fence_i());
 471             gs.dr_write_jump(3);
 472             gs.fence_i_required = false;
 473           } else {
 474             gs.dr_write_jump(2);
 475           }
 476           gs.dr_write(SLOT_DATA0, gs.dpc);
 477           gs.set_interrupt(0);
 478           return false;
 479
 480         case 1:
 481           gs.dr_write_load(0, S0, SLOT_DATA0);
 482           gs.dr_write32(1, csrw(S0, CSR_MSTATUS));
 483           gs.dr_write_jump(2);
 484           gs.dr_write(SLOT_DATA0, gs.mstatus);
 485           gs.set_interrupt(0);
 486           return false;
 487
 488         case 2:
 489           gs.dr_write32(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 490           gs.dr_write32(1, csrw(S0, CSR_DCSR));
 491           gs.dr_write_jump(2);
 492
 493           reg_t dcsr = set_field(gs.dcsr, DCSR_HALT, 0);
 494           dcsr = set_field(dcsr, DCSR_STEP, single_step);
 495           // Software breakpoints should go here.
 496           dcsr = set_field(dcsr, DCSR_EBREAKM, 1);
 497           dcsr = set_field(dcsr, DCSR_EBREAKH, 1);
 498           dcsr = set_field(dcsr, DCSR_EBREAKS, 1);
 499           dcsr = set_field(dcsr, DCSR_EBREAKU, 1);
 500           gs.dr_write32(4, dcsr);
 501
 502           gs.set_interrupt(0);
 503           return true;
 504       }
 505       return false;
 506     }
 507
 508   private:
 509     bool single_step;
 510 };
 511
 512 class general_registers_read_op_t : public operation_t
 513 {
 514   // Register order that gdb expects is:
 515   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 516   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 517   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 518   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 519
 520   // Each byte of register data is described by two hex digits. The bytes with
 521   // the register are transmitted in target byte order. The size of each
 522   // register and their position within the ‘g’ packet are determined by the
 523   // gdb internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and
 524   // gdbarch_register_name.
 525
 526   public:
 527     general_registers_read_op_t(gdbserver_t& gdbserver) :
 528       operation_t(gdbserver) {};
 529
 530     bool perform_step(unsigned int step)
 531     {
 532       D(fprintf(stderr, "register_read step %d\n", step));
 533       if (step == 0) {
 534         gs.start_packet();
 535
 536         // x0 is always zero.
 537         if (gs.xlen == 32) {
 538           gs.send((uint32_t) 0);
 539         } else {
 540           gs.send((uint64_t) 0);
 541         }
 542
 543         gs.dr_write_store(0, 1, SLOT_DATA0);
 544         gs.dr_write_store(1, 2, SLOT_DATA1);
 545         gs.dr_write_jump(2);
 546         gs.set_interrupt(0);
 547         return false;
 548       }
 549
 550       if (gs.xlen == 32) {
 551         gs.send((uint32_t) gs.dr_read(SLOT_DATA0));
 552       } else {
 553         gs.send((uint64_t) gs.dr_read(SLOT_DATA0));
 554       }
 555       if (step >= 16) {
 556         gs.end_packet();
 557         return true;
 558       }
 559
 560       if (gs.xlen == 32) {
 561         gs.send((uint32_t) gs.dr_read(SLOT_DATA1));
 562       } else {
 563         gs.send((uint64_t) gs.dr_read(SLOT_DATA1));
 564       }
 565
 566       unsigned int current_reg = 2 * step + 1;
 567       unsigned int i = 0;
 568       if (current_reg == S1) {
 569         gs.dr_write_load(i++, S1, SLOT_DATA_LAST);
 570       }
 571       gs.dr_write_store(i++, current_reg, SLOT_DATA0);
 572       if (current_reg + 1 == S0) {
 573         gs.dr_write32(i++, csrr(S0, CSR_DSCRATCH));
 574       }
 575       if (step < 15) {
 576         gs.dr_write_store(i++, current_reg+1, SLOT_DATA1);
 577       }
 578       gs.dr_write_jump(i);
 579       gs.set_interrupt(0);
 580
 581       return false;
 582     }
 583 };
 584
 585 class register_read_op_t : public operation_t
 586 {
 587   public:
 588     register_read_op_t(gdbserver_t& gdbserver, unsigned int reg) :
 589       operation_t(gdbserver), reg(reg) {};
 590
 591     bool perform_step(unsigned int step)
 592     {
 593       switch (step) {
 594         case 0:
 595           if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 596             die("handle_register_read");
 597             // send(p->state.XPR[reg - REG_XPR0]);
 598           } else if (reg == REG_PC) {
 599             gs.start_packet();
 600             if (gs.xlen == 32) {
 601               gs.send((uint32_t) gs.dpc);
 602             } else {
 603               gs.send(gs.dpc);
 604             }
 605             gs.end_packet();
 606             return true;
 607           } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 608             // send(p->state.FPR[reg - REG_FPR0]);
 609             if (gs.xlen == 32) {
 610               gs.dr_write32(0, fsw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 611             } else {
 612               gs.dr_write32(0, fsd(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 613             }
 614             gs.dr_write_jump(1);
 615           } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 616             gs.dr_write32(0, csrr(S0, reg - REG_CSR0));
 617             gs.dr_write_store(1, S0, SLOT_DATA0);
 618             gs.dr_write_jump(2);
 619             // If we hit an exception reading the CSR, we'll end up returning ~0 as
 620             // the register's value, which is what we want. (Right?)
 621             gs.dr_write(SLOT_DATA0, ~(uint64_t) 0);
 622           } else {
 623             gs.send_packet("E02");
 624             return true;
 625           }
 626           gs.set_interrupt(0);
 627           return false;
 628
 629         case 1:
 630           gs.start_packet();
 631           if (gs.xlen == 32) {
 632             gs.send(gs.dr_read32(4));
 633           } else {
 634             gs.send(gs.dr_read(SLOT_DATA0));
 635           }
 636           gs.end_packet();
 637           return true;
 638       }
 639       return false;
 640     }
 641
 642   private:
 643     unsigned int reg;
 644 };
 645
 646 class register_write_op_t : public operation_t
 647 {
 648   public:
 649     register_write_op_t(gdbserver_t& gdbserver, unsigned int reg, reg_t value) :
 650       operation_t(gdbserver), reg(reg), value(value) {};
 651
 652     bool perform_step(unsigned int step)
 653     {
 654       gs.dr_write_load(0, S0, SLOT_DATA0);
 655       gs.dr_write(SLOT_DATA0, value);
 656       if (reg == S0) {
 657         gs.dr_write32(1, csrw(S0, CSR_DSCRATCH));
 658         gs.dr_write_jump(2);
 659       } else if (reg == S1) {
 660         gs.dr_write_store(1, S0, SLOT_DATA_LAST);
 661         gs.dr_write_jump(2);
 662       } else if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 663         gs.dr_write32(1, addi(reg, S0, 0));
 664         gs.dr_write_jump(2);
 665       } else if (reg == REG_PC) {
 666         gs.dpc = value;
 667         return true;
 668       } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 669         if (gs.xlen == 32) {
 670           gs.dr_write32(0, flw(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 671         } else {
 672           gs.dr_write32(0, fld(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 673         }
 674         gs.dr_write_jump(1);
 675       } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 676         gs.dr_write32(1, csrw(S0, reg - REG_CSR0));
 677         gs.dr_write_jump(2);
 678         if (reg == REG_CSR0 + CSR_SPTBR) {
 679           gs.sptbr = value;
 680           gs.sptbr_valid = true;
 681         }
 682       } else {
 683         gs.send_packet("E02");
 684         return true;
 685       }
 686       gs.set_interrupt(0);
 687       gs.send_packet("OK");
 688       return true;
 689     }
 690
 691   private:
 692     unsigned int reg;
 693     reg_t value;
 694 };
 695
 696 class memory_read_op_t : public operation_t
 697 {
 698   public:
 699     // Read length bytes from vaddr, storing the result into data.
 700     // If data is NULL, send the result straight to gdb.
 701     memory_read_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
 702         unsigned char *data=NULL) :
 703       operation_t(gdbserver), vaddr(vaddr), length(length), data(data) {};
 704
 705     bool perform_step(unsigned int step)
 706     {
 707       if (step == 0) {
 708         // address goes in S0
 709         paddr = gs.translate(vaddr);
 710         access_size = gs.find_access_size(paddr, length);
 711
 712         gs.dr_write_load(0, S0, SLOT_DATA0);
 713         switch (access_size) {
 714           case 1:
 715             gs.dr_write32(1, lb(S1, S0, 0));
 716             break;
 717           case 2:
 718             gs.dr_write32(1, lh(S1, S0, 0));
 719             break;
 720           case 4:
 721             gs.dr_write32(1, lw(S1, S0, 0));
 722             break;
 723           case 8:
 724             gs.dr_write32(1, ld(S1, S0, 0));
 725             break;
 726         }
 727         gs.dr_write_store(2, S1, SLOT_DATA1);
 728         gs.dr_write_jump(3);
 729         gs.dr_write(SLOT_DATA0, paddr);
 730         gs.set_interrupt(0);
 731
 732         if (!data) {
 733           gs.start_packet();
 734         }
 735         return false;
 736       }
 737
 738       char buffer[3];
 739       reg_t value = gs.dr_read(SLOT_DATA1);
 740       for (unsigned int i = 0; i < access_size; i++) {
 741         if (data) {
 742           *(data++) = value & 0xff;
 743           D(fprintf(stderr, "%02x", (unsigned int) (value & 0xff)));
 744         } else {
 745           sprintf(buffer, "%02x", (unsigned int) (value & 0xff));
 746           gs.send(buffer);
 747         }
 748         value >>= 8;
 749       }
 750       if (data && debug_gdbserver) {
 751         D(fprintf(stderr, "\n"));
 752       }
 753       length -= access_size;
 754       paddr += access_size;
 755
 756       if (length == 0) {
 757         if (!data) {
 758           gs.end_packet();
 759         }
 760         return true;
 761       } else {
 762         gs.dr_write(SLOT_DATA0, paddr);
 763         gs.set_interrupt(0);
 764         return false;
 765       }
 766     }
 767
 768   private:
 769     reg_t vaddr;
 770     unsigned int length;
 771     unsigned char* data;
 772     reg_t paddr;
 773     unsigned int access_size;
 774 };
 775
 776 class memory_write_op_t : public operation_t
 777 {
 778   public:
 779     memory_write_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
 780         const unsigned char *data) :
 781       operation_t(gdbserver), vaddr(vaddr), offset(0), length(length), data(data) {};
 782
 783     ~memory_write_op_t() {
 784       delete[] data;
 785     }
 786
 787     bool perform_step(unsigned int step)
 788     {
 789       reg_t paddr = gs.translate(vaddr);
 790
 791       unsigned int data_offset;
 792       switch (gs.xlen) {
 793         case 32:
 794           data_offset = slot_offset32[SLOT_DATA1];
 795           break;
 796         case 64:
 797           data_offset = slot_offset64[SLOT_DATA1];
 798           break;
 799         case 128:
 800           data_offset = slot_offset128[SLOT_DATA1];
 801           break;
 802         default:
 803           abort();
 804       }
 805
 806       if (step == 0) {
 807         access_size = gs.find_access_size(paddr, length);
 808
 809         D(fprintf(stderr, "write to 0x%lx -> 0x%lx (access=%d): ", vaddr, paddr,
 810             access_size));
 811         for (unsigned int i = 0; i < length; i++) {
 812           D(fprintf(stderr, "%02x", data[i]));
 813         }
 814         D(fprintf(stderr, "\n"));
 815
 816         // address goes in S0
 817         gs.dr_write_load(0, S0, SLOT_DATA0);
 818         switch (access_size) {
 819           case 1:
 820             gs.dr_write32(1, lb(S1, 0, (uint16_t) DEBUG_RAM_START + 4*data_offset));
 821             gs.dr_write32(2, sb(S1, S0, 0));
 822             gs.dr_write32(data_offset, data[0]);
 823             break;
 824           case 2:
 825             gs.dr_write32(1, lh(S1, 0, (uint16_t) DEBUG_RAM_START + 4*data_offset));
 826             gs.dr_write32(2, sh(S1, S0, 0));
 827             gs.dr_write32(data_offset, data[0] | (data[1] << 8));
 828             break;
 829           case 4:
 830             gs.dr_write32(1, lw(S1, 0, (uint16_t) DEBUG_RAM_START + 4*data_offset));
 831             gs.dr_write32(2, sw(S1, S0, 0));
 832             gs.dr_write32(data_offset, data[0] | (data[1] << 8) |
 833                 (data[2] << 16) | (data[3] << 24));
 834             break;
 835           case 8:
 836             gs.dr_write32(1, ld(S1, 0, (uint16_t) DEBUG_RAM_START + 4*data_offset));
 837             gs.dr_write32(2, sd(S1, S0, 0));
 838             gs.dr_write32(data_offset, data[0] | (data[1] << 8) |
 839                 (data[2] << 16) | (data[3] << 24));
 840             gs.dr_write32(data_offset+1, data[4] | (data[5] << 8) |
 841                 (data[6] << 16) | (data[7] << 24));
 842             break;
 843           default:
 844             fprintf(stderr, "gdbserver error: write %d bytes to 0x%lx -> 0x%lx; "
 845                 "access_size=%d\n", length, vaddr, paddr, access_size);
 846             gs.send_packet("E12");
 847             return true;
 848         }
 849         gs.dr_write_jump(3);
 850         gs.dr_write(SLOT_DATA0, paddr);
 851         gs.set_interrupt(0);
 852
 853         return false;
 854       }
 855
 856       if (gs.dr_read32(DEBUG_RAM_SIZE / 4 - 1)) {
 857         fprintf(stderr, "Exception happened while writing to 0x%lx -> 0x%lx\n",
 858             vaddr, paddr);
 859       }
 860
 861       offset += access_size;
 862       if (offset >= length) {
 863         gs.send_packet("OK");
 864         return true;
 865       } else {
 866         const unsigned char *d = data + offset;
 867         switch (access_size) {
 868           case 1:
 869             gs.dr_write32(data_offset, d[0]);
 870             break;
 871           case 2:
 872             gs.dr_write32(data_offset, d[0] | (d[1] << 8));
 873             break;
 874           case 4:
 875             gs.dr_write32(data_offset, d[0] | (d[1] << 8) |
 876                 (d[2] << 16) | (d[3] << 24));
 877             break;
 878           case 8:
 879             gs.dr_write32(data_offset, d[0] | (d[1] << 8) |
 880                 (d[2] << 16) | (d[3] << 24));
 881             gs.dr_write32(data_offset+1, d[4] | (d[5] << 8) |
 882                 (d[6] << 16) | (d[7] << 24));
 883             break;
 884           default:
 885             gs.send_packet("E13");
 886             return true;
 887         }
 888         gs.dr_write(SLOT_DATA0, paddr + offset);
 889         gs.set_interrupt(0);
 890         return false;
 891       }
 892     }
 893
 894   private:
 895     reg_t vaddr;
 896     unsigned int offset;
 897     unsigned int length;
 898     unsigned int access_size;
 899     const unsigned char *data;
 900 };
 901
 902 class collect_translation_info_op_t : public operation_t
 903 {
 904   public:
 905     // Read sufficient information from the target into gdbserver structures so
 906     // that it's possible to translate vaddr, vaddr+length, and all addresses
 907     // in between to physical addresses.
 908     collect_translation_info_op_t(gdbserver_t& gdbserver, reg_t vaddr, size_t length) :
 909       operation_t(gdbserver), state(STATE_START), vaddr(vaddr), length(length) {};
 910
 911     bool perform_step(unsigned int step)
 912     {
 913       unsigned int vm = gs.virtual_memory();
 914
 915       if (step == 0) {
 916         switch (vm) {
 917           case VM_MBARE:
 918             // Nothing to be done.
 919             return true;
 920
 921           case VM_SV32:
 922             levels = 2;
 923             ptidxbits = 10;
 924             ptesize = 4;
 925             break;
 926           case VM_SV39:
 927             levels = 3;
 928             ptidxbits = 9;
 929             ptesize = 8;
 930             break;
 931           case VM_SV48:
 932             levels = 4;
 933             ptidxbits = 9;
 934             ptesize = 8;
 935             break;
 936
 937           default:
 938             {
 939               char buf[100];
 940               sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
 941               die(buf);
 942               return true;        // die doesn't return, but gcc doesn't know that.
 943             }
 944         }
 945       }
 946
 947       // Perform any reads from the just-completed action.
 948       switch (state) {
 949         case STATE_START:
 950           break;
 951         case STATE_READ_SPTBR:
 952           gs.sptbr = ((uint64_t) gs.dr_read32(5) << 32) | gs.dr_read32(4);
 953           gs.sptbr_valid = true;
 954           break;
 955         case STATE_READ_PTE:
 956           gs.pte_cache[pte_addr] = ((uint64_t) gs.dr_read32(5) << 32) |
 957             gs.dr_read32(4);
 958           D(fprintf(stderr, "pte_cache[0x%lx] = 0x%lx\n", pte_addr, gs.pte_cache[pte_addr]));
 959           break;
 960       }
 961
 962       // Set up the next action.
 963       // We only get here for VM_SV32/39/38.
 964
 965       if (!gs.sptbr_valid) {
 966         state = STATE_READ_SPTBR;
 967         gs.dr_write32(0, csrr(S0, CSR_SPTBR));
 968         gs.dr_write32(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 969         gs.dr_write32(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 970         gs.set_interrupt(0);
 971         return false;
 972       }
 973
 974       reg_t base = gs.sptbr << PGSHIFT;
 975       int ptshift = (levels - 1) * ptidxbits;
 976       for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
 977         reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
 978
 979         pte_addr = base + idx * ptesize;
 980         auto it = gs.pte_cache.find(pte_addr);
 981         if (it == gs.pte_cache.end()) {
 982           state = STATE_READ_PTE;
 983           if (ptesize == 4) {
 984             gs.dr_write32(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 985             gs.dr_write32(1, lw(S1, S0, 0));
 986             gs.dr_write32(2, sw(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 987           } else {
 988             assert(gs.xlen >= 64);
 989             gs.dr_write32(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 990             gs.dr_write32(1, ld(S1, S0, 0));
 991             gs.dr_write32(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 992           }
 993           gs.dr_write32(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 994           gs.dr_write32(4, pte_addr);
 995           gs.dr_write32(5, pte_addr >> 32);
 996           gs.set_interrupt(0);
 997           return false;
 998         }
 999
1000         reg_t pte = gs.pte_cache[pte_addr];
1001         reg_t ppn = pte >> PTE_PPN_SHIFT;
1002
1003         if (PTE_TABLE(pte)) { // next level of page table
1004           base = ppn << PGSHIFT;
1005         } else {
1006           // We've collected all the data required for the translation.
1007           return true;
1008         }
1009       }
1010       fprintf(stderr,
1011           "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%lx\n",
1012           vaddr);
1013       return true;
1014     }
1015
1016   private:
1017     enum {
1018       STATE_START,
1019       STATE_READ_SPTBR,
1020       STATE_READ_PTE
1021     } state;
1022     reg_t vaddr;
1023     size_t length;
1024     unsigned int levels;
1025     unsigned int ptidxbits;
1026     unsigned int ptesize;
1027     reg_t pte_addr;
1028 };
1029
1030 ////////////////////////////// gdbserver itself
1031
1032 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
1033   xlen(0),
1034   sim(sim),
1035   client_fd(0),
1036   recv_buf(64 * 1024), send_buf(64 * 1024)
1037 {
1038   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
1039   if (socket_fd == -1) {
1040     fprintf(stderr, "failed to make socket: %s (%d)\n", strerror(errno), errno);
1041     abort();
1042   }
1043
1044   fcntl(socket_fd, F_SETFL, O_NONBLOCK);
1045   int reuseaddr = 1;
1046   if (setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,
1047         sizeof(int)) == -1) {
1048     fprintf(stderr, "failed setsockopt: %s (%d)\n", strerror(errno), errno);
1049     abort();
1050   }
1051
1052   struct sockaddr_in addr;
1053   memset(&addr, 0, sizeof(addr));
1054   addr.sin_family = AF_INET;
1055   addr.sin_addr.s_addr = INADDR_ANY;
1056   addr.sin_port = htons(port);
1057
1058   if (bind(socket_fd, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
1059     fprintf(stderr, "failed to bind socket: %s (%d)\n", strerror(errno), errno);
1060     abort();
1061   }
1062
1063   if (listen(socket_fd, 1) == -1) {
1064     fprintf(stderr, "failed to listen on socket: %s (%d)\n", strerror(errno), errno);
1065     abort();
1066   }
1067 }
1068
1069 unsigned int gdbserver_t::find_access_size(reg_t address, int length)
1070 {
1071   reg_t composite = address | length;
1072   if ((composite & 0x7) == 0 && xlen >= 64)
1073     return 8;
1074   if ((composite & 0x3) == 0)
1075     return 4;
1076   return 1;
1077 }
1078
1079 reg_t gdbserver_t::translate(reg_t vaddr)
1080 {
1081   unsigned int vm = virtual_memory();
1082   unsigned int levels, ptidxbits, ptesize;
1083
1084   switch (vm) {
1085     case VM_MBARE:
1086       return vaddr;
1087
1088     case VM_SV32:
1089       levels = 2;
1090       ptidxbits = 10;
1091       ptesize = 4;
1092       break;
1093     case VM_SV39:
1094       levels = 3;
1095       ptidxbits = 9;
1096       ptesize = 8;
1097       break;
1098     case VM_SV48:
1099       levels = 4;
1100       ptidxbits = 9;
1101       ptesize = 8;
1102       break;
1103
1104     default:
1105       {
1106         char buf[100];
1107         sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
1108         die(buf);
1109         return true;        // die doesn't return, but gcc doesn't know that.
1110       }
1111   }
1112
1113   // Handle page tables here. There's a bunch of duplicated code with
1114   // collect_translation_info_op_t. :-(
1115   reg_t base = sptbr << PGSHIFT;
1116   int ptshift = (levels - 1) * ptidxbits;
1117   for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
1118     reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
1119
1120     reg_t pte_addr = base + idx * ptesize;
1121     auto it = pte_cache.find(pte_addr);
1122     if (it == pte_cache.end()) {
1123       fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx without first "
1124           "collecting the relevant PTEs.\n", vaddr);
1125       die("gdbserver_t::translate()");
1126     }
1127
1128     reg_t pte = pte_cache[pte_addr];
1129     reg_t ppn = pte >> PTE_PPN_SHIFT;
1130
1131     if (PTE_TABLE(pte)) { // next level of page table
1132       base = ppn << PGSHIFT;
1133     } else {
1134       // We've collected all the data required for the translation.
1135       reg_t vpn = vaddr >> PGSHIFT;
1136       reg_t paddr = (ppn | (vpn & ((reg_t(1) << ptshift) - 1))) << PGSHIFT;
1137       paddr += vaddr & (PGSIZE-1);
1138       D(fprintf(stderr, "gdbserver translate 0x%lx -> 0x%lx\n", vaddr, paddr));
1139       return paddr;
1140     }
1141   }
1142
1143   fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx but the relevant "
1144       "PTEs are invalid.\n", vaddr);
1145   // TODO: Is it better to throw an exception here?
1146   return -1;
1147 }
1148
1149 unsigned int gdbserver_t::privilege_mode()
1150 {
1151   unsigned int mode = get_field(dcsr, DCSR_PRV);
1152   if (get_field(mstatus, MSTATUS_MPRV))
1153     mode = get_field(mstatus, MSTATUS_MPP);
1154   return mode;
1155 }
1156
1157 unsigned int gdbserver_t::virtual_memory()
1158 {
1159   unsigned int mode = privilege_mode();
1160   if (mode == PRV_M)
1161     return VM_MBARE;
1162   return get_field(mstatus, MSTATUS_VM);
1163 }
1164
1165 void gdbserver_t::dr_write32(unsigned int index, uint32_t value)
1166 {
1167   sim->debug_module.ram_write32(index, value);
1168 }
1169
1170 void gdbserver_t::dr_write64(unsigned int index, uint64_t value)
1171 {
1172   dr_write32(index, value);
1173   dr_write32(index+1, value >> 32);
1174 }
1175
1176 void gdbserver_t::dr_write(enum slot slot, uint64_t value)
1177 {
1178   switch (xlen) {
1179     case 32:
1180       dr_write32(slot_offset32[slot], value);
1181       break;
1182     case 64:
1183       dr_write64(slot_offset64[slot], value);
1184       break;
1185     case 128:
1186     default:
1187       abort();
1188   }
1189 }
1190
1191 void gdbserver_t::dr_write_jump(unsigned int index)
1192 {
1193   dr_write32(index, jal(0,
1194         (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*index))));
1195 }
1196
1197 void gdbserver_t::dr_write_store(unsigned int index, unsigned int reg, enum slot slot)
1198 {
1199   assert(slot != SLOT_INST0 || index > 2);
1200   assert(slot != SLOT_DATA0 || index < 4 || index > 6);
1201   assert(slot != SLOT_DATA1 || index < 5 || index > 10);
1202   assert(slot != SLOT_DATA_LAST || index < 6 || index > 14);
1203   switch (xlen) {
1204     case 32:
1205       return dr_write32(index,
1206           sw(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset32[slot]));
1207     case 64:
1208       return dr_write32(index,
1209           sd(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset64[slot]));
1210     case 128:
1211       return dr_write32(index,
1212           sq(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset128[slot]));
1213     default:
1214       fprintf(stderr, "xlen is %d!\n", xlen);
1215       abort();
1216   }
1217 }
1218
1219 void gdbserver_t::dr_write_load(unsigned int index, unsigned int reg, enum slot slot)
1220 {
1221   switch (xlen) {
1222     case 32:
1223       return dr_write32(index,
1224           lw(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset32[slot]));
1225     case 64:
1226       return dr_write32(index,
1227           ld(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset64[slot]));
1228     case 128:
1229       return dr_write32(index,
1230           lq(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset128[slot]));
1231     default:
1232       fprintf(stderr, "xlen is %d!\n", xlen);
1233       abort();
1234   }
1235 }
1236
1237 uint32_t gdbserver_t::dr_read32(unsigned int index)
1238 {
1239   uint32_t value = sim->debug_module.ram_read32(index);
1240   D(fprintf(stderr, "read32(%d) -> 0x%x\n", index, value));
1241   return value;
1242 }
1243
1244 uint64_t gdbserver_t::dr_read64(unsigned int index)
1245 {
1246   return ((uint64_t) dr_read32(index+1) << 32) | dr_read32(index);
1247 }
1248
1249 uint64_t gdbserver_t::dr_read(enum slot slot)
1250 {
1251   switch (xlen) {
1252     case 32:
1253       return dr_read32(slot_offset32[slot]);
1254     case 64:
1255       return dr_read64(slot_offset64[slot]);
1256     case 128:
1257       abort();
1258     default:
1259       abort();
1260   }
1261 }
1262
1263 void gdbserver_t::add_operation(operation_t* operation)
1264 {
1265   operation_queue.push(operation);
1266 }
1267
1268 void gdbserver_t::accept()
1269 {
1270   client_fd = ::accept(socket_fd, NULL, NULL);
1271   if (client_fd == -1) {
1272     if (errno == EAGAIN) {
1273       // No client waiting to connect right now.
1274     } else {
1275       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno),
1276           errno);
1277       abort();
1278     }
1279   } else {
1280     fcntl(client_fd, F_SETFL, O_NONBLOCK);
1281
1282     expect_ack = false;
1283     extended_mode = false;
1284
1285     // gdb wants the core to be halted when it attaches.
1286     add_operation(new halt_op_t(*this));
1287   }
1288 }
1289
1290 void gdbserver_t::read()
1291 {
1292   // Reading from a non-blocking socket still blocks if there is no data
1293   // available.
1294
1295   size_t count = recv_buf.contiguous_empty_size();
1296   assert(count > 0);
1297   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
1298   if (bytes == -1) {
1299     if (errno == EAGAIN) {
1300       // We'll try again the next call.
1301     } else {
1302       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
1303       abort();
1304     }
1305   } else if (bytes == 0) {
1306     // The remote disconnected.
1307     client_fd = 0;
1308     processor_t *p = sim->get_core(0);
1309     // TODO p->set_halted(false, HR_NONE);
1310     recv_buf.reset();
1311     send_buf.reset();
1312   } else {
1313     recv_buf.data_added(bytes);
1314   }
1315 }
1316
1317 void gdbserver_t::write()
1318 {
1319   if (send_buf.empty())
1320     return;
1321
1322   while (!send_buf.empty()) {
1323     unsigned int count = send_buf.contiguous_data_size();
1324     assert(count > 0);
1325     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
1326     if (bytes == -1) {
1327       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
1328       abort();
1329     } else if (bytes == 0) {
1330       // Client can't take any more data right now.
1331       break;
1332     } else {
1333       D(fprintf(stderr, "wrote %ld bytes: ", bytes));
1334       for (unsigned int i = 0; i < bytes; i++) {
1335         D(fprintf(stderr, "%c", send_buf[i]));
1336       }
1337       D(fprintf(stderr, "\n"));
1338       send_buf.consume(bytes);
1339     }
1340   }
1341 }
1342
1343 void print_packet(const std::vector<uint8_t> &packet)
1344 {
1345   for (uint8_t c : packet) {
1346     if (c >= ' ' and c <= '~')
1347       fprintf(stderr, "%c", c);
1348     else
1349       fprintf(stderr, "\\x%02x", c);
1350   }
1351   fprintf(stderr, "\n");
1352 }
1353
1354 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
1355 {
1356   uint8_t checksum = 0;
1357   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
1358     checksum += *i;
1359   }
1360   return checksum;
1361 }
1362
1363 uint8_t character_hex_value(uint8_t character)
1364 {
1365   if (character >= '0' && character <= '9')
1366     return character - '0';
1367   if (character >= 'a' && character <= 'f')
1368     return 10 + character - 'a';
1369   if (character >= 'A' && character <= 'F')
1370     return 10 + character - 'A';
1371   return 0xff;
1372 }
1373
1374 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
1375 {
1376   return character_hex_value(*(packet.end() - 1)) +
1377     16 * character_hex_value(*(packet.end() - 2));
1378 }
1379
1380 void gdbserver_t::process_requests()
1381 {
1382   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
1383
1384   while (!recv_buf.empty()) {
1385     std::vector<uint8_t> packet;
1386     for (unsigned int i = 0; i < recv_buf.size(); i++) {
1387       uint8_t b = recv_buf[i];
1388
1389       if (packet.empty() && expect_ack && b == '+') {
1390         recv_buf.consume(1);
1391         break;
1392       }
1393
1394       if (packet.empty() && b == 3) {
1395         D(fprintf(stderr, "Received interrupt\n"));
1396         recv_buf.consume(1);
1397         handle_interrupt();
1398         break;
1399       }
1400
1401       if (b == '$') {
1402         // Start of new packet.
1403         if (!packet.empty()) {
1404           fprintf(stderr, "Received malformed %ld-byte packet from debug client: ",
1405               packet.size());
1406           print_packet(packet);
1407           recv_buf.consume(i);
1408           break;
1409         }
1410       }
1411
1412       packet.push_back(b);
1413
1414       // Packets consist of $<packet-data>#<checksum>
1415       // where <checksum> is
1416       if (packet.size() >= 4 &&
1417           packet[packet.size()-3] == '#') {
1418         handle_packet(packet);
1419         recv_buf.consume(i+1);
1420         break;
1421       }
1422     }
1423     // There's a partial packet in the buffer. Wait until we get more data to
1424     // process it.
1425     if (packet.size()) {
1426       break;
1427     }
1428   }
1429 }
1430
1431 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
1432 {
1433   send_packet("S00");
1434 }
1435
1436 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
1437 {
1438   add_operation(new general_registers_read_op_t(*this));
1439 }
1440
1441 void gdbserver_t::set_interrupt(uint32_t hartid) {
1442   sim->debug_module.set_interrupt(hartid);
1443 }
1444
1445 // First byte is the most-significant one.
1446 // Eg. "08675309" becomes 0x08675309.
1447 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
1448     std::vector<uint8_t>::const_iterator end)
1449 {
1450   uint64_t value = 0;
1451
1452   while (iter != end) {
1453     uint8_t c = *iter;
1454     uint64_t c_value = character_hex_value(c);
1455     if (c_value > 15)
1456       break;
1457     iter++;
1458     value <<= 4;
1459     value += c_value;
1460   }
1461   return value;
1462 }
1463
1464 // First byte is the least-significant one.
1465 // Eg. "08675309" becomes 0x09536708
1466 uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
1467     std::vector<uint8_t>::const_iterator end)
1468 {
1469   uint64_t value = 0;
1470   unsigned int shift = 4;
1471
1472   while (iter != end) {
1473     uint8_t c = *iter;
1474     uint64_t c_value = character_hex_value(c);
1475     if (c_value > 15)
1476       break;
1477     iter++;
1478     value |= c_value << shift;
1479     if ((shift % 8) == 0)
1480       shift += 12;
1481     else
1482       shift -= 4;
1483   }
1484   return value;
1485 }
1486
1487 void consume_string(std::string &str, std::vector<uint8_t>::const_iterator &iter,
1488     std::vector<uint8_t>::const_iterator end, uint8_t separator)
1489 {
1490   while (iter != end && *iter != separator) {
1491     str.append(1, (char) *iter);
1492     iter++;
1493   }
1494 }
1495
1496 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
1497 {
1498   // p n
1499
1500   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1501   unsigned int n = consume_hex_number(iter, packet.end());
1502   if (*iter != '#')
1503     return send_packet("E01");
1504
1505   add_operation(new register_read_op_t(*this, n));
1506 }
1507
1508 void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
1509 {
1510   // P n...=r...
1511
1512   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1513   unsigned int n = consume_hex_number(iter, packet.end());
1514   if (*iter != '=')
1515     return send_packet("E05");
1516   iter++;
1517
1518   reg_t value = consume_hex_number_le(iter, packet.end());
1519   if (*iter != '#')
1520     return send_packet("E06");
1521
1522   processor_t *p = sim->get_core(0);
1523
1524   add_operation(new register_write_op_t(*this, n, value));
1525
1526   return send_packet("OK");
1527 }
1528
1529 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
1530 {
1531   // m addr,length
1532   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1533   reg_t address = consume_hex_number(iter, packet.end());
1534   if (*iter != ',')
1535     return send_packet("E10");
1536   iter++;
1537   reg_t length = consume_hex_number(iter, packet.end());
1538   if (*iter != '#')
1539     return send_packet("E11");
1540
1541   add_operation(new collect_translation_info_op_t(*this, address, length));
1542   add_operation(new memory_read_op_t(*this, address, length));
1543 }
1544
1545 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
1546 {
1547   // X addr,length:XX...
1548   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1549   reg_t address = consume_hex_number(iter, packet.end());
1550   if (*iter != ',')
1551     return send_packet("E20");
1552   iter++;
1553   reg_t length = consume_hex_number(iter, packet.end());
1554   if (*iter != ':')
1555     return send_packet("E21");
1556   iter++;
1557
1558   if (length == 0) {
1559     return send_packet("OK");
1560   }
1561
1562   unsigned char *data = new unsigned char[length];
1563   for (unsigned int i = 0; i < length; i++) {
1564     if (iter == packet.end()) {
1565       return send_packet("E22");
1566     }
1567     uint8_t c = *iter;
1568     iter++;
1569     if (c == '}') {
1570       // The binary data representation uses 7d (ascii ‘}’) as an escape
1571       // character. Any escaped byte is transmitted as the escape character
1572       // followed by the original character XORed with 0x20. For example, the
1573       // byte 0x7d would be transmitted as the two bytes 0x7d 0x5d. The bytes
1574       // 0x23 (ascii ‘#’), 0x24 (ascii ‘$’), and 0x7d (ascii ‘}’) must always
1575       // be escaped.
1576       if (iter == packet.end()) {
1577         return send_packet("E23");
1578       }
1579       c = (*iter) ^ 0x20;
1580       iter++;
1581     }
1582     data[i] = c;
1583   }
1584   if (*iter != '#')
1585     return send_packet("E4b"); // EOVERFLOW
1586
1587   add_operation(new collect_translation_info_op_t(*this, address, length));
1588   add_operation(new memory_write_op_t(*this, address, length, data));
1589 }
1590
1591 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
1592 {
1593   // c [addr]
1594   processor_t *p = sim->get_core(0);
1595   if (packet[2] != '#') {
1596     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1597     dpc = consume_hex_number(iter, packet.end());
1598     if (*iter != '#')
1599       return send_packet("E30");
1600   }
1601
1602   add_operation(new continue_op_t(*this, false));
1603 }
1604
1605 void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
1606 {
1607   // s [addr]
1608   if (packet[2] != '#') {
1609     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1610     die("handle_step");
1611     //p->state.pc = consume_hex_number(iter, packet.end());
1612     if (*iter != '#')
1613       return send_packet("E40");
1614   }
1615
1616   add_operation(new continue_op_t(*this, true));
1617 }
1618
1619 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
1620 {
1621   // k
1622   // The exact effect of this packet is not specified.
1623   // Looks like OpenOCD disconnects?
1624   // TODO
1625 }
1626
1627 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
1628 {
1629   // Enable extended mode. In extended mode, the remote server is made
1630   // persistent. The ‘R’ packet is used to restart the program being debugged.
1631   send_packet("OK");
1632   extended_mode = true;
1633 }
1634
1635 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
1636 {
1637   // insert: Z type,addr,kind
1638   // remove: z type,addr,kind
1639
1640   software_breakpoint_t bp;
1641   bool insert = (packet[1] == 'Z');
1642   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1643   int type = consume_hex_number(iter, packet.end());
1644   if (*iter != ',')
1645     return send_packet("E50");
1646   iter++;
1647   bp.address = consume_hex_number(iter, packet.end());
1648   if (*iter != ',')
1649     return send_packet("E51");
1650   iter++;
1651   bp.size = consume_hex_number(iter, packet.end());
1652   // There may be more options after a ; here, but we don't support that.
1653   if (*iter != '#')
1654     return send_packet("E52");
1655
1656   if (type != 0) {
1657     // Only software breakpoints are supported.
1658     return send_packet("");
1659   }
1660
1661   if (bp.size != 2 && bp.size != 4) {
1662     return send_packet("E53");
1663   }
1664
1665   fence_i_required = true;
1666   add_operation(new collect_translation_info_op_t(*this, bp.address, bp.size));
1667   if (insert) {
1668     unsigned char* swbp = new unsigned char[4];
1669     if (bp.size == 2) {
1670       swbp[0] = C_EBREAK & 0xff;
1671       swbp[1] = (C_EBREAK >> 8) & 0xff;
1672     } else {
1673       swbp[0] = EBREAK & 0xff;
1674       swbp[1] = (EBREAK >> 8) & 0xff;
1675       swbp[2] = (EBREAK >> 16) & 0xff;
1676       swbp[3] = (EBREAK >> 24) & 0xff;
1677     }
1678
1679     breakpoints[bp.address] = new software_breakpoint_t(bp);
1680     add_operation(new memory_read_op_t(*this, bp.address, bp.size,
1681           breakpoints[bp.address]->instruction));
1682     add_operation(new memory_write_op_t(*this, bp.address, bp.size, swbp));
1683
1684   } else {
1685     software_breakpoint_t *found_bp;
1686     found_bp = breakpoints[bp.address];
1687     unsigned char* instruction = new unsigned char[4];
1688     memcpy(instruction, found_bp->instruction, 4);
1689     add_operation(new memory_write_op_t(*this, found_bp->address,
1690           found_bp->size, instruction));
1691     breakpoints.erase(bp.address);
1692     delete found_bp;
1693   }
1694
1695   return send_packet("OK");
1696 }
1697
1698 void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
1699 {
1700   std::string name;
1701   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1702
1703   consume_string(name, iter, packet.end(), ':');
1704   if (iter != packet.end())
1705     iter++;
1706   if (name == "Supported") {
1707     start_packet();
1708     while (iter != packet.end()) {
1709       std::string feature;
1710       consume_string(feature, iter, packet.end(), ';');
1711       if (iter != packet.end())
1712         iter++;
1713       if (feature == "swbreak+") {
1714         send("swbreak+;");
1715       }
1716     }
1717     send("PacketSize=131072;");
1718     return end_packet();
1719   }
1720
1721   D(fprintf(stderr, "Unsupported query %s\n", name.c_str()));
1722   return send_packet("");
1723 }
1724
1725 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
1726 {
1727   if (compute_checksum(packet) != extract_checksum(packet)) {
1728     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
1729     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
1730     print_packet(packet);
1731     send("-");
1732     return;
1733   }
1734
1735   D(fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size()));
1736   D(print_packet(packet));
1737   send("+");
1738
1739   switch (packet[1]) {
1740     case '!':
1741       return handle_extended(packet);
1742     case '?':
1743       return handle_halt_reason(packet);
1744     case 'g':
1745       return handle_general_registers_read(packet);
1746 //    case 'k':
1747 //      return handle_kill(packet);
1748     case 'm':
1749       return handle_memory_read(packet);
1750 //    case 'M':
1751 //      return handle_memory_write(packet);
1752     case 'X':
1753       return handle_memory_binary_write(packet);
1754     case 'p':
1755       return handle_register_read(packet);
1756     case 'P':
1757       return handle_register_write(packet);
1758     case 'c':
1759       return handle_continue(packet);
1760     case 's':
1761       return handle_step(packet);
1762     case 'z':
1763     case 'Z':
1764       return handle_breakpoint(packet);
1765     case 'q':
1766     case 'Q':
1767       return handle_query(packet);
1768   }
1769
1770   // Not supported.
1771   D(fprintf(stderr, "** Unsupported packet: "));
1772   D(print_packet(packet));
1773   send_packet("");
1774 }
1775
1776 void gdbserver_t::handle_interrupt()
1777 {
1778   processor_t *p = sim->get_core(0);
1779   add_operation(new halt_op_t(*this, true));
1780 }
1781
1782 void gdbserver_t::handle()
1783 {
1784   if (client_fd > 0) {
1785     processor_t *p = sim->get_core(0);
1786
1787     bool interrupt = sim->debug_module.get_interrupt(0);
1788
1789     if (!interrupt && !operation_queue.empty()) {
1790       operation_t *operation = operation_queue.front();
1791       if (operation->step()) {
1792         operation_queue.pop();
1793         delete operation;
1794       }
1795     }
1796
1797     bool halt_notification = sim->debug_module.get_halt_notification(0);
1798     if (halt_notification) {
1799       sim->debug_module.clear_halt_notification(0);
1800       add_operation(new halt_op_t(*this, true));
1801     }
1802
1803     this->read();
1804     this->write();
1805
1806   } else {
1807     this->accept();
1808   }
1809
1810   if (operation_queue.empty()) {
1811     this->process_requests();
1812   }
1813 }
1814
1815 void gdbserver_t::send(const char* msg)
1816 {
1817   unsigned int length = strlen(msg);
1818   for (const char *c = msg; *c; c++)
1819     running_checksum += *c;
1820   send_buf.append((const uint8_t *) msg, length);
1821 }
1822
1823 void gdbserver_t::send(uint64_t value)
1824 {
1825   char buffer[3];
1826   for (unsigned int i = 0; i < 8; i++) {
1827     sprintf(buffer, "%02x", (int) (value & 0xff));
1828     send(buffer);
1829     value >>= 8;
1830   }
1831 }
1832
1833 void gdbserver_t::send(uint32_t value)
1834 {
1835   char buffer[3];
1836   for (unsigned int i = 0; i < 4; i++) {
1837     sprintf(buffer, "%02x", (int) (value & 0xff));
1838     send(buffer);
1839     value >>= 8;
1840   }
1841 }
1842
1843 void gdbserver_t::send_packet(const char* data)
1844 {
1845   start_packet();
1846   send(data);
1847   end_packet();
1848   expect_ack = true;
1849 }
1850
1851 void gdbserver_t::start_packet()
1852 {
1853   send("$");
1854   running_checksum = 0;
1855 }
1856
1857 void gdbserver_t::end_packet(const char* data)
1858 {
1859   if (data) {
1860     send(data);
1861   }
1862
1863   char checksum_string[4];
1864   sprintf(checksum_string, "#%02x", running_checksum);
1865   send(checksum_string);
1866   expect_ack = true;
1867 }