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