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