path: root/decoders/socks/socks_decoder.cpp

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

extern "C" {
#include "stellar/stellar.h"
#include "stellar/session.h"
#include "stellar/stellar_mq.h"
#include "stellar/stellar_exdata.h"
}

#include "socks_decoder_internal.h"
#include "stellar/log.h"

#define UNUSED(x) (void)(x)


static void socks_decoder_session_exdata_free(int idx, void *ex_ptr, void *arg)
{
    UNUSED(idx);
    UNUSED(arg);

    if (ex_ptr == NULL)
    {
        return;
    }

    free(ex_ptr);

    return;
}

//packet format refer to https://zh.wikipedia.org/wiki/SOCKS
static int socks_rough_rec(const char *payload, size_t payload_len)
{
	//for socks4
	if (payload_len < SOCKS5_REQUEST_METHOD_MIN_LEN)
	{
		return -1;
	}

	if (payload[0] == SOCKS_FIELD_VER_4 && payload[payload_len-1] == 0)
	{
		if (payload_len >= 9)
		{
			return 0;	
		}
	}

	if (payload[0] == SOCKS_FIELD_VER_5)
	{
        if (2 + (unsigned int)payload[1] == payload_len)
        {

            return 0;	
        }
	}
	return -1;
}

static int socks5_method_request(const char *payload, size_t payload_len)
{
	if (payload_len < SOCKS5_REQUEST_METHOD_MIN_LEN)
	{
		return -1;
	}

	if (payload[0] != SOCKS_FIELD_VER_5)
	{
		return -1;
	}

	if ((size_t)(2 + payload[1]) != payload_len)
	{
		return -1;
	}

	return 0;
}

static int socks4_request(struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
	if (payload_len < 9)
		return -1;	
	if (payload[0] != SOCKS_FIELD_VER_4 || payload[payload_len-1] != 0)
		return -1;

	struct socks_addr *addr = &(stream->info.dst_addr);
    
	memcpy(&(addr->port), payload + 2, 2);
	memcpy(&(addr->ipv4), payload + 4, 4);

	if (addr->ipv4 & 0x00FFFFFF)
	{
		addr->type = SOCKS_ADDR_IPV4;
		return 0;
	}
	else if (addr->ipv4 & 0xFF000000)//socks4a hostname
	{
		size_t pos = 8;
		while (pos < payload_len - 2 && payload[pos] != 0)
		{
            pos++;
        }

		if (payload[pos] == 0)
		{
			if (payload_len - 1 - pos < 2)
				return -1;

			addr->fqdn.iov_len = payload_len - 1 - pos - 1;
            addr->fqdn.iov_base = (unsigned char *)calloc(1, addr->fqdn.iov_len + 1);

			memcpy(addr->fqdn.iov_base, payload+pos+1, addr->fqdn.iov_len);

			addr->type = SOCKS_ADDR_FQDN;
	
			return 0;
		}
		else 
		{
			return -1;
		}
	}

	return -1;
}

static int socks5_request(struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
	unsigned int port_offset = 0;
    struct socks_addr *addr = &(stream->info.dst_addr);

	if (payload_len < SOCKS5_REQUEST_MIN_LEN)
	{
		return -1;
	}

	if (payload[0] != SOCKS_FIELD_VER_5)
	{
		return -1;
	}

	if (payload[1] != SOCKS_CMD_CONNECT && payload[1] != SOCKS_CMD_BIND)
	{	
		return -1;
	}

	if (payload[2] != SOCKS5_RSV_DEFAULT)
	{
		return -1;
	}

	switch (payload[3])
	{
		case SOCKS_ATYPE_IP4:
			{
				if (payload_len != 10)
				{
					return -1;
				}

                addr->type = SOCKS_ADDR_IPV4;
				memcpy(&(addr->ipv4), payload+4, 4);
                
				port_offset = 8;

				break;
			}
        //should be processed
		case SOCKS_ATYPE_IP6:
			{
				if(payload_len != 22)
				{
					return -1;
				}
                addr->type = SOCKS_ADDR_IPV6;
				memcpy(addr->ipv6, payload+4, 16);
				port_offset = 20;

				break;
			}
		case SOCKS_ATYPE_FQDN:
			{
				if (payload[4] < 0 || payload_len != (size_t)(7 + payload[4]))
				{
					return -1;
				}
				addr->type = SOCKS_ADDR_FQDN;
                addr->fqdn.iov_len = payload[4];
                addr->fqdn.iov_base = (char *)calloc(1, addr->fqdn.iov_len + 1);
                memcpy(addr->fqdn.iov_base, payload+5, payload[4]);

				port_offset = 5 + payload[4];
			}
			break;
		default:
			break;
	}
	
	memcpy(&(addr->port), payload+port_offset, 2);

	return 0;
}

