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