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