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       // Perform any reads from the just-completed action.
 991       switch (state) {
 992         case STATE_START:
 993           break;
 994         case STATE_READ_SPTBR:
 995           gs.sptbr = gs.dr_read(SLOT_DATA0);
 996           gs.sptbr_valid = true;
 997           vm = decode_vm_info(gs.xlen, gs.privilege_mode(), gs.sptbr);
 998           if (vm.levels == 0)
 999             return true;
1000           break;
1001         case STATE_READ_PTE:
1002           if (vm.ptesize == 4) {
1003               gs.pte_cache[pte_addr] = gs.dr_read32(4);
1004           } else {
1005               gs.pte_cache[pte_addr] = ((uint64_t) gs.dr_read32(5) << 32) |
1006                   gs.dr_read32(4);
1007           }
1008           D(fprintf(stderr, "pte_cache[0x%" PRIx64 "] = 0x%" PRIx64 "\n", pte_addr,
1009                     gs.pte_cache[pte_addr]));
1010           break;
1011       }
1012
1013       // Set up the next action.
1014       // We only get here for VM_SV32/39/38.
1015
1016       if (!gs.sptbr_valid) {
1017         state = STATE_READ_SPTBR;
1018         gs.dr_write32(0, csrr(S0, CSR_SPTBR));
1019         gs.dr_write_store(1, S0, SLOT_DATA0);
1020         gs.dr_write_jump(2);
1021         gs.set_interrupt(0);
1022         return false;
1023       }
1024
1025       reg_t base = vm.ptbase;
1026       for (int i = vm.levels - 1; i >= 0; i--) {
1027         int ptshift = i * vm.idxbits;
1028         reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << vm.idxbits) - 1);
1029
1030         pte_addr = base + idx * vm.ptesize;
1031         auto it = gs.pte_cache.find(pte_addr);
1032         if (it == gs.pte_cache.end()) {
1033           state = STATE_READ_PTE;
1034           if (vm.ptesize == 4) {
1035             gs.dr_write32(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
1036             gs.dr_write32(1, lw(S1, S0, 0));
1037             gs.dr_write32(2, sw(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
1038           } else {
1039             assert(gs.xlen >= 64);
1040             gs.dr_write32(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
1041             gs.dr_write32(1, ld(S1, S0, 0));
1042             gs.dr_write32(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
1043           }
1044           gs.dr_write_jump(3);
1045           gs.dr_write32(4, pte_addr);
1046           gs.dr_write32(5, pte_addr >> 32);
1047           gs.set_interrupt(0);
1048           return false;
1049         }
1050
1051         reg_t pte = gs.pte_cache[pte_addr];
1052         reg_t ppn = pte >> PTE_PPN_SHIFT;
1053
1054         if (PTE_TABLE(pte)) { // next level of page table
1055           base = ppn << PGSHIFT;
1056         } else {
1057           // We've collected all the data required for the translation.
1058           return true;
1059         }
1060       }
1061       fprintf(stderr,
1062           "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%016" PRIx64 "\n",
1063           vaddr);
1064       return true;
1065     }
1066
1067   private:
1068     enum {
1069       STATE_START,
1070       STATE_READ_SPTBR,
1071       STATE_READ_PTE
1072     } state;
1073     reg_t vaddr;
1074     size_t length;
1075     vm_info vm;
1076     reg_t pte_addr;
1077 };
1078
1079 class hardware_breakpoint_insert_op_t : public operation_t
1080 {
1081   public:
1082     hardware_breakpoint_insert_op_t(gdbserver_t& gdbserver,
1083         hardware_breakpoint_t bp) :
1084       operation_t(gdbserver), state(STATE_START), bp(bp) {};
1085
1086     void write_new_index_program()
1087     {
1088       gs.dr_write_load(0, S0, SLOT_DATA1);
1089       gs.dr_write32(1, csrw(S0, CSR_TSELECT));
1090       gs.dr_write32(2, csrr(S0, CSR_TSELECT));
1091       gs.dr_write_store(3, S0, SLOT_DATA1);
1092       gs.dr_write_jump(4);
1093       gs.dr_write(SLOT_DATA1, bp.index);
1094     }
1095
1096     bool perform_step(unsigned int step)
1097     {
1098       switch (state) {
1099         case STATE_START:
1100           bp.index = 0;
1101           write_new_index_program();
1102           state = STATE_CHECK_INDEX;
1103           break;
1104
1105         case STATE_CHECK_INDEX:
1106           if (gs.dr_read(SLOT_DATA1) != bp.index) {
1107             // We've exhausted breakpoints without finding an appropriate one.
1108             gs.send_packet("E58");
1109             return true;
1110           }
1111
1112           gs.dr_write32(0, csrr(S0, CSR_TDATA1));
1113           gs.dr_write_store(1, S0, SLOT_DATA0);
1114           gs.dr_write_jump(2);
1115           state = STATE_CHECK_MCONTROL;
1116           break;
1117
1118         case STATE_CHECK_MCONTROL:
1119           {
1120             reg_t mcontrol = gs.dr_read(SLOT_DATA0);
1121             unsigned int type = mcontrol >> (gs.xlen - 4);
1122             if (type == 0) {
1123               // We've exhausted breakpoints without finding an appropriate one.
1124               gs.send_packet("E58");
1125               return true;
1126             }
1127
1128             if (type == 2 &&
1129                 !get_field(mcontrol, MCONTROL_EXECUTE) &&
1130                 !get_field(mcontrol, MCONTROL_LOAD) &&
1131                 !get_field(mcontrol, MCONTROL_STORE)) {
1132               // Found an unused trigger.
1133               gs.dr_write_load(0, S0, SLOT_DATA1);
1134               gs.dr_write32(1, csrw(S0, CSR_TDATA1));
1135               gs.dr_write_jump(2);
1136               mcontrol = set_field(0, MCONTROL_ACTION, MCONTROL_ACTION_DEBUG_MODE);
1137               mcontrol = set_field(mcontrol, MCONTROL_DMODE(gs.xlen), 1);
1138               mcontrol = set_field(mcontrol, MCONTROL_MATCH, MCONTROL_MATCH_EQUAL);
1139               mcontrol = set_field(mcontrol, MCONTROL_M, 1);
1140               mcontrol = set_field(mcontrol, MCONTROL_H, 1);
1141               mcontrol = set_field(mcontrol, MCONTROL_S, 1);
1142               mcontrol = set_field(mcontrol, MCONTROL_U, 1);
1143               mcontrol = set_field(mcontrol, MCONTROL_EXECUTE, bp.execute);
1144               mcontrol = set_field(mcontrol, MCONTROL_LOAD, bp.load);
1145               mcontrol = set_field(mcontrol, MCONTROL_STORE, bp.store);
1146               // For store triggers it's nicer to fire just before the
1147               // instruction than just after. However, gdb doesn't clear the
1148               // breakpoints and step before resuming from a store trigger.
1149               // That means that without extra code, you'll keep hitting the
1150               // same watchpoint over and over again. That's not useful at all.
1151               // Instead of fixing this the right way, just set timing=1 for
1152               // those triggers.
1153               if (bp.load || bp.store)
1154                 mcontrol = set_field(mcontrol, MCONTROL_TIMING, 1);
1155
1156               gs.dr_write(SLOT_DATA1, mcontrol);
1157               state = STATE_WRITE_ADDRESS;
1158             } else {
1159               bp.index++;
1160               write_new_index_program();
1161               state = STATE_CHECK_INDEX;
1162             }
1163           }
1164           break;
1165
1166         case STATE_WRITE_ADDRESS:
1167           {
1168             gs.dr_write_load(0, S0, SLOT_DATA1);
1169             gs.dr_write32(1, csrw(S0, CSR_TDATA2));
1170             gs.dr_write_jump(2);
1171             gs.dr_write(SLOT_DATA1, bp.vaddr);
1172             gs.set_interrupt(0);
1173             gs.send_packet("OK");
1174
1175             gs.hardware_breakpoints.insert(bp);
1176
1177             return true;
1178           }
1179       }
1180
1181       gs.set_interrupt(0);
1182       return false;
1183     }
1184
1185   private:
1186     enum {
1187       STATE_START,
1188       STATE_CHECK_INDEX,
1189       STATE_CHECK_MCONTROL,
1190       STATE_WRITE_ADDRESS
1191     } state;
1192     hardware_breakpoint_t bp;
1193 };
1194
1195 class maybe_save_tselect_op_t : public operation_t
1196 {
1197   public:
1198     maybe_save_tselect_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
1199     bool perform_step(unsigned int step) {
1200       if (gs.tselect_valid)
1201         return true;
1202
1203       switch (step) {
1204         case 0:
1205           gs.dr_write32(0, csrr(S0, CSR_TDATA1));
1206           gs.dr_write_store(1, S0, SLOT_DATA0);
1207           gs.dr_write_jump(2);
1208           gs.set_interrupt(0);
1209           return false;
1210         case 1:
1211           gs.tselect = gs.dr_read(SLOT_DATA0);
1212           gs.tselect_valid = true;
1213           break;
1214       }
1215       return true;
1216     }
1217 };
1218
1219 class maybe_restore_tselect_op_t : public operation_t
1220 {
1221   public:
1222     maybe_restore_tselect_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
1223     bool perform_step(unsigned int step) {
1224       if (gs.tselect_valid) {
1225         gs.dr_write_load(0, S0, SLOT_DATA1);
1226         gs.dr_write32(1, csrw(S0, CSR_TSELECT));
1227         gs.dr_write_jump(2);
1228         gs.dr_write(SLOT_DATA1, gs.tselect);
1229       }
1230       return true;
1231     }
1232 };
1233
1234 class maybe_restore_mstatus_op_t : public operation_t
1235 {
1236   public:
1237     maybe_restore_mstatus_op_t(gdbserver_t& gdbserver) : operation_t(gdbserver) {};
1238     bool perform_step(unsigned int step) {
1239       if (gs.mstatus_dirty) {
1240         gs.dr_write_load(0, S0, SLOT_DATA1);
1241         gs.dr_write32(1, csrw(S0, CSR_MSTATUS));
1242         gs.dr_write_jump(2);
1243         gs.dr_write(SLOT_DATA1, gs.mstatus);
1244       }
1245       return true;
1246     }
1247 };
1248
1249 class hardware_breakpoint_remove_op_t : public operation_t
1250 {
1251   public:
1252     hardware_breakpoint_remove_op_t(gdbserver_t& gdbserver,
1253         hardware_breakpoint_t bp) :
1254       operation_t(gdbserver), bp(bp) {};
1255
1256     bool perform_step(unsigned int step) {
1257       gs.dr_write32(0, addi(S0, ZERO, bp.index));
1258       gs.dr_write32(1, csrw(S0, CSR_TSELECT));
1259       gs.dr_write32(2, csrw(ZERO, CSR_TDATA1));
1260       gs.dr_write_jump(3);
1261       gs.set_interrupt(0);
1262       return true;
1263     }
1264
1265   private:
1266     hardware_breakpoint_t bp;
1267 };
1268
1269 ////////////////////////////// gdbserver itself
1270
1271 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
1272   xlen(0),
1273   sim(sim),
1274   client_fd(0),
1275   // gdb likes to send 0x100000 bytes at once when downloading.
1276   recv_buf(0x180000), send_buf(64 * 1024)
1277 {
1278   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
1279   if (socket_fd == -1) {
1280     fprintf(stderr, "failed to make socket: %s (%d)\n", strerror(errno), errno);
1281     abort();
1282   }
1283
1284   fcntl(socket_fd, F_SETFL, O_NONBLOCK);
1285   int reuseaddr = 1;
1286   if (setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,
1287         sizeof(int)) == -1) {
1288     fprintf(stderr, "failed setsockopt: %s (%d)\n", strerror(errno), errno);
1289     abort();
1290   }
1291
1292   struct sockaddr_in addr;
1293   memset(&addr, 0, sizeof(addr));
1294   addr.sin_family = AF_INET;
1295   addr.sin_addr.s_addr = INADDR_ANY;
1296   addr.sin_port = htons(port);
1297
1298   if (bind(socket_fd, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
1299     fprintf(stderr, "failed to bind socket: %s (%d)\n", strerror(errno), errno);
1300     abort();
1301   }
1302
1303   if (listen(socket_fd, 1) == -1) {
1304     fprintf(stderr, "failed to listen on socket: %s (%d)\n", strerror(errno), errno);
1305     abort();
1306   }
1307 }
1308
1309 unsigned int gdbserver_t::find_access_size(reg_t address, int length)
1310 {
1311   reg_t composite = address | length;
1312   if ((composite & 0x7) == 0 && xlen >= 64)
1313     return 8;
1314   if ((composite & 0x3) == 0)
1315     return 4;
1316   return 1;
1317 }
1318
1319 reg_t gdbserver_t::translate(reg_t vaddr)
1320 {
1321   vm_info vm = decode_vm_info(xlen, privilege_mode(), sptbr);
1322   if (vm.levels == 0)
1323     return vaddr;
1324
1325   // Handle page tables here. There's a bunch of duplicated code with
1326   // collect_translation_info_op_t. :-(
1327   reg_t base = vm.ptbase;
1328   for (int i = vm.levels - 1; i >= 0; i--) {
1329     int ptshift = i * vm.idxbits;
1330     reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << vm.idxbits) - 1);
1331
1332     reg_t pte_addr = base + idx * vm.ptesize;
1333     auto it = pte_cache.find(pte_addr);
1334     if (it == pte_cache.end()) {
1335       fprintf(stderr, "ERROR: gdbserver tried to translate 0x%016" PRIx64
1336           " without first collecting the relevant PTEs.\n", vaddr);
1337       die("gdbserver_t::translate()");
1338     }
1339
1340     reg_t pte = pte_cache[pte_addr];
1341     reg_t ppn = pte >> PTE_PPN_SHIFT;
1342
1343     if (PTE_TABLE(pte)) { // next level of page table
1344       base = ppn << PGSHIFT;
1345     } else {
1346       // We've collected all the data required for the translation.
1347       reg_t vpn = vaddr >> PGSHIFT;
1348       reg_t paddr = (ppn | (vpn & ((reg_t(1) << ptshift) - 1))) << PGSHIFT;
1349       paddr += vaddr & (PGSIZE-1);
1350       D(fprintf(stderr, "gdbserver translate 0x%" PRIx64 " -> 0x%" PRIx64 "\n", vaddr, paddr));
1351       return paddr;
1352     }
1353   }
1354
1355   fprintf(stderr, "ERROR: gdbserver tried to translate 0x%016" PRIx64
1356           " but the relevant PTEs are invalid.\n", vaddr);
1357   // TODO: Is it better to throw an exception here?
1358   return -1;
1359 }
1360
1361 unsigned int gdbserver_t::privilege_mode()
1362 {
1363   unsigned int mode = get_field(dcsr, DCSR_PRV);
1364   if (get_field(mstatus, MSTATUS_MPRV))
1365     mode = get_field(mstatus, MSTATUS_MPP);
1366   return mode;
1367 }
1368
1369 void gdbserver_t::dr_write32(unsigned int index, uint32_t value)
1370 {
1371   sim->debug_module.ram_write32(index, value);
1372 }
1373
1374 void gdbserver_t::dr_write64(unsigned int index, uint64_t value)
1375 {
1376   dr_write32(index, value);
1377   dr_write32(index+1, value >> 32);
1378 }
1379
1380 void gdbserver_t::dr_write(enum slot slot, uint64_t value)
1381 {
1382   switch (xlen) {
1383     case 32:
1384       dr_write32(slot_offset32[slot], value);
1385       break;
1386     case 64:
1387       dr_write64(slot_offset64[slot], value);
1388       break;
1389     case 128:
1390     default:
1391       abort();
1392   }
1393 }
1394
1395 void gdbserver_t::dr_write_jump(unsigned int index)
1396 {
1397   dr_write32(index, jal(0,
1398         (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*index))));
1399 }
1400
1401 void gdbserver_t::dr_write_store(unsigned int index, unsigned int reg, enum slot slot)
1402 {
1403   assert(slot != SLOT_INST0 || index > 2);
1404   assert(slot != SLOT_DATA0 || index < 4 || index > 6);
1405   assert(slot != SLOT_DATA1 || index < 5 || index > 10);
1406   assert(slot != SLOT_DATA_LAST || index < 6 || index > 14);
1407   switch (xlen) {
1408     case 32:
1409       return dr_write32(index,
1410           sw(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset32[slot]));
1411     case 64:
1412       return dr_write32(index,
1413           sd(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset64[slot]));
1414     case 128:
1415       return dr_write32(index,
1416           sq(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset128[slot]));
1417     default:
1418       fprintf(stderr, "xlen is %d!\n", xlen);
1419       abort();
1420   }
1421 }
1422
1423 void gdbserver_t::dr_write_load(unsigned int index, unsigned int reg, enum slot slot)
1424 {
1425   switch (xlen) {
1426     case 32:
1427       return dr_write32(index,
1428           lw(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset32[slot]));
1429     case 64:
1430       return dr_write32(index,
1431           ld(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset64[slot]));
1432     case 128:
1433       return dr_write32(index,
1434           lq(reg, 0, (uint16_t) DEBUG_RAM_START + 4 * slot_offset128[slot]));
1435     default:
1436       fprintf(stderr, "xlen is %d!\n", xlen);
1437       abort();
1438   }
1439 }
1440
1441 uint32_t gdbserver_t::dr_read32(unsigned int index)
1442 {
1443   uint32_t value = sim->debug_module.ram_read32(index);
1444   D(fprintf(stderr, "read32(%d) -> 0x%x\n", index, value));
1445   return value;
1446 }
1447
1448 uint64_t gdbserver_t::dr_read64(unsigned int index)
1449 {
1450   return ((uint64_t) dr_read32(index+1) << 32) | dr_read32(index);
1451 }
1452
1453 uint64_t gdbserver_t::dr_read(enum slot slot)
1454 {
1455   switch (xlen) {
1456     case 32:
1457       return dr_read32(slot_offset32[slot]);
1458     case 64:
1459       return dr_read64(slot_offset64[slot]);
1460     case 128:
1461       abort();
1462     default:
1463       abort();
1464   }
1465 }
1466
1467 void gdbserver_t::add_operation(operation_t* operation)
1468 {
1469   operation_queue.push(operation);
1470 }
1471
1472 void gdbserver_t::accept()
1473 {
1474   client_fd = ::accept(socket_fd, NULL, NULL);
1475   if (client_fd == -1) {
1476     if (errno == EAGAIN) {
1477       // No client waiting to connect right now.
1478     } else {
1479       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno),
1480           errno);
1481       abort();
1482     }
1483   } else {
1484     fcntl(client_fd, F_SETFL, O_NONBLOCK);
1485
1486     expect_ack = false;
1487     extended_mode = false;
1488
1489     // gdb wants the core to be halted when it attaches.
1490     add_operation(new halt_op_t(*this));
1491   }
1492 }
1493
1494 void gdbserver_t::read()
1495 {
1496   // Reading from a non-blocking socket still blocks if there is no data
1497   // available.
1498
1499   size_t count = recv_buf.contiguous_empty_size();
1500   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
1501   if (bytes == -1) {
1502     if (errno == EAGAIN) {
1503       // We'll try again the next call.
1504     } else {
1505       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
1506       abort();
1507     }
1508   } else if (bytes == 0) {
1509     // The remote disconnected.
1510     client_fd = 0;
1511     processor_t *p = sim->get_core(0);
1512     // TODO p->set_halted(false, HR_NONE);
1513     recv_buf.reset();
1514     send_buf.reset();
1515   } else {
1516     recv_buf.data_added(bytes);
1517   }
1518 }
1519
1520 void gdbserver_t::write()
1521 {
1522   if (send_buf.empty())
1523     return;
1524
1525   while (!send_buf.empty()) {
1526     unsigned int count = send_buf.contiguous_data_size();
1527     assert(count > 0);
1528     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
1529     if (bytes == -1) {
1530       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
1531       abort();
1532     } else if (bytes == 0) {
1533       // Client can't take any more data right now.
1534       break;
1535     } else {
1536       D(fprintf(stderr, "wrote %zd bytes: ", bytes));
1537       for (int i = 0; i < bytes; i++) {
1538         D(fprintf(stderr, "%c", send_buf[i]));
1539       }
1540       D(fprintf(stderr, "\n"));
1541       send_buf.consume(bytes);
1542     }
1543   }
1544 }
1545
1546 void print_packet(const std::vector<uint8_t> &packet)
1547 {
1548   for (uint8_t c : packet) {
1549     if (c >= ' ' and c <= '~')
1550       fprintf(stderr, "%c", c);
1551     else
1552       fprintf(stderr, "\\x%02x", c);
1553   }
1554   fprintf(stderr, "\n");
1555 }
1556
1557 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
1558 {
1559   uint8_t checksum = 0;
1560   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
1561     checksum += *i;
1562   }
1563   return checksum;
1564 }
1565
1566 uint8_t character_hex_value(uint8_t character)
1567 {
1568   if (character >= '0' && character <= '9')
1569     return character - '0';
1570   if (character >= 'a' && character <= 'f')
1571     return 10 + character - 'a';
1572   if (character >= 'A' && character <= 'F')
1573     return 10 + character - 'A';
1574   return 0xff;
1575 }
1576
1577 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
1578 {
1579   return character_hex_value(*(packet.end() - 1)) +
1580     16 * character_hex_value(*(packet.end() - 2));
1581 }
1582
1583 void gdbserver_t::process_requests()
1584 {
1585   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
1586
1587   while (!recv_buf.empty()) {
1588     std::vector<uint8_t> packet;
1589     for (unsigned int i = 0; i < recv_buf.size(); i++) {
1590       uint8_t b = recv_buf[i];
1591
1592       if (packet.empty() && expect_ack && b == '+') {
1593         recv_buf.consume(1);
1594         break;
1595       }
1596
1597       if (packet.empty() && b == 3) {
1598         D(fprintf(stderr, "Received interrupt\n"));
1599         recv_buf.consume(1);
1600         handle_interrupt();
1601         break;
1602       }
1603
1604       if (b == '$') {
1605         // Start of new packet.
1606         if (!packet.empty()) {
1607           fprintf(stderr, "Received malformed %zd-byte packet from debug client: ",
1608               packet.size());
1609           print_packet(packet);
1610           recv_buf.consume(i);
1611           break;
1612         }
1613       }
1614
1615       packet.push_back(b);
1616
1617       // Packets consist of $<packet-data>#<checksum>
1618       // where <checksum> is
1619       if (packet.size() >= 4 &&
1620           packet[packet.size()-3] == '#') {
1621         handle_packet(packet);
1622         recv_buf.consume(i+1);
1623         break;
1624       }
1625     }
1626     // There's a partial packet in the buffer. Wait until we get more data to
1627     // process it.
1628     if (packet.size()) {
1629       break;
1630     }
1631   }
1632
1633   if (recv_buf.full()) {
1634     fprintf(stderr,
1635         "Receive buffer is full, but no complete packet was found!\n");
1636     for (unsigned line = 0; line < 8; line++) {
1637       for (unsigned i = 0; i < 16; i++) {
1638         fprintf(stderr, "%02x ", recv_buf.entry(line * 16 + i));
1639       }
1640       for (unsigned i = 0; i < 16; i++) {
1641         uint8_t e = recv_buf.entry(line * 16 + i);
1642         if (e >= ' ' && e <= '~')
1643           fprintf(stderr, "%c", e);
1644         else
1645           fprintf(stderr, ".");
1646       }
1647       fprintf(stderr, "\n");
1648     }
1649     assert(!recv_buf.full());
1650   }
1651 }
1652
1653 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
1654 {
1655   send_packet("S00");
1656 }
1657
1658 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
1659 {
1660   add_operation(new general_registers_read_op_t(*this));
1661 }
1662
1663 void gdbserver_t::set_interrupt(uint32_t hartid) {
1664   sim->debug_module.set_interrupt(hartid);
1665 }
1666
1667 // First byte is the most-significant one.
1668 // Eg. "08675309" becomes 0x08675309.
1669 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
1670     std::vector<uint8_t>::const_iterator end)
1671 {
1672   uint64_t value = 0;
1673
1674   while (iter != end) {
1675     uint8_t c = *iter;
1676     uint64_t c_value = character_hex_value(c);
1677     if (c_value > 15)
1678       break;
1679     iter++;
1680     value <<= 4;
1681     value += c_value;
1682   }
1683   return value;
1684 }
1685
1686 // First byte is the least-significant one.
1687 // Eg. "08675309" becomes 0x09536708
1688 uint64_t gdbserver_t::consume_hex_number_le(
1689     std::vector<uint8_t>::const_iterator &iter,
1690     std::vector<uint8_t>::const_iterator end)
1691 {
1692   uint64_t value = 0;
1693   unsigned int shift = 4;
1694
1695   while (iter != end) {
1696     uint8_t c = *iter;
1697     uint64_t c_value = character_hex_value(c);
1698     if (c_value > 15)
1699       break;
1700     iter++;
1701     value |= c_value << shift;
1702     if ((shift % 8) == 0)
1703       shift += 12;
1704     else
1705       shift -= 4;
1706   }
1707   if (shift > (xlen+4)) {
1708     fprintf(stderr,
1709         "gdb sent too many data bytes. That means it thinks XLEN is greater "
1710         "than %d.\nTo fix that, tell gdb: set arch riscv:rv%d\n",
1711         xlen, xlen);
1712   }
1713   return value;
1714 }
1715
1716 void consume_string(std::string &str, std::vector<uint8_t>::const_iterator &iter,
1717     std::vector<uint8_t>::const_iterator end, uint8_t separator)
1718 {
1719   while (iter != end && *iter != separator) {
1720     str.append(1, (char) *iter);
1721     iter++;
1722   }
1723 }
1724
1725 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
1726 {
1727   // p n
1728
1729   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1730   unsigned int n = consume_hex_number(iter, packet.end());
1731   if (*iter != '#')
1732     return send_packet("E01");
1733
1734   add_operation(new register_read_op_t(*this, n));
1735 }
1736
1737 void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
1738 {
1739   // P n...=r...
1740
1741   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1742   unsigned int n = consume_hex_number(iter, packet.end());
1743   if (*iter != '=')
1744     return send_packet("E05");
1745   iter++;
1746
1747   reg_t value = consume_hex_number_le(iter, packet.end());
1748   if (*iter != '#')
1749     return send_packet("E06");
1750
1751   processor_t *p = sim->get_core(0);
1752
1753   add_operation(new register_write_op_t(*this, n, value));
1754 }
1755
1756 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
1757 {
1758   // m addr,length
1759   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1760   reg_t address = consume_hex_number(iter, packet.end());
1761   if (*iter != ',')
1762     return send_packet("E10");
1763   iter++;
1764   reg_t length = consume_hex_number(iter, packet.end());
1765   if (*iter != '#')
1766     return send_packet("E11");
1767
1768   add_operation(new collect_translation_info_op_t(*this, address, length));
1769   add_operation(new memory_read_op_t(*this, address, length));
1770 }
1771
1772 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
1773 {
1774   // X addr,length:XX...
1775   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1776   reg_t address = consume_hex_number(iter, packet.end());
1777   if (*iter != ',')
1778     return send_packet("E20");
1779   iter++;
1780   reg_t length = consume_hex_number(iter, packet.end());
1781   if (*iter != ':')
1782     return send_packet("E21");
1783   iter++;
1784
1785   if (length == 0) {
1786     return send_packet("OK");
1787   }
1788
1789   unsigned char *data = new unsigned char[length];
1790   for (unsigned int i = 0; i < length; i++) {
1791     if (iter == packet.end()) {
1792       return send_packet("E22");
1793     }
1794     uint8_t c = *iter;
1795     iter++;
1796     if (c == '}') {
1797       // The binary data representation uses 7d (ascii ‘}’) as an escape
1798       // character. Any escaped byte is transmitted as the escape character
1799       // followed by the original character XORed with 0x20. For example, the
1800       // byte 0x7d would be transmitted as the two bytes 0x7d 0x5d. The bytes
1801       // 0x23 (ascii ‘#’), 0x24 (ascii ‘$’), and 0x7d (ascii ‘}’) must always
1802       // be escaped.
1803       if (iter == packet.end()) {
1804         return send_packet("E23");
1805       }
1806       c = (*iter) ^ 0x20;
1807       iter++;
1808     }
1809     data[i] = c;
1810   }
1811   if (*iter != '#')
1812     return send_packet("E4b"); // EOVERFLOW
1813
1814   add_operation(new collect_translation_info_op_t(*this, address, length));
1815   add_operation(new memory_write_op_t(*this, address, length, data));
1816 }
1817
1818 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
1819 {
1820   // c [addr]
1821   processor_t *p = sim->get_core(0);
1822   if (packet[2] != '#') {
1823     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1824     dpc = consume_hex_number(iter, packet.end());
1825     if (*iter != '#')
1826       return send_packet("E30");
1827   }
1828
1829   add_operation(new maybe_restore_tselect_op_t(*this));
1830   add_operation(new maybe_restore_mstatus_op_t(*this));
1831   add_operation(new continue_op_t(*this, false));
1832 }
1833
1834 void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
1835 {
1836   // s [addr]
1837   if (packet[2] != '#') {
1838     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1839     die("handle_step");
1840     //p->state.pc = consume_hex_number(iter, packet.end());
1841     if (*iter != '#')
1842       return send_packet("E40");
1843   }
1844
1845   add_operation(new maybe_restore_tselect_op_t(*this));
1846   add_operation(new continue_op_t(*this, true));
1847 }
1848
1849 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
1850 {
1851   // k
1852   // The exact effect of this packet is not specified.
1853   // Looks like OpenOCD disconnects?
1854   // TODO
1855 }
1856
1857 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
1858 {
1859   // Enable extended mode. In extended mode, the remote server is made
1860   // persistent. The ‘R’ packet is used to restart the program being debugged.
1861   send_packet("OK");
1862   extended_mode = true;
1863 }
1864
1865 void gdbserver_t::software_breakpoint_insert(reg_t vaddr, unsigned int size)
1866 {
1867   fence_i_required = true;
1868   add_operation(new collect_translation_info_op_t(*this, vaddr, size));
1869   unsigned char* inst = new unsigned char[4];
1870   if (size == 2) {
1871     inst[0] = MATCH_C_EBREAK & 0xff;
1872     inst[1] = (MATCH_C_EBREAK >> 8) & 0xff;
1873   } else {
1874     inst[0] = MATCH_EBREAK & 0xff;
1875     inst[1] = (MATCH_EBREAK >> 8) & 0xff;
1876     inst[2] = (MATCH_EBREAK >> 16) & 0xff;
1877     inst[3] = (MATCH_EBREAK >> 24) & 0xff;
1878   }
1879
1880   software_breakpoint_t bp = {
1881     .vaddr = vaddr,
1882     .size = size
1883   };
1884   software_breakpoints[vaddr] = bp;
1885   add_operation(new memory_read_op_t(*this, bp.vaddr, bp.size,
1886         software_breakpoints[bp.vaddr].instruction));
1887   add_operation(new memory_write_op_t(*this, bp.vaddr, bp.size, inst));
1888 }
1889
1890 void gdbserver_t::software_breakpoint_remove(reg_t vaddr, unsigned int size)
1891 {
1892   fence_i_required = true;
1893   add_operation(new collect_translation_info_op_t(*this, vaddr, size));
1894
1895   software_breakpoint_t found_bp = software_breakpoints[vaddr];
1896   unsigned char* instruction = new unsigned char[4];
1897   memcpy(instruction, found_bp.instruction, 4);
1898   add_operation(new memory_write_op_t(*this, found_bp.vaddr,
1899         found_bp.size, instruction));
1900   software_breakpoints.erase(vaddr);
1901 }
1902
1903 void gdbserver_t::hardware_breakpoint_insert(const hardware_breakpoint_t &bp)
1904 {
1905   add_operation(new maybe_save_tselect_op_t(*this));
1906   add_operation(new hardware_breakpoint_insert_op_t(*this, bp));
1907 }
1908
1909 void gdbserver_t::hardware_breakpoint_remove(const hardware_breakpoint_t &bp)
1910 {
1911   add_operation(new maybe_save_tselect_op_t(*this));
1912   hardware_breakpoint_t found = *hardware_breakpoints.find(bp);
1913   add_operation(new hardware_breakpoint_remove_op_t(*this, found));
1914 }
1915
1916 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
1917 {
1918   // insert: Z type,addr,length
1919   // remove: z type,addr,length
1920
1921   // type: 0 - software breakpoint, 1 - hardware breakpoint, 2 - write
1922   // watchpoint, 3 - read watchpoint, 4 - access watchpoint; addr is address;
1923   // length is in bytes. For a software breakpoint, length specifies the size
1924   // of the instruction to be patched. For hardware breakpoints and watchpoints
1925   // length specifies the memory region to be monitored. To avoid potential
1926   // problems with duplicate packets, the operations should be implemented in
1927   // an idempotent way.
1928
1929   bool insert = (packet[1] == 'Z');
1930   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1931   gdb_breakpoint_type_t type = static_cast<gdb_breakpoint_type_t>(
1932       consume_hex_number(iter, packet.end()));
1933   if (*iter != ',')
1934     return send_packet("E50");
1935   iter++;
1936   reg_t address = consume_hex_number(iter, packet.end());
1937   if (*iter != ',')
1938     return send_packet("E51");
1939   iter++;
1940   unsigned int size = consume_hex_number(iter, packet.end());
1941   // There may be more options after a ; here, but we don't support that.
1942   if (*iter != '#')
1943     return send_packet("E52");
1944
1945   switch (type) {
1946     case GB_SOFTWARE:
1947       if (size != 2 && size != 4) {
1948         return send_packet("E53");
1949       }
1950       if (insert) {
1951         software_breakpoint_insert(address, size);
1952       } else {
1953         software_breakpoint_remove(address, size);
1954       }
1955       break;
1956
1957     case GB_HARDWARE:
1958     case GB_WRITE:
1959     case GB_READ:
1960     case GB_ACCESS:
1961       {
1962         hardware_breakpoint_t bp = {
1963           .vaddr = address,
1964           .size = size
1965         };
1966         bp.load = (type == GB_READ || type == GB_ACCESS);
1967         bp.store = (type == GB_WRITE || type == GB_ACCESS);
1968         bp.execute = (type == GB_HARDWARE || type == GB_ACCESS);
1969         if (insert) {
1970           hardware_breakpoint_insert(bp);
1971           // Insert might fail if there's no space, so the insert operation will
1972           // send its own OK (or not).
1973           return;
1974         } else {
1975           hardware_breakpoint_remove(bp);
1976         }
1977       }
1978       break;
1979
1980     default:
1981       return send_packet("E56");
1982   }
1983
1984   return send_packet("OK");
1985 }
1986
1987 void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
1988 {
1989   std::string name;
1990   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1991
1992   consume_string(name, iter, packet.end(), ':');
1993   if (iter != packet.end())
1994     iter++;
1995   if (name == "Supported") {
1996     start_packet();
1997     while (iter != packet.end()) {
1998       std::string feature;
1999       consume_string(feature, iter, packet.end(), ';');
2000       if (iter != packet.end())
2001         iter++;
2002       if (feature == "swbreak+") {
2003         send("swbreak+;");
2004       }
2005     }
2006     send("PacketSize=131072;");
2007     return end_packet();
2008   }
2009
2010   D(fprintf(stderr, "Unsupported query %s\n", name.c_str()));
2011   return send_packet("");
2012 }
2013
2014 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
2015 {
2016   if (compute_checksum(packet) != extract_checksum(packet)) {
2017     fprintf(stderr, "Received %zd-byte packet with invalid checksum\n", packet.size());
2018     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
2019     print_packet(packet);
2020     send("-");
2021     return;
2022   }
2023
2024   D(fprintf(stderr, "Received %zd-byte packet from debug client: ", packet.size()));
2025   D(print_packet(packet));
2026   send("+");
2027
2028   switch (packet[1]) {
2029     case '!':
2030       return handle_extended(packet);
2031     case '?':
2032       return handle_halt_reason(packet);
2033     case 'g':
2034       return handle_general_registers_read(packet);
2035 //    case 'k':
2036 //      return handle_kill(packet);
2037     case 'm':
2038       return handle_memory_read(packet);
2039 //    case 'M':
2040 //      return handle_memory_write(packet);
2041     case 'X':
2042       return handle_memory_binary_write(packet);
2043     case 'p':
2044       return handle_register_read(packet);
2045     case 'P':
2046       return handle_register_write(packet);
2047     case 'c':
2048       return handle_continue(packet);
2049     case 's':
2050       return handle_step(packet);
2051     case 'z':
2052     case 'Z':
2053       return handle_breakpoint(packet);
2054     case 'q':
2055     case 'Q':
2056       return handle_query(packet);
2057   }
2058
2059   // Not supported.
2060   D(fprintf(stderr, "** Unsupported packet: "));
2061   D(print_packet(packet));
2062   send_packet("");
2063 }
2064
2065 void gdbserver_t::handle_interrupt()
2066 {
2067   processor_t *p = sim->get_core(0);
2068   add_operation(new halt_op_t(*this, true));
2069 }
2070
2071 void gdbserver_t::handle()
2072 {
2073   if (client_fd > 0) {
2074     processor_t *p = sim->get_core(0);
2075
2076     bool interrupt = sim->debug_module.get_interrupt(0);
2077
2078     if (!interrupt && !operation_queue.empty()) {
2079       operation_t *operation = operation_queue.front();
2080       if (operation->step()) {
2081         operation_queue.pop();
2082         delete operation;
2083       }
2084     }
2085
2086     bool halt_notification = sim->debug_module.get_halt_notification(0);
2087     if (halt_notification) {
2088       sim->debug_module.clear_halt_notification(0);
2089       add_operation(new halt_op_t(*this, true));
2090     }
2091
2092     this->read();
2093     this->write();
2094
2095   } else {
2096     this->accept();
2097   }
2098
2099   if (operation_queue.empty()) {
2100     this->process_requests();
2101   }
2102 }
2103
2104 void gdbserver_t::send(const char* msg)
2105 {
2106   unsigned int length = strlen(msg);
2107   for (const char *c = msg; *c; c++)
2108     running_checksum += *c;
2109   send_buf.append((const uint8_t *) msg, length);
2110 }
2111
2112 void gdbserver_t::send(uint64_t value)
2113 {
2114   char buffer[3];
2115   for (unsigned int i = 0; i < 8; i++) {
2116     sprintf(buffer, "%02x", (int) (value & 0xff));
2117     send(buffer);
2118     value >>= 8;
2119   }
2120 }
2121
2122 void gdbserver_t::send(uint32_t value)
2123 {
2124   char buffer[3];
2125   for (unsigned int i = 0; i < 4; i++) {
2126     sprintf(buffer, "%02x", (int) (value & 0xff));
2127     send(buffer);
2128     value >>= 8;
2129   }
2130 }
2131
2132 void gdbserver_t::send(uint8_t value)
2133 {
2134   char buffer[3];
2135   sprintf(buffer, "%02x", (int) value);
2136   send(buffer);
2137 }
2138
2139 void gdbserver_t::send_packet(const char* data)
2140 {
2141   start_packet();
2142   send(data);
2143   end_packet();
2144   expect_ack = true;
2145 }
2146
2147 void gdbserver_t::start_packet()
2148 {
2149   send("$");
2150   running_checksum = 0;
2151 }
2152
2153 void gdbserver_t::end_packet(const char* data)
2154 {
2155   if (data) {
2156     send(data);
2157   }
2158
2159   char checksum_string[4];
2160   sprintf(checksum_string, "#%02x", running_checksum);
2161   send(checksum_string);
2162   expect_ack = true;
2163 }