Socks5 handshake logic

src/net_tools.c

@@ -160,6 +160,11 @@ void fill_buffer(size_t* written, char* dest, void *src, size_t n) {
     *written += n;
 }
 
+void fill_buffer2(size_t* written, char* dest, void *start, void *stop) {
+    memcpy(dest+*written, start, stop - start);
+    *written += stop - start;
+}
+
 /*
  * Trying with Level Triggered for now --------
  * Be careful, if configured as edge triggered and not level triggered

src/net_tools.h

@@ -19,3 +19,4 @@ void add_fd_to_epoll(int epollfd, int fd, uint32_t flags);
 void update_fd_epoll(int epollfd, int fd, uint32_t flags);
 int read_entity(int fd, void* entity, int size);
 void fill_buffer(size_t* written, char* dest, void *src, size_t n);
+void fill_buffer2(size_t* written, char* dest, void *start, void *stop);

src/socks5.c

@@ -347,25 +347,91 @@ void socks5_server_handle_req(struct evt_core_ctx* ctx, int fd) {
   s5ctx->port = 0;
   s5ctx->addr = NULL;
 
-
   // 3. Set our handshake answer
   s5ctx->sh.ver = VER_SOCKS5;
   s5ctx->sh.method = METHOD_NOAUTH;
 
+  // 4. Set our CONNECT reply (yes we hardcode a lot, shame on me)
+  s5ctx->sr.ver = VER_SOCKS5;
+  s5ctx->sr.rep = SOCKS5_REP_SUCCESS;
+  s5ctx->sr.rsv = 0x00;
+  s5ctx->sr.atyp = ATYP_IPV4;
+  if (inet_aton("127.0.0.1", (struct in_addr*) &s5ctx->sr.bind_addr.ipv4) == 0) goto error;
+  s5ctx->sr.port = htons(0);
+
+  // 5 Generate server reply buffer
+  s5ctx->sr_size = 0;
+  fill_buffer2(&s5ctx->sr_size, s5ctx->sr_buffer, &s5ctx->sr, &s5ctx->sr.bind_addr);
+  fill_buffer2(&s5ctx->sr_size, s5ctx->sr_buffer, &s5ctx->sr.bind_addr.ipv4, &s5ctx->sr.bind_addr.ipv4 + 1);
+  fill_buffer2(&s5ctx->sr_size, s5ctx->sr_buffer, &s5ctx->sr.port, &s5ctx->sr + 1);
+
+  reg_fdinfo = evt_core_add_fd (ctx, &fdinfo);
+
+  return;
+error:
+  perror("failed to init socks5 server.");
+  exit(EXIT_FAILURE);
 }
 
 int on_socks5_server_recv_handshake(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
+  struct socks5_ctx* s5ctx = fdinfo->other;
+
+  // 1. We have read the whole buffer, change category
+  if (s5ctx->ch_cursor > 2 && s5ctx->ch_cursor >= s5ctx->ch.nmethods + 2) {
+    printf("[%s][socks5] read client handshake version: %d, nmethods: %d\n", current_human_datetime (), s5ctx->ch.ver, s5ctx->ch.nmethods);
+    for (int i = 0; i < s5ctx->ch.nmethods; i++) {
+      if (s5ctx->ch.methods[i] == METHOD_NOAUTH) {
+        evt_core_mv_fd2 (ctx, fdinfo, "socks5-server-send-handshake");
+        return EVT_CORE_FD_EXHAUSTED; // Success, we are compatible with client, moving to next state
+      }
+    }
+    // We failed to find a NOAUTH
+    fprintf(stderr, "Unable to find a NOAUTH method in the list of available method\n");
+    evt_core_mv_fd2 (ctx, fdinfo, "socks5-server-failed");
+    return EVT_CORE_FD_EXHAUSTED;
+  }
+
+  // 2. We need to read more
+  size_t to_read = 0;
+  if (s5ctx->ch_cursor < 2) to_read = 2 - s5ctx->ch_cursor;
+  else to_read = s5ctx->ch.nmethods - (s5ctx->ch_cursor - 2);
+  ssize_t nread = recv(fdinfo->fd, ((void*)&s5ctx->ch) + s5ctx->ch_cursor, to_read, 0);
+  if (nread == -1 && errno == EAGAIN) return EVT_CORE_FD_EXHAUSTED;
+  if (nread == -1) goto serv_handshake_err;
+  s5ctx->ch_cursor += nread;
 
   return EVT_CORE_FD_UNFINISHED;
+serv_handshake_err:
+  perror("[socks5] unable to read handshake from socket.");
+  evt_core_mv_fd2 (ctx, fdinfo, "socks5-server-failed");
+  return EVT_CORE_FD_EXHAUSTED;
 }
 
 int on_socks5_server_send_handshake(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
+  struct socks5_ctx* s5ctx = fdinfo->other;
+
+  // 1. We have sent the whole buffer
+  if (s5ctx->sh_cursor >= sizeof(s5ctx->sh)) {
+    printf("[%s][socks5] sent server handshake\n", current_human_datetime ());
+    evt_core_mv_fd2 (ctx, fdinfo, "socks5-server-recv-client-req");
+    return EVT_CORE_FD_EXHAUSTED;
+  }
+
+  ssize_t nsent = send(fdinfo->fd, &s5ctx->sh, sizeof(s5ctx->sh) - s5ctx->sh_cursor, 0);
+  if (nsent == -1 && errno == EAGAIN) return EVT_CORE_FD_EXHAUSTED;
+  if (nsent == -1) goto send_hs_error;
+  s5ctx->sh_cursor += nsent;
 
   return EVT_CORE_FD_UNFINISHED;
+send_hs_error:
+  perror("[socks5] unable to send handshake to socket.");
+  evt_core_mv_fd2 (ctx, fdinfo, "socks5-server-failed");
+  return EVT_CORE_FD_EXHAUSTED;
 }
 
 int on_socks5_server_recv_client_req(struct evt_core_ctx* ctx, struct evt_core_fdinfo* fdinfo) {
 
+
   return EVT_CORE_FD_UNFINISHED;
 }
 

src/socks5.h

@@ -6,6 +6,7 @@
 #include <unistd.h>
 #include <netdb.h>
 #include <string.h>
+#include <arpa/inet.h>
 #include "net_tools.h"
 #include "evt_core.h"
 
@@ -120,8 +121,10 @@ struct socks5_ctx {
   uint64_t cr_cursor;
   uint64_t sr_cursor;
   char cr_buffer[262];
+  char sr_buffer[263];
   size_t ch_size;
   size_t cr_size;
+  size_t sr_size;
   uint8_t sr_host_read;
 };