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