static int socks5_pass_request(struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
	unsigned char *ubuf = (unsigned char*)payload;
	unsigned char *cur;
    struct socks_info *info = &stream->info;

	if (ubuf[0]!=1)
	{
		return -1;
	}
	if (payload_len < 2 || payload_len < (size_t)(3+ubuf[1]) || payload_len != (size_t)(3+ubuf[1]+ubuf[2+ubuf[1]]))
	{
		return -1;
	}

	info->user_name.iov_len = ubuf[1];
    info->user_name.iov_base = (char *)calloc(1, info->user_name.iov_len + 1);
	memcpy(info->user_name.iov_base, ubuf+2, info->user_name.iov_len);

    info->password.iov_len = ubuf[2+ubuf[1]];
    info->password.iov_base = (char *)calloc(1, info->password.iov_len + 1);
    cur = ubuf + 2 + info->user_name.iov_len + 1;
	memcpy(info->password.iov_base, cur, info->password.iov_len);

	return 0;
}

static void c2s_handler(struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
	switch (stream->client_state)
	{
        case SS_BEGIN:
            if (socks5_method_request(payload, payload_len) == 0)
            {
                stream->client_state = SS_SUB;
                stream->info.version = SOCKS_VERSION_5;
            }
            else if (socks4_request(stream, payload, payload_len) == 0)
            {
                stream->client_state = SS_END;
                stream->info.version = SOCKS_VERSION_4;
            }
            else
            {
                stream->client_state = SS_FAILED;
            }
            break;
        case SS_SUB:
            if (socks5_request(stream, payload, payload_len) == 0)
            {
                stream->client_state = SS_END;            
            }
            else if (socks5_pass_request(stream, payload, payload_len) != 0)
            {
                stream->client_state = SS_FAILED;
            }
            break;
        default:
            break;
	}
}

static int socks5_method_replay(const char *payload, size_t payload_len)
{
	if (payload_len != 2)
	{
		return -1;
	}
	if (payload[0] != SOCKS_FIELD_VER_5)
	{
		return -1;
	}
	if (payload[1] != 0 && payload[1]!=2)
	{
		return -1;
	}

	////printf("socks5 method replay!\n");
	return 0;
}

static int socks4_replay(const char *payload, size_t payload_len)
{
	if (payload_len != 8)
		return -1;
	if (payload[0] != 0 || payload[1] < 90 || payload[1] > 93)
		return -1;
	
	return 0; 
}

static int socks5_replay(const char *payload, size_t payload_len)
{   
	if (payload_len < 6)
	{
		return -1;
	}
	if (payload[0] != SOCKS_FIELD_VER_5)
	{
		return -1;
	}
	if (payload[1] != 0)
	{
		return -1;
	}
	if (payload[2] != 0)
	{
		return -1;
	}

    switch(payload[3])
    {
        case SOCKS_ATYPE_IP4:
            if (payload_len < 10)  
            {
                return -1;
            }
            break;
        case SOCKS_ATYPE_IP6:
            if (payload_len != 22)
            {
                return -1;
            }
            break;
        case SOCKS_ATYPE_FQDN:
            if (payload[4] < 0 || payload_len < (size_t)(7 + payload[4]))
            {
                return -1;
            }
            break;
        default:
            return -1;
    }

	return 0;
}

static int socks5_pass_reply(const char *payload, size_t payload_len)
{
	if (payload_len != 2)
		return -1;
	if (payload[0] != 1 || payload[1] != 0)
		return -1;
	return 0;
}

static void s2c_handler(struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
	switch (stream->server_state)
	{
	case SS_BEGIN:
		if (stream->info.version == SOCKS_VERSION_5 && socks5_method_replay(payload, payload_len) == 0)
		{
			stream->server_state = SS_SUB;
		}
		else if (stream->info.version == SOCKS_VERSION_4 && socks4_replay(payload, payload_len) == 0)
		{
			stream->server_state = SS_END;
		}
		else
		{
			stream->server_state = SS_FAILED;
			//printf("socks error, s2c beign\n");
		}
		break;
	case SS_SUB:
		if (socks5_replay(payload, payload_len) == 0)
		{
			stream->server_state = SS_END;
		}
		else if (socks5_pass_reply(payload, payload_len) != 0)
		{
			stream->server_state = SS_FAILED;

			//printf("socks error, s2c sub\n");
		}
		break;
	default:
		//error!
		break;
	}
}

