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_HWBP:
 313               case DCSR_CAUSE_DEBUGINT:
 314               case DCSR_CAUSE_STEP:
 315               case DCSR_CAUSE_HALT:
 316                 // There's no gdb code for this.
 317                 gs.send_packet("T05");
 318                 break;
 319               case DCSR_CAUSE_SWBP:
 320                 gs.send_packet("T05swbreak:;");
 321                 break;
 322             }
 323           }
 324
 325           return true;
 326       }
 327       return false;
 328     }
 329
 330   private:
 331     bool send_status;
 332 };
 333
 334 class continue_op_t : public operation_t
 335 {
 336   public:
 337     continue_op_t(gdbserver_t& gdbserver, bool single_step) :
 338       operation_t(gdbserver), single_step(single_step) {};
 339
 340     bool perform_step(unsigned int step) {
 341       switch (step) {
 342         case 0:
 343           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 344           gs.write_debug_ram(1, csrw(S0, DPC_ADDRESS));
 345           if (gs.fence_i_required) {
 346             gs.write_debug_ram(2, fence_i());
 347             gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 348             gs.fence_i_required = false;
 349           } else {
 350             gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 351           }
 352           gs.write_debug_ram(4, gs.saved_dpc);
 353           gs.write_debug_ram(5, gs.saved_dpc >> 32);
 354           gs.set_interrupt(0);
 355           return false;
 356
 357         case 1:
 358           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 359           gs.write_debug_ram(1, csrw(S0, CSR_MBADADDR));
 360           gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 361           gs.write_debug_ram(4, gs.saved_mbadaddr);
 362           gs.write_debug_ram(5, gs.saved_mbadaddr >> 32);
 363           gs.set_interrupt(0);
 364           return false;
 365
 366         case 2:
 367           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+16));
 368           gs.write_debug_ram(1, csrw(S0, CSR_MSTATUS));
 369           gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 370           gs.write_debug_ram(4, gs.saved_mstatus);
 371           gs.write_debug_ram(5, gs.saved_mstatus >> 32);
 372           gs.set_interrupt(0);
 373           return false;
 374
 375         case 3:
 376           gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START+24));
 377           gs.write_debug_ram(1, csrw(S0, CSR_MCAUSE));
 378           gs.write_debug_ram(2, lw(S0, 0, (uint16_t) DEBUG_RAM_START+20));
 379           gs.write_debug_ram(3, csrw(S0, CSR_DCSR));
 380           gs.write_debug_ram(4, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*4))));
 381
 382           reg_t dcsr = gs.dcsr & ~DCSR_HALT_MASK;
 383           if (single_step)
 384             dcsr |= DCSR_STEP_MASK;
 385           else
 386             dcsr &= ~DCSR_STEP_MASK;
 387           gs.write_debug_ram(5, dcsr);
 388
 389           gs.write_debug_ram(6, gs.saved_mcause);
 390           gs.write_debug_ram(7, gs.saved_mcause >> 32);
 391           gs.set_interrupt(0);
 392           return true;
 393       }
 394       return false;
 395     }
 396
 397   private:
 398     bool single_step;
 399 };
 400
 401 class general_registers_read_op_t : public operation_t
 402 {
 403   // Register order that gdb expects is:
 404   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 405   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 406   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 407   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 408
 409   // Each byte of register data is described by two hex digits. The bytes with
 410   // the register are transmitted in target byte order. The size of each
 411   // register and their position within the ‘g’ packet are determined by the
 412   // gdb internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and
 413   // gdbarch_register_name.
 414
 415   public:
 416     general_registers_read_op_t(gdbserver_t& gdbserver) :
 417       operation_t(gdbserver) {};
 418
 419     bool perform_step(unsigned int step)
 420     {
 421       if (step == 0) {
 422         gs.start_packet();
 423
 424         // x0 is always zero.
 425         gs.send((reg_t) 0);
 426
 427         gs.write_debug_ram(0, sd(1, 0, (uint16_t) DEBUG_RAM_START + 16));
 428         gs.write_debug_ram(1, sd(2, 0, (uint16_t) DEBUG_RAM_START + 0));
 429         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 430         gs.set_interrupt(0);
 431         return false;
 432       }
 433
 434       gs.send(((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4));
 435       if (step >= 16) {
 436         gs.end_packet();
 437         return true;
 438       }
 439
 440       gs.send(((uint64_t) gs.read_debug_ram(1) << 32) | gs.read_debug_ram(0));
 441
 442       unsigned int current_reg = 2 * step + 1;
 443       unsigned int i = 0;
 444       if (current_reg == S1) {
 445         gs.write_debug_ram(i++, ld(S1, 0, (uint16_t) DEBUG_RAM_END - 8));
 446       }
 447       gs.write_debug_ram(i++, sd(current_reg, 0, (uint16_t) DEBUG_RAM_START + 16));
 448       if (current_reg + 1 == S0) {
 449         gs.write_debug_ram(i++, csrr(S0, CSR_DSCRATCH));
 450       }
 451       gs.write_debug_ram(i++, sd(current_reg+1, 0, (uint16_t) DEBUG_RAM_START + 0));
 452       gs.write_debug_ram(i, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*i))));
 453       gs.set_interrupt(0);
 454
 455       return false;
 456     }
 457 };
 458
 459 class register_read_op_t : public operation_t
 460 {
 461   public:
 462     register_read_op_t(gdbserver_t& gdbserver, unsigned int reg) :
 463       operation_t(gdbserver), reg(reg) {};
 464
 465     bool perform_step(unsigned int step)
 466     {
 467       switch (step) {
 468         case 0:
 469           if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 470             die("handle_register_read");
 471             // send(p->state.XPR[reg - REG_XPR0]);
 472           } else if (reg == REG_PC) {
 473             gs.start_packet();
 474             gs.send(gs.saved_dpc);
 475             gs.end_packet();
 476             return true;
 477           } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 478             // send(p->state.FPR[reg - REG_FPR0]);
 479             gs.write_debug_ram(0, fsd(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 480             gs.write_debug_ram(1, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*1))));
 481           } else if (reg == REG_CSR0 + CSR_MBADADDR) {
 482             gs.start_packet();
 483             gs.send(gs.saved_mbadaddr);
 484             gs.end_packet();
 485             return true;
 486           } else if (reg == REG_CSR0 + CSR_MCAUSE) {
 487             gs.start_packet();
 488             gs.send(gs.saved_mcause);
 489             gs.end_packet();
 490             return true;
 491           } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 492             gs.write_debug_ram(0, csrr(S0, reg - REG_CSR0));
 493             gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 494             gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 495             // If we hit an exception reading the CSR, we'll end up returning ~0 as
 496             // the register's value, which is what we want. (Right?)
 497             gs.write_debug_ram(4, 0xffffffff);
 498             gs.write_debug_ram(5, 0xffffffff);
 499           } else {
 500             gs.send_packet("E02");
 501             return true;
 502           }
 503           gs.set_interrupt(0);
 504           return false;
 505
 506         case 1:
 507           gs.start_packet();
 508           gs.send(((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4));
 509           gs.end_packet();
 510           return true;
 511       }
 512       return false;
 513     }
 514
 515   private:
 516     unsigned int reg;
 517 };
 518
 519 class register_write_op_t : public operation_t
 520 {
 521   public:
 522     register_write_op_t(gdbserver_t& gdbserver, unsigned int reg, reg_t value) :
 523       operation_t(gdbserver), reg(reg), value(value) {};
 524
 525     bool perform_step(unsigned int step)
 526     {
 527       gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 528       gs.write_debug_ram(4, value);
 529       gs.write_debug_ram(5, value >> 32);
 530       if (reg == S0) {
 531         gs.write_debug_ram(1, csrw(S0, CSR_DSCRATCH));
 532         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 533       } else if (reg == S1) {
 534         gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_END - 8));
 535         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 536       } else if (reg >= REG_XPR0 && reg <= REG_XPR31) {
 537         gs.write_debug_ram(1, addi(reg, S0, 0));
 538         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 539       } else if (reg == REG_PC) {
 540         gs.saved_dpc = value;
 541         return true;
 542       } else if (reg >= REG_FPR0 && reg <= REG_FPR31) {
 543         // send(p->state.FPR[reg - REG_FPR0]);
 544         gs.write_debug_ram(0, fld(reg - REG_FPR0, 0, (uint16_t) DEBUG_RAM_START + 16));
 545         gs.write_debug_ram(1, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*1))));
 546       } else if (reg == REG_CSR0 + CSR_MBADADDR) {
 547         gs.saved_mbadaddr = value;
 548         return true;
 549       } else if (reg == REG_CSR0 + CSR_MCAUSE) {
 550         gs.saved_mcause = value;
 551         return true;
 552       } else if (reg >= REG_CSR0 && reg <= REG_CSR4095) {
 553         gs.write_debug_ram(1, csrw(S0, reg - REG_CSR0));
 554         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 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 = (paddr % length);
 584         if (access_size == 0)
 585           access_size = length;
 586         if (access_size > 8)
 587           access_size = 8;
 588
 589         gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 590         switch (access_size) {
 591           case 1:
 592             gs.write_debug_ram(1, lb(S1, S0, 0));
 593             break;
 594           case 2:
 595             gs.write_debug_ram(1, lh(S1, S0, 0));
 596             break;
 597           case 4:
 598             gs.write_debug_ram(1, lw(S1, S0, 0));
 599             break;
 600           case 8:
 601             gs.write_debug_ram(1, ld(S1, S0, 0));
 602             break;
 603         }
 604         gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 605         gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 606         gs.write_debug_ram(4, paddr);
 607         gs.write_debug_ram(5, paddr >> 32);
 608         gs.set_interrupt(0);
 609
 610         if (!data) {
 611           gs.start_packet();
 612         }
 613         return false;
 614       }
 615
 616       char buffer[3];
 617       reg_t value = ((uint64_t) gs.read_debug_ram(7) << 32) | gs.read_debug_ram(6);
 618       for (unsigned int i = 0; i < access_size; i++) {
 619         if (data) {
 620           *(data++) = value & 0xff;
 621           fprintf(stderr, "%02x", (unsigned int) (value & 0xff));
 622         } else {
 623           sprintf(buffer, "%02x", (unsigned int) (value & 0xff));
 624           gs.send(buffer);
 625         }
 626         value >>= 8;
 627       }
 628       if (data)
 629         fprintf(stderr, "\n");
 630       length -= access_size;
 631       paddr += access_size;
 632
 633       if (length == 0) {
 634         if (!data) {
 635           gs.end_packet();
 636         }
 637         return true;
 638       } else {
 639         gs.write_debug_ram(4, paddr);
 640         gs.write_debug_ram(5, paddr >> 32);
 641         gs.set_interrupt(0);
 642         return false;
 643       }
 644     }
 645
 646   private:
 647     reg_t vaddr;
 648     unsigned int length;
 649     unsigned char* data;
 650     reg_t paddr;
 651     unsigned int access_size;
 652 };
 653
 654 class memory_write_op_t : public operation_t
 655 {
 656   public:
 657     memory_write_op_t(gdbserver_t& gdbserver, reg_t vaddr, unsigned int length,
 658         const unsigned char *data) :
 659       operation_t(gdbserver), vaddr(vaddr), offset(0), length(length), data(data) {};
 660
 661     ~memory_write_op_t() {
 662       delete[] data;
 663     }
 664
 665     bool perform_step(unsigned int step)
 666     {
 667       reg_t paddr = gs.translate(vaddr);
 668       if (step == 0) {
 669         // address goes in S0
 670         access_size = (paddr % length);
 671         if (access_size == 0)
 672           access_size = length;
 673
 674         fprintf(stderr, "write to 0x%lx -> 0x%lx: ", vaddr, paddr);
 675         for (unsigned int i = 0; i < length; i++)
 676           fprintf(stderr, "%02x", data[i]);
 677         fprintf(stderr, "\n");
 678
 679         gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 680         switch (access_size) {
 681           case 1:
 682             gs.write_debug_ram(1, lb(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 683             gs.write_debug_ram(2, sb(S1, S0, 0));
 684             gs.write_debug_ram(6, data[0]);
 685             break;
 686           case 2:
 687             gs.write_debug_ram(1, lh(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 688             gs.write_debug_ram(2, sh(S1, S0, 0));
 689             gs.write_debug_ram(6, data[0] | (data[1] << 8));
 690             break;
 691           case 4:
 692             gs.write_debug_ram(1, lw(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 693             gs.write_debug_ram(2, sw(S1, S0, 0));
 694             gs.write_debug_ram(6, data[0] | (data[1] << 8) |
 695                 (data[2] << 16) | (data[3] << 24));
 696             break;
 697           case 8:
 698             gs.write_debug_ram(1, ld(S1, 0, (uint16_t) DEBUG_RAM_START + 24));
 699             gs.write_debug_ram(2, sd(S1, S0, 0));
 700             gs.write_debug_ram(6, data[0] | (data[1] << 8) |
 701                 (data[2] << 16) | (data[3] << 24));
 702             gs.write_debug_ram(7, data[4] | (data[5] << 8) |
 703                 (data[6] << 16) | (data[7] << 24));
 704             break;
 705           default:
 706             gs.send_packet("E12");
 707             return true;
 708         }
 709         gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 710         gs.write_debug_ram(4, paddr);
 711         gs.write_debug_ram(5, paddr >> 32);
 712         gs.set_interrupt(0);
 713
 714         return false;
 715       }
 716
 717       offset += access_size;
 718       if (offset >= length) {
 719         gs.send_packet("OK");
 720         return true;
 721       } else {
 722         const unsigned char *d = data + offset;
 723         switch (access_size) {
 724           case 1:
 725             gs.write_debug_ram(6, d[0]);
 726             break;
 727           case 2:
 728             gs.write_debug_ram(6, d[0] | (d[1] << 8));
 729             break;
 730           case 4:
 731             gs.write_debug_ram(6, d[0] | (d[1] << 8) |
 732                 (d[2] << 16) | (d[3] << 24));
 733             break;
 734           case 8:
 735             gs.write_debug_ram(6, d[0] | (d[1] << 8) |
 736                 (d[2] << 16) | (d[3] << 24));
 737             gs.write_debug_ram(7, d[4] | (d[5] << 8) |
 738                 (d[6] << 16) | (d[7] << 24));
 739             break;
 740           default:
 741             gs.send_packet("E12");
 742             return true;
 743         }
 744         gs.write_debug_ram(4, paddr + offset);
 745         gs.write_debug_ram(5, (paddr + offset) >> 32);
 746         gs.set_interrupt(0);
 747         return false;
 748       }
 749     }
 750
 751   private:
 752     reg_t vaddr;
 753     unsigned int offset;
 754     unsigned int length;
 755     unsigned int access_size;
 756     const unsigned char *data;
 757 };
 758
 759 class collect_translation_info_op_t : public operation_t
 760 {
 761   public:
 762     // Read sufficient information from the target into gdbserver structures so
 763     // that it's possible to translate vaddr, vaddr+length, and all addresses
 764     // in between to physical addresses.
 765     collect_translation_info_op_t(gdbserver_t& gdbserver, reg_t vaddr, size_t length) :
 766       operation_t(gdbserver), state(STATE_START), vaddr(vaddr), length(length) {};
 767
 768     bool perform_step(unsigned int step)
 769     {
 770       unsigned int vm = gs.virtual_memory();
 771
 772       if (step == 0) {
 773         switch (vm) {
 774           case VM_MBARE:
 775             // Nothing to be done.
 776             return true;
 777
 778           case VM_SV32:
 779             levels = 2;
 780             ptidxbits = 10;
 781             ptesize = 4;
 782             break;
 783           case VM_SV39:
 784             levels = 3;
 785             ptidxbits = 9;
 786             ptesize = 8;
 787             break;
 788           case VM_SV48:
 789             levels = 4;
 790             ptidxbits = 9;
 791             ptesize = 8;
 792             break;
 793
 794           default:
 795             {
 796               char buf[100];
 797               sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
 798               die(buf);
 799               return true;        // die doesn't return, but gcc doesn't know that.
 800             }
 801         }
 802       }
 803
 804       // Perform any reads from the just-completed action.
 805       switch (state) {
 806         case STATE_START:
 807           break;
 808         case STATE_READ_SPTBR:
 809           gs.sptbr = ((uint64_t) gs.read_debug_ram(5) << 32) | gs.read_debug_ram(4);
 810           gs.sptbr_valid = true;
 811           break;
 812         case STATE_READ_PTE:
 813           gs.pte_cache[pte_addr] = ((uint64_t) gs.read_debug_ram(5) << 32) |
 814             gs.read_debug_ram(4);
 815           fprintf(stderr, "pte_cache[0x%lx] = 0x%lx\n", pte_addr, gs.pte_cache[pte_addr]);
 816           break;
 817       }
 818
 819       // Set up the next action.
 820       // We only get here for VM_SV32/39/38.
 821
 822       if (!gs.sptbr_valid) {
 823         state = STATE_READ_SPTBR;
 824         gs.write_debug_ram(0, csrr(S0, CSR_SPTBR));
 825         gs.write_debug_ram(1, sd(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 826         gs.write_debug_ram(2, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*2))));
 827         gs.set_interrupt(0);
 828         return false;
 829       }
 830
 831       reg_t base = gs.sptbr << PGSHIFT;
 832       int ptshift = (levels - 1) * ptidxbits;
 833       for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
 834         reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
 835
 836         pte_addr = base + idx * ptesize;
 837         auto it = gs.pte_cache.find(pte_addr);
 838         if (it == gs.pte_cache.end()) {
 839           state = STATE_READ_PTE;
 840           if (ptesize == 4) {
 841             gs.write_debug_ram(0, lw(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 842             gs.write_debug_ram(1, lw(S1, S0, 0));
 843             gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 844           } else {
 845             gs.write_debug_ram(0, ld(S0, 0, (uint16_t) DEBUG_RAM_START + 16));
 846             gs.write_debug_ram(1, ld(S1, S0, 0));
 847             gs.write_debug_ram(2, sd(S1, 0, (uint16_t) DEBUG_RAM_START + 16));
 848           }
 849           gs.write_debug_ram(3, jal(0, (uint32_t) (DEBUG_ROM_RESUME - (DEBUG_RAM_START + 4*3))));
 850           gs.write_debug_ram(4, pte_addr);
 851           gs.write_debug_ram(5, pte_addr >> 32);
 852           gs.set_interrupt(0);
 853           return false;
 854         }
 855
 856         reg_t pte = gs.pte_cache[pte_addr];
 857         reg_t ppn = pte >> PTE_PPN_SHIFT;
 858
 859         if (PTE_TABLE(pte)) { // next level of page table
 860           base = ppn << PGSHIFT;
 861         } else {
 862           // We've collected all the data required for the translation.
 863           return true;
 864         }
 865       }
 866       fprintf(stderr,
 867           "ERROR: gdbserver couldn't find appropriate PTEs to translate 0x%lx\n",
 868           vaddr);
 869       return true;
 870     }
 871
 872   private:
 873     enum {
 874       STATE_START,
 875       STATE_READ_SPTBR,
 876       STATE_READ_PTE
 877     } state;
 878     reg_t vaddr;
 879     size_t length;
 880     unsigned int levels;
 881     unsigned int ptidxbits;
 882     unsigned int ptesize;
 883     reg_t pte_addr;
 884 };
 885
 886 ////////////////////////////// gdbserver itself
 887
 888 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
 889   sim(sim),
 890   client_fd(0),
 891   recv_buf(64 * 1024), send_buf(64 * 1024)
 892 {
 893   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
 894   if (socket_fd == -1) {
 895     fprintf(stderr, "failed to make socket: %s (%d)\n", strerror(errno), errno);
 896     abort();
 897   }
 898
 899   fcntl(socket_fd, F_SETFL, O_NONBLOCK);
 900   int reuseaddr = 1;
 901   if (setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,
 902         sizeof(int)) == -1) {
 903     fprintf(stderr, "failed setsockopt: %s (%d)\n", strerror(errno), errno);
 904     abort();
 905   }
 906
 907   struct sockaddr_in addr;
 908   memset(&addr, 0, sizeof(addr));
 909   addr.sin_family = AF_INET;
 910   addr.sin_addr.s_addr = INADDR_ANY;
 911   addr.sin_port = htons(port);
 912
 913   if (bind(socket_fd, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
 914     fprintf(stderr, "failed to bind socket: %s (%d)\n", strerror(errno), errno);
 915     abort();
 916   }
 917
 918   if (listen(socket_fd, 1) == -1) {
 919     fprintf(stderr, "failed to listen on socket: %s (%d)\n", strerror(errno), errno);
 920     abort();
 921   }
 922 }
 923
 924 reg_t gdbserver_t::translate(reg_t vaddr)
 925 {
 926   unsigned int vm = virtual_memory();
 927   unsigned int levels, ptidxbits, ptesize;
 928
 929   switch (vm) {
 930     case VM_MBARE:
 931       return vaddr;
 932
 933     case VM_SV32:
 934       levels = 2;
 935       ptidxbits = 10;
 936       ptesize = 4;
 937       break;
 938     case VM_SV39:
 939       levels = 3;
 940       ptidxbits = 9;
 941       ptesize = 8;
 942       break;
 943     case VM_SV48:
 944       levels = 4;
 945       ptidxbits = 9;
 946       ptesize = 8;
 947       break;
 948
 949     default:
 950       {
 951         char buf[100];
 952         sprintf(buf, "VM mode %d is not supported by gdbserver.cc.", vm);
 953         die(buf);
 954         return true;        // die doesn't return, but gcc doesn't know that.
 955       }
 956   }
 957
 958   // Handle page tables here. There's a bunch of duplicated code with
 959   // collect_translation_info_op_t. :-(
 960   reg_t base = sptbr << PGSHIFT;
 961   int ptshift = (levels - 1) * ptidxbits;
 962   for (unsigned int i = 0; i < levels; i++, ptshift -= ptidxbits) {
 963     reg_t idx = (vaddr >> (PGSHIFT + ptshift)) & ((1 << ptidxbits) - 1);
 964
 965     reg_t pte_addr = base + idx * ptesize;
 966     auto it = pte_cache.find(pte_addr);
 967     if (it == pte_cache.end()) {
 968       fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx without first "
 969           "collecting the relevant PTEs.\n", vaddr);
 970       die("gdbserver_t::translate()");
 971     }
 972
 973     reg_t pte = pte_cache[pte_addr];
 974     reg_t ppn = pte >> PTE_PPN_SHIFT;
 975
 976     if (PTE_TABLE(pte)) { // next level of page table
 977       base = ppn << PGSHIFT;
 978     } else {
 979       // We've collected all the data required for the translation.
 980       reg_t vpn = vaddr >> PGSHIFT;
 981       reg_t paddr = (ppn | (vpn & ((reg_t(1) << ptshift) - 1))) << PGSHIFT;
 982       paddr += vaddr & (PGSIZE-1);
 983       fprintf(stderr, "gdbserver translate 0x%lx -> 0x%lx\n", vaddr, paddr);
 984       return paddr;
 985     }
 986   }
 987
 988   fprintf(stderr, "ERROR: gdbserver tried to translate 0x%lx but the relevant "
 989       "PTEs are invalid.\n", vaddr);
 990   // TODO: Is it better to throw an exception here?
 991   return -1;
 992 }
 993
 994 unsigned int gdbserver_t::privilege_mode()
 995 {
 996   unsigned int mode = get_field(dcsr, DCSR_PRV);
 997   if (get_field(saved_mstatus, MSTATUS_MPRV))
 998     mode = get_field(saved_mstatus, MSTATUS_MPP);
 999   return mode;
1000 }
1001
1002 unsigned int gdbserver_t::virtual_memory()
1003 {
1004   unsigned int mode = privilege_mode();
1005   if (mode == PRV_M)
1006     return VM_MBARE;
1007   return get_field(saved_mstatus, MSTATUS_VM);
1008 }
1009
1010 void gdbserver_t::write_debug_ram(unsigned int index, uint32_t value)
1011 {
1012   sim->debug_module.ram_write32(index, value);
1013 }
1014
1015 uint32_t gdbserver_t::read_debug_ram(unsigned int index)
1016 {
1017   return sim->debug_module.ram_read32(index);
1018 }
1019
1020 void gdbserver_t::add_operation(operation_t* operation)
1021 {
1022   operation_queue.push(operation);
1023 }
1024
1025 void gdbserver_t::accept()
1026 {
1027   client_fd = ::accept(socket_fd, NULL, NULL);
1028   if (client_fd == -1) {
1029     if (errno == EAGAIN) {
1030       // No client waiting to connect right now.
1031     } else {
1032       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno),
1033           errno);
1034       abort();
1035     }
1036   } else {
1037     fcntl(client_fd, F_SETFL, O_NONBLOCK);
1038
1039     expect_ack = false;
1040     extended_mode = false;
1041
1042     // gdb wants the core to be halted when it attaches.
1043     add_operation(new halt_op_t(*this));
1044   }
1045 }
1046
1047 void gdbserver_t::read()
1048 {
1049   // Reading from a non-blocking socket still blocks if there is no data
1050   // available.
1051
1052   size_t count = recv_buf.contiguous_empty_size();
1053   assert(count > 0);
1054   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
1055   if (bytes == -1) {
1056     if (errno == EAGAIN) {
1057       // We'll try again the next call.
1058     } else {
1059       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
1060       abort();
1061     }
1062   } else if (bytes == 0) {
1063     // The remote disconnected.
1064     client_fd = 0;
1065     processor_t *p = sim->get_core(0);
1066     // TODO p->set_halted(false, HR_NONE);
1067     recv_buf.reset();
1068     send_buf.reset();
1069   } else {
1070     recv_buf.data_added(bytes);
1071   }
1072 }
1073
1074 void gdbserver_t::write()
1075 {
1076   if (send_buf.empty())
1077     return;
1078
1079   while (!send_buf.empty()) {
1080     unsigned int count = send_buf.contiguous_data_size();
1081     assert(count > 0);
1082     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
1083     if (bytes == -1) {
1084       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
1085       abort();
1086     } else if (bytes == 0) {
1087       // Client can't take any more data right now.
1088       break;
1089     } else {
1090       fprintf(stderr, "wrote %ld bytes: ", bytes);
1091       for (unsigned int i = 0; i < bytes; i++) {
1092         fprintf(stderr, "%c", send_buf[i]);
1093       }
1094       fprintf(stderr, "\n");
1095       send_buf.consume(bytes);
1096     }
1097   }
1098 }
1099
1100 void print_packet(const std::vector<uint8_t> &packet)
1101 {
1102   for (uint8_t c : packet) {
1103     if (c >= ' ' and c <= '~')
1104       fprintf(stderr, "%c", c);
1105     else
1106       fprintf(stderr, "\\x%x", c);
1107   }
1108   fprintf(stderr, "\n");
1109 }
1110
1111 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
1112 {
1113   uint8_t checksum = 0;
1114   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
1115     checksum += *i;
1116   }
1117   return checksum;
1118 }
1119
1120 uint8_t character_hex_value(uint8_t character)
1121 {
1122   if (character >= '0' && character <= '9')
1123     return character - '0';
1124   if (character >= 'a' && character <= 'f')
1125     return 10 + character - 'a';
1126   if (character >= 'A' && character <= 'F')
1127     return 10 + character - 'A';
1128   return 0xff;
1129 }
1130
1131 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
1132 {
1133   return character_hex_value(*(packet.end() - 1)) +
1134     16 * character_hex_value(*(packet.end() - 2));
1135 }
1136
1137 void gdbserver_t::process_requests()
1138 {
1139   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
1140
1141   while (!recv_buf.empty()) {
1142     std::vector<uint8_t> packet;
1143     for (unsigned int i = 0; i < recv_buf.size(); i++) {
1144       uint8_t b = recv_buf[i];
1145
1146       if (packet.empty() && expect_ack && b == '+') {
1147         recv_buf.consume(1);
1148         break;
1149       }
1150
1151       if (packet.empty() && b == 3) {
1152         fprintf(stderr, "Received interrupt\n");
1153         recv_buf.consume(1);
1154         handle_interrupt();
1155         break;
1156       }
1157
1158       if (b == '$') {
1159         // Start of new packet.
1160         if (!packet.empty()) {
1161           fprintf(stderr, "Received malformed %ld-byte packet from debug client: ",
1162               packet.size());
1163           print_packet(packet);
1164           recv_buf.consume(i);
1165           break;
1166         }
1167       }
1168
1169       packet.push_back(b);
1170
1171       // Packets consist of $<packet-data>#<checksum>
1172       // where <checksum> is
1173       if (packet.size() >= 4 &&
1174           packet[packet.size()-3] == '#') {
1175         handle_packet(packet);
1176         recv_buf.consume(i+1);
1177         break;
1178       }
1179     }
1180     // There's a partial packet in the buffer. Wait until we get more data to
1181     // process it.
1182     if (packet.size()) {
1183       break;
1184     }
1185   }
1186 }
1187
1188 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
1189 {
1190   send_packet("S00");
1191 }
1192
1193 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
1194 {
1195   add_operation(new general_registers_read_op_t(*this));
1196 }
1197
1198 void gdbserver_t::set_interrupt(uint32_t hartid) {
1199   sim->debug_module.set_interrupt(hartid);
1200 }
1201
1202 // First byte is the most-significant one.
1203 // Eg. "08675309" becomes 0x08675309.
1204 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
1205     std::vector<uint8_t>::const_iterator end)
1206 {
1207   uint64_t value = 0;
1208
1209   while (iter != end) {
1210     uint8_t c = *iter;
1211     uint64_t c_value = character_hex_value(c);
1212     if (c_value > 15)
1213       break;
1214     iter++;
1215     value <<= 4;
1216     value += c_value;
1217   }
1218   return value;
1219 }
1220
1221 // First byte is the least-significant one.
1222 // Eg. "08675309" becomes 0x09536708
1223 uint64_t consume_hex_number_le(std::vector<uint8_t>::const_iterator &iter,
1224     std::vector<uint8_t>::const_iterator end)
1225 {
1226   uint64_t value = 0;
1227   unsigned int shift = 4;
1228
1229   while (iter != end) {
1230     uint8_t c = *iter;
1231     uint64_t c_value = character_hex_value(c);
1232     if (c_value > 15)
1233       break;
1234     iter++;
1235     value |= c_value << shift;
1236     if ((shift % 8) == 0)
1237       shift += 12;
1238     else
1239       shift -= 4;
1240   }
1241   return value;
1242 }
1243
1244 void consume_string(std::string &str, std::vector<uint8_t>::const_iterator &iter,
1245     std::vector<uint8_t>::const_iterator end, uint8_t separator)
1246 {
1247   while (iter != end && *iter != separator) {
1248     str.append(1, (char) *iter);
1249     iter++;
1250   }
1251 }
1252
1253 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
1254 {
1255   // p n
1256
1257   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1258   unsigned int n = consume_hex_number(iter, packet.end());
1259   if (*iter != '#')
1260     return send_packet("E01");
1261
1262   add_operation(new register_read_op_t(*this, n));
1263 }
1264
1265 void gdbserver_t::handle_register_write(const std::vector<uint8_t> &packet)
1266 {
1267   // P n...=r...
1268
1269   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1270   unsigned int n = consume_hex_number(iter, packet.end());
1271   if (*iter != '=')
1272     return send_packet("E05");
1273   iter++;
1274
1275   reg_t value = consume_hex_number_le(iter, packet.end());
1276   if (*iter != '#')
1277     return send_packet("E06");
1278
1279   processor_t *p = sim->get_core(0);
1280
1281   add_operation(new register_write_op_t(*this, n, value));
1282
1283   return send_packet("OK");
1284 }
1285
1286 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
1287 {
1288   // m addr,length
1289   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1290   reg_t address = consume_hex_number(iter, packet.end());
1291   if (*iter != ',')
1292     return send_packet("E10");
1293   iter++;
1294   reg_t length = consume_hex_number(iter, packet.end());
1295   if (*iter != '#')
1296     return send_packet("E11");
1297
1298   add_operation(new collect_translation_info_op_t(*this, address, length));
1299   add_operation(new memory_read_op_t(*this, address, length));
1300 }
1301
1302 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
1303 {
1304   // X addr,length:XX...
1305   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1306   reg_t address = consume_hex_number(iter, packet.end());
1307   if (*iter != ',')
1308     return send_packet("E20");
1309   iter++;
1310   reg_t length = consume_hex_number(iter, packet.end());
1311   if (*iter != ':')
1312     return send_packet("E21");
1313   iter++;
1314
1315   if (length == 0) {
1316     return send_packet("OK");
1317   }
1318
1319   unsigned char *data = new unsigned char[length];
1320   for (unsigned int i = 0; i < length; i++) {
1321     if (iter == packet.end()) {
1322       return send_packet("E22");
1323     }
1324     data[i] = *iter;
1325     iter++;
1326   }
1327   if (*iter != '#')
1328     return send_packet("E4b"); // EOVERFLOW
1329
1330   add_operation(new collect_translation_info_op_t(*this, address, length));
1331   add_operation(new memory_write_op_t(*this, address, length, data));
1332 }
1333
1334 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
1335 {
1336   // c [addr]
1337   processor_t *p = sim->get_core(0);
1338   if (packet[2] != '#') {
1339     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1340     saved_dpc = consume_hex_number(iter, packet.end());
1341     if (*iter != '#')
1342       return send_packet("E30");
1343   }
1344
1345   add_operation(new continue_op_t(*this, false));
1346 }
1347
1348 void gdbserver_t::handle_step(const std::vector<uint8_t> &packet)
1349 {
1350   // s [addr]
1351   if (packet[2] != '#') {
1352     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1353     die("handle_step");
1354     //p->state.pc = consume_hex_number(iter, packet.end());
1355     if (*iter != '#')
1356       return send_packet("E40");
1357   }
1358
1359   add_operation(new continue_op_t(*this, true));
1360 }
1361
1362 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
1363 {
1364   // k
1365   // The exact effect of this packet is not specified.
1366   // Looks like OpenOCD disconnects?
1367   // TODO
1368 }
1369
1370 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
1371 {
1372   // Enable extended mode. In extended mode, the remote server is made
1373   // persistent. The ‘R’ packet is used to restart the program being debugged.
1374   send_packet("OK");
1375   extended_mode = true;
1376 }
1377
1378 void gdbserver_t::handle_breakpoint(const std::vector<uint8_t> &packet)
1379 {
1380   // insert: Z type,addr,kind
1381   // remove: z type,addr,kind
1382
1383   software_breakpoint_t bp;
1384   bool insert = (packet[1] == 'Z');
1385   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1386   int type = consume_hex_number(iter, packet.end());
1387   if (*iter != ',')
1388     return send_packet("E50");
1389   iter++;
1390   bp.address = consume_hex_number(iter, packet.end());
1391   if (*iter != ',')
1392     return send_packet("E51");
1393   iter++;
1394   bp.size = consume_hex_number(iter, packet.end());
1395   // There may be more options after a ; here, but we don't support that.
1396   if (*iter != '#')
1397     return send_packet("E52");
1398
1399   if (bp.size != 2 && bp.size != 4) {
1400     return send_packet("E53");
1401   }
1402
1403   fence_i_required = true;
1404   add_operation(new collect_translation_info_op_t(*this, bp.address, bp.size));
1405   if (insert) {
1406     unsigned char* swbp = new unsigned char[4];
1407     if (bp.size == 2) {
1408       swbp[0] = C_EBREAK & 0xff;
1409       swbp[1] = (C_EBREAK >> 8) & 0xff;
1410     } else {
1411       swbp[0] = EBREAK & 0xff;
1412       swbp[1] = (EBREAK >> 8) & 0xff;
1413       swbp[2] = (EBREAK >> 16) & 0xff;
1414       swbp[3] = (EBREAK >> 24) & 0xff;
1415     }
1416
1417     breakpoints[bp.address] = new software_breakpoint_t(bp);
1418     add_operation(new memory_read_op_t(*this, bp.address, bp.size,
1419           breakpoints[bp.address]->instruction));
1420     add_operation(new memory_write_op_t(*this, bp.address, bp.size, swbp));
1421
1422   } else {
1423     software_breakpoint_t *found_bp;
1424     found_bp = breakpoints[bp.address];
1425     unsigned char* instruction = new unsigned char[4];
1426     memcpy(instruction, found_bp->instruction, 4);
1427     add_operation(new memory_write_op_t(*this, found_bp->address,
1428           found_bp->size, instruction));
1429     breakpoints.erase(bp.address);
1430     delete found_bp;
1431   }
1432
1433   return send_packet("OK");
1434 }
1435
1436 void gdbserver_t::handle_query(const std::vector<uint8_t> &packet)
1437 {
1438   std::string name;
1439   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
1440
1441   consume_string(name, iter, packet.end(), ':');
1442   if (iter != packet.end())
1443     iter++;
1444   if (name == "Supported") {
1445     start_packet();
1446     while (iter != packet.end()) {
1447       std::string feature;
1448       consume_string(feature, iter, packet.end(), ';');
1449       if (iter != packet.end())
1450         iter++;
1451       if (feature == "swbreak+") {
1452         send("swbreak+;");
1453       }
1454     }
1455     return end_packet();
1456   }
1457
1458   fprintf(stderr, "Unsupported query %s\n", name.c_str());
1459   return send_packet("");
1460 }
1461
1462 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
1463 {
1464   if (compute_checksum(packet) != extract_checksum(packet)) {
1465     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
1466     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
1467     print_packet(packet);
1468     send("-");
1469     return;
1470   }
1471
1472   fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size());
1473   print_packet(packet);
1474   send("+");
1475
1476   switch (packet[1]) {
1477     case '!':
1478       return handle_extended(packet);
1479     case '?':
1480       return handle_halt_reason(packet);
1481     case 'g':
1482       return handle_general_registers_read(packet);
1483     case 'k':
1484       return handle_kill(packet);
1485     case 'm':
1486       return handle_memory_read(packet);
1487 //    case 'M':
1488 //      return handle_memory_write(packet);
1489     case 'X':
1490       return handle_memory_binary_write(packet);
1491     case 'p':
1492       return handle_register_read(packet);
1493     case 'P':
1494       return handle_register_write(packet);
1495     case 'c':
1496       return handle_continue(packet);
1497     case 's':
1498       return handle_step(packet);
1499     case 'z':
1500     case 'Z':
1501       return handle_breakpoint(packet);
1502     case 'q':
1503     case 'Q':
1504       return handle_query(packet);
1505   }
1506
1507   // Not supported.
1508   fprintf(stderr, "** Unsupported packet: ");
1509   print_packet(packet);
1510   send_packet("");
1511 }
1512
1513 void gdbserver_t::handle_interrupt()
1514 {
1515   processor_t *p = sim->get_core(0);
1516   // TODO p->set_halted(true, HR_INTERRUPT);
1517   send_packet("S02");   // Pretend program received SIGINT.
1518   // TODO running = false;
1519 }
1520
1521 void gdbserver_t::handle()
1522 {
1523   if (client_fd > 0) {
1524     processor_t *p = sim->get_core(0);
1525
1526     bool interrupt = sim->debug_module.get_interrupt(0);
1527
1528     if (!interrupt && !operation_queue.empty()) {
1529       operation_t *operation = operation_queue.front();
1530       if (operation->step()) {
1531         operation_queue.pop();
1532         delete operation;
1533       }
1534     }
1535
1536     bool halt_notification = sim->debug_module.get_halt_notification(0);
1537     if (halt_notification) {
1538       sim->debug_module.clear_halt_notification(0);
1539       add_operation(new halt_op_t(*this, true));
1540     }
1541
1542     this->read();
1543     this->write();
1544
1545   } else {
1546     this->accept();
1547   }
1548
1549   if (operation_queue.empty()) {
1550     this->process_requests();
1551   }
1552 }
1553
1554 void gdbserver_t::send(const char* msg)
1555 {
1556   unsigned int length = strlen(msg);
1557   for (const char *c = msg; *c; c++)
1558     running_checksum += *c;
1559   send_buf.append((const uint8_t *) msg, length);
1560 }
1561
1562 void gdbserver_t::send(uint64_t value)
1563 {
1564   char buffer[3];
1565   for (unsigned int i = 0; i < 8; i++) {
1566     sprintf(buffer, "%02x", (int) (value & 0xff));
1567     send(buffer);
1568     value >>= 8;
1569   }
1570 }
1571
1572 void gdbserver_t::send(uint32_t value)
1573 {
1574   char buffer[3];
1575   for (unsigned int i = 0; i < 4; i++) {
1576     sprintf(buffer, "%02x", (int) (value & 0xff));
1577     send(buffer);
1578     value >>= 8;
1579   }
1580 }
1581
1582 void gdbserver_t::send_packet(const char* data)
1583 {
1584   start_packet();
1585   send(data);
1586   end_packet();
1587   expect_ack = true;
1588 }
1589
1590 void gdbserver_t::start_packet()
1591 {
1592   send("$");
1593   running_checksum = 0;
1594 }
1595
1596 void gdbserver_t::end_packet(const char* data)
1597 {
1598   if (data) {
1599     send(data);
1600   }
1601
1602   char checksum_string[4];
1603   sprintf(checksum_string, "#%02x", running_checksum);
1604   send(checksum_string);
1605   expect_ack = true;
1606 }