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