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