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