stun

commit 7c94056263faf3a17ef80ea333143b09fc2178b3
Author: sin <sin@2f30.org>
Date:   Thu, 17 Mar 2016 18:36:28 +0000

Initial commit

Diffstat:
A
arg.h
 | 
65
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
A
stun.c
 | 
375
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

2 files changed, 440 insertions(+), 0 deletions(-)
diff --git a/arg.h b/arg.h
@@ -0,0 +1,65 @@
+/*
+ * Copy me if you can.
+ * by 20h
+ */
+
+#ifndef ARG_H__
+#define ARG_H__
+
+extern char *argv0;
+
+/* use main(int argc, char *argv[]) */
+#define ARGBEGIN	for (argv0 = *argv, argv++, argc--;\
+					argv[0] && argv[0][0] == '-'\
+					&& argv[0][1];\
+					argc--, argv++) {\
+				char argc_;\
+				char **argv_;\
+				int brk_;\
+				if (argv[0][1] == '-' && argv[0][2] == '\0') {\
+					argv++;\
+					argc--;\
+					break;\
+				}\
+				for (brk_ = 0, argv[0]++, argv_ = argv;\
+						argv[0][0] && !brk_;\
+						argv[0]++) {\
+					if (argv_ != argv)\
+						break;\
+					argc_ = argv[0][0];\
+					switch (argc_)
+
+/* Handles obsolete -NUM syntax */
+#define ARGNUM				case '0':\
+					case '1':\
+					case '2':\
+					case '3':\
+					case '4':\
+					case '5':\
+					case '6':\
+					case '7':\
+					case '8':\
+					case '9'
+
+#define ARGEND			}\
+			}
+
+#define ARGC()		argc_
+
+#define ARGNUMF()	(brk_ = 1, estrtonum(argv[0], 0, INT_MAX))
+
+#define EARGF(x)	((argv[0][1] == '\0' && argv[1] == NULL)?\
+				((x), abort(), (char *)0) :\
+				(brk_ = 1, (argv[0][1] != '\0')?\
+					(&argv[0][1]) :\
+					(argc--, argv++, argv[0])))
+
+#define ARGF()		((argv[0][1] == '\0' && argv[1] == NULL)?\
+				(char *)0 :\
+				(brk_ = 1, (argv[0][1] != '\0')?\
+					(&argv[0][1]) :\
+					(argc--, argv++, argv[0])))
+
+#define LNGARG()	&argv[0][0]
+
+#endif
diff --git a/stun.c b/stun.c
@@ -0,0 +1,375 @@
+/*
+ * Copyright (c) 2016 Dimitris Papastamos <sin@2f30.org>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <net/if.h>
+#include <net/if_tun.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <err.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <openssl/aes.h>
+#include <openssl/evp.h>
+
+#include "arg.h"
+
+#define HDRLEN		4
+#define MTU		1440
+
+EVP_CIPHER_CTX enc, dec;
+char *argv0;
+char *tundev;
+char *host;
+int port = 12080;
+int debug;
+int sflag;
+
+int
+aesinit(unsigned char *pw, int pwlen, unsigned char *salt,
+        EVP_CIPHER_CTX *ectx, EVP_CIPHER_CTX *dctx)
+{
+	unsigned char key[32], iv[32];
+	int ret, nrounds = 5;
+
+	ret = EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha1(),
+	                     salt, pw, pwlen, nrounds, key, iv);
+	if (ret != 32)
+		errx(1, "wrong key size: %d\n", ret);
+	EVP_CIPHER_CTX_init(ectx);
+	EVP_EncryptInit_ex(ectx, EVP_aes_256_cbc(), NULL, key, iv);
+	EVP_CIPHER_CTX_init(dctx);
+	EVP_DecryptInit_ex(dctx, EVP_aes_256_cbc(), NULL, key, iv);
+	return 0;
+}
+
+int
+aesenc(EVP_CIPHER_CTX *ectx, unsigned char *ciphertext,
+       unsigned char *plaintext, int len)
+{
+	int clen = len + AES_BLOCK_SIZE, flen = 0;
+
+	EVP_EncryptInit_ex(ectx, NULL, NULL, NULL, NULL);
+	EVP_EncryptUpdate(ectx, ciphertext, &clen, plaintext, len);
+	EVP_EncryptFinal_ex(ectx, ciphertext + clen, &flen);
+	return clen + flen;
+}
+
+int
+aesdec(EVP_CIPHER_CTX *ectx, unsigned char *plaintext,
+       unsigned char *ciphertext, int len)
+{
+	int plen = len, flen = 0;
+
+	EVP_DecryptInit_ex(ectx, NULL, NULL, NULL, NULL);
+	EVP_DecryptUpdate(ectx, plaintext, &plen, ciphertext, len);
+	EVP_DecryptFinal_ex(ectx, plaintext + plen, &flen);
+	return plen + flen;
+}
+
+int
+opentun(char *tundev)
+{
+	struct tuninfo ti;
+	int fd;
+	int mode = IFF_UP | IFF_POINTOPOINT;
+
+	fd = open(tundev, O_RDWR);
+	if (fd < 0)
+		err(1, "open %s", tundev);
+	if (ioctl(fd, TUNSIFMODE, &mode) < 0)
+		err(1, "ioctl: TUNSIFMODE %s", tundev);
+	if (ioctl(fd, TUNGIFINFO, &ti) < 0)
+		err(1, "ioctl: TUNGIFINFO %s", tundev);
+	ti.mtu = MTU;
+	if (ioctl(fd, TUNSIFINFO, &ti) < 0)
+		err(1, "ioctl: TUNSIFINFO %s", tundev);
+	return fd;
+}
+
+ssize_t
+writetun(int fd, char *buf, size_t len)
+{
+	struct iovec iov[2];
+	uint32_t type = htonl(AF_INET);
+	ssize_t n;
+
+	iov[0].iov_base = &type;
+	iov[0].iov_len = sizeof(type);
+	iov[1].iov_base = buf;
+	iov[1].iov_len = len;
+	n = writev(fd, iov, 2);
+	if (n < 0)
+		err(1, "writev");
+	else if (n > 0)
+		return n - sizeof(type);
+	return n;
+}
+
+ssize_t
+readtun(int fd, char *buf, size_t len)
+{
+	struct iovec iov[2];
+	uint32_t type;
+	ssize_t n;
+
+	iov[0].iov_base = &type;
+	iov[0].iov_len = sizeof(type);
+	iov[1].iov_base = buf;
+	iov[1].iov_len = len;
+	n = readv(fd, iov, 2);
+	if (n < 0)
+		err(1, "readv");
+	else if (n > 0)
+		return n - sizeof(type);
+	return n;
+}
+
+void
+packint(unsigned char *buf, int n)
+{
+	buf[0] = n >> 24 & 0xff;
+	buf[1] = n >> 16 & 0xff;
+	buf[2] = n >>  8 & 0xff;
+	buf[3] = n & 0xff;
+}
+
+int
+unpackint(unsigned char *buf)
+{
+	return (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
+}
+
+/* handles partial writes */
+int
+writeall(int fd, void *buf, int count)
+{
+	unsigned char *p = buf;
+	int n, total = 0;
+
+	while (count > 0) {
+		n = write(fd, p + total, count);
+		if (n < 0) {
+			warn("write");
+			total = -1;
+			break;
+		} else if (n == 0) {
+			break;
+		}
+		total += n;
+		count -= n;
+	}
+	return total;
+}
+
+/* encrypt a packet and send it to the remote peer */
+int
+writenet(int fd, unsigned char *buf, int buflen)
+{
+	unsigned char encbuf[MTU + AES_BLOCK_SIZE + HDRLEN];
+	int pktlen;
+
+	pktlen = aesenc(&enc, &encbuf[HDRLEN], buf, buflen);
+	packint(encbuf, pktlen);
+	pktlen += HDRLEN;
+	return writeall(fd, encbuf, pktlen);
+}
+
+/* handles partial reads */
+int
+readall(int fd, void *buf, int count)
+{
+	unsigned char *p = buf;
+	int n, total = 0;
+
+	while (count > 0) {
+		n = read(fd, p + total, count);
+		if (n < 0) {
+			warn("read");
+			total = -1;
+			break;
+		} else if (n == 0) {
+			break;
+		}
+		total += n;
+		count -= n;
+	}
+	return total;
+}
+
+/* read a packet from the remote peer, decrypt it and pass it back through buf */
+int
+readnet(int fd, unsigned char *buf, int count)
+{
+	unsigned char decbuf[MTU + AES_BLOCK_SIZE + HDRLEN];
+	unsigned char hdr[HDRLEN];
+	int n, pktlen;
+
+	n = readall(fd, hdr, sizeof(hdr));
+	if (n <= 0)
+		return n;
+	pktlen = unpackint(hdr);
+	if (pktlen < 0 || pktlen > MTU + AES_BLOCK_SIZE) {
+		warnx("bogus payload length: %d", pktlen);
+		return -1;
+	}
+	n = readall(fd, buf, pktlen);
+	if (n <= 0)
+		return n;
+	pktlen = aesdec(&dec, decbuf, buf, pktlen);
+	memcpy(buf, decbuf, pktlen);
+	return pktlen;
+}
+
+int
+loop(int netfd, int tunfd)
+{
+	unsigned char buf[MTU + AES_BLOCK_SIZE + HDRLEN];
+	struct pollfd pfd[2];
+	int ret, n;
+
+	pfd[0].fd = netfd;
+	pfd[0].events = POLLIN;
+	pfd[1].fd = tunfd;
+	pfd[1].events = POLLIN;
+	while (1) {
+		ret = poll(pfd, 2, -1);
+		if (ret < 0)
+			err(1, "poll");
+		if (pfd[0].revents & (POLLERR | POLLNVAL) ||
+		    pfd[1].revents & (POLLERR | POLLNVAL))
+			errx(1, "bad fd");
+		if (pfd[1].revents & (POLLIN | POLLHUP))
+			if ((n = readtun(tunfd, buf, MTU)) <= 0 ||
+			    (n = writenet(netfd, buf, n)) <= 0)
+				return -1;
+		if (pfd[0].revents & (POLLIN | POLLHUP))
+			if ((n = readnet(netfd, buf, MTU)) <= 0 ||
+			    (n = writetun(tunfd, buf, n)) <= 0)
+				return -1;
+	}
+	return 0; /* unreachable */
+}
+
+void
+usage(void)
+{
+	fprintf(stderr, "usage: %s [-ds] [-h host] [-p port] interface\n", argv0);
+	exit(1);
+}
+
+int
+main(int argc, char *argv[])
+{
+	struct sockaddr_in local, remote;
+	char *pw;
+	const char *errstr;
+	int tunfd, netfd = -1, listenfd;
+	int ret;
+
+	ARGBEGIN {
+	case 'd':
+		debug = 1;
+		break;
+	case 's':
+		sflag = 1;
+		break;
+	case 'h':
+		host = EARGF(usage());
+		break;
+	case 'p':
+		port = strtonum(EARGF(usage()), 1, 65535, &errstr);
+		if (errstr)
+			errx(1, "invalid port number");
+		break;
+	default:
+		usage();
+	} ARGEND
+
+	if (argc != 1) usage();
+	tundev = argv[0];
+
+	tunfd = opentun(tundev);
+
+	pw = getenv("STUNPW");
+	if (!pw)
+		errx(1, "STUNPW is not set");
+	if (aesinit(pw, strlen(pw), NULL, &enc, &dec) < 0)
+		errx(1, "couldn't initialize AES cipher");
+	explicit_bzero(pw, strlen(pw));
+
+	if (sflag) {
+		/* server */
+		listenfd = socket(AF_INET, SOCK_STREAM, 0);
+		if (listenfd < 0)
+			err(1, "socket");
+		setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR, (int []){1}, sizeof(int));
+		memset(&local, 0, sizeof(local));
+		local.sin_family = AF_INET;
+		local.sin_addr.s_addr = htonl(INADDR_ANY);
+		local.sin_port = htons(port);
+		ret = bind(listenfd, (struct sockaddr *)&local, sizeof(local));
+		if (ret < 0)
+			err(1, "bind");
+		ret = listen(listenfd, 5);
+		if (ret < 0)
+			err(1, "listen");
+	} else {
+		/* client */
+		if (!host)
+			usage();
+		netfd = socket(AF_INET, SOCK_STREAM, 0);
+		if (netfd < 0)
+			err(1, "socket");
+		memset(&remote, 0, sizeof(remote));
+		remote.sin_family = AF_INET;
+		remote.sin_addr.s_addr = inet_addr(host);
+		remote.sin_port = htons(port);
+		ret = connect(netfd, (struct sockaddr *)&remote, sizeof(remote));
+		if (ret < 0)
+			err(1, "connect");
+	}
+
+again:
+	if (sflag) {
+		if (netfd != -1)
+			close(netfd);
+		netfd = accept(listenfd, (struct sockaddr *)&remote,
+		               (int []){sizeof(remote)});
+		if (ret < 0)
+			err(1, "accept");
+		if (debug)
+			printf("client connected\n");
+	}
+	setsockopt(netfd, IPPROTO_TCP, TCP_NODELAY, (int []){1}, sizeof(int));
+	if (loop(netfd, tunfd) < 0 && sflag)
+		goto again;
+	warnx("connection dropped");
+	exit(1);
+}