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