riscv/gdbserver.cc

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