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   }
 149 }
 150
 151 void gdbserver_t::read()
 152 {
 153   // Reading from a non-blocking socket still blocks if there is no data
 154   // available.
 155
 156   size_t count = recv_buf.contiguous_empty_size();
 157   assert(count > 0);
 158   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
 159   if (bytes == -1) {
 160     if (errno == EAGAIN) {
 161       // We'll try again the next call.
 162     } else {
 163       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
 164       abort();
 165     }
 166   } else if (bytes == 0) {
 167     // The remote disconnected.
 168     client_fd = 0;
 169     recv_buf.reset();
 170     send_buf.reset();
 171   } else {
 172     printf("read %ld bytes\n", bytes);
 173     recv_buf.data_added(bytes);
 174   }
 175 }
 176
 177 void gdbserver_t::write()
 178 {
 179   if (send_buf.empty())
 180     return;
 181
 182   while (!send_buf.empty()) {
 183     unsigned int count = send_buf.contiguous_data_size();
 184     assert(count > 0);
 185     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
 186     if (bytes == -1) {
 187       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
 188       abort();
 189     } else if (bytes == 0) {
 190       // Client can't take any more data right now.
 191       break;
 192     } else {
 193       printf("wrote %ld bytes:\n", bytes);
 194       for (unsigned int i = 0; i < bytes; i++) {
 195         printf("%c", send_buf[i]);
 196       }
 197       printf("\n");
 198       send_buf.consume(bytes);
 199     }
 200   }
 201 }
 202
 203 void print_packet(const std::vector<uint8_t> &packet)
 204 {
 205   for (uint8_t c : packet) {
 206     fprintf(stderr, "%c", c);
 207   }
 208   fprintf(stderr, "\n");
 209 }
 210
 211 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
 212 {
 213   uint8_t checksum = 0;
 214   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
 215     checksum += *i;
 216   }
 217   return checksum;
 218 }
 219
 220 uint8_t character_hex_value(uint8_t character)
 221 {
 222   if (character >= '0' && character <= '9')
 223     return character - '0';
 224   if (character >= 'a' && character <= 'f')
 225     return 10 + character - 'a';
 226   if (character >= 'A' && character <= 'F')
 227     return 10 + character - 'A';
 228   return 0xff;
 229 }
 230
 231 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
 232 {
 233   return character_hex_value(*(packet.end() - 1)) +
 234     16 * character_hex_value(*(packet.end() - 2));
 235 }
 236
 237 void gdbserver_t::process_requests()
 238 {
 239   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
 240
 241   while (!recv_buf.empty()) {
 242     std::vector<uint8_t> packet;
 243     for (unsigned int i = 0; i < recv_buf.size(); i++) {
 244       uint8_t b = recv_buf[i];
 245
 246       if (packet.empty() && expect_ack && b == '+') {
 247         fprintf(stderr, "Received ack\n");
 248         recv_buf.consume(1);
 249         break;
 250       }
 251
 252       if (b == '$') {
 253         // Start of new packet.
 254         if (!packet.empty()) {
 255           fprintf(stderr, "Received malformed %ld-byte packet from debug client\n", packet.size());
 256           print_packet(packet);
 257           recv_buf.consume(i);
 258           break;
 259         }
 260       }
 261
 262       packet.push_back(b);
 263
 264       // Packets consist of $<packet-data>#<checksum>
 265       // where <checksum> is
 266       if (packet.size() >= 4 &&
 267           packet[packet.size()-3] == '#') {
 268         handle_packet(packet);
 269         recv_buf.consume(i+1);
 270         break;
 271       }
 272     }
 273     // There's a partial packet in the buffer. Wait until we get more data to
 274     // process it.
 275     if (packet.size())
 276       break;
 277   }
 278 }
 279
 280 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
 281 {
 282   send_packet("S00");
 283 }
 284
 285 void gdbserver_t::handle_read_general_registers(const std::vector<uint8_t> &packet)
 286 {
 287   // Register order that gdb expects is:
 288   //   "x0",  "x1",  "x2",  "x3",  "x4",  "x5",  "x6",  "x7",
 289   //   "x8",  "x9",  "x10", "x11", "x12", "x13", "x14", "x15",
 290   //   "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23",
 291   //   "x24", "x25", "x26", "x27", "x28", "x29", "x30", "x31",
 292   //   "pc",
 293   //   "f0",  "f1",  "f2",  "f3",  "f4",  "f5",  "f6",  "f7",
 294   //   "f8",  "f9",  "f10", "f11", "f12", "f13", "f14", "f15",
 295   //   "f16", "f17", "f18", "f19", "f20", "f21", "f22", "f23",
 296   //   "f24", "f25", "f26", "f27", "f28", "f29", "f30", "f31",
 297
 298   send("$");
 299   running_checksum = 0;
 300   char buffer[17];
 301   processor_t *p = sim->get_core(0);
 302   for (int r = 0; r < 32; r++) {
 303     sprintf(buffer, "%08lx", p->state.XPR[r]);
 304     send(buffer);
 305   }
 306   send_running_checksum();
 307   expect_ack = true;
 308 }
 309
 310 uint64_t consume_hex_number(std::vector<uint8_t>::const_iterator &iter,
 311     std::vector<uint8_t>::const_iterator end)
 312 {
 313   uint64_t value = 0;
 314
 315   while (iter != end) {
 316     uint8_t c = *iter;
 317     uint64_t c_value = character_hex_value(c);
 318     if (c_value > 15)
 319       break;
 320     iter++;
 321     value <<= 4;
 322     value += c_value;
 323   }
 324   return value;
 325 }
 326
 327 void gdbserver_t::handle_read_memory(const std::vector<uint8_t> &packet)
 328 {
 329   // m addr,length
 330   std::vector<uint8_t>::const_iterator iter = packet.begin() + 2;
 331   reg_t address = consume_hex_number(iter, packet.end());
 332   printf("address=%lx %c\n", address, *iter);
 333   if (*iter != ',')
 334     return send_packet("E16"); // EINVAL
 335   iter++;
 336   reg_t length = consume_hex_number(iter, packet.end());
 337   printf("length=%lx %c\n", length, *iter);
 338   if (*iter != '#')
 339     return send_packet("E16"); // EINVAL
 340
 341   send("$");
 342   running_checksum = 0;
 343   char buffer[3];
 344   processor_t *p = sim->get_core(0);
 345   mmu_t* mmu = sim->debug_mmu;
 346
 347   for (reg_t i = 0; i < length; i++) {
 348     sprintf(buffer, "%02x", mmu->load_uint8(address + i));
 349     send(buffer);
 350   }
 351   send_running_checksum();
 352 }
 353
 354 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 355 {
 356   if (compute_checksum(packet) != extract_checksum(packet)) {
 357     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
 358     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
 359     print_packet(packet);
 360     send("-");
 361     return;
 362   }
 363
 364   fprintf(stderr, "Received %ld-byte packet from debug client\n", packet.size());
 365   print_packet(packet);
 366   send("+");
 367
 368   switch (packet[1]) {
 369     case '?':
 370       return handle_halt_reason(packet);
 371     case 'g':
 372       return handle_read_general_registers(packet);
 373     case 'm':
 374       return handle_read_memory(packet);
 375   }
 376
 377   // Not supported.
 378   send_packet("");
 379 }
 380
 381 void gdbserver_t::handle()
 382 {
 383   if (client_fd > 0) {
 384     this->read();
 385     this->write();
 386
 387   } else {
 388     this->accept();
 389   }
 390
 391   this->process_requests();
 392 }
 393
 394 void gdbserver_t::send(const char* msg)
 395 {
 396   unsigned int length = strlen(msg);
 397   for (const char *c = msg; *c; c++)
 398     running_checksum += *c;
 399   send_buf.append((const uint8_t *) msg, length);
 400 }
 401
 402 void gdbserver_t::send_packet(const char* data)
 403 {
 404   send("$");
 405   running_checksum = 0;
 406   send(data);
 407   send_running_checksum();
 408   expect_ack = true;
 409 }
 410
 411 void gdbserver_t::send_running_checksum()
 412 {
 413   char checksum_string[4];
 414   sprintf(checksum_string, "#%02x", running_checksum);
 415   send(checksum_string);
 416 }