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 "gdbserver.h"
  16
  17 template <typename T>
  18 unsigned int circular_buffer_t<T>::size() const
  19 {
  20   if (end >= start)
  21     return end - start;
  22   else
  23     return end + capacity - start;
  24 }
  25
  26 template <typename T>
  27 void circular_buffer_t<T>::consume(unsigned int bytes)
  28 {
  29   start = (start + bytes) % capacity;
  30 }
  31
  32 template <typename T>
  33 unsigned int circular_buffer_t<T>::contiguous_empty_size() const
  34 {
  35   if (end >= start)
  36     if (start == 0)
  37       return capacity - end - 1;
  38     else
  39       return capacity - end;
  40   else
  41     return start - end - 1;
  42 }
  43
  44 template <typename T>
  45 unsigned int circular_buffer_t<T>::contiguous_data_size() const
  46 {
  47   if (end >= start)
  48     return end - start;
  49   else
  50     return capacity - start;
  51 }
  52
  53 template <typename T>
  54 void circular_buffer_t<T>::data_added(unsigned int bytes)
  55 {
  56   end += bytes;
  57   assert(end <= capacity);
  58   if (end == capacity)
  59     end = 0;
  60 }
  61
  62 template <typename T>
  63 void circular_buffer_t<T>::reset()
  64 {
  65   start = 0;
  66   end = 0;
  67 }
  68
  69 template <typename T>
  70 void circular_buffer_t<T>::append(const T *src, unsigned int count)
  71 {
  72   unsigned int copy = std::min(count, contiguous_empty_size());
  73   memcpy(contiguous_empty(), src, copy * sizeof(T));
  74   data_added(copy);
  75   count -= copy;
  76   if (count > 0) {
  77     assert(count < contiguous_empty_size());
  78     memcpy(contiguous_empty(), src, count * sizeof(T));
  79     data_added(count);
  80   }
  81 }
  82
  83 // Code inspired by/copied from OpenOCD server/server.c.
  84
  85 gdbserver_t::gdbserver_t(uint16_t port) :
  86   client_fd(0),
  87   recv_buf(64 * 1024), send_buf(64 * 1024)
  88 {
  89   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
  90   if (socket_fd == -1) {
  91     fprintf(stderr, "error creating socket: %s\n", strerror(errno));
  92     abort();
  93   }
  94
  95   int so_reuseaddr_option = 1;
  96   setsockopt(socket_fd,
  97       SOL_SOCKET,
  98       SO_REUSEADDR,
  99       (void *)&so_reuseaddr_option,
 100       sizeof(int));
 101
 102   int oldopts = fcntl(socket_fd, F_GETFL, 0);
 103   fcntl(socket_fd, F_SETFL, oldopts | O_NONBLOCK);
 104
 105   struct sockaddr_in sin;
 106   memset(&sin, 0, sizeof(sin));
 107   sin.sin_family = AF_INET;
 108   sin.sin_addr.s_addr = INADDR_ANY;
 109   sin.sin_port = htons(port);
 110
 111   if (bind(socket_fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
 112     fprintf(stderr, "couldn't bind to socket: %s\n", strerror(errno));
 113     abort();
 114   }
 115
 116   /* These setsockopt()s must happen before the listen() */
 117   int window_size = 128 * 1024;
 118   setsockopt(socket_fd, SOL_SOCKET, SO_SNDBUF,
 119       (char *)&window_size, sizeof(window_size));
 120   setsockopt(socket_fd, SOL_SOCKET, SO_RCVBUF,
 121       (char *)&window_size, sizeof(window_size));
 122
 123   if (listen(socket_fd, 1) == -1) {
 124     fprintf(stderr, "couldn't listen on socket: %s\n", strerror(errno));
 125     abort();
 126   }
 127 }
 128
 129 void gdbserver_t::accept()
 130 {
 131   struct sockaddr client_addr;
 132   socklen_t address_size = sizeof(client_addr);
 133   client_fd = ::accept(socket_fd, &client_addr, &address_size);
 134   if (client_fd == -1) {
 135     if (errno == EAGAIN) {
 136       // We'll try again in the next call.
 137     } else {
 138       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno), errno);
 139       abort();
 140     }
 141   } else {
 142     int oldopts = fcntl(client_fd, F_GETFL, 0);
 143     fcntl(client_fd, F_SETFL, oldopts | O_NONBLOCK);
 144     expect_ack = false;
 145   }
 146 }
 147
 148 void gdbserver_t::read()
 149 {
 150   // Reading from a non-blocking socket still blocks if there is no data
 151   // available.
 152
 153   size_t count = recv_buf.contiguous_empty_size();
 154   assert(count > 0);
 155   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
 156   if (bytes == -1) {
 157     if (errno == EAGAIN) {
 158       // We'll try again the next call.
 159     } else {
 160       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
 161       abort();
 162     }
 163   } else if (bytes == 0) {
 164     // The remote disconnected.
 165     client_fd = 0;
 166     recv_buf.reset();
 167     send_buf.reset();
 168   } else {
 169     printf("read %ld bytes\n", bytes);
 170     recv_buf.data_added(bytes);
 171   }
 172 }
 173
 174 void gdbserver_t::write()
 175 {
 176   if (send_buf.empty())
 177     return;
 178
 179   while (!send_buf.empty()) {
 180     unsigned int count = send_buf.contiguous_data_size();
 181     assert(count > 0);
 182     ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
 183     if (bytes == -1) {
 184       fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
 185       abort();
 186     } else if (bytes == 0) {
 187       // Client can't take any more data right now.
 188       break;
 189     } else {
 190       printf("wrote %ld bytes:\n", bytes);
 191       for (unsigned int i = 0; i < bytes; i++) {
 192         printf("%c", send_buf[i]);
 193       }
 194       printf("\n");
 195       send_buf.consume(bytes);
 196     }
 197   }
 198 }
 199
 200 void print_packet(const std::vector<uint8_t> &packet)
 201 {
 202   for (uint8_t c : packet) {
 203     fprintf(stderr, "%c", c);
 204   }
 205   fprintf(stderr, "\n");
 206 }
 207
 208 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
 209 {
 210   uint8_t checksum = 0;
 211   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
 212     checksum += *i;
 213   }
 214   return checksum;
 215 }
 216
 217 uint8_t character_hex_value(uint8_t character)
 218 {
 219   if (character >= '0' && character <= '9')
 220     return character - '0';
 221   if (character >= 'a' && character <= 'f')
 222     return 10 + character - 'a';
 223   if (character >= 'A' && character <= 'F')
 224     return 10 + character - 'A';
 225   return 0;
 226 }
 227
 228 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
 229 {
 230   return character_hex_value(*(packet.end() - 1)) +
 231     16 * character_hex_value(*(packet.end() - 2));
 232 }
 233
 234 void gdbserver_t::process_requests()
 235 {
 236   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
 237
 238   while (!recv_buf.empty()) {
 239     std::vector<uint8_t> packet;
 240     for (unsigned int i = 0; i < recv_buf.size(); i++) {
 241       uint8_t b = recv_buf[i];
 242
 243       if (packet.empty() && expect_ack && b == '+') {
 244         fprintf(stderr, "Received ack\n");
 245         recv_buf.consume(1);
 246         break;
 247       }
 248
 249       if (b == '$') {
 250         // Start of new packet.
 251         if (!packet.empty()) {
 252           fprintf(stderr, "Received malformed %ld-byte packet from debug client\n", packet.size());
 253           print_packet(packet);
 254           recv_buf.consume(i);
 255           break;
 256         }
 257       }
 258
 259       packet.push_back(b);
 260
 261       // Packets consist of $<packet-data>#<checksum>
 262       // where <checksum> is
 263       if (packet.size() >= 4 &&
 264           packet[packet.size()-3] == '#') {
 265         handle_packet(packet);
 266         recv_buf.consume(i+1);
 267         break;
 268       }
 269     }
 270     // There's a partial packet in the buffer. Wait until we get more data to
 271     // process it.
 272     if (packet.size())
 273       break;
 274   }
 275 }
 276
 277 void gdbserver_t::handle_set_threadid(const std::vector<uint8_t> &packet)
 278 {
 279   if (packet[2] == 'g' && packet[3] == '0') {
 280     // Use thread 0 for all operations.
 281     send("OK");
 282   } else {
 283     send("$#00");
 284   }
 285 }
 286
 287 void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
 288 {
 289   send_packet("S00");
 290 }
 291
 292 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 293 {
 294   if (compute_checksum(packet) != extract_checksum(packet)) {
 295     fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
 296     fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
 297     print_packet(packet);
 298     send("-");
 299     return;
 300   }
 301
 302   fprintf(stderr, "Received %ld-byte packet from debug client\n", packet.size());
 303   print_packet(packet);
 304   send("+");
 305
 306   switch (packet[1]) {
 307     case 'H':
 308       return handle_set_threadid(packet);
 309     case '?':
 310       return handle_halt_reason(packet);
 311   }
 312
 313   // Not supported.
 314   send_packet("");
 315 }
 316
 317 void gdbserver_t::handle()
 318 {
 319   if (client_fd > 0) {
 320     this->read();
 321     this->write();
 322
 323   } else {
 324     this->accept();
 325   }
 326
 327   this->process_requests();
 328 }
 329
 330 void gdbserver_t::send(const char* msg)
 331 {
 332   unsigned int length = strlen(msg);
 333   send_buf.append((const uint8_t *) msg, length);
 334 }
 335
 336 void gdbserver_t::send_packet(const char* data)
 337 {
 338   send("$");
 339   send(data);
 340   send("#");
 341
 342   uint8_t checksum = 0;
 343   for ( ; *data; data++) {
 344     checksum += *data;
 345   }
 346   char checksum_string[3];
 347   sprintf(checksum_string, "%02x", checksum);
 348   send(checksum_string);
 349   expect_ack = true;
 350 }