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