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