--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.c	Fri Feb 07 23:28:39 2014 +0100
@@ -0,0 +1,335 @@
+/* Copyright 2014 Ivo Smits <Ivo@UCIS.nl>. All rights reserved.
+   Redistribution and use in source and binary forms, with or without modification, are
+   permitted provided that the following conditions are met:
+
+   1. Redistributions of source code must retain the above copyright notice, this list of
+      conditions and the following disclaimer.
+
+   2. Redistributions in binary form must reproduce the above copyright notice, this list
+      of conditions and the following disclaimer in the documentation and/or other materials
+      provided with the distribution.
+
+   THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED
+   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+   FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR
+   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+   ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+   The views and conclusions contained in the software and documentation are those of the
+   authors and should not be interpreted as representing official policies, either expressed
+   or implied, of Ivo Smits.*/
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sodium/crypto_box_curve25519xsalsa20poly1305.h>
+#include <sodium/crypto_scalarmult_curve25519.h>
+#include <sodium/crypto_hash_sha512.h>
+#include "include.h"
+
+static unsigned char workbuffer[2000];
+
+static int devurandomfd = -1;
+static bool randombytes(unsigned char* buffer, int len) {
+	if (devurandomfd == -1) devurandomfd = open("/dev/urandom", O_RDONLY);
+	if (devurandomfd == -1) return errorexit("could not open /dev/urandom");
+	while (len > 0) {
+		int got = read(devurandomfd, buffer, len);
+		if (got < 0) return errorexitp("could not read from /dev/urandom");
+		buffer += got;
+		len -= got;
+	}
+	return true;
+}
+
+static void dumphex(char* lbl, unsigned char* buffer, int len) {
+	fprintf(stderr, "%s: ", lbl);
+	for (; len > 0; len--, buffer++) fprintf(stderr, "%02x", *buffer);
+	fprintf(stderr, "\n");
+}
+
+static void nonceinc(unsigned char* nonce) {
+	int i;
+	for (i = 23; i >= 0 && (++nonce[i] == 0); i--);
+}
+
+static bool send_all(int socket, unsigned char* buffer, int length) {
+	while (length) {
+		int sent = write(socket, buffer, length);
+		if (sent <= 0) return errorexitp("socket write failed");
+		buffer += sent;
+		length -= sent;
+	}
+	return true;
+}
+static bool send_packet(connection_context* context, unsigned char* buffer, int length, bool controlflag) {
+	fprintf(stderr, "Send packet (size=%d type=%d control=%d encrypt=%d)\n", length, buffer[0], controlflag, context->send_encrypted);
+	if (context->send_encrypted) {
+		if (length + 32 > sizeof(workbuffer)) return errorexit("packet too big for encryption buffer");
+		memmove(workbuffer + 32, buffer, length);
+		memset(workbuffer, 0, 32);
+		crypto_box_curve25519xsalsa20poly1305_afternm(workbuffer, workbuffer, length + 32, context->send_nonce, context->send_key);
+		nonceinc(context->send_nonce);
+		buffer = workbuffer + 16;
+		length += 16;
+	}
+	if (length > 0x7FFF) return errorexit("packet too big for framing");
+	unsigned char lbuf[2];
+	length = (length & 0x7FFF) | (controlflag ? 0x8000 : 0);
+	lbuf[0] = (length >> 8) & 0xFF;
+	lbuf[1] = length & 0xFF;
+	if (!send_all(context->send_socket, lbuf, 2)) return false;
+	if (!send_all(context->send_socket, buffer, length & 0x7FFF)) return false;
+	return true;
+}
+static bool send_command_ack(connection_context* context, unsigned char code) {
+	workbuffer[0] = code;
+	return send_packet(context, workbuffer, 1, true);
+}
+
+
+static bool check_tunnel_ready(connection_context* context) {
+	if (context->local_tunnelready) return true;
+	if (context->require_key_authentication) return true;
+	if (context->require_password_authentication) return true;
+	if (context->require_encryption && !context->recv_encrypted) return true;
+	if (!send_command_ack(context, 0)) return false;
+	context->local_tunnelready = true;
+	return true;
+}
+
+static bool process_network_packet(connection_context* context, unsigned char* buffer, int pktlen, int controlflag);
+bool connection_read(connection_context* context) {
+	int got = read(context->recv_socket, context->recv_buffer + context->recv_offset, sizeof(context->recv_buffer) - context->recv_offset);
+	if (got < 0) return errorexitp("read failure on socket");
+	context->recv_offset += got;
+	while (context->recv_offset >= 2) {
+		int pktlen = ((unsigned char)context->recv_buffer[0] << 8) | (unsigned char)context->recv_buffer[1];
+		int controlflag = pktlen & 0x8000;
+		pktlen &= 0x7FFF;
+		if (pktlen > sizeof(context->recv_buffer)) return errorexit("received packet too big for buffer");
+		if (context->recv_offset < pktlen + 2) break;
+		if (!process_network_packet(context, context->recv_buffer + 2, pktlen, !!controlflag)) return false;
+		context->recv_offset -= 2 + pktlen;
+		memmove(context->recv_buffer, context->recv_buffer + 2 + pktlen, context->recv_offset);
+	}
+	if (!context->local_tunnelready) if (!check_tunnel_ready(context)) return false;
+	return true;
+}
+static bool send_start_crypt_auth(connection_context* context, const unsigned char* key) {
+	if (!context->key_updated) return context->startcryptauthsent && !key;
+	if (key) {
+		memcpy(context->local_seckey_next, key, 32);
+	} else {
+		randombytes(context->local_seckey_next, 32);
+	}
+	workbuffer[0] = 1;
+	crypto_scalarmult_curve25519_base(workbuffer + 1, context->local_seckey_next);
+	memcpy(workbuffer + 1 + 32, context->nonce_next, 24);
+	if (!send_packet(context, workbuffer, 1 + 32 + 24, true)) return false;
+	crypto_box_curve25519xsalsa20poly1305_beforenm(context->send_key, context->remote_pubkey, context->local_seckey_next);
+	memcpy(context->send_nonce + 12, context->nonce_next + 12, 12);
+	context->send_encrypted = true;
+	context->startcryptauthsent = true;
+	context->key_updated = false;
+	return true;
+}
+static bool begin_update_key(connection_context* context, char* seckey) {
+	if (!context->key_updated) return !seckey;
+	if (seckey) {
+		memcpy(context->local_seckey_next, seckey, 32);
+	} else {
+		randombytes(context->local_seckey_next, 32);
+	}
+	workbuffer[0] = 3;
+	crypto_scalarmult_curve25519_base(workbuffer + 1, context->local_seckey_next);
+	if (!send_packet(context, workbuffer, 1 + 32, true)) return false;
+	crypto_box_curve25519xsalsa20poly1305_beforenm(context->send_key, context->remote_pubkey, context->local_seckey_next);
+	context->send_encrypted = true;
+	context->key_updated = false;
+	return true;
+}
+static bool password_hash(unsigned char* output, unsigned char* salt, int saltlength, char* password) {
+	int pwlen = password ? strlen(password) : 0;
+	if (saltlength + pwlen > sizeof(workbuffer)) return errorexit("password and salt too big for buffer");
+	if (saltlength) memmove(workbuffer, salt, saltlength);
+	if (pwlen) memcpy(workbuffer + saltlength, password, pwlen);
+	crypto_hash_sha512(output, workbuffer, saltlength + pwlen);
+	return true;
+}
+static bool process_network_packet(connection_context* context, unsigned char* buffer, int pktlen, int controlflag) {
+	if (context->recv_encrypted) {
+		if (pktlen + 16 > sizeof(workbuffer)) return errorexit("received packet too big for decrypt buffer");
+		memmove(workbuffer + 16, buffer, pktlen);
+		memset(workbuffer, 0, 16);
+		if (crypto_box_curve25519xsalsa20poly1305_open_afternm(workbuffer, workbuffer, pktlen + 16, context->recv_nonce, context->recv_key)) return false;
+		nonceinc(context->recv_nonce);
+		buffer = workbuffer + 32;
+		pktlen -= 16;
+	}
+	if (controlflag) {
+		fprintf(stderr, "Received packet (size=%d type=%d control=%d encrypt=%d)\n", pktlen, buffer[0], controlflag, context->recv_encrypted);
+		if (pktlen < 1) return errorexit("zero length control packet");
+		switch (buffer[0]) {
+			case 0:
+				fprintf(stderr, "Control: tunnel ready\n");
+				context->remote_tunnelready = true;
+				break;
+			case 1:
+				fprintf(stderr, "Control: crypto auth req\n");
+				if (pktlen < 1 + 32 + 24) return errorexit("short control packet 1");
+				if (context->require_key_authentication) {
+					if (memcmp(buffer + 1, context->remote_pubkey_expect, 32)) return errorexit("incorrect crypto auth key");
+					context->require_key_authentication = true;
+				}
+				if (!context->startcryptauthsent) send_start_crypt_auth(context, NULL);
+				if (!send_command_ack(context, 2)) return false;
+				memcpy(context->remote_pubkey, buffer + 1, 32);
+				memcpy(context->send_nonce, buffer + 1 + 32, 12);
+				memcpy(context->recv_nonce + 12, buffer + 1 + 32 + 12, 12);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->recv_key, context->remote_pubkey, context->local_seckey_current);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->send_key, context->remote_pubkey, context->local_seckey_next);
+				context->recv_encrypted = context->send_encrypted = true;
+				break;
+			case 2:
+				fprintf(stderr, "Control: crypto auth ack\n");
+				memcpy(context->local_seckey_current, context->local_seckey_next, 32);
+				memcpy(context->recv_nonce, context->nonce_next, 12);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->recv_key, context->remote_pubkey, context->local_seckey_current);
+				context->recv_encrypted = true;
+				context->key_updated = true;
+				begin_update_key(context, NULL);
+				break;
+			case 3:
+				fprintf(stderr, "Control: key update req\n");
+				if (pktlen < 1 + 32) return errorexit("short control packet 3");
+				if (!send_command_ack(context, 4)) return false;
+				memcpy(context->remote_pubkey, buffer + 1, 32);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->recv_key, context->remote_pubkey, context->local_seckey_current);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->send_key, context->remote_pubkey, context->local_seckey_next);
+				context->recv_encrypted = context->send_encrypted = true;
+				break;
+			case 4:
+				fprintf(stderr, "Control: key update ack\n");
+				memcpy(context->local_seckey_current, context->local_seckey_next, 32);
+				crypto_box_curve25519xsalsa20poly1305_beforenm(context->recv_key, context->remote_pubkey, context->local_seckey_current);
+				context->recv_encrypted = true;
+				context->key_updated = true;
+				break;
+			case 5:
+				fprintf(stderr, "Control: password authentication req\n");
+				unsigned char pwhash[64];
+				if (!password_hash(pwhash, buffer + 1, pktlen - 1, context->password)) return false;
+				workbuffer[0] = 6;
+				memcpy(workbuffer + 1, pwhash, 64);
+				if (!send_packet(context, workbuffer, 1 + 64, true)) return false;
+				break;
+			case 6:
+				fprintf(stderr, "Control: password authentication ack\n");
+				if (pktlen < 1 + 64) return errorexit("short control packet 6");;
+				char pwhashcheck[64];
+				memcpy(pwhashcheck, buffer + 1, 64);
+				if (!password_hash(pwhash, context->nonce_next, 12, context->password)) return false;
+				if (memcmp(pwhash, pwhashcheck, 64)) return errorexit("incorrect password auth");;
+				context->require_password_authentication = false;
+				break;
+			case 7:
+				fprintf(stderr, "Control: disable encryption req\n");
+				context->recv_encrypted = context->send_encrypted = false;
+				if (!send_command_ack(context, 8)) return false;
+				break;
+			case 8:
+				fprintf(stderr, "Control: disable encryption ack\n");
+				context->recv_encrypted = false;
+				break;
+			case 9:
+				fprintf(stderr, "Control: echo req\n");
+				if (pktlen > sizeof(workbuffer)) return errorexit("echo request too big");;
+				memmove(workbuffer, buffer, pktlen);
+				workbuffer[0] = 10;
+				if (!send_packet(context, workbuffer, pktlen, true)) return false;
+				break;
+			case 10:
+				fprintf(stderr, "Control: echo resp\n");
+				context->pong = true;
+				break;
+			case 13:
+				if (!context->local_tunnelready || !context->remote_tunnelready) return errorexit("received data packet while tunnel not ready");;
+				if (context->tunnel && !tunnel_write_data(context->tunnel, buffer + 1, pktlen - 1)) return false;
+				break;
+			case 11:
+			case 12:
+			case 17:
+			case 81:
+				break;
+			default:
+				fprintf(stderr, "Unknown control type %d\n", buffer[0]);
+				break;
+		}
+	} else {
+		if (!context->local_tunnelready || !context->remote_tunnelready) return errorexit("received data packet while tunnel not ready");;
+		fprintf(stderr, "Tunnel data %d\n", pktlen);
+		if (context->tunnel && !tunnel_write_data(context->tunnel, buffer, pktlen)) return false;
+	}
+	return true;
+}
+
+bool connection_ping(connection_context* context) {
+	if (!context->pong) return false;
+	context->pong = false;
+	send_command_ack(context, 9);
+	return true;
+}
+
+bool connection_init(connection_context* context) {
+	memset(context, 0, sizeof(connection_context));
+	char localpubkey[32];
+	crypto_scalarmult_curve25519_base(context->remote_pubkey, context->local_seckey_current);
+	randombytes(context->nonce_next, 24);
+	context->key_updated = true;
+	context->pong = true;
+	return true;
+}
+bool connection_init_socket(connection_context* context, const int recvsocket, const int sendsocket) {
+	context->recv_socket = recvsocket;
+	context->send_socket = sendsocket;
+	if (!send_packet(context, (unsigned char*)"QUICKTUN", 8, true)) return false;
+	return true;
+}
+bool connection_init_encryption(connection_context* context, const unsigned char* localseckey, const unsigned char* remotepubkey) {
+	if (remotepubkey) {
+		memcpy(context->remote_pubkey_expect, remotepubkey, 32);
+		context->require_key_authentication = true;
+	}
+	context->require_encryption = true;
+	if (!send_start_crypt_auth(context, localseckey)) return false;
+	return true;
+}
+bool connection_init_passwordauth(connection_context* context, char* password) {
+	context->password = password;
+	context->require_password_authentication = true;
+	workbuffer[0] = 5;
+	memcpy(workbuffer + 1, context->nonce_next, 12);
+	if (!send_packet(context, workbuffer, 1 + 12, true)) return false;
+	return true;
+}
+
+bool connection_write_data(connection_context* context, unsigned char* buffer, int len) {
+	return send_packet(context, buffer, len, false);
+}
+
+bool connection_init_done(connection_context* context) {
+	return check_tunnel_ready(context);
+}
+
+bool connection_update_key(connection_context* context) {
+	return begin_update_key(context, NULL);
+}
+