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
  19 template <typename T>
  20 unsigned int circular_buffer_t<T>::size() const
  21 {
  22   if (end >= start)
  23     return end - start;
  24   else
  25     return end + capacity - start;
  26 }
  27
  28 template <typename T>
  29 void circular_buffer_t<T>::consume(unsigned int bytes)
  30 {
  31   start = (start + bytes) % capacity;
  32 }
  33
  34 template <typename T>
  35 unsigned int circular_buffer_t<T>::contiguous_empty_size() const
  36 {
  37   if (end >= start)
  38     if (start == 0)
  39       return capacity - end - 1;
  40     else
  41       return capacity - end;
  42   else
  43     return start - end - 1;
  44 }
  45
  46 template <typename T>
  47 unsigned int circular_buffer_t<T>::contiguous_data_size() const
  48 {
  49   if (end >= start)
  50     return end - start;
  51   else
  52     return capacity - start;
  53 }
  54
  55 template <typename T>
  56 void circular_buffer_t<T>::data_added(unsigned int bytes)
  57 {
  58   end += bytes;
  59   assert(end <= capacity);
  60   if (end == capacity)
  61     end = 0;
  62 }
  63
  64 template <typename T>
  65 void circular_buffer_t<T>::reset()
  66 {
  67   start = 0;
  68   end = 0;
  69 }
  70
  71 template <typename T>
  72 void circular_buffer_t<T>::append(const T *src, unsigned int count)
  73 {
  74   unsigned int copy = std::min(count, contiguous_empty_size());
  75   memcpy(contiguous_empty(), src, copy * sizeof(T));
  76   data_added(copy);
  77   count -= copy;
  78   if (count > 0) {
  79     assert(count < contiguous_empty_size());
  80     memcpy(contiguous_empty(), src, count * sizeof(T));
  81     data_added(count);
  82   }
  83 }
  84
  85 // Code inspired by/copied from OpenOCD server/server.c.
  86
  87 gdbserver_t::gdbserver_t(uint16_t port, sim_t *sim) :
  88   sim(sim),
  89   client_fd(0),
  90   recv_buf(64 * 1024), send_buf(64 * 1024)
  91 {
  92   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
  93   if (socket_fd == -1) {
  94     fprintf(stderr, "error creating socket: %s\n", strerror(errno));
  95     abort();
  96   }
  97
  98   int so_reuseaddr_option = 1;
  99   setsockopt(socket_fd,
 100       SOL_SOCKET,
 101       SO_REUSEADDR,
 102       (void *)&so_reuseaddr_option,
 103       sizeof(int));
 104
 105   int oldopts = fcntl(socket_fd, F_GETFL, 0);
 106   fcntl(socket_fd, F_SETFL, oldopts | O_NONBLOCK);
 107
 108   struct sockaddr_in sin;
 109   memset(&sin, 0, sizeof(sin));
 110   sin.sin_family = AF_INET;
 111   sin.sin_addr.s_addr = INADDR_ANY;
 112   sin.sin_port = htons(port);
 113
 114   if (bind(socket_fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
 115     fprintf(stderr, "couldn't bind to socket: %s\n", strerror(errno));
 116     abort();
 117   }
 118
 119   /* These setsockopt()s must happen before the listen() */
 120   int window_size = 128 * 1024;
 121   setsockopt(socket_fd, SOL_SOCKET, SO_SNDBUF,
 122       (char *)&window_size, sizeof(window_size));
 123   setsockopt(socket_fd, SOL_SOCKET, SO_RCVBUF,
 124       (char *)&window_size, sizeof(window_size));
 125
 126   if (listen(socket_fd, 1) == -1) {
 127     fprintf(stderr, "couldn't listen on socket: %s\n", strerror(errno));
 128     abort();
 129   }
 130 }
 131
 132 void gdbserver_t::accept()
 133 {
 134   struct sockaddr client_addr;
 135   socklen_t address_size = sizeof(client_addr);
 136   client_fd = ::accept(socket_fd, &client_addr, &address_size);
 137   if (client_fd == -1) {
 138     if (errno == EAGAIN) {
 139       // We'll try again in the next call.
 140     } else {
 141       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno), errno);
 142       abort();
 143     }
 144   } else {
 145     int oldopts = fcntl(client_fd, F_GETFL, 0);
 146     fcntl(client_fd, F_SETFL, oldopts | O_NONBLOCK);
 147     expect_ack = false;
 148     extended_mode = false;
 149
 150     // gdb wants the core to be halted when it attaches.
 151     processor_t *p = sim->get_core(0);
 152     p->set_halted(true);
 153   }
 154 }
 155
 156 void gdbserver_t::read()
 157 {
 158   // Reading from a non-blocking socket still blocks if there is no data
 159   // available.
 160
 161   size_t count = recv_buf.contiguous_empty_size();
 162   assert(count > 0);
 163   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
 164   if (bytes == -1) {
 165     if (errno == EAGAIN) {
 166       // We'll try again the next call.
 167     } else {
 168       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
 169       abort();
 170     }
 171   } else if (bytes == 0) {
 172     // The remote disconnected.
 173     client_fd = 0;
 174     processor_t *p = sim->get_core(0);
 175     p->set_halted(false);
 176     recv_buf.reset();
 177     send_buf.reset();
 178   } else {
 179     recv_buf.data_added(bytes);
 180   }
 181 }
 182
 183 void gdbserver_t::write()
 184 {
 185   if (send_buf.empty())
 186     return;
 187
 188   while (!send_buf.empty()) {
 189     unsigned int count = send_buf.contiguous_data_size();
 190     assert(count > 0);
 191     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
 192     if (bytes == -1) {
 193       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
 194       abort();
 195     } else if (bytes == 0) {
 196       // Client can't take any more data right now.
 197       break;
 198     } else {
 199       printf("wrote %ld bytes: ", bytes);
 200       for (unsigned int i = 0; i < bytes; i++) {
 201         printf("%c", send_buf[i]);
 202       }
 203       printf("\n");
 204       send_buf.consume(bytes);
 205     }
 206   }
 207 }
 208
 209 void print_packet(const std::vector<uint8_t> &packet)
 210 {
 211   for (uint8_t c : packet) {
 212     if (c >= ' ' and c <= '~')
 213       fprintf(stderr, "%c", c);
 214     else
 215       fprintf(stderr, "\\x%x", c);
 216   }
 217   fprintf(stderr, "\n");
 218 }
 219
 220 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
 221 {
 222   uint8_t checksum = 0;
 223   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
 224     checksum += *i;
 225   }
 226   return checksum;
 227 }
 228
 229 uint8_t character_hex_value(uint8_t character)
 230 {
 231   if (character >= '0' && character <= '9')
 232     return character - '0';
 233   if (character >= 'a' && character <= 'f')
 234     return 10 + character - 'a';
 235   if (character >= 'A' && character <= 'F')
 236     return 10 + character - 'A';
 237   return 0xff;
 238 }
 239
 240 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
 241 {
 242   return character_hex_value(*(packet.end() - 1)) +
 243     16 * character_hex_value(*(packet.end() - 2));
 244 }
 245
 246 void gdbserver_t::process_requests()
 247 {
 248   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
 249
 250   while (!recv_buf.empty()) {
 251     std::vector<uint8_t> packet;
 252     for (unsigned int i = 0; i < recv_buf.size(); i++) {
 253       uint8_t b = recv_buf[i];
 254
 255       if (packet.empty() && expect_ack && b == '+') {
 256         recv_buf.consume(1);
 257         break;
 258       }
 259
 260       if (packet.empty() && b == 3) {
 261         fprintf(stderr, "Received interrupt\n");
 262         recv_buf.consume(1);
 263         handle_interrupt();
 264         break;
 265       }
 266
 267       if (b == '$') {
 268         // Start of new packet.
 269         if (!packet.empty()) {
 270           fprintf(stderr, "Received malformed %ld-byte packet from debug client: ", packet.size());
 271           print_packet(packet);
 272           recv_buf.consume(i);
 273           break;
 274         }
 275       }
 276
 277       packet.push_back(b);
 278
 279       // Packets consist of $<packet-data>#<checksum>
 280       // where <checksum> is
 281       if (packet.size() >= 4 &&
 282           packet[packet.size()-3] == '#') {
 283         handle_packet(packet);
 284         recv_buf.consume(i+1);
 285         break;
 286       }
 287     }
 288     // There's a partial packet in the buffer. Wait until we get more data to
 289     // process it.
 290     if (packet.size()) {
 291       break;
 292     }
 293   }
 294 }
 295
 296 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
 297 {
 298   send_packet("S00");
 299 }
 300
 301 void gdbserver_t::handle_general_registers_read(const std::vector<uint8_t> &packet)
 302 {
 303   // Register order that gdb expects is:
 304   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 305   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 306   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 307   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 308   //   "pc",
 309   //   "f0",  "f1",  "f2",  "f3",  "f4",  "f5",  "f6",  "f7",
 310   //   "f8",  "f9",  "f10", "f11", "f12", "f13", "f14", "f15",
 311   //   "f16", "f17", "f18", "f19", "f20", "f21", "f22", "f23",
 312   //   "f24", "f25", "f26", "f27", "f28", "f29", "f30", "f31",
 313
 314   // Each byte of register data is described by two hex digits. The bytes with
 315   // the register are transmitted in target byte order. The size of each
 316   // register and their position within the ‘g’ packet are determined by the
 317   // gdb internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and
 318   // gdbarch_register_name.
 319
 320   send("$");
 321   running_checksum = 0;
 322   processor_t *p = sim->get_core(0);
 323   for (int r = 0; r < 32; r++) {
 324     send(p->state.XPR[r]);
 325   }
 326   send_running_checksum();
 327   expect_ack = true;
 328 }
 329
 330 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
 331     std::vector<uint8_t>::const_iterator end)
 332 {
 333   uint64_t value = 0;
 334
 335   while (iter != end) {
 336     uint8_t c = *iter;
 337     uint64_t c_value = character_hex_value(c);
 338     if (c_value > 15)
 339       break;
 340     iter++;
 341     value <<= 4;
 342     value += c_value;
 343   }
 344   return value;
 345 }
 346
 347 void gdbserver_t::handle_register_read(const std::vector<uint8_t> &packet)
 348 {
 349   // p n
 350
 351   // Register order that gdb expects is:
 352   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 353   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 354   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 355   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 356   //   "pc",
 357   //   "f0",  "f1",  "f2",  "f3",  "f4",  "f5",  "f6",  "f7",
 358   //   "f8",  "f9",  "f10", "f11", "f12", "f13", "f14", "f15",
 359   //   "f16", "f17", "f18", "f19", "f20", "f21", "f22", "f23",
 360   //   "f24", "f25", "f26", "f27", "f28", "f29", "f30", "f31",
 361
 362   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 363   unsigned int n = consume_hex_number(iter, packet.end());
 364   if (*iter != '#')
 365     return send_packet("E16"); // EINVAL
 366
 367   processor_t *p = sim->get_core(0);
 368   send("$");
 369   running_checksum = 0;
 370   if (n < 32) {
 371     send(p->state.XPR[n]);
 372   } else if (n == 0x20) {
 373     send(p->state.pc);
 374   } else {
 375     send("E16");        // EINVAL
 376   }
 377
 378   send_running_checksum();
 379   expect_ack = true;
 380 }
 381
 382 void gdbserver_t::handle_memory_read(const std::vector<uint8_t> &packet)
 383 {
 384   // m addr,length
 385   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 386   reg_t address = consume_hex_number(iter, packet.end());
 387   if (*iter != ',')
 388     return send_packet("E16"); // EINVAL
 389   iter++;
 390   reg_t length = consume_hex_number(iter, packet.end());
 391   if (*iter != '#')
 392     return send_packet("E16"); // EINVAL
 393
 394   send("$");
 395   running_checksum = 0;
 396   char buffer[3];
 397   processor_t *p = sim->get_core(0);
 398   mmu_t* mmu = sim->debug_mmu;
 399
 400   for (reg_t i = 0; i < length; i++) {
 401     sprintf(buffer, "%02x", mmu->load_uint8(address + i));
 402     send(buffer);
 403   }
 404   send_running_checksum();
 405 }
 406
 407 void gdbserver_t::handle_memory_binary_write(const std::vector<uint8_t> &packet)
 408 {
 409   // X addr,length:XX...
 410   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 411   reg_t address = consume_hex_number(iter, packet.end());
 412   if (*iter != ',')
 413     return send_packet("E16"); // EINVAL
 414   iter++;
 415   reg_t length = consume_hex_number(iter, packet.end());
 416   if (*iter != ':')
 417     return send_packet("E16"); // EINVAL
 418   iter++;
 419
 420   processor_t *p = sim->get_core(0);
 421   mmu_t* mmu = sim->debug_mmu;
 422   for (unsigned int i = 0; i < length; i++) {
 423     if (iter == packet.end()) {
 424       return send_packet("E16"); // EINVAL
 425     }
 426     mmu->store_uint8(address + i, *iter);
 427     iter++;
 428   }
 429   if (*iter != '#')
 430     return send_packet("E4b"); // EOVERFLOW
 431
 432   send_packet("OK");
 433 }
 434
 435 void gdbserver_t::handle_continue(const std::vector<uint8_t> &packet)
 436 {
 437   // c [addr]
 438   processor_t *p = sim->get_core(0);
 439   if (packet[2] != '#') {
 440     std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 441     p->state.pc = consume_hex_number(iter, packet.end());
 442     if (*iter != '#')
 443       return send_packet("E16"); // EINVAL
 444   }
 445
 446   p->set_halted(false);
 447 }
 448
 449 void gdbserver_t::handle_kill(const std::vector<uint8_t> &packet)
 450 {
 451   // k
 452   // The exact effect of this packet is not specified.
 453   // Looks like OpenOCD disconnects?
 454   // TODO
 455 }
 456
 457 void gdbserver_t::handle_extended(const std::vector<uint8_t> &packet)
 458 {
 459   // Enable extended mode. In extended mode, the remote server is made
 460   // persistent. The ‘R’ packet is used to restart the program being debugged.
 461   send_packet("OK");
 462   extended_mode = true;
 463 }
 464
 465 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 466 {
 467   if (compute_checksum(packet) != extract_checksum(packet)) {
 468     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
 469     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
 470     print_packet(packet);
 471     send("-");
 472     return;
 473   }
 474
 475   fprintf(stderr, "Received %ld-byte packet from debug client: ", packet.size());
 476   print_packet(packet);
 477   send("+");
 478
 479   switch (packet[1]) {
 480     case '!':
 481       return handle_extended(packet);
 482     case '?':
 483       return handle_halt_reason(packet);
 484     case 'g':
 485       return handle_general_registers_read(packet);
 486     case 'k':
 487       return handle_kill(packet);
 488     case 'm':
 489       return handle_memory_read(packet);
 490 //    case 'M':
 491 //      return handle_memory_write(packet);
 492     case 'X':
 493       return handle_memory_binary_write(packet);
 494     case 'p':
 495       return handle_register_read(packet);
 496     case 'c':
 497       return handle_continue(packet);
 498   }
 499
 500   // Not supported.
 501   fprintf(stderr, "** Unsupported packet: ");
 502   print_packet(packet);
 503   send_packet("");
 504 }
 505
 506 void gdbserver_t::handle_interrupt()
 507 {
 508   processor_t *p = sim->get_core(0);
 509   p->set_halted(true);
 510   send_packet("S02");   // Pretend program received SIGINT.
 511 }
 512
 513 void gdbserver_t::handle()
 514 {
 515   if (client_fd > 0) {
 516     this->read();
 517     this->write();
 518
 519   } else {
 520     this->accept();
 521   }
 522
 523   this->process_requests();
 524 }
 525
 526 void gdbserver_t::send(const char* msg)
 527 {
 528   unsigned int length = strlen(msg);
 529   for (const char *c = msg; *c; c++)
 530     running_checksum += *c;
 531   send_buf.append((const uint8_t *) msg, length);
 532 }
 533
 534 void gdbserver_t::send(uint64_t value)
 535 {
 536   char buffer[3];
 537   for (unsigned int i = 0; i < 8; i++) {
 538     sprintf(buffer, "%02x", (int) (value & 0xff));
 539     send(buffer);
 540     value >>= 8;
 541   }
 542 }
 543
 544 void gdbserver_t::send(uint32_t value)
 545 {
 546   char buffer[3];
 547   for (unsigned int i = 0; i < 4; i++) {
 548     sprintf(buffer, "%02x", (int) (value & 0xff));
 549     send(buffer);
 550     value >>= 8;
 551   }
 552 }
 553
 554 void gdbserver_t::send_packet(const char* data)
 555 {
 556   send("$");
 557   running_checksum = 0;
 558   send(data);
 559   send_running_checksum();
 560   expect_ack = true;
 561 }
 562
 563 void gdbserver_t::send_running_checksum()
 564 {
 565   char checksum_string[4];
 566   sprintf(checksum_string, "#%02x", running_checksum);
 567   send(checksum_string);
 568 }