Hack to the point where gdb reads a register.

2 files changed, 134 insertions, 28 deletions

diff --git a/riscv/gdbserver.cc b/riscv/gdbserver.cc
index 83587f9..e2481bc 100644
--- a/riscv/gdbserver.cc
+++ b/riscv/gdbserver.cc
@@ -7,6 +7,7 @@
 #include <sys/types.h>
 #include <unistd.h>
 
+#include <algorithm>
 #include <cassert>
 #include <cstdio>
 #include <vector>
@@ -29,7 +30,7 @@ void circular_buffer_t<T>::consume(unsigned int bytes)
 }
 
 template <typename T>
-unsigned int circular_buffer_t<T>::contiguous_space() const
+unsigned int circular_buffer_t<T>::contiguous_empty_size() const
 {
   if (end >= start)
     if (start == 0)
@@ -41,6 +42,15 @@ unsigned int circular_buffer_t<T>::contiguous_space() const
 }
 
 template <typename T>
+unsigned int circular_buffer_t<T>::contiguous_data_size() const
+{
+  if (end >= start)
+    return end - start;
+  else
+    return capacity - start;
+}
+
+template <typename T>
 void circular_buffer_t<T>::data_added(unsigned int bytes)
 {
   end += bytes;
@@ -56,11 +66,25 @@ void circular_buffer_t<T>::reset()
   end = 0;
 }
 
+template <typename T>
+void circular_buffer_t<T>::append(const T *src, unsigned int count)
+{
+  unsigned int copy = std::min(count, contiguous_empty_size());
+  memcpy(contiguous_empty(), src, copy * sizeof(T));
+  data_added(copy);
+  count -= copy;
+  if (count > 0) {
+    assert(count < contiguous_empty_size());
+    memcpy(contiguous_empty(), src, count * sizeof(T));
+    data_added(count);
+  }
+}
+
 // Code inspired by/copied from OpenOCD server/server.c.
 
 gdbserver_t::gdbserver_t(uint16_t port) :
-  recv_buf(64 * 1024),
-  send_start(0), send_end(0)
+  client_fd(0),
+  recv_buf(64 * 1024), send_buf(64 * 1024)
 {
   socket_fd = socket(AF_INET, SOCK_STREAM, 0);
   if (socket_fd == -1) {
@@ -117,7 +141,7 @@ void gdbserver_t::accept()
   } else {
     int oldopts = fcntl(client_fd, F_GETFL, 0);
     fcntl(client_fd, F_SETFL, oldopts | O_NONBLOCK);
-    ack_mode = true;
+    expect_ack = false;
   }
 }
 
@@ -126,9 +150,9 @@ void gdbserver_t::read()
   // Reading from a non-blocking socket still blocks if there is no data
   // available.
 
-  size_t count = recv_buf.contiguous_space();
+  size_t count = recv_buf.contiguous_empty_size();
   assert(count > 0);
-  ssize_t bytes = ::read(client_fd, recv_buf.contiguous_data(), count);
+  ssize_t bytes = ::read(client_fd, recv_buf.contiguous_empty(), count);
   if (bytes == -1) {
     if (errno == EAGAIN) {
       // We'll try again the next call.
@@ -140,14 +164,39 @@ void gdbserver_t::read()
     // The remote disconnected.
     client_fd = 0;
     recv_buf.reset();
-    send_start = 0;
-    send_end = 0;
+    send_buf.reset();
   } else {
     printf("read %ld bytes\n", bytes);
     recv_buf.data_added(bytes);
   }
 }
 
