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 <cassert>
  11 #include <cstdio>
  12 #include <vector>
  13
  14 #include "gdbserver.h"
  15
  16 template <typename T>
  17 unsigned int circular_buffer_t<T>::size() const
  18 {
  19   if (end >= start)
  20     return end - start;
  21   else
  22     return end + capacity - start;
  23 }
  24
  25 template <typename T>
  26 void circular_buffer_t<T>::consume(unsigned int bytes)
  27 {
  28   start = (start + bytes) % capacity;
  29 }
  30
  31 template <typename T>
  32 unsigned int circular_buffer_t<T>::contiguous_space() const
  33 {
  34   if (end >= start)
  35     if (start == 0)
  36       return capacity - end - 1;
  37     else
  38       return capacity - end;
  39   else
  40     return start - end - 1;
  41 }
  42
  43 template <typename T>
  44 void circular_buffer_t<T>::data_added(unsigned int bytes)
  45 {
  46   end += bytes;
  47   assert(end <= capacity);
  48   if (end == capacity)
  49     end = 0;
  50 }
  51
  52 template <typename T>
  53 void circular_buffer_t<T>::reset()
  54 {
  55   start = 0;
  56   end = 0;
  57 }
  58
  59 // Code inspired by/copied from OpenOCD server/server.c.
  60
  61 gdbserver_t::gdbserver_t(uint16_t port) :
  62   recv_buf(64 * 1024),
  63   send_start(0), send_end(0)
  64 {
  65   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
  66   if (socket_fd == -1) {
  67     fprintf(stderr, "error creating socket: %s\n", strerror(errno));
  68     abort();
  69   }
  70
  71   int so_reuseaddr_option = 1;
  72   setsockopt(socket_fd,
  73       SOL_SOCKET,
  74       SO_REUSEADDR,
  75       (void *)&so_reuseaddr_option,
  76       sizeof(int));
  77
  78   int oldopts = fcntl(socket_fd, F_GETFL, 0);
  79   fcntl(socket_fd, F_SETFL, oldopts | O_NONBLOCK);
  80
  81   struct sockaddr_in sin;
  82   memset(&sin, 0, sizeof(sin));
  83   sin.sin_family = AF_INET;
  84   sin.sin_addr.s_addr = INADDR_ANY;
  85   sin.sin_port = htons(port);
  86
  87   if (bind(socket_fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
  88     fprintf(stderr, "couldn't bind to socket: %s\n", strerror(errno));
  89     abort();
  90   }
  91
  92   /* These setsockopt()s must happen before the listen() */
  93   int window_size = 128 * 1024;
  94   setsockopt(socket_fd, SOL_SOCKET, SO_SNDBUF,
  95       (char *)&window_size, sizeof(window_size));
  96   setsockopt(socket_fd, SOL_SOCKET, SO_RCVBUF,
  97       (char *)&window_size, sizeof(window_size));
  98
  99   if (listen(socket_fd, 1) == -1) {
 100     fprintf(stderr, "couldn't listen on socket: %s\n", strerror(errno));
 101     abort();
 102   }
 103 }
 104
 105 void gdbserver_t::accept()
 106 {
 107   struct sockaddr client_addr;
 108   socklen_t address_size = sizeof(client_addr);
 109   client_fd = ::accept(socket_fd, &client_addr, &address_size);
 110   if (client_fd == -1) {
 111     if (errno == EAGAIN) {
 112       // We'll try again in the next call.
 113     } else {
 114       fprintf(stderr, "failed to accept on socket: %s (%d)\n", strerror(errno), errno);
 115       abort();
 116     }
 117   } else {
 118     int oldopts = fcntl(client_fd, F_GETFL, 0);
 119     fcntl(client_fd, F_SETFL, oldopts | O_NONBLOCK);
 120     ack_mode = true;
 121   }
 122 }
 123
 124 void gdbserver_t::read()
 125 {
 126   // Reading from a non-blocking socket still blocks if there is no data
 127   // available.
 128
 129   size_t count = recv_buf.contiguous_space();
 130   assert(count > 0);
 131   ssize_t bytes = ::read(client_fd, recv_buf.contiguous_data(), count);
 132   if (bytes == -1) {
 133     if (errno == EAGAIN) {
 134       // We'll try again the next call.
 135     } else {
 136       fprintf(stderr, "failed to read on socket: %s (%d)\n", strerror(errno), errno);
 137       abort();
 138     }
 139   } else if (bytes == 0) {
 140     // The remote disconnected.
 141     client_fd = 0;
 142     recv_buf.reset();
 143     send_start = 0;
 144     send_end = 0;
 145   } else {
 146     printf("read %ld bytes\n", bytes);
 147     recv_buf.data_added(bytes);
 148   }
 149 }
 150
 151 void print_packet(const std::vector<uint8_t> &packet)
 152 {
 153   for (uint8_t c : packet) {
 154     fprintf(stderr, "%c", c);
 155   }
 156   fprintf(stderr, "\n");
 157 }
 158
 159 uint8_t compute_checksum(const std::vector<uint8_t> &packet)
 160 {
 161   uint8_t checksum = 0;
 162   for (auto i = packet.begin() + 1; i != packet.end() - 3; i++ ) {
 163     checksum += *i;
 164   }
 165   return checksum;
 166 }
 167
 168 uint8_t character_hex_value(uint8_t character)
 169 {
 170   if (character >= '0' && character <= '9')
 171     return character - '0';
 172   if (character >= 'a' && character <= 'f')
 173     return 10 + character - 'a';
 174   if (character >= 'A' && character <= 'F')
 175     return 10 + character - 'A';
 176   return 0;
 177 }
 178
 179 uint8_t extract_checksum(const std::vector<uint8_t> &packet)
 180 {
 181   return character_hex_value(*(packet.end() - 1)) +
 182     16 * character_hex_value(*(packet.end() - 2));
 183 }
 184
 185 void gdbserver_t::process_requests()
 186 {
 187   // See https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html
 188
 189   while (!recv_buf.empty()) {
 190     std::vector<uint8_t> packet;
 191     for (unsigned int i = 0; i < recv_buf.size(); i++) {
 192       uint8_t b = recv_buf[i];
 193       if (b == '$') {
 194         // Start of new packet.
 195         if (!packet.empty()) {
 196           fprintf(stderr, "Received malformed %ld-byte packet from debug client\n", packet.size());
 197           print_packet(packet);
 198           recv_buf.consume(i);
 199           break;
 200         }
 201       }
 202
 203       packet.push_back(b);
 204
 205       // Packets consist of $<packet-data>#<checksum>
 206       // where <checksum> is
 207       if (packet.size() >= 4 &&
 208           packet[packet.size()-3] == '#') {
 209         if (compute_checksum(packet) == extract_checksum(packet)) {
 210           handle_packet(packet);
 211         } else {
 212           fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
 213           fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
 214           print_packet(packet);
 215           send("-");
 216         }
 217         recv_buf.consume(i+1);
 218         break;
 219       }
 220     }
 221     // There's a partial packet in the buffer. Wait until we get more data to
 222     // process it.
 223     if (packet.size())
 224       break;
 225   }
 226 }
 227
 228 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 229 {
 230   fprintf(stderr, "Received %ld-byte packet from debug client\n", packet.size());
 231   print_packet(packet);
 232   send("+");
 233 }
 234
 235 void gdbserver_t::handle()
 236 {
 237   if (client_fd > 0) {
 238     this->read();
 239
 240   } else {
 241     this->accept();
 242   }
 243
 244   this->process_requests();
 245 }
 246
 247 void gdbserver_t::send(const char* msg)
 248 {
 249   unsigned int length = strlen(msg);
 250   unsigned int count;
 251 }