int socks_process(struct socks_decoder_info *socks_decoder_info, struct session *sess, struct socks_tunnel_stream *stream, const char *payload, size_t payload_len)
{
    if (payload_len == 0)
    {
        return 0;
    }

    switch(stream->state)
    {
        case STATE_INIT:
            if (socks_rough_rec(payload, payload_len) < 0)
            {
                stream->state = STATE_FAILED;
                return -1;
            }
            stream->state = STATE_OPENING;
            [[fallthrough]];//continue execute STATE_OPENING
        case STATE_OPENING:     
            switch (session_get_flow_type(sess))
            {
                case FLOW_TYPE_C2S:
                    c2s_handler(stream, payload, payload_len);
                    break;
                case FLOW_TYPE_S2C:
                    s2c_handler(stream, payload, payload_len);
                    break;
                default:
                    break;
            }     

            if (stream->client_state == SS_END && stream->server_state == SS_END)
            {
                if (session_mq_publish_message(sess, socks_decoder_info->socks_decoder_topic_id, &stream->info) < 0)
                {
					STELLAR_LOG_FATAL(socks_decoder_info->log_handle, SOCKS_LOG_MOUDLE, "session_mq_publish_message OPENING failed");
                }
                stellar_session_plugin_dettach_current_session(sess);
            }
            else if (stream->client_state == SS_FAILED || stream->server_state == SS_FAILED)
            {
                //not a socks proxy
                stream->state = STATE_FAILED;
                return -1;
            }
            break;
        default:
            return -1;
    }

    return 0;
}

void socks_decoder_on_message(struct session *sess, int topic_id, const void *msg, void *per_session_ctx, void *plugin_env)
{
    UNUSED(per_session_ctx);
    UNUSED(topic_id);
    UNUSED(msg);

    struct socks_decoder_info *socks_decoder_info = (struct socks_decoder_info *)plugin_env;
    struct socks_tunnel_stream *socks_tunel_stream = (struct socks_tunnel_stream *)session_exdata_get(sess, socks_decoder_info->sess_exdata_idx);
    const char *payload = NULL;
    size_t payload_len = 0;
	const struct packet *pkt = NULL;

    if (socks_tunel_stream == NULL)
    {
        socks_tunel_stream = (struct socks_tunnel_stream *)calloc(1, sizeof(struct socks_tunnel_stream));
        session_exdata_set(sess, socks_decoder_info->sess_exdata_idx, socks_tunel_stream);
    }

	pkt = session_get_current_packet(sess);
	payload = packet_get_payload(pkt);
	payload_len = packet_get_payload_len(pkt);

    if (socks_process(socks_decoder_info, sess, socks_tunel_stream, payload, payload_len) < 0)
    {
        stellar_session_plugin_dettach_current_session(sess);
    }
}

extern "C" void *socks_decoder_init(struct stellar *st)
{
    struct socks_decoder_info *socks_decoder_info = (struct socks_decoder_info *)calloc(1, sizeof(struct socks_decoder_info));

	socks_decoder_info->sess_exdata_idx= stellar_exdata_new_index(st, "SOCKS_DECODER_SESS_CTX", socks_decoder_session_exdata_free, NULL);

    socks_decoder_info->st = st;
    socks_decoder_info->log_handle = stellar_get_logger(st);

    socks_decoder_info->plugin_id = stellar_session_plugin_register(st, NULL, NULL, socks_decoder_info);
	if (socks_decoder_info->plugin_id < 0)
	{
		STELLAR_LOG_FATAL(socks_decoder_info->log_handle, SOCKS_LOG_MOUDLE, "stellar_session_plugin_register failed");
		goto ERROR;
	
	}

    socks_decoder_info->socks_decoder_topic_id = stellar_mq_create_topic(st, SOCKS_MESSAGE_TOPIC, NULL, NULL);
    if (socks_decoder_info->socks_decoder_topic_id < 0)
    {
		STELLAR_LOG_FATAL(socks_decoder_info->log_handle, SOCKS_LOG_MOUDLE, "stellar_session_mq_create_topic failed");
        goto ERROR;
    }

    socks_decoder_info->tcp_topic_id = stellar_mq_get_topic_id(st, TOPIC_TCP_STREAM);
    if (socks_decoder_info->tcp_topic_id < 0)
    {
		STELLAR_LOG_FATAL(socks_decoder_info->log_handle, SOCKS_LOG_MOUDLE, "stellar_session_mq_get_topic_id failed");
        goto ERROR;
    }

    if (stellar_session_mq_subscribe(st, socks_decoder_info->tcp_topic_id, socks_decoder_on_message, socks_decoder_info->plugin_id) < 0)
    {
		STELLAR_LOG_FATAL(socks_decoder_info->log_handle, SOCKS_LOG_MOUDLE, "stellar_session_mq_subscribe tcp_topic_id failed");
        goto ERROR;
    }

    return socks_decoder_info;

ERROR:
    if (socks_decoder_info != NULL)
    {
        free(socks_decoder_info);
    }
    perror("socks_decoder init failed");
	exit(-1);
}

extern "C" void socks_decoder_exit(void *plugin_env)
{
    struct socks_decoder_info *socks_decoder_info = (struct socks_decoder_info *)plugin_env;

    if (socks_decoder_info != NULL)
    {
        free(socks_decoder_info);
    }

    return;
}