D&C

Still WIP because the buffer manipulation is so f**king problematic.
Hardware: Arduino Uno + W5100 Ethernet Module
Tested working through curl.

#include <SPI.h>
#include <Ethernet.h>
const uint8_t MAX_CONN = 4;
//////////////////////////////////////////////////////////////////////////////
//Socks5 Server
// Ref: https://github.com/mfontanini/Programs-Scripts/blob/master/socks5/socks5.cpp
//
const uint16_t SERVER_PORT = 23;
const int BUFFER_SIZE = 512;
byte socks_buffer[BUFFER_SIZE];
#define METHOD_NOAUTH       0
#define METHOD_AUTH         2
#define METHOD_NOTAVAILABLE 0xff
/* Responses */
#define RESP_SUCCEDED       0
#define RESP_GEN_ERROR      1
/* Address type constants */
#define ATYP_IPV4   1
#define ATYP_DNAME  3
#define ATYP_IPV6   4
/* Command constants */
#define CMD_CONNECT         1
#define CMD_BIND            2
#define CMD_UDP_ASSOCIATIVE 3
#define htons(x) ( ((x)<< 8 & 0xFF00) | \
                   ((x)>> 8 & 0x00FF) )
#define ntohs(x) htons(x)
#define htonl(x) ( ((x)<<24 & 0xFF000000UL) | \
                   ((x)<< 8 & 0x00FF0000UL) | \
                   ((x)>> 8 & 0x0000FF00UL) | \
                   ((x)>>24 & 0x000000FFUL) )
