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