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