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 const int debug_gdbserver = 0;
  33
  34 void die(const char* msg)
  35 {
  36   fprintf(stderr, "gdbserver code died: %s\n", msg);
  37   abort();
  38 }
  39
  40 // gdb's register list is defined in riscv_gdb_reg_names gdb/riscv-tdep.c in
  41 // its source tree. We must interpret the numbers the same here.
  42 enum {
  43   REG_XPR0 = 0,
  44   REG_XPR31 = 31,
  45   REG_PC = 32,
  46   REG_FPR0 = 33,
  47   REG_FPR31 = 64,
  48   REG_CSR0 = 65,
  49   REG_CSR4095 = 4160,
  50   REG_END = 4161
  51 };
  52
  53 // Return access size to use when writing length bytes to address, so that
  54 // every write will be aligned.
  55 unsigned int find_access_size(reg_t address, int length)
  56 {
  57   reg_t composite = address | length;
  58   if ((composite & 0x7) == 0)
  59     return 8;
  60   if ((composite & 0x3) == 0)
  61     return 4;
  62   return 1;
  63 }
  64
  65 //////////////////////////////////////// Functions to generate RISC-V opcodes.
  66
  67 // TODO: Does this already exist somewhere?
  68
  69 // Using regnames.cc as source. The RVG Calling Convention of the 2.0 RISC-V
  70 // spec says it should be 2 and 3.
  71 #define S0      8
  72 #define S1      9
  73 static uint32_t bits(uint32_t value, unsigned int hi, unsigned int lo) {
  74   return (value >> lo) & ((1 << (hi+1-lo)) - 1);
  75 }
  76
  77 static uint32_t bit(uint32_t value, unsigned int b) {
  78   return (value >> b) & 1;
  79 }
  80
  81 static uint32_t jal(unsigned int rd, uint32_t imm) {
  82   return (bit(imm, 20) << 31) |
  83     (bits(imm, 10, 1) << 21) |
  84     (bit(imm, 11) << 20) |
  85     (bits(imm, 19, 12) << 12) |
  86     (rd << 7) |
  87     MATCH_JAL;
  88 }
  89
  90 static uint32_t csrsi(unsigned int csr, uint16_t imm) {
  91   return (csr << 20) |
  92     (bits(imm, 4, 0) << 15) |
  93     MATCH_CSRRSI;
  94 }
  95
  96 static uint32_t csrci(unsigned int csr, uint16_t imm) {
  97   return (csr << 20) |
  98     (bits(imm, 4, 0) << 15) |
  99     MATCH_CSRRCI;
 100 }
 101
 102 static uint32_t csrr(unsigned int rd, unsigned int csr) {
 103   return (csr << 20) | (rd << 7) | MATCH_CSRRS;
 104 }
 105
 106 static uint32_t csrw(unsigned int source, unsigned int csr) {
 107   return (csr << 20) | (source << 15) | MATCH_CSRRW;
 108 }
 109
 110 static uint32_t fence_i()
 111 {
 112   return MATCH_FENCE_I;
 113 }
 114
 115 static uint32_t sb(unsigned int src, unsigned int base, uint16_t offset)
 116 {
 117   return (bits(offset, 11, 5) << 25) |
 118     (src << 20) |
 119     (base << 15) |
 120     (bits(offset, 4, 0) << 7) |
 121     MATCH_SB;
 122 }
 123
 124 static uint32_t sh(unsigned int src, unsigned int base, uint16_t offset)
 125 {
 126   return (bits(offset, 11, 5) << 25) |
 127     (src << 20) |
 128     (base << 15) |
 129     (bits(offset, 4, 0) << 7) |
 130     MATCH_SH;
 131 }
 132
 133 static uint32_t sw(unsigned int src, unsigned int base, uint16_t offset)
 134 {
 135   return (bits(offset, 11, 5) << 25) |
 136     (src << 20) |
 137     (base << 15) |
 138     (bits(offset, 4, 0) << 7) |
 139     MATCH_SW;
 140 }
 141
 142 static uint32_t sd(unsigned int src, unsigned int base, uint16_t offset)
 143 {
 144   return (bits(offset, 11, 5) << 25) |
 145     (bits(src, 4, 0) << 20) |
 146     (base << 15) |
 147     (bits(offset, 4, 0) << 7) |
 148     MATCH_SD;
 149 }
 150
 151 static uint32_t ld(unsigned int rd, unsigned int base, uint16_t offset)
 152 {
 153   return (bits(offset, 11, 0) << 20) |
 154     (base << 15) |
 155     (bits(rd, 4, 0) << 7) |
 156     MATCH_LD;
 157 }
 158
 159 static uint32_t lw(unsigned int rd, unsigned int base, uint16_t offset)
 160 {
 161   return (bits(offset, 11, 0) << 20) |
 162     (base << 15) |
 163     (bits(rd, 4, 0) << 7) |
 164     MATCH_LW;
 165 }
 166
 167 static uint32_t lh(unsigned int rd, unsigned int base, uint16_t offset)
 168 {
 169   return (bits(offset, 11, 0) << 20) |
 170     (base << 15) |
 171     (bits(rd, 4, 0) << 7) |
 172     MATCH_LH;
 173 }
 174
 175 static uint32_t lb(unsigned int rd, unsigned int base, uint16_t offset)
 176 {
 177   return (bits(offset, 11, 0) << 20) |
 178     (base << 15) |
 179     (bits(rd, 4, 0) << 7) |
 180     MATCH_LB;
 181 }
 182
 183 static uint32_t fsd(unsigned int src, unsigned int base, uint16_t offset)
 184 {
 185   return (bits(offset, 11, 5) << 25) |
 186     (bits(src, 4, 0) << 20) |
 187     (base << 15) |
 188     (bits(offset, 4, 0) << 7) |
 189     MATCH_FSD;
 190 }
 191
 192 static uint32_t fld(unsigned int src, unsigned int base, uint16_t offset)
 193 {
 194   return (bits(offset, 11, 5) << 25) |
 195     (bits(src, 4, 0) << 20) |
 196     (base << 15) |
 197     (bits(offset, 4, 0) << 7) |
 198     MATCH_FLD;
 199 }
 200
 201 static uint32_t addi(unsigned int dest, unsigned int src, uint16_t imm)
 202 {
 203   return (bits(imm, 11, 0) << 20) |
 204     (src << 15) |
 205     (dest << 7) |
 206     MATCH_ADDI;
 207 }
 208
 209 static uint32_t ori(unsigned int dest, unsigned int src, uint16_t imm)
 210 {
 211   return (bits(imm, 11, 0) << 20) |
 212     (src << 15) |
 213     (dest << 7) |
 214     MATCH_ORI;
 215 }
 216
 217 static uint32_t nop()
 218 {
 219   return addi(0, 0, 0);
 220 }
 221
 222 template <typename T>
 223 unsigned int circular_buffer_t<T>::size() const
 224 {
 225   if (end >= start)
 226     return end - start;
 227   else
 228     return end + capacity - start;
 229 }
 230
 231 template <typename T>
 232 void circular_buffer_t<T>::consume(unsigned int bytes)
 233 {
 234   start = (start + bytes) % capacity;
 235 }
 236
 237 template <typename T>
 238 unsigned int circular_buffer_t<T>::contiguous_empty_size() const
 239 {
 240   if (end >= start)
 241     if (start == 0)
 242       return capacity - end - 1;
 243     else
 244       return capacity - end;
 245   else
 246     return start - end - 1;
 247 }
 248
 249 template <typename T>
 250 unsigned int circular_buffer_t<T>::contiguous_data_size() const
 251 {
 252   if (end >= start)
 253     return end - start;
 254   else
 255     return capacity - start;
 256 }
 257
 258 template <typename T>
 259 void circular_buffer_t<T>::data_added(unsigned int bytes)
 260 {
 261   end += bytes;
 262   assert(end <= capacity);
 263   if (end == capacity)
 264     end = 0;
 265 }
 266
 267 template <typename T>
 268 void circular_buffer_t<T>::reset()
 269 {
 270   start = 0;
 271   end = 0;
 272 }
 273
 274 template <typename T>
 275 void circular_buffer_t<T>::append(const T *src, unsigned int count)
 276 {
 277   unsigned int copy = std::min(count, contiguous_empty_size());
 278   memcpy(contiguous_empty(), src, copy * sizeof(T));
 279   data_added(copy);
 280   count -= copy;
 281   if (count > 0) {
 282     assert(count < contiguous_empty_size());
 283     memcpy(contiguous_empty(), src, count * sizeof(T));
 284     data_added(count);
 285   }
 286 }
 287
 288 ////////////////////////////// Debug Operations
 289
 290 class halt_op_t : public operation_t
 291 {
 292   public:
 293     halt_op_t(gdbserver_t& gdbserver, bool send_status=false) :
 294       operation_t(gdbserver), send_status(send_status) {};
 295
 296     bool perform_step(unsigned int step) {
 297       switch (step) {
 298         case 0:
 299           // TODO: For now we just assume the target is 64-bit.
 300           gs.write_debug_ram(0, csrsi(CSR_DCSR, DCSR_HALT));
 301           gs.write_debug_ram(1, csrr(S0, CSR_DPC));
 302           gs.write_debug_ram(2, sd(S0, 0, (uint16_t) DEBUG_RAM_START));
 303           gs.write_debug_ram(3, csrr(S0, CSR_MSTATUS));
 304           gs.write_debug_ram(4, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 8));
 305           gs.write_debug_ram(5, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*5))));
 306           gs.set_interrupt(0);
 307           // We could read more registers here, but only on 64-bit targets. I'm
 308           // trying to keep The patterns here usable for 32-bit ISAs as well.
 309           return false;
 310
 311         case 1:
 312           gs.dpc = ((uint64_t) gs.read_debug_ram(1) << 32) | gs.read_debug_ram(0);
 313           gs.mstatus = ((uint64_t) gs.read_debug_ram(3) << 32) | gs.read_debug_ram(2);
 314           gs.write_debug_ram(0, csrr(S0, CSR_DCSR));
 315           gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 316           gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 317           gs.set_interrupt(0);
 318           return false;
 319
 320         case 2:
 321           gs.dcsr = ((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4);
 322
 323           gs.sptbr_valid = false;
 324           gs.pte_cache.clear();
 325
 326           if (send_status) {
 327             switch (get_field(gs.dcsr, DCSR_CAUSE)) {
 328               case DCSR_CAUSE_NONE:
 329                 fprintf(stderr, "Internal error. Processor halted without reason.\n");
 330                 abort();
 331
 332               case DCSR_CAUSE_DEBUGINT:
 333                 gs.send_packet("S02");   // Pretend program received SIGINT.
 334                 break;
 335
 336               case DCSR_CAUSE_HWBP:
 337               case DCSR_CAUSE_STEP:
 338               case DCSR_CAUSE_HALT:
 339                 // There's no gdb code for this.
 340                 gs.send_packet("T05");
 341                 break;
 342               case DCSR_CAUSE_SWBP:
 343                 gs.send_packet("T05swbreak:;");
 344                 break;
 345             }
 346           }
 347
 348           return true;
 349       }
 350       return false;
 351     }
 352
 353   private:
 354     bool send_status;
 355 };
 356
 357 class continue_op_t : public operation_t
 358 {
 359   public:
 360     continue_op_t(gdbserver_t& gdbserver, bool single_step) :
 361       operation_t(gdbserver), single_step(single_step) {};
 362
 363     bool perform_step(unsigned int step) {
 364       switch (step) {
 365         case 0:
 366           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 367           gs.write_debug_ram(1, csrw(S0, CSR_DPC));
 368           if (gs.fence_i_required) {
 369             gs.write_debug_ram(2, fence_i());
 370             gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 371             gs.fence_i_required = false;
 372           } else {
 373             gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 374           }
 375           gs.write_debug_ram(4, gs.dpc);
 376           gs.write_debug_ram(5, gs.dpc >> 32);
 377           gs.set_interrupt(0);
 378           return false;
 379
 380         case 1:
 381           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 382           gs.write_debug_ram(1, csrw(S0, CSR_MSTATUS));
 383           gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 384           gs.write_debug_ram(4, gs.mstatus);
 385           gs.write_debug_ram(5, gs.mstatus >> 32);
 386           gs.set_interrupt(0);
 387           return false;
 388
 389         case 2:
 390           gs.write_debug_ram(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 391           gs.write_debug_ram(1, csrw(S0, CSR_DCSR));
 392           gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 393
 394           reg_t dcsr = set_field(gs.dcsr, DCSR_HALT, 0);
 395           dcsr = set_field(dcsr, DCSR_STEP, single_step);
 396           // Software breakpoints should go here.
 397           dcsr = set_field(dcsr, DCSR_EBREAKM, 1);
 398           dcsr = set_field(dcsr, DCSR_EBREAKH, 1);
 399           dcsr = set_field(dcsr, DCSR_EBREAKS, 1);
 400           dcsr = set_field(dcsr, DCSR_EBREAKU, 1);
 401           gs.write_debug_ram(4, dcsr);
 402
 403           gs.set_interrupt(0);
 404           return true;
 405       }
 406       return false;
 407     }
 408
 409   private:
 410     bool single_step;
 411 };
 412
 413 class general_registers_read_op_t : public operation_t
 414 {
 415   // Register order that gdb expects is:
 416   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 417   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 418   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 419   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 420
 421   // Each byte of register data is described by two hex digits. The bytes with
 422   // the register are transmitted in target byte order. The size of each
 423   // register and their position within the ‘g’ packet are determined by the
 424   // gdb internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and
 425   // gdbarch_register_name.
 426
 427   public:
 428     general_registers_read_op_t(gdbserver_t& gdbserver) :
 429       operation_t(gdbserver) {};
 430
 431     bool perform_step(unsigned int step)
 432     {
 433       if (step == 0) {
 434         gs.start_packet();
 435
 436         // x0 is always zero.
 437         gs.send((reg_t) 0);
 438
 439         gs.write_debug_ram(0, sd(1, 0, (uint16_t) DEBUG_RAM_START + 16));
 440         gs.write_debug_ram(1, sd(2, 0, (uint16_t) DEBUG_RAM_START + 0));
 441         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 442         gs.set_interrupt(0);
 443         return false;
 444       }
 445
 446       gs.send(((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4));
 447       if (step >= 16) {
 448         gs.end_packet();
 449         return true;
 450       }
 451
 452       gs.send(((uint64_t) gs.read_debug_ram(1) << 32) | gs.read_debug_ram(0));
 453
 454       unsigned int current_reg = 2 * step + 1;
 455       unsigned int i = 0;
 456       if (current_reg == S1) {
 457         gs.write_debug_ram(i++, ld(S1, 0, (uint16_t) DEBUG_RAM_END - 8));
 458       }
 459       gs.write_debug_ram(i++, sd(current_reg, 0, (uint16_t) DEBUG_RAM_START + 16));
 460       if (current_reg + 1 == S0) {
 461         gs.write_debug_ram(i++, csrr(S0, CSR_DSCRATCH));
 462       }
 463       gs.write_debug_ram(i++, sd(current_reg+1, 0, (uint16_t) DEBUG_RAM_START + 0));
 464       gs.write_debug_ram(i, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*i))));
 465       gs.set_interrupt(0);
 466
 467       return false;
 468     }
 469 };
 470
 471 class register_read_op_t : public operation_t
 472 {
 473   public:
 474     register_read_op_t(gdbserver_t& gdbserver, unsigned int reg) :
 475       operation_t(gdbserver), reg(reg) {};
 476
 477     bool perform_step(unsigned int step)
 478     {
 479       switch (step) {
 480         case 0:
 481           if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 482             die("handle_register_read");
 483             // send(p->state.XPR[reg - REG_XPR0]);
 484           } else if (reg == REG_PC) {
 485             gs.start_packet();
 486             gs.send(gs.dpc);
 487             gs.end_packet();
 488             return true;
 489           } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 490             // send(p->state.FPR[reg - REG_FPR0]);
 491             gs.write_debug_ram(0, fsd(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 492             gs.write_debug_ram(1, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*1))));
 493           } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 494             gs.write_debug_ram(0, csrr(S0, reg - REG_CSR0));
 495             gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 496             gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 497             // If we hit an exception reading the CSR, we'll end up returning ~0 as
 498             // the register's value, which is what we want. (Right?)
 499             gs.write_debug_ram(4, 0xffffffff);
 500             gs.write_debug_ram(5, 0xffffffff);
 501           } else {
 502             gs.send_packet("E02");
 503             return true;
 504           }
 505           gs.set_interrupt(0);
 506           return false;
 507
 508         case 1:
 509           gs.start_packet();
 510           gs.send(((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4));
 511           gs.end_packet();
 512           return true;
 513       }
 514       return false;
 515     }
 516
 517   private:
 518     unsigned int reg;
 519 };
 520
 521 class register_write_op_t : public operation_t
 522 {
 523   public:
 524     register_write_op_t(gdbserver_t& gdbserver, unsigned int reg, reg_t value) :
 525       operation_t(gdbserver), reg(reg), value(value) {};
 526
 527     bool perform_step(unsigned int step)
 528     {
 529       gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 530       gs.write_debug_ram(4, value);
 531       gs.write_debug_ram(5, value >> 32);
 532       if (reg == S0) {
 533         gs.write_debug_ram(1, csrw(S0, CSR_DSCRATCH));
 534         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 535       } else if (reg == S1) {
 536         gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_END - 8));
 537         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 538       } else if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 539         gs.write_debug_ram(1, addi(reg, S0, 0));
 540         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 541       } else if (reg == REG_PC) {
 542         gs.dpc = value;
 543         return true;
 544       } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 545         // send(p->state.FPR[reg - REG_FPR0]);
 546         gs.write_debug_ram(0, fld(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 547         gs.write_debug_ram(1, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*1))));
 548       } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 549         gs.write_debug_ram(1, csrw(S0, reg - REG_CSR0));
 550         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 551         if (reg == REG_CSR0 + CSR_SPTBR) {
 552           gs.sptbr = value;
 553           gs.sptbr_valid = true;
 554         }
 555       } else {
 556         gs.send_packet("E02");
 557         return true;
 558       }
 559       gs.set_interrupt(0);
 560       gs.send_packet("OK");
 561       return true;
 562     }
 563
 564   private:
 565     unsigned int reg;
 566     reg_t value;
 567 };
 568
 569 class memory_read_op_t : public operation_t
 570 {
 571   public:
 572     // Read length bytes from vaddr, storing the result into data.
 573     // If data is NULL, send the result straight to gdb.
 574     memory_read_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
 575         unsigned char *data=NULL) :
 576       operation_t(gdbserver), vaddr(vaddr), length(length), data(data) {};
 577
 578     bool perform_step(unsigned int step)
 579     {
 580       if (step == 0) {
 581         // address goes in S0
 582         paddr = gs.translate(vaddr);
 583         access_size = find_access_size(paddr, length);
 584
 585         gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 586         switch (access_size) {
 587           case 1:
 588             gs.write_debug_ram(1, lb(S1, S0, 0));
 589             break;
 590           case 2:
 591             gs.write_debug_ram(1, lh(S1, S0, 0));
 592             break;
 593           case 4:
 594             gs.write_debug_ram(1, lw(S1, S0, 0));
 595             break;
 596           case 8:
 597             gs.write_debug_ram(1, ld(S1, S0, 0));
 598             break;
 599         }
 600         gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 601         gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 602         gs.write_debug_ram(4, paddr);
 603         gs.write_debug_ram(5, paddr >> 32);
 604         gs.set_interrupt(0);
 605
 606         if (!data) {
 607           gs.start_packet();
 608         }
 609         return false;
 610       }
 611
 612       char buffer[3];
 613       reg_t value = ((uint64_t) gs.read_debug_ram(7) << 32) | gs.read_debug_ram(6);
 614       for (unsigned int i = 0; i < access_size; i++) {
 615         if (data) {
 616           *(data++) = value & 0xff;
 617           D(fprintf(stderr, "%02x", (unsigned int) (value & 0xff)));
 618         } else {
 619           sprintf(buffer, "%02x", (unsigned int) (value & 0xff));
 620           gs.send(buffer);
 621         }
 622         value >>= 8;
 623       }
 624       if (data && debug_gdbserver) {
 625         D(fprintf(stderr, "\n"));
 626       }
 627       length -= access_size;
 628       paddr += access_size;
 629
 630       if (length == 0) {
 631         if (!data) {
 632           gs.end_packet();
 633         }
 634         return true;
 635       } else {
 636         gs.write_debug_ram(4, paddr);
 637         gs.write_debug_ram(5, paddr >> 32);
 638         gs.set_interrupt(0);
 639         return false;
 640       }
 641     }
 642
 643   private:
 644     reg_t vaddr;
 645     unsigned int length;
 646     unsigned char* data;
 647     reg_t paddr;
 648     unsigned int access_size;
 649 };
 650
 651 class memory_write_op_t : public operation_t
 652 {
 653   public:
 654     memory_write_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
 655         const unsigned char *data) :
 656       operation_t(gdbserver), vaddr(vaddr), offset(0), length(length), data(data) {};
 657
 658     ~memory_write_op_t() {
 659       delete[] data;
 660     }
 661
 662     bool perform_step(unsigned int step)
 663     {
 664       reg_t paddr = gs.translate(vaddr);
 665       if (step == 0) {
 666         access_size = find_access_size(paddr, length);
 667
 668         D(fprintf(stderr, "write to 0x%lx -> 0x%lx (access=%d): ", vaddr, paddr,
 669             access_size));
 670         for (unsigned int i = 0; i < length; i++) {
 671           D(fprintf(stderr, "%02x", data[i]));
 672         }
 673         D(fprintf(stderr, "\n"));
 674
 675         // address goes in S0
 676         gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 677         switch (access_size) {
 678           case 1:
 679             gs.write_debug_ram(1, lb(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 680             gs.write_debug_ram(2, sb(S1, S0, 0));
 681             gs.write_debug_ram(6, data[0]);
 682             break;
 683           case 2:
 684             gs.write_debug_ram(1, lh(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 685             gs.write_debug_ram(2, sh(S1, S0, 0));
 686             gs.write_debug_ram(6, data[0] | (data[1] << 8));
 687             break;
 688           case 4:
 689             gs.write_debug_ram(1, lw(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 690             gs.write_debug_ram(2, sw(S1, S0, 0));
 691             gs.write_debug_ram(6, data[0] | (data[1] << 8) |
 692                 (data[2] << 16) | (data[3] << 24));
 693             break;
 694           case 8:
 695             gs.write_debug_ram(1, ld(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 696             gs.write_debug_ram(2, sd(S1, S0, 0));
 697             gs.write_debug_ram(6, data[0] | (data[1] << 8) |
 698                 (data[2] << 16) | (data[3] << 24));
 699             gs.write_debug_ram(7, data[4] | (data[5] << 8) |
 700                 (data[6] << 16) | (data[7] << 24));
 701             break;
 702           default:
 703             fprintf(stderr, "gdbserver error: write %d bytes to 0x%lx -> 0x%lx; "
 704                 "access_size=%d\n", length, vaddr, paddr, access_size);
 705             gs.send_packet("E12");
 706             return true;
 707         }
 708         gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 709         gs.write_debug_ram(4, paddr);
 710         gs.write_debug_ram(5, paddr >> 32);
 711         gs.set_interrupt(0);
 712
 713         return false;
 714       }
 715
 716       if (gs.read_debug_ram(DEBUG_RAM_SIZE / 4 - 1)) {
 717         fprintf(stderr, "Exception happened while writing to 0x%lx -> 0x%lx\n",
 718             vaddr, paddr);
 719       }
 720
 721       offset += access_size;
 722       if (offset >= length) {
 723         gs.send_packet("OK");
 724         return true;
 725       } else {
 726         const unsigned char *d = data + offset;
 727         switch (access_size) {
 728           case 1:
 729             gs.write_debug_ram(6, d[0]);
 730             break;
 731           case 2:
 732             gs.write_debug_ram(6, d[0] | (d[1] << 8));
 733             break;
 734           case 4:
 735             gs.write_debug_ram(6, d[0] | (d[1] << 8) |
 736                 (d[2] << 16) | (d[3] << 24));
 737             break;
 738           case 8:
 739             gs.write_debug_ram(6, d[0] | (d[1] << 8) |
 740                 (d[2] << 16) | (d[3] << 24));
 741             gs.write_debug_ram(7, d[4] | (d[5] << 8) |
 742                 (d[6] << 16) | (d[7] << 24));
 743             break;
 744           default:
 745             gs.send_packet("E13");
 746             return true;
 747         }
 748         gs.write_debug_ram(4, paddr + offset);
 749         gs.write_debug_ram(5, (paddr + offset) >> 32);
 750         gs.set_interrupt(0);
 751         return false;
 752       }
 753     }
 754
 755   private:
 756     reg_t vaddr;
 757     unsigned int offset;
 758     unsigned int length;
 759     unsigned int access_size;
 760     const unsigned char *data;
 761 };
 762
 763 class collect_translation_info_op_t : public operation_t
 764 {
 765   public:
 766     // Read sufficient information from the target into gdbserver structures so
 767     // that it's possible to translate vaddr, vaddr+length, and all addresses
 768     // in between to physical addresses.
 769     collect_translation_info_op_t(gdbserver_t& gdbserver, reg_t vaddr, size_t length) :
 770       operation_t(gdbserver), state(STATE_START), vaddr(vaddr), length(length) {};
 771
 772     bool perform_step(unsigned int step)
 773     {
 774       unsigned int vm = gs.virtual_memory();
 775
 776       if (step == 0) {
 777         switch (vm) {
 778           case VM_MBARE:
 779             // Nothing to be done.
 780             return true;
 781
 782           case VM_SV32:
 783             levels = 2;
 784             ptidxbits = 10;
 785             ptesize = 4;
 786             break;
 787           case VM_SV39:
 788             levels = 3;
 789             ptidxbits = 9;
 790             ptesize = 8;
 791             break;
 792           case VM_SV48:
 793             levels = 4;
 794             ptidxbits = 9;
 795             ptesize = 8;
 796             break;
 797
 798           default:
 799             {
 800               char buf[100];
 801               sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
 802               die(buf);
 803               return true;        // die doesn't return, but gcc doesn't know that.
 804             }
 805         }
 806       }
 807
 808       // Perform any reads from the just-completed action.
 809       switch (state) {
 810         case STATE_START:
 811           break;
 812         case STATE_READ_SPTBR:
 813           gs.sptbr = ((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4);
 814           gs.sptbr_valid = true;
 815           break;
 816         case STATE_READ_PTE:
 817           gs.pte_cache[pte_addr] = ((uint64_t) gs.read_debug_ram(5) << 32) |
 818             gs.read_debug_ram(4);
 819           D(fprintf(stderr, "pte_cache[0x%lx] = 0x%lx\n", pte_addr, gs.pte_cache[pte_addr]));
 820           break;
 821       }
 822
 823       // Set up the next action.
 824       // We only get here for VM_SV32/39/38.
 825
 826       if (!gs.sptbr_valid) {
 827         state = STATE_READ_SPTBR;
 828         gs.write_debug_ram(0, csrr(S0, CSR_SPTBR));
 829         gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 830         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 831         gs.set_interrupt(0);
 832         return false;
 833       }
 834
 835       reg_t base = gs.sptbr << PGSHIFT;
 836       int ptshift = (levels - 1) * ptidxbits;
 837       for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
 838         reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
 839
 840         pte_addr = base + idx * ptesize;
 841         auto it = gs.pte_cache.find(pte_addr);
 842         if (it == gs.pte_cache.end()) {
 843           state = STATE_READ_PTE;
 844           if (ptesize == 4) {
 845             gs.write_debug_ram(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 846             gs.write_debug_ram(1, lw(S1, S0, 0));
 847             gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 848           } else {
 849             gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 850             gs.write_debug_ram(1, ld(S1, S0, 0));
 851             gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 852           }
 853           gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 854           gs.write_debug_ram(4, pte_addr);
 855           gs.write_debug_ram(5, pte_addr >> 32);
 856           gs.set_interrupt(0);
 857           return false;
 858         }
 859
 860         reg_t pte = gs.pte_cache[pte_addr];
 861         reg_t ppn = pte >> PTE_PPN_SHIFT;
 862
 863         if (PTE_TABLE(pte)) { // next level of page table
 864           base = ppn << PGSHIFT;
 865         } else {
 866           // We've collected all the data required for the translation.
 867           return true;
 868         }
 869       }
 870       fprintf(stderr,
 871           "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%lx\n",
 872           vaddr);
 873       return true;
 874     }
 875
 876   private:
 877     enum {
 878       STATE_START,
 879       STATE_READ_SPTBR,
 880       STATE_READ_PTE
 881     } state;
 882     reg_t vaddr;
 883     size_t length;
 884     unsigned int levels;
 885     unsigned int ptidxbits;
 886     unsigned int ptesize;
 887     reg_t pte_addr;
 888 };
 889
 890 ////////////////////////////// gdbserver itself
 891
 892 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
 893   sim(sim),
 894   client_fd(0),
 895   recv_buf(64 * 1024), send_buf(64 * 1024)
 896 {
 897   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
 898   if (socket_fd == -1) {
 899     fprintf(stderr, "failed to make socket: %s (%d)\n", strerror(errno), errno);
 900     abort();
 901   }
 902
 903   fcntl(socket_fd, F_SETFL, O_NONBLOCK);
 904   int reuseaddr = 1;
 905   if (setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,
 906         sizeof(int)) == -1) {
 907     fprintf(stderr, "failed setsockopt: %s (%d)\n", strerror(errno), errno);
 908     abort();
 909   }
 910
 911   struct sockaddr_in addr;
 912   memset(&addr, 0, sizeof(addr));
 913   addr.sin_family = AF_INET;
 914   addr.sin_addr.s_addr = INADDR_ANY;
 915   addr.sin_port = htons(port);
 916
 917   if (bind(socket_fd, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
 918     fprintf(stderr, "failed to bind socket: %s (%d)\n", strerror(errno), errno);
 919     abort();
 920   }
 921
 922   if (listen(socket_fd, 1) == -1) {
 923     fprintf(stderr, "failed to listen on socket: %s (%d)\n", strerror(errno), errno);
 924     abort();
 925   }
 926 }
 927
 928 reg_t gdbserver_t::translate(reg_t vaddr)
 929 {
 930   unsigned int vm = virtual_memory();
 931   unsigned int levels, ptidxbits, ptesize;
 932
 933   switch (vm) {
 934     case VM_MBARE:
 935       return vaddr;
 936
 937     case VM_SV32:
 938       levels = 2;
 939       ptidxbits = 10;
 940       ptesize = 4;
 941       break;
 942     case VM_SV39:
 943       levels = 3;
 944       ptidxbits = 9;
 945       ptesize = 8;
 946       break;
 947     case VM_SV48:
 948       levels = 4;
 949       ptidxbits = 9;
 950       ptesize = 8;
 951       break;
 952
 953     default:
 954       {
 955         char buf[100];
 956         sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
 957         die(buf);
 958         return true;        // die doesn't return, but gcc doesn't know that.
 959       }
 960   }
 961
 962   // Handle page tables here. There's a bunch of duplicated code with
 963   // collect_translation_info_op_t. :-(
 964   reg_t base = sptbr << PGSHIFT;
 965   int ptshift = (levels - 1) * ptidxbits;
 966   for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
 967     reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
 968
 969     reg_t pte_addr = base + idx * ptesize;
 970     auto it = pte_cache.find(pte_addr);
 971     if (it == pte_cache.end()) {
 972       fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx without first "
 973           "collecting the relevant PTEs.\n", vaddr);
 974       die("gdbserver_t::translate()");
 975     }
 976
 977     reg_t pte = pte_cache[pte_addr];
 978     reg_t ppn = pte >> PTE_PPN_SHIFT;
 979
 980     if (PTE_TABLE(pte)) { // next level of page table
 981       base = ppn << PGSHIFT;
 982     } else {
 983       // We've collected all the data required for the translation.
 984       reg_t vpn = vaddr >> PGSHIFT;
 985       reg_t paddr = (ppn | (vpn & ((reg_t(1) << ptshift) - 1))) << PGSHIFT;
 986       paddr += vaddr & (PGSIZE-1);
 987       D(fprintf(stderr, "gdbserver translate 0x%lx -> 0x%lx\n", vaddr, paddr));
 988       return paddr;
 989     }
 990   }
 991
 992   fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx but the relevant "
 993       "PTEs are invalid.\n", vaddr);
 994   // TODO: Is it better to throw an exception here?
 995   return -1;
 996 }
 997
 998 unsigned int gdbserver_t::privilege_mode()
 999 {
1000   unsigned int mode = get_field(dcsr, DCSR_PRV);
1001   if (get_field(mstatus, MSTATUS_MPRV))
1002     mode = get_field(mstatus, MSTATUS_MPP);
1003   return mode;
1004 }
1005
1006 unsigned int gdbserver_t::virtual_memory()
1007 {
1008   unsigned int mode = privilege_mode();
1009   if (mode == PRV_M)
1010     return VM_MBARE;
1011   return get_field(mstatus, MSTATUS_VM);
1012 }
1013
1014 void gdbserver_t::write_debug_ram(unsigned int index, uint32_t value)
1015 {
1016   sim->debug_module.ram_write32(index, value);
1017 }
1018
1019 uint32_t gdbserver_t::read_debug_ram(unsigned int index)
1020 {
1021   return sim->debug_module.ram_read32(index);
1022 }
1023
1024 void gdbserver_t::add_operation(operation_t* operation)
1025 {
1026   operation_queue.push(operation);
1027 }
1028
1029 void gdbserver_t::accept()
1030 {
1031   client_fd = ::accept(socket_fd, NULL, NULL);
1032   if (client_fd == -1) {
1033     if (errno == EAGAIN) {
1034       // No client waiting to connect right now.
1035     } else {
1036       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno),
1037           errno);
1038       abort();
1039     }
1040   } else {
1041     fcntl(client_fd, F_SETFL, O_NONBLOCK);
1042
1043     expect_ack = false;
1044     extended_mode = false;
1045
1046     // gdb wants the core to be halted when it attaches.
1047     add_operation(new halt_op_t(*this));
1048   }
1049 }
1050
1051 void gdbserver_t::read()
1052 {
1053   // Reading from a non-blocking socket still blocks if there is no data
1054   // available.
1055
1056   size_t count = recv_buf.contiguous_empty_size();
1057   assert(count > 0);
1058   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
1059   if (bytes == -1) {
1060     if (errno == EAGAIN) {
1061       // We'll try again the next call.
1062     } else {
1063       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
1064       abort();
1065     }
1066   } else if (bytes == 0) {
1067     // The remote disconnected.
1068     client_fd = 0;
1069     processor_t *p = sim->get_core(0);
1070     // TODO p->set_halted(false, HR_NONE);
1071     recv_buf.reset();
1072     send_buf.reset();
1073   } else {
1074     recv_buf.data_added(bytes);
1075   }
1076 }
1077
1078 void gdbserver_t::write()
1079 {
1080   if (send_buf.empty())
1081     return;
1082
1083   while (!send_buf.empty()) {
1084     unsigned int count = send_buf.contiguous_data_size();
1085     assert(count > 0);
1086     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
1087     if (bytes == -1) {
1088       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
1089       abort();
1090     } else if (bytes == 0) {
1091       // Client can't take any more data right now.
1092       break;
1093     } else {
1094       D(fprintf(stderr, "wrote %ld bytes: ", bytes));
1095       for (unsigned int i = 0; i < bytes; i++) {
1096         D(fprintf(stderr, "%c", send_buf[i]));
1097       }
1098       D(fprintf(stderr, "\n"));
1099       send_buf.consume(bytes);
1100     }
1101   }
1102 }
1103
1104 void print_packet(const std::vector<uint8_t> &packet)
1105 {
1106   for (uint8_t c : packet) {
1107     if (c >= ' ' and c <= '~')
1108       fprintf(stderr, "%c", c);
1109     else
1110       fprintf(stderr, "\\x%02x", c);
1111   }
1112   fprintf(stderr, "\n");
1113 }
1114
1115 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
1116 {
1117   uint8_t checksum = 0;
1118   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
1119     checksum += *i;
1120   }
1121   return checksum;
1122 }
1123
1124 uint8_t character_hex_value(uint8_t character)
1125 {
1126   if (character >= '0' && character <= '9')
1127     return character - '0';
1128   if (character >= 'a' && character <= 'f')
1129     return 10 + character - 'a';
1130   if (character >= 'A' && character <= 'F')
1131     return 10 + character - 'A';
1132   return 0xff;
1133 }
1134
1135 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
1136 {
1137   return character_hex_value(*(packet.end() - 1)) +
1138     16 * character_hex_value(*(packet.end() - 2));
1139 }
1140
1141 void gdbserver_t::process_requests()
1142 {
1143   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
1144
1145   while (!recv_buf.empty()) {
1146     std::vector<uint8_t> packet;
1147     for (unsigned int i = 0; i < recv_buf.size(); i++) {
1148       uint8_t b = recv_buf[i];
1149
1150       if (packet.empty() && expect_ack && b == '+') {
1151         recv_buf.consume(1);
1152         break;
1153       }
1154
1155       if (packet.empty() && b == 3) {
1156         D(fprintf(stderr, "Received interrupt\n"));
1157         recv_buf.consume(1);
1158         handle_interrupt();
1159         break;
1160       }
1161
1162       if (b == '$') {
1163         // Start of new packet.
1164         if (!packet.empty()) {
1165           fprintf(stderr, "Received malformed %ld-byte packet from debug client: ",
1166               packet.size());
1167           print_packet(packet);
1168           recv_buf.consume(i);
1169           break;
1170         }
1171       }
1172
1173       packet.push_back(b);
1174
1175       // Packets consist of $<packet-data>#<checksum>
1176       // where <checksum> is
1177       if (packet.size() >= 4 &&
1178           packet[packet.size()-3] == '#') {
1179         handle_packet(packet);
1180         recv_buf.consume(i+1);
1181         break;
1182       }
1183     }
1184     // There's a partial packet in the buffer. Wait until we get more data to
1185     // process it.
1186     if (packet.size()) {
1187       break;
1188     }
1189   }
1190 }
1191
1192 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
1193 {
1194   send_packet("S00");
1195 }
1196
1197 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
1198 {
1199   add_operation(new general_registers_read_op_t(*this));
1200 }
1201
1202 void gdbserver_t::set_interrupt(uint32_t hartid) {
1203   sim->debug_module.set_interrupt(hartid);
1204 }
1205
1206 // First byte is the most-significant one.
1207 // Eg. "08675309" becomes 0x08675309.
1208 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
1209     std::vector<uint8_t>::const_iterator end)
1210 {
1211   uint64_t value = 0;
1212
1213   while (iter != end) {
1214     uint8_t c = *iter;
1215     uint64_t c_value = character_hex_value(c);
1216     if (c_value > 15)
1217       break;
1218     iter++;
1219     value <<= 4;
1220     value += c_value;
1221   }
1222   return value;
1223 }
1224
1225 // First byte is the least-significant one.
1226 // Eg. "08675309" becomes 0x09536708
1227 uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
1228     std::vector<uint8_t>::const_iterator end)
1229 {
1230   uint64_t value = 0;
1231   unsigned int shift = 4;
1232
1233   while (iter != end) {
1234     uint8_t c = *iter;
1235     uint64_t c_value = character_hex_value(c);
1236     if (c_value > 15)
1237       break;
1238     iter++;
1239     value |= c_value << shift;
1240     if ((shift % 8) == 0)
1241       shift += 12;
1242     else
1243       shift -= 4;
1244   }
1245   return value;
1246 }
1247
1248 void consume_string(std::string &str, std::vector<uint8_t>::const_iterator &iter,
1249     std::vector<uint8_t>::const_iterator end, uint8_t separator)
1250 {
1251   while (iter != end && *iter != separator) {
1252     str.append(1, (char) *iter);
1253     iter++;
1254   }
1255 }
1256
1257 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
1258 {
1259   // p n
1260
1261   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1262   unsigned int n = consume_hex_number(iter, packet.end());
1263   if (*iter != '#')
1264     return send_packet("E01");
1265
1266   add_operation(new register_read_op_t(*this, n));
1267 }
1268
1269 void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
1270 {
1271   // P n...=r...
1272
1273   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1274   unsigned int n = consume_hex_number(iter, packet.end());
1275   if (*iter != '=')
1276     return send_packet("E05");
1277   iter++;
1278
1279   reg_t value = consume_hex_number_le(iter, packet.end());
1280   if (*iter != '#')
1281     return send_packet("E06");
1282
1283   processor_t *p = sim->get_core(0);
1284
1285   add_operation(new register_write_op_t(*this, n, value));
1286
1287   return send_packet("OK");
1288 }
1289
1290 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
1291 {
1292   // m addr,length
1293   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1294   reg_t address = consume_hex_number(iter, packet.end());
1295   if (*iter != ',')
1296     return send_packet("E10");
1297   iter++;
1298   reg_t length = consume_hex_number(iter, packet.end());
1299   if (*iter != '#')
1300     return send_packet("E11");
1301
1302   add_operation(new collect_translation_info_op_t(*this, address, length));
1303   add_operation(new memory_read_op_t(*this, address, length));
1304 }
1305
1306 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
1307 {
1308   // X addr,length:XX...
1309   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1310   reg_t address = consume_hex_number(iter, packet.end());
1311   if (*iter != ',')
1312     return send_packet("E20");
1313   iter++;
1314   reg_t length = consume_hex_number(iter, packet.end());
1315   if (*iter != ':')
1316     return send_packet("E21");
1317   iter++;
1318
1319   if (length == 0) {
1320     return send_packet("OK");
1321   }
1322
1323   unsigned char *data = new unsigned char[length];
1324   for (unsigned int i = 0; i < length; i++) {
1325     if (iter == packet.end()) {
1326       return send_packet("E22");
1327     }
1328     uint8_t c = *iter;
1329     iter++;
1330     if (c == '}') {
1331       // The binary data representation uses 7d (ascii ‘}’) as an escape
1332       // character. Any escaped byte is transmitted as the escape character
1333       // followed by the original character XORed with 0x20. For example, the
1334       // byte 0x7d would be transmitted as the two bytes 0x7d 0x5d. The bytes
1335       // 0x23 (ascii ‘#’), 0x24 (ascii ‘$’), and 0x7d (ascii ‘}’) must always
1336       // be escaped.
1337       if (iter == packet.end()) {
1338         return send_packet("E23");
1339       }
1340       c = (*iter) ^ 0x20;
1341       iter++;
1342     }
1343     data[i] = c;
1344   }
1345   if (*iter != '#')
1346     return send_packet("E4b"); // EOVERFLOW
1347
1348   add_operation(new collect_translation_info_op_t(*this, address, length));
1349   add_operation(new memory_write_op_t(*this, address, length, data));
1350 }
1351
1352 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
1353 {
1354   // c [addr]
1355   processor_t *p = sim->get_core(0);
1356   if (packet[2] != '#') {
1357     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1358     dpc = consume_hex_number(iter, packet.end());
1359     if (*iter != '#')
1360       return send_packet("E30");
1361   }
1362
1363   add_operation(new continue_op_t(*this, false));
1364 }
1365
1366 void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
1367 {
1368   // s [addr]
1369   if (packet[2] != '#') {
1370     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1371     die("handle_step");
1372     //p->state.pc = consume_hex_number(iter, packet.end());
1373     if (*iter != '#')
1374       return send_packet("E40");
1375   }
1376
1377   add_operation(new continue_op_t(*this, true));
1378 }
1379
1380 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
1381 {
1382   // k
1383   // The exact effect of this packet is not specified.
1384   // Looks like OpenOCD disconnects?
1385   // TODO
1386 }
1387
1388 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
1389 {
1390   // Enable extended mode. In extended mode, the remote server is made
1391   // persistent. The ‘R’ packet is used to restart the program being debugged.
1392   send_packet("OK");
1393   extended_mode = true;
1394 }
1395
1396 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
1397 {
1398   // insert: Z type,addr,kind
1399   // remove: z type,addr,kind
1400
1401   software_breakpoint_t bp;
1402   bool insert = (packet[1] == 'Z');
1403   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1404   int type = consume_hex_number(iter, packet.end());
1405   if (*iter != ',')
1406     return send_packet("E50");
1407   iter++;
1408   bp.address = consume_hex_number(iter, packet.end());
1409   if (*iter != ',')
1410     return send_packet("E51");
1411   iter++;
1412   bp.size = consume_hex_number(iter, packet.end());
1413   // There may be more options after a ; here, but we don't support that.
1414   if (*iter != '#')
1415     return send_packet("E52");
1416
1417   if (bp.size != 2 && bp.size != 4) {
1418     return send_packet("E53");
1419   }
1420
1421   fence_i_required = true;
1422   add_operation(new collect_translation_info_op_t(*this, bp.address, bp.size));
1423   if (insert) {
1424     unsigned char* swbp = new unsigned char[4];
1425     if (bp.size == 2) {
1426       swbp[0] = C_EBREAK & 0xff;
1427       swbp[1] = (C_EBREAK >> 8) & 0xff;
1428     } else {
1429       swbp[0] = EBREAK & 0xff;
1430       swbp[1] = (EBREAK >> 8) & 0xff;
1431       swbp[2] = (EBREAK >> 16) & 0xff;
1432       swbp[3] = (EBREAK >> 24) & 0xff;
1433     }
1434
1435     breakpoints[bp.address] = new software_breakpoint_t(bp);
1436     add_operation(new memory_read_op_t(*this, bp.address, bp.size,
1437           breakpoints[bp.address]->instruction));
1438     add_operation(new memory_write_op_t(*this, bp.address, bp.size, swbp));
1439
1440   } else {
1441     software_breakpoint_t *found_bp;
1442     found_bp = breakpoints[bp.address];
1443     unsigned char* instruction = new unsigned char[4];
1444     memcpy(instruction, found_bp->instruction, 4);
1445     add_operation(new memory_write_op_t(*this, found_bp->address,
1446           found_bp->size, instruction));
1447     breakpoints.erase(bp.address);
1448     delete found_bp;
1449   }
1450
1451   return send_packet("OK");
1452 }
1453
1454 void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
1455 {
1456   std::string name;
1457   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1458
1459   consume_string(name, iter, packet.end(), ':');
1460   if (iter != packet.end())
1461     iter++;
1462   if (name == "Supported") {
1463     start_packet();
1464     while (iter != packet.end()) {
1465       std::string feature;
1466       consume_string(feature, iter, packet.end(), ';');
1467       if (iter != packet.end())
1468         iter++;
1469       if (feature == "swbreak+") {
1470         send("swbreak+;");
1471       }
1472     }
1473     send("PacketSize=131072;");
1474     return end_packet();
1475   }
1476
1477   D(fprintf(stderr, "Unsupported query %s\n", name.c_str()));
1478   return send_packet("");
1479 }
1480
1481 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
1482 {
1483   if (compute_checksum(packet) != extract_checksum(packet)) {
1484     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
1485     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
1486     print_packet(packet);
1487     send("-");
1488     return;
1489   }
1490
1491   D(fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size()));
1492   D(print_packet(packet));
1493   send("+");
1494
1495   switch (packet[1]) {
1496     case '!':
1497       return handle_extended(packet);
1498     case '?':
1499       return handle_halt_reason(packet);
1500     case 'g':
1501       return handle_general_registers_read(packet);
1502 //    case 'k':
1503 //      return handle_kill(packet);
1504     case 'm':
1505       return handle_memory_read(packet);
1506 //    case 'M':
1507 //      return handle_memory_write(packet);
1508     case 'X':
1509       return handle_memory_binary_write(packet);
1510     case 'p':
1511       return handle_register_read(packet);
1512     case 'P':
1513       return handle_register_write(packet);
1514     case 'c':
1515       return handle_continue(packet);
1516     case 's':
1517       return handle_step(packet);
1518     case 'z':
1519     case 'Z':
1520       return handle_breakpoint(packet);
1521     case 'q':
1522     case 'Q':
1523       return handle_query(packet);
1524   }
1525
1526   // Not supported.
1527   D(fprintf(stderr, "** Unsupported packet: "));
1528   D(print_packet(packet));
1529   send_packet("");
1530 }
1531
1532 void gdbserver_t::handle_interrupt()
1533 {
1534   processor_t *p = sim->get_core(0);
1535   add_operation(new halt_op_t(*this, true));
1536 }
1537
1538 void gdbserver_t::handle()
1539 {
1540   if (client_fd > 0) {
1541     processor_t *p = sim->get_core(0);
1542
1543     bool interrupt = sim->debug_module.get_interrupt(0);
1544
1545     if (!interrupt && !operation_queue.empty()) {
1546       operation_t *operation = operation_queue.front();
1547       if (operation->step()) {
1548         operation_queue.pop();
1549         delete operation;
1550       }
1551     }
1552
1553     bool halt_notification = sim->debug_module.get_halt_notification(0);
1554     if (halt_notification) {
1555       sim->debug_module.clear_halt_notification(0);
1556       add_operation(new halt_op_t(*this, true));
1557     }
1558
1559     this->read();
1560     this->write();
1561
1562   } else {
1563     this->accept();
1564   }
1565
1566   if (operation_queue.empty()) {
1567     this->process_requests();
1568   }
1569 }
1570
1571 void gdbserver_t::send(const char* msg)
1572 {
1573   unsigned int length = strlen(msg);
1574   for (const char *c = msg; *c; c++)
1575     running_checksum += *c;
1576   send_buf.append((const uint8_t *) msg, length);
1577 }
1578
1579 void gdbserver_t::send(uint64_t value)
1580 {
1581   char buffer[3];
1582   for (unsigned int i = 0; i < 8; i++) {
1583     sprintf(buffer, "%02x", (int) (value & 0xff));
1584     send(buffer);
1585     value >>= 8;
1586   }
1587 }
1588
1589 void gdbserver_t::send(uint32_t value)
1590 {
1591   char buffer[3];
1592   for (unsigned int i = 0; i < 4; i++) {
1593     sprintf(buffer, "%02x", (int) (value & 0xff));
1594     send(buffer);
1595     value >>= 8;
1596   }
1597 }
1598
1599 void gdbserver_t::send_packet(const char* data)
1600 {
1601   start_packet();
1602   send(data);
1603   end_packet();
1604   expect_ack = true;
1605 }
1606
1607 void gdbserver_t::start_packet()
1608 {
1609   send("$");
1610   running_checksum = 0;
1611 }
1612
1613 void gdbserver_t::end_packet(const char* data)
1614 {
1615   if (data) {
1616     send(data);
1617   }
1618
1619   char checksum_string[4];
1620   sprintf(checksum_string, "#%02x", running_checksum);
1621   send(checksum_string);
1622   expect_ack = true;
1623 }