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