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