#define ntohl(x) htonl(x)
struct MethodIdentificationPacket
{
  uint8_t version_;
  uint8_t nmethods_;
} __attribute__((packed));
struct MethodSelectionPacket
{
  uint8_t version_;
  uint8_t method_;
  MethodSelectionPacket() : version_(5) {
  }
} __attribute__((packed));
struct SOCKS5RequestHeader
{
  uint8_t version_;
  uint8_t cmd_;
  uint8_t rsv_;
  uint8_t atyp_;
} __attribute__((packed));
struct SOCK5IP4RequestBody
{
  uint32_t ip_dst_;
  uint16_t port_;
} __attribute__((packed));
struct SOCK5DNameRequestBody
{
  uint8_t length_;
} __attribute__((packed));
struct SOCKS5Response
{
  uint8_t version_;
  uint8_t cmd_;
  uint8_t rsv_;
  uint8_t atyp_;
  uint32_t ip_src_;
  uint16_t port_src_;
  SOCKS5Response(bool succ = true) :
    version_(5),
    cmd_(succ ? RESP_SUCCEDED : RESP_GEN_ERROR),
    rsv_(0),
    atyp_(ATYP_IPV4)
  { }
} __attribute__((packed));
bool handle_handshake(EthernetClient* client)
{
  MethodIdentificationPacket packet;
  int count = client->read((byte*)&packet, sizeof(packet));
  if (count != sizeof(packet) || packet.version_ != 5)
  {
    Serial.print("wrong handshake version!");
    return false;
  }
  Serial.print((int)packet.version_);
  Serial.print(",");
  Serial.println((int)packet.nmethods_);
  count = client->read(socks_buffer, packet.nmethods_);
  if (count != packet.nmethods_)
  {
    Serial.print("wrong methods length:");
    Serial.println(count);
    return false;
  }
  MethodSelectionPacket response;
  for (int i = 0; i < packet.nmethods_; ++i){
    if (socks_buffer[i] == METHOD_NOAUTH) {
      response.method_ = METHOD_NOAUTH;
      break;
    } else if (socks_buffer[i] == METHOD_AUTH) {
      response.method_ = METHOD_AUTH;
      break;
    } else {
      response.method_ = METHOD_NOTAVAILABLE;
    }
  }
  if (response.method_ != METHOD_NOAUTH)
  {
    Serial.print("method not supported:");
    Serial.println((int)response.method_);
    return false;
  }
  count = client->write((byte*)&response, sizeof(response));
  if (count != sizeof(response))
  {
    Serial.println("response send failure");
  }
  return true;
}
EthernetClient* connect_to_host(uint32_t ip, uint16_t port) {
  Serial.print("connect to ");
  Serial.print(ip);
  Serial.print("@");
  Serial.println(port);
  EthernetClient* host_conn = new EthernetClient();
  int result = host_conn->connect(ip, port);
  if (host_conn->connected())
  {
    Serial.println("connected");
    return host_conn;
  }
  else
  {
    Serial.print("failed to connect:");
    Serial.println(result);
    delete host_conn;
  }
  return NULL;
}
void do_proxy(EthernetClient* remote, EthernetClient* client) {
  //TODO: set a counter to break and let others connect
  while (remote->connected() && client->connected()) {
    // see if we have any requst from client
    int recv = client->read(socks_buffer, sizeof(socks_buffer));
    if (recv > 0) {
      remote->write(socks_buffer, recv);
      Serial.print("recv from remote: ");
      Serial.println(recv);
    }
    // see if we have any response from remote
    recv = remote->read(socks_buffer, sizeof(socks_buffer));
    if (recv > 0) {
      client->write(socks_buffer, recv);
      Serial.print("recv from client: ");
      Serial.println(recv);
    }
  }
}
bool handle_request(EthernetClient* client) {
  SOCKS5RequestHeader header;
  int count = -1;
  while (count < 0)
  {
    count = client->read((byte*)&header, sizeof(header));
  }
  if (header.version_ != 5 || header.cmd_ != CMD_CONNECT || header.rsv_ != 0)
  {
    Serial.println("request header mismatch");
    return false;
  }
  EthernetClient* client_sock = NULL;
  switch(header.atyp_) {
    case ATYP_IPV4:
    {
      SOCK5IP4RequestBody req;
      count = client->read((byte*)&req, sizeof(req));
      if (count != sizeof(req))
      {
        Serial.println("SOCK5IP4RequestBody recv size mismatch");
        return false;
      }
      client_sock = connect_to_host(req.ip_dst_, ntohs(req.port_));
      break;
    }
    case ATYP_DNAME:
      break;
    default:
      return false;
  }
  if (client_sock == NULL)
    return false;
  SOCKS5Response response;
  response.ip_src_ = 0;
  response.port_src_ = SERVER_PORT;
  client->write((byte*)&response, sizeof(response));
  do_proxy(client_sock, client);
  if (client_sock != NULL)
  {
    client_sock->stop();
    delete client_sock;
    Serial.println("stopped host connection");
  }
}
//////////////////////////////////////////////////////////////////////////////
// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network.
// gateway and subnet are optional:
byte mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xAA, 0xED
};
IPAddress ip(192, 168, 31, 207);
IPAddress myDns(192, 168, 31, 1);
IPAddress gateway(192, 168, 31, 1);
IPAddress subnet(255, 255, 255, 0);
// telnet defaults to port 23
EthernetServer server(SERVER_PORT);
EthernetClient clients[MAX_CONN];
bool clients_handshaken[MAX_CONN];
void setup() {
  // You can use Ethernet.init(pin) to configure the CS pin
  Ethernet.init(10);  // Most Arduino shields
  //Ethernet.init(5);   // MKR ETH shield
  //Ethernet.init(0);   // Teensy 2.0
  //Ethernet.init(20);  // Teensy++ 2.0
  //Ethernet.init(15);  // ESP8266 with Adafruit Featherwing Ethernet
  //Ethernet.init(33);  // ESP32 with Adafruit Featherwing Ethernet
  // initialize the Ethernet device
  Ethernet.begin(mac, ip, myDns, gateway, subnet);
  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  // Check for Ethernet hardware present
  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
    Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
    while (true) {
      delay(1); // do nothing, no point running without Ethernet hardware
    }
  }
  if (Ethernet.linkStatus() == LinkOFF) {
    Serial.println("Ethernet cable is not connected.");
  }
  // start listening for clients
  server.begin();
  Serial.print("Chat server address:");
  Serial.println(Ethernet.localIP());
}
void loop() {
  // check for any new client connecting, and say hello (before any incoming data)
  EthernetClient newClient = server.accept();
  if (newClient) {
    for (byte i = 0; i < MAX_CONN; i++) {
      if (!clients[i]) {
        Serial.print("We have a new client #");
        Serial.println(i);
        // Once we "accept", the client is no longer tracked by EthernetServer
        // so we must store it into our list of clients
        clients[i] = newClient;
        clients_handshaken[i] = false;
        break;
      }
    }
  }
  // check for incoming data from all clients
  for (byte i = 0; i < MAX_CONN; i++) {
    if (clients[i] && clients[i].available() > 0) {
      if (clients_handshaken[i] == true) {
        Serial.println(">>>");
        handle_request(&clients[i]);
      }else if (handle_handshake(&clients[i])) {
        Serial.println("===");
        clients_handshaken[i] = true;
        handle_request(&clients[i]);
      }
    }
  }
  // stop any clients which disconnect
  for (byte i = 0; i < MAX_CONN; i++) {
    if (clients[i] && !clients[i].connected()) {
      Serial.print("disconnect client #");
      Serial.println(i);
      clients[i].stop();
      clients_handshaken[i] = false;
    }
  }
}