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