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