ccx-utils

Miscellaneous utilities written in C
git clone https://ccx.te2000.cz/git/ccx-utils
Log | Files | Refs

ucspi-socksserver.c (10103B)


      1 #include <errno.h>
      2 #include <fcntl.h>
      3 #include <unistd.h>
      4 #include <stdbool.h>
      5 #include <stdio.h>  /* for log_message */
      6 #include <stdarg.h>  /* for log_message */
      7 #include <assert.h>
      8 #include <arpa/inet.h>
      9 
     10 #include <skalibs/exec.h>
     11 #include <skalibs/strerr.h>
     12 
     13 #include "socks.h"
     14 
     15 #define PROG "ucspi-socketserver"
     16 
     17 #define MAX_DOMAIN_LENGTH 256
     18 
     19 void fd_block(int fd);
     20 void read0(void *buf, ssize_t n);
     21 void write1(const void *buf, ssize_t n);
     22 void socks_read_header(socks_header *hdr);
     23 unsigned short socks_read_port(void);
     24 void interact(void);
     25 void handle_socks5(char methods);
     26 void socks5_auth(int methods_count, socks5_auth_mode_t supported_auth_method);
     27 void socks5_auth_userpass(void);
     28 void socks5_auth_noauth(void);
     29 void socks5_auth_notsupported(void);
     30 void socks5_command(socks5_request_header *rhdr);
     31 void handle_socks4(void);
     32 void do_connect(unsigned char socks_version, socks5_addr_type_t addr_type, char *buf, unsigned short int port);
     33 
     34 char const *const *new_argv = NULL;
     35 
     36 int main (int argc, char const *const *argv)
     37 {
     38     fd_block(STDIN_FILENO);
     39     fd_block(STDOUT_FILENO);
     40     new_argv = argv+1;
     41     interact();
     42 }
     43 
     44 // TODO: get rid of stdio dependency
     45 void log_message(const char *message, ...)
     46 {
     47     int ret;
     48     va_list args;
     49     va_start(args, message);
     50     ret = vdprintf(STDERR_FILENO, message, args);
     51     va_end(args);
     52     if (ret < 0) {
     53         strerr_dief1sys(111, "log_message() failed");
     54     }
     55     write(2, "\n", 1);
     56 }
     57 
     58 void fd_block(int fd)
     59 {
     60     int flags = fcntl(fd, F_GETFL);
     61     if(flags == -1) {
     62         strerr_dief1sys(111, "fcntl() getfd");
     63     }
     64     if(fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
     65         strerr_dief1sys(111, "fcntl() setfd");
     66     }
     67 }
     68 
     69 void read0(void *buf, ssize_t n)
     70 {
     71     ssize_t nread, left = n;
     72     while (left > 0) {
     73         if ((nread = read(STDIN_FILENO, buf, left)) < 0) {
     74             if (errno == EINTR || errno == EAGAIN) {
     75                 continue;
     76             }
     77             strerr_diefu1sys(111, "read(stdin)");
     78         } else {
     79             if (nread == 0) {
     80                 strerr_diefu1x(111, "read(stdin): end of file");
     81             } else {
     82                 assert(left >= nread);
     83                 left -= nread;
     84                 buf += nread;
     85             }
     86         }
     87     }
     88 }
     89 
     90 void write1(const void *buf, ssize_t n)
     91 {
     92     ssize_t nwrite, left = n;
     93     while (left > 0) {
     94         if ((nwrite = write(STDOUT_FILENO, buf, left)) < 0) {
     95             if (errno == EINTR || errno == EAGAIN) {
     96                 continue;
     97             }
     98             strerr_diefu1sys(111, "write(stdout)");
     99         } else {
    100             assert(left >= nwrite);
    101             left -= nwrite;
    102             buf += nwrite;
    103         }
    104     }
    105 }
    106 
    107 void read0_cstring(char *buf, size_t n)
    108 {
    109     size_t left = n;
    110     while (left > 0) {
    111         read0(buf, 1);
    112         if(*buf == 0) {
    113             return;
    114         }
    115         left--;
    116         buf++;
    117     }
    118     strerr_diefu1x(0, "read0_cstring: exceeded maximum buffer size");
    119 }
    120 
    121 /* start here */
    122 void interact(void)
    123 {
    124     socks_header hdr;
    125     socks_read_header(&hdr);
    126 
    127     switch (hdr.version) {
    128         case SOCKS_VERSION_5:
    129             handle_socks5(hdr.methods);
    130             break;
    131         case SOCKS_VERSION_4:
    132             if (hdr.methods == 1) {
    133                 handle_socks4();
    134             } else {
    135                 strerr_dief1x(0, "Unsupported SOCKS4 mode");
    136             }
    137             break;
    138     }
    139 }
    140 
    141 void socks_read_header(socks_header *hdr)
    142 {
    143     read0(hdr, sizeof(socks_header));
    144     // log_message("Received header: 0x%hhX 0x%hhX", hdr->version, hdr->method);
    145     if (hdr->version != SOCKS_VERSION_5 && hdr->version != SOCKS_VERSION_4) {
    146         strerr_dief1x(0, "Incompatible version!");
    147     }
    148 }
    149 
    150 unsigned short socks_read_port(void) {
    151     unsigned short port;
    152     read0(&port, sizeof(port));
    153     return ntohs(port);
    154 }
    155 
    156 void handle_socks5(char methods)
    157 {
    158     socks5_auth(methods, SOCKS5_AUTH_METHOD_NOAUTH); /* TODO: authentication support */
    159     socks5_request_header rhdr;
    160     socks5_command(&rhdr);
    161     unsigned short port;
    162 
    163     switch (rhdr.atyp) {
    164         case SOCKS5_ADDR_TYPE_IP4:
    165             char ip4[4];
    166             read0(ip4, sizeof(ip4));
    167             port = socks_read_port();
    168 
    169             do_connect(5, SOCKS5_ADDR_TYPE_IP4, ip4, port);
    170             break;
    171         case SOCKS5_ADDR_TYPE_IP6:
    172             char ip6[16];
    173             read0(ip6, sizeof(ip6));
    174             port = socks_read_port();
    175 
    176             do_connect(5, SOCKS5_ADDR_TYPE_IP6, ip6, port);
    177             break;
    178         case SOCKS5_ADDR_TYPE_DOMAIN:
    179             unsigned char size;
    180             char domain[MAX_DOMAIN_LENGTH];
    181             read0(&size, sizeof(size));
    182             assert(size < MAX_DOMAIN_LENGTH);
    183             read0(domain, size);
    184             domain[size] = 0;
    185             port = socks_read_port();
    186 
    187             do_connect(5, SOCKS5_ADDR_TYPE_DOMAIN, domain, port);
    188             break;
    189         default:
    190             strerr_dief1x(1, "Unsupported SOCKS5 command");
    191             break;
    192     }
    193 }
    194 
    195 void socks5_auth(int methods_count, socks5_auth_mode_t supported_auth_method)
    196 {
    197     bool supported = false;
    198     for (int i = 0; i < methods_count; i++) {
    199         char type;
    200         read0(&type, 1);
    201         // log_message("Method AUTH 0x%hhX", type);
    202         if (type == supported_auth_method) {
    203             supported = true;
    204         }
    205     }
    206     if (!supported) {
    207         socks5_auth_notsupported();
    208         strerr_dief1x(1, "No supported authentication method");
    209     }
    210     switch (supported_auth_method) {
    211         case SOCKS5_AUTH_METHOD_NOAUTH:
    212             socks5_auth_noauth();
    213             break;
    214         case SOCKS5_AUTH_METHOD_USERPASS:
    215             socks5_auth_userpass();
    216             break;
    217         default:
    218             strerr_dief1x(110, "Internal error: unhandled authentication method");
    219             break;
    220     }
    221 }
    222 
    223 void socks5_auth_userpass(void)
    224 {
    225     strerr_dief1x(110, "TODO: userpass auth");
    226 }
    227 
    228 void socks5_auth_noauth(void)
    229 {
    230     const char answer[2] = { SOCKS_VERSION_5, SOCKS5_AUTH_METHOD_NOAUTH };
    231     write1(answer, sizeof(answer));
    232 }
    233 
    234 void socks5_auth_notsupported(void)
    235 {
    236     const char answer[2] = { SOCKS_VERSION_5, SOCKS5_AUTH_METHOD_NOMETHOD };
    237     write1(answer, sizeof(answer));
    238 }
    239 
    240 void socks5_command(socks5_request_header *rhdr)
    241 {
    242     read0(rhdr, sizeof(socks5_request_header));
    243     // log_message("Command %02hhX %02hhX %02hhX %02hhX",
    244     //        rhdr->version, rhdr->cmd, rhdr->reserved, rhdr->atyp);
    245     assert(rhdr->version == SOCKS_VERSION_5);
    246     assert(rhdr->cmd == SOCKS5_CMD_CONNECT);
    247 }
    248 
    249 
    250 void handle_socks4(void)
    251 {
    252     char ident[255];
    253     unsigned short int port;
    254     char ip[4];
    255 
    256     port = socks_read_port();
    257     read0(&ip, sizeof(ip));
    258     read0_cstring(ident, sizeof(ident));
    259 
    260     if (ip[0] == 0 && ip[1] == 0 && ip[2] == 0 && ip[3] != 0) {
    261         char domain[MAX_DOMAIN_LENGTH];
    262         read0_cstring(domain, sizeof(domain));
    263         // log_message("Socks4A: ident:%s; domain:%s;", ident, domain);
    264         do_connect(4, SOCKS5_ADDR_TYPE_DOMAIN, domain, port);
    265     } else {
    266         // log_message("Socks4: connect by ip & port");
    267         do_connect(4, SOCKS5_ADDR_TYPE_IP4, ip, port);
    268     }
    269 
    270 }
    271 
    272 void xenv(char const *s, char const *t) {
    273     if (!env_mexec(s, t)) {
    274         strerr_diefu1sys(111, "env_mexec");
    275     }
    276 }
    277 
    278 void socks_env_ip4(const char *ip) {
    279     char v4_address[16];
    280     snprintf(v4_address, sizeof(v4_address), "%hhu.%hhu.%hhu.%hhu",
    281             ip[0], ip[1], ip[2], ip[3]);
    282     xenv("SOCKS_ADDR", v4_address);
    283     xenv("SOCKS_ADDR_TYPE", "ip4");
    284 }
    285 
    286 void socks_env_ip6(const char *ip) {
    287     char v6_address[41];
    288     snprintf(v6_address, sizeof(v6_address),
    289             "%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx",
    290             ip[0x0], ip[0x1], ip[0x2], ip[0x3], ip[0x4], ip[0x5], ip[0x6], ip[0x7],
    291             ip[0x8], ip[0x9], ip[0xa], ip[0xb], ip[0xc], ip[0xd], ip[0xe], ip[0xf]);
    292     xenv("SOCKS_ADDR", v6_address);
    293     xenv("SOCKS_ADDR_TYPE", "ip6");
    294 }
    295 
    296 void socks_env_dns(const char *domain) {
    297     xenv("SOCKS_ADDR", domain);
    298     xenv("SOCKS_ADDR_TYPE", "dns");
    299 }
    300 
    301 void log_connect(unsigned char socks_version, socks5_addr_type_t addr_type, char *buf, unsigned short int port) {
    302     char address[43];
    303     switch (addr_type) {
    304         case SOCKS5_ADDR_TYPE_IP4:
    305             snprintf(address, sizeof(address), "%hhu.%hhu.%hhu.%hhu",
    306                     buf[0], buf[1], buf[2], buf[3]);
    307             break;
    308         case SOCKS5_ADDR_TYPE_IP6:
    309             snprintf(address, sizeof(address),
    310                 "[%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx:%02hhx%02hhx]",
    311                 buf[0x0], buf[0x1], buf[0x2], buf[0x3], buf[0x4], buf[0x5], buf[0x6], buf[0x7],
    312                 buf[0x8], buf[0x9], buf[0xa], buf[0xb], buf[0xc], buf[0xd], buf[0xe], buf[0xf]);
    313             break;
    314         case SOCKS5_ADDR_TYPE_DOMAIN:
    315             break;
    316         default:
    317             strerr_dief1x(110, "Internal error: unhandled address type");
    318     }
    319     log_message("SOCKS%hhd %s:%hd", socks_version, addr_type == SOCKS5_ADDR_TYPE_DOMAIN ? buf : address, port);
    320 }
    321 
    322 void do_connect(unsigned char socks_version, socks5_addr_type_t addr_type, char *buf, unsigned short int port) {
    323     switch(socks_version) {
    324         case 4:
    325             xenv("SOCKS_VERSION", "4");
    326             break;
    327         case 5:
    328             xenv("SOCKS_VERSION", "5");
    329             break;
    330         default:
    331             strerr_dief1x(110, "Internal error: unhandled protocol version");
    332             break;
    333     }
    334 
    335     char port_string[6];
    336     snprintf(port_string, sizeof(port_string), "%d", port);
    337     xenv("SOCKS_PORT", port_string);
    338 
    339     switch (addr_type) {
    340         case SOCKS5_ADDR_TYPE_IP4:
    341             socks_env_ip4(buf);
    342             break;
    343         case SOCKS5_ADDR_TYPE_IP6:
    344             socks_env_ip6(buf);
    345             break;
    346         case SOCKS5_ADDR_TYPE_DOMAIN:
    347             socks_env_dns(buf);
    348             break;
    349         default:
    350             strerr_dief1x(110, "Internal error: unhandled address type");
    351     }
    352 
    353     log_connect(socks_version, addr_type, buf, port);
    354     xmexec(new_argv);
    355 }
    356 
    357 /* vim: sw=4 sts=4 et
    358 */