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