+void gdbserver_t::write()
+{
+  if (send_buf.empty())
+    return;
+
+  while (!send_buf.empty()) {
+    unsigned int count = send_buf.contiguous_data_size();
+    assert(count > 0);
+    ssize_t bytes = ::write(client_fd, send_buf.contiguous_data(), count);
+    if (bytes == -1) {
+      fprintf(stderr, "failed to write to socket: %s (%d)\n", strerror(errno), errno);
+      abort();
+    } else if (bytes == 0) {
+      // Client can't take any more data right now.
+      break;
+    } else {
+      printf("wrote %ld bytes:\n", bytes);
+      for (unsigned int i = 0; i < bytes; i++) {
+        printf("%c", send_buf[i]);
+      }
+      printf("\n");
+      send_buf.consume(bytes);
+    }
+  }
+}
+
 void print_packet(const std::vector<uint8_t> &packet)
 {
   for (uint8_t c : packet) {
@@ -190,6 +239,13 @@ void gdbserver_t::process_requests()
     std::vector<uint8_t> packet;
     for (unsigned int i = 0; i < recv_buf.size(); i++) {
       uint8_t b = recv_buf[i];
+
+      if (packet.empty() && expect_ack && b == '+') {
+        fprintf(stderr, "Received ack\n");
+        recv_buf.consume(1);
+        break;
+      }
+
       if (b == '$') {
         // Start of new packet.
         if (!packet.empty()) {
@@ -206,14 +262,7 @@ void gdbserver_t::process_requests()
       // where <checksum> is 
       if (packet.size() >= 4 &&
           packet[packet.size()-3] == '#') {
-        if (compute_checksum(packet) == extract_checksum(packet)) {
-          handle_packet(packet);
-        } else {
-          fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
-          fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
-          print_packet(packet);
-          send("-");
-        }
+        handle_packet(packet);
         recv_buf.consume(i+1);
         break;
       }
@@ -225,17 +274,51 @@ void gdbserver_t::process_requests()
   }
 }
 
+void gdbserver_t::handle_set_threadid(const std::vector<uint8_t> &packet)
+{
+  if (packet[2] == 'g' && packet[3] == '0') {
+    // Use thread 0 for all operations.
+    send("OK");
+  } else {
+    send("$#00");
+  }
+}
+
+void gdbserver_t::handle_halt_reason(const std::vector<uint8_t> &packet)
+{
+  send_packet("S00");
+}
+
 void gdbserver_t::handle_packet(const std::vector<uint8_t> &packet)
 {
+  if (compute_checksum(packet) != extract_checksum(packet)) {
+    fprintf(stderr, "Received %ld-byte packet with invalid checksum\n", packet.size());
+    fprintf(stderr, "Computed checksum: %x\n", compute_checksum(packet));
+    print_packet(packet);
+    send("-");
+    return;
+  }
+
   fprintf(stderr, "Received %ld-byte packet from debug client\n", packet.size());
   print_packet(packet);
   send("+");
+
+  switch (packet[1]) {
+    case 'H':
+      return handle_set_threadid(packet);
+    case '?':
+      return handle_halt_reason(packet);
+  }
+
+  // Not supported.
+  send_packet("");
 }
 
 void gdbserver_t::handle()
 {
   if (client_fd > 0) {
     this->read();
+    this->write();
 
   } else {
     this->accept();
@@ -247,5 +330,21 @@ void gdbserver_t::handle()
 void gdbserver_t::send(const char* msg)
 {
   unsigned int length = strlen(msg);
-  unsigned int count;
+  send_buf.append((const uint8_t *) msg, length);
+}
+
+void gdbserver_t::send_packet(const char* data)
+{
+  send("$");
+  send(data);
+  send("#");
+
+  uint8_t checksum = 0;
+  for ( ; *data; data++) {
+    checksum += *data;
+  }
+  char checksum_string[3];
+  sprintf(checksum_string, "%02x", checksum);
+  send(checksum_string);
+  expect_ack = true;
 }
diff --git a/riscv/gdbserver.h b/riscv/gdbserver.h
index c947936..675add6 100644
--- a/riscv/gdbserver.h
+++ b/riscv/gdbserver.h
@@ -19,21 +19,25 @@ public:
   unsigned int capacity;    // Size of the buffer.
   unsigned int size() const;
   bool empty() const { return start == end; }
-  // Tell the buffer that some bytes were consumed from the start of the
-  // buffer.
-  void consume(unsigned int bytes);
+  bool full() const { return ((end+1) % capacity) == start; }
 
   // Return size and address of the block of RAM where more data can be copied
   // to be added to the buffer.
-  unsigned int contiguous_space() const;
-  T *contiguous_data() { return data + end; }
+  unsigned int contiguous_empty_size() const;
+  T *contiguous_empty() { return data + end; }
   void data_added(unsigned int bytes);
 
+  unsigned int contiguous_data_size() const;
+  T *contiguous_data() { return data + start; }
+  // Tell the buffer that some bytes were consumed from the start of the
+  // buffer.
+  void consume(unsigned int bytes);
+
   void reset();
 
-  T operator[](unsigned int i) {
-    return data[(start + i) % capacity];
-  }
+  T operator[](unsigned int i) const { return data[(start + i) % capacity]; }
+
+  void append(const T *src, unsigned int count);
 };
 
 class gdbserver_t
@@ -47,24 +51,27 @@ public:
   void handle();
 
   void handle_packet(const std::vector<uint8_t> &packet);
+  void handle_set_threadid(const std::vector<uint8_t> &packet);
+  void handle_halt_reason(const std::vector<uint8_t> &packet);
 
 private:
   int socket_fd;
   int client_fd;
   circular_buffer_t<uint8_t> recv_buf;
-  uint8_t send_buf[64 * 1024];          // Circular buffer.
-  unsigned int send_start, send_end;    // Data start (inclusive)/end (exclusive)pointers.
+  circular_buffer_t<uint8_t> send_buf;
 
-  bool ack_mode;
+  bool expect_ack;
 
   // Read pending data from the client.
   void read();
+  void write();
   // Accept a new client if there isn't one already connected.
   void accept();
   // Process all complete requests in recv_buf.
   void process_requests();
   // Add the given message to send_buf.
   void send(const char* msg);
+  void send_packet(const char* data);
 };
 
 #endif