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 */