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;
}
}
}