Add support for transporting audio data using udpudp-3

With crypt.c importet from umurmur, changed encrypt key
to use raw_key instead of decrypt_iv.

8 files changed, 576 insertions, 35 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 18979d0..a929bbf 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -1,15 +1,15 @@
 bin_PROGRAMS = cmumble
 
 noinst_HEADERS = cmumble.h message.h varint.h io.h \
-		 connection.h audio.h commands.h util.h
+		 connection.h audio.h commands.h util.h crypt.h
 nodist_noinst_HEADERS = mumble.pb-c.h message_list.h
 
 cmumble_SOURCES = cmumble.c message.c varint.c io.c \
-		  connection.c audio.c commands.c util.c
+		  connection.c audio.c commands.c util.c crypt.c
 nodist_cmumble_SOURCES = mumble.pb-c.c
 
 cmumble_LDADD = $(PROTOBUF_LIBS) $(GLIB_LIBS) $(GIO_LIBS) \
-		$(GSTREAMER_LIBS) $(CELT_LIBS)
+		$(GSTREAMER_LIBS) $(CELT_LIBS) -lssl
 AM_CPPFLAGS = $(PROTOBUF_CFLAGS) $(GLIB_CFLAGS) $(GIO_CFLAGS) \
 	      $(GSTREAMER_CFLAGS) $(CELT_CFLAGS)
 
diff --git a/src/audio.c b/src/audio.c
index 3096edf..1ac1103 100644
--- a/src/audio.c
+++ b/src/audio.c
@@ -21,9 +21,8 @@ pull_buffer(GstAppSink *sink, gpointer user_data)
 {
 	struct cmumlbe *cm = user_data;
 	GstBuffer *buf;
-	uint8_t data[1024];
+	uint8_t data[1020];
 	uint32_t write = 0, pos = 0;
-	MumbleProto__UDPTunnel tunnel;
 	static int seq = 0;
 
 	/* FIXME: Make this more generic/disable pulling
@@ -54,10 +53,7 @@ pull_buffer(GstAppSink *sink, gpointer user_data)
 
 	gst_buffer_unref(buf);
 
-	mumble_proto__udptunnel__init(&tunnel);
-	tunnel.packet.data = data;
-	tunnel.packet.len = pos;
-	cmumble_send_msg(cm, &tunnel.base);
+	cmumble_connection_send_udp_data(cm, data, pos);
 
 	return GST_FLOW_OK;
 }
diff --git a/src/cmumble.c b/src/cmumble.c
index e76920e..640b6ea 100644
--- a/src/cmumble.c
+++ b/src/cmumble.c
@@ -9,14 +9,12 @@
 #include "util.h"
 
 static void
-recv_udp_tunnel(MumbleProto__UDPTunnel *tunnel, struct cmumlbe *cm)
+cmumble_read_audio_packet(struct cmumlbe *cm, uint8_t *data, size_t len)
 {
 	int64_t session, sequence;
 	uint32_t pos = 1, read = 0;
 	uint8_t frame_len, terminator;
 	struct cmumble_user *user = NULL;
-	uint8_t *data = tunnel->packet.data;
-	size_t len = tunnel->packet.len;
 
 	session  = decode_varint(&data[pos], &read, len-pos);
 	pos += read;
@@ -45,6 +43,34 @@ recv_udp_tunnel(MumbleProto__UDPTunnel *tunnel, struct cmumlbe *cm)
 	} while (terminator);
 }
 
+void
+cmumble_read_udp_data(struct cmumlbe *cm, guint8 *data, gsize size)
+{
+	guint num;
+	gint64 timestamp;
+
+	switch ((data[0] >> 5) & 0x03) {
+	case udp_ping:
+		timestamp = decode_varint(&data[1], &num, size - 1);
+		g_print("received udp ping, timestamp: %ld\n", timestamp);
+		break;
+	case udp_voice_celt_alpha:
+	case udp_voice_speex:
+	case udp_voice_celt_beta:
+		cmumble_read_audio_packet(cm, data, size);
+		break;
+	default:
+		g_printerr("Invalid udp message received\n");
+		break;
+	}
+}
+
+static void
+recv_udp_tunnel(MumbleProto__UDPTunnel *tunnel, struct cmumlbe *cm)
+{
+	cmumble_read_udp_data(cm, tunnel->packet.data, tunnel->packet.len);
+}
+
 static void
 recv_version(MumbleProto__Version *version, struct cmumlbe *cm)
 {
@@ -98,28 +124,17 @@ recv_server_sync(MumbleProto__ServerSync *sync, struct cmumlbe *cm)
 static void
 recv_crypt_setup(MumbleProto__CryptSetup *crypt, struct cmumlbe *cm)
 {
-#if 0
-	int i;
-
-	if (crypt->has_key) {
-		g_print("key: 0x");
-		for (i = 0; i < crypt->key.len; ++i)
-			g_print("%x", crypt->key.data[i]);
-		g_print("\n");
-	}
-	if (crypt->has_client_nonce) {
-		g_print("client nonce: 0x");
-		for (i = 0; i < crypt->client_nonce.len; ++i)
-			g_print("%x", crypt->client_nonce.data[i]);
-		g_print("\n");
-	}
-	if (crypt->has_server_nonce) {
-		g_print("server nonce: 0x");
-		for (i = 0; i < crypt->server_nonce.len; ++i)
-			g_print("%x", crypt->server_nonce.data[i]);
-		g_print("\n");
-	}
-#endif
+	g_return_if_fail(crypt->key.len == 16 &&
+			 crypt->client_nonce.len == 16 &&
+			 crypt->server_nonce.len == 16);
+
+	CryptState_init(&cm->crypt);
+	CryptState_setKey(&cm->crypt,
+			  crypt->key.data,
+			  crypt->client_nonce.data,
+			  crypt->server_nonce.data);
+
+	cmumble_connection_udp_init(cm);
 }
 
 static void
diff --git a/src/cmumble.h b/src/cmumble.h
index cc40149..3c9722b 100644
--- a/src/cmumble.h
+++ b/src/cmumble.h
@@ -12,6 +12,8 @@
 #include "audio.h"
 #include "commands.h"
 
+#include "crypt.h"
+
 typedef void (*callback_t)(ProtobufCMessage *msg, struct cmumlbe *);
 
 struct cmumlbe {
@@ -34,6 +36,7 @@ struct cmumlbe {
 	GList *channels;
 
 	struct cmumble_user *user;
+	cryptState_t crypt;
 };
 
 struct cmumble_user {
@@ -74,4 +77,7 @@ enum udp_message_target {
 void
 cmumble_protocol_init(struct cmumlbe *cm);
 
+void
+cmumble_read_udp_data(struct cmumlbe *cm, guint8 *data, gsize size);
+
 #endif
diff --git a/src/connection.c b/src/connection.c
index 7fefdb3..e7f74bd 100644
--- a/src/connection.c
+++ b/src/connection.c
@@ -3,6 +3,7 @@
 
 #include "connection.h"
 #include "cmumble.h"
+#include "varint.h"
 
 static gboolean
 read_cb(GObject *pollable_stream, gpointer data)
@@ -18,6 +19,118 @@ read_cb(GObject *pollable_stream, gpointer data)
 	return TRUE;
 }
 
+static gboolean
+read_udp_socket(GSocket *socket, GIOCondition condition, gpointer user_data)
+{
+	struct cmumlbe *cm = user_data;
+	GError *error = NULL;
+	guchar data[1024];
+	guchar plain[1020];
+	gssize size;
+
+	size = g_socket_receive(socket, (gchar *) data, sizeof(data),
+				NULL, &error);
+	if (size <= 4)
+		return TRUE;
+
+	if (!CryptState_decrypt(&cm->crypt, data, plain, size))
+		return TRUE;
+
+	cm->con.udp.connected = TRUE;
+
+	cmumble_read_udp_data(cm, plain, size - 4);
+
+	return TRUE;
+}
+
+static void
+send_udp_tcptunnel(struct cmumlbe *cm, guint8 *data, gsize len)
+{
+	MumbleProto__UDPTunnel tunnel;
+
+	mumble_proto__udptunnel__init(&tunnel);
+	tunnel.packet.data = data;
+	tunnel.packet.len = len;
+	cmumble_send_msg(cm, &tunnel.base);
+}
+
+static void
+send_udp(struct cmumlbe *cm, guint8 *data, gsize len)
+{
+	guint8 encrypted[1024];
+	GError *error = NULL;
+
+	g_assert(len <= (sizeof(encrypted) - 4));
+
+	CryptState_encrypt(&cm->crypt, data, encrypted, len);
+	g_socket_send(cm->con.udp.sock, (gchar *) encrypted, len + 4,
+		      NULL, &error);
+}
+
+void
+cmumble_connection_send_udp_data(struct cmumlbe *cm, guint8 *data, gsize len)
+{
+	if (cm->con.udp.connected)
+		send_udp(cm, data, len);
+	else
+		send_udp_tcptunnel(cm, data, len);
+}
+
+static void
+do_udp_ping(struct cmumlbe *cm)
+{
+	uint8_t data[16];
+	uint32_t written = 0, pos = 0;
+	GTimeVal tv;
+
+	g_get_current_time(&tv);
+
+	data[pos++] = (udp_ping << 5);
+	encode_varint(&data[pos], &written, tv.tv_sec, sizeof(data)-pos);
+	pos += written;
+
+	send_udp(cm, data, pos);
+
+	g_print("udp ping sent: timestamp: %ld\n", tv.tv_sec);
+}
+
+void
+cmumble_connection_udp_init(struct cmumlbe *cm)
+{
+	GError *error = NULL;
+	GSocketAddress *saddr;
+
+	cm->con.udp.connected = FALSE;
+	cm->con.udp.sock = g_socket_new(G_SOCKET_FAMILY_IPV4,
+					G_SOCKET_TYPE_DATAGRAM,
+					G_SOCKET_PROTOCOL_UDP,
+					&error);
+	g_assert(error == NULL);
+
+	saddr = g_socket_connection_get_remote_address(cm->con.conn, &error);
+	if (saddr == NULL) {
+		g_printerr("Failed to get remote address for udp setup: %s\n",
+			   error->message);
+		return;
+	}
+
+	if (!g_socket_connect(cm->con.udp.sock, saddr, NULL, &error)) {
+		g_object_unref(saddr);
+		g_object_unref(cm->con.udp.sock);
+		cm->con.udp.sock = NULL;
+		return;
+	}
+	g_object_unref(saddr);
+
+	cm->con.udp.source = g_socket_create_source(cm->con.udp.sock,
+						    G_IO_IN, NULL);
+	g_source_set_callback(cm->con.udp.source,
+			      (GSourceFunc) read_udp_socket, cm, NULL);
+	g_source_attach(cm->con.udp.source, NULL);
+
+	do_udp_ping(cm);
+}
+
 static void
 connection_ready(GObject *source_object, GAsyncResult *res, gpointer user_data)
 {
@@ -48,7 +161,6 @@ connection_ready(GObject *source_object, GAsyncResult *res, gpointer user_data)
 	con->source = g_pollable_input_stream_create_source(con->input, NULL);
 	g_source_set_callback(con->source, (GSourceFunc) read_cb, cm, NULL);
 	g_source_attach(con->source, NULL);
-	g_source_unref(con->source);
 
 	cmumble_protocol_init(cm);
 }
diff --git a/src/connection.h b/src/connection.h
index 508a71c..7696d07 100644
--- a/src/connection.h
+++ b/src/connection.h
@@ -12,6 +12,12 @@ struct cmumble_connection {
 	GOutputStream *output;
 
 	GSource *source;
+
+	struct {
+		GSocket *sock;
+		gboolean connected;
+		GSource *source;
+	} udp;
 };
 
 struct cmumlbe;
@@ -23,4 +29,11 @@ cmumble_connection_init(struct cmumlbe *cm,
 int
 cmumble_connection_fini(struct cmumlbe *cm);
 
+void
+cmumble_connection_udp_init(struct cmumlbe *cm);
+
+void
+cmumble_connection_send_udp_data(struct cmumlbe *cm,
+				 guint8 *data, gsize len);
+
 #endif /* _CONNECTION_H_ */
diff --git a/src/crypt.c b/src/crypt.c
new file mode 100644
index 0000000..f51ceb8
--- /dev/null
+++ b/src/crypt.c
@@ -0,0 +1,319 @@
+/* Copyright (C) 2009-2011, Martin Johansson <martin@fatbob.nu>
+   Copyright (C) 2005-2011, Thorvald Natvig <thorvald@natvig.com>
+
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
+
+   - Redistributions of source code must retain the above copyright notice,
+     this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of the Developers nor the names of its contributors may
+     be used to endorse or promote products derived from this software without
+     specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+   ``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 FOUNDATION 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.
+*/
+
+/*
+ * This code implements OCB-AES128.
+ * In the US, OCB is covered by patents. The inventor has given a license
+ * to all programs distributed under the GPL.
+ * uMurmur is BSD (revised) licensed, meaning you can use the code in a
+ * closed-source program. If you do, you'll have to either replace
+ * OCB with something else or get yourself a license.
+ */
+
+#include <string.h>
+#include <arpa/inet.h>
+#include "crypt.h"
+
+static void CryptState_ocb_encrypt(cryptState_t *cs, const unsigned char *plain, unsigned char *encrypted, unsigned int len, const unsigned char *nonce, unsigned char *tag);
+static void CryptState_ocb_decrypt(cryptState_t *cs, const unsigned char *encrypted, unsigned char *plain, unsigned int len, const unsigned char *nonce, unsigned char *tag);
+
+
+void CryptState_init(cryptState_t *cs)
+{
+	memset(cs->decrypt_history, 0, 0xff);
+	memset(cs->raw_key, 0, AES_BLOCK_SIZE);
+	memset(cs->encrypt_iv, 0, AES_BLOCK_SIZE);
+	memset(cs->decrypt_iv, 0, AES_BLOCK_SIZE);
+	cs->bInit = false;
+	cs->uiGood = cs->uiLate = cs->uiLost = cs->uiResync = 0;
+	cs->uiRemoteGood = cs->uiRemoteLate = cs->uiRemoteLost = cs->uiRemoteResync = 0;
+	/*
+	Timer_init(&cs->tLastGood);
+	Timer_init(&cs->tLastRequest);
+	*/
+}
+
+void CryptState_setKey(cryptState_t *cs, const unsigned char *rkey, const unsigned char *eiv, const unsigned char *div)
+{
+	memcpy(cs->raw_key, rkey, AES_BLOCK_SIZE);
+	memcpy(cs->encrypt_iv, eiv, AES_BLOCK_SIZE);
+	memcpy(cs->decrypt_iv, div, AES_BLOCK_SIZE);
+
+	AES_set_encrypt_key(cs->raw_key, 128, &cs->encrypt_key);
+	AES_set_decrypt_key(cs->raw_key, 128, &cs->decrypt_key);
+
+	cs->bInit = true;
+}
+
+void CryptState_setDecryptIV(cryptState_t *cs, const unsigned char *iv)
+{
+	memcpy(cs->decrypt_iv, iv, AES_BLOCK_SIZE);
+}
+
+void CryptState_encrypt(cryptState_t *cs, const unsigned char *source, unsigned char *dst, unsigned int plain_length)
+{
+	unsigned char tag[AES_BLOCK_SIZE];
+	int i;
+	// First, increase our IV.
+	for (i = 0; i < AES_BLOCK_SIZE; i++)
+		if (++cs->encrypt_iv[i])
+			break;
+
+	CryptState_ocb_encrypt(cs, source, dst+4, plain_length, cs->encrypt_iv, tag);
+
+	dst[0] = cs->encrypt_iv[0];
+	dst[1] = tag[0];
+	dst[2] = tag[1];
+	dst[3] = tag[2];
+}
+
+bool_t CryptState_decrypt(cryptState_t *cs, const unsigned char *source, unsigned char *dst, unsigned int crypted_length)
+{
+	if (crypted_length < 4)
+		return false;
+
+	unsigned int plain_length = crypted_length - 4;
+
+	unsigned char saveiv[AES_BLOCK_SIZE];
+	unsigned char ivbyte = source[0];
+	bool_t restore = false;
+	unsigned char tag[AES_BLOCK_SIZE];
+
+	int lost = 0;
+	int late = 0;
+
+	memcpy(saveiv, cs->decrypt_iv, AES_BLOCK_SIZE);
+
+	if (((cs->decrypt_iv[0] + 1) & 0xFF) == ivbyte) {
+		// In order as expected.
+		if (ivbyte > cs->decrypt_iv[0]) {
+			cs->decrypt_iv[0] = ivbyte;
+		} else if (ivbyte < cs->decrypt_iv[0]) {
+			int i;
+			cs->decrypt_iv[0] = ivbyte;
+			for (i = 1; i < AES_BLOCK_SIZE; i++)
+				if (++cs->decrypt_iv[i])
+					break;
+		} else {
+			return false;
+		}
+	} else {
+		// This is either out of order or a repeat.
+
+		int diff = ivbyte - cs->decrypt_iv[0];
+		if (diff > 128)
+			diff = diff-256;
+		else if (diff < -128)
+			diff = diff+256;
+
+		if ((ivbyte < cs->decrypt_iv[0]) && (diff > -30) && (diff < 0)) {
+			// Late packet, but no wraparound.
+			late = 1;
+			lost = -1;
+			cs->decrypt_iv[0] = ivbyte;
+			restore = true;
+		} else if ((ivbyte > cs->decrypt_iv[0]) && (diff > -30) && (diff < 0)) {
+			int i;
+			// Last was 0x02, here comes 0xff from last round
+			late = 1;
+			lost = -1;
+			cs->decrypt_iv[0] = ivbyte;
+			for (i = 1; i < AES_BLOCK_SIZE; i++)
+				if (cs->decrypt_iv[i]--)
+					break;
+			restore = true;
+		} else if ((ivbyte > cs->decrypt_iv[0]) && (diff > 0)) {
+			// Lost a few packets, but beyond that we're good.
+			lost = ivbyte - cs->decrypt_iv[0] - 1;
+			cs->decrypt_iv[0] = ivbyte;
+		} else if ((ivbyte < cs->decrypt_iv[0]) && (diff > 0)) {
+			int i;
+			// Lost a few packets, and wrapped around
+			lost = 256 - cs->decrypt_iv[0] + ivbyte - 1;
+			cs->decrypt_iv[0] = ivbyte;
+			for (i = 1; i < AES_BLOCK_SIZE; i++)
+				if (++cs->decrypt_iv[i])
+					break;
+		} else {
+			return false;
+		}
+
+		if (cs->decrypt_history[cs->decrypt_iv[0]] == cs->decrypt_iv[1]) {
+			memcpy(cs->decrypt_iv, saveiv, AES_BLOCK_SIZE);
+			return false;
+		}
+	}
+
+	CryptState_ocb_decrypt(cs, source+4, dst, plain_length, cs->decrypt_iv, tag);
+
+	if (memcmp(tag, source+1, 3) != 0) {
+		memcpy(cs->decrypt_iv, saveiv, AES_BLOCK_SIZE);		
+		return false;
+	}
+	cs->decrypt_history[cs->decrypt_iv[0]] = cs->decrypt_iv[1];
+
+	if (restore)
+		memcpy(cs->decrypt_iv, saveiv, AES_BLOCK_SIZE);
+
+	cs->uiGood++;
+	cs->uiLate += late;
+	cs->uiLost += lost;
+
+	//Timer_restart(&cs->tLastGood);
+	return true;
+}
+
+#if defined(__LP64__)
+#define BLOCKSIZE 2
+#define SHIFTBITS 63
+typedef uint64_t subblock;
+
+#if __BYTE_ORDER == __BIG_ENDIAN
+#define SWAPPED(x) (x)
+#else
+#ifdef __x86_64__
+#define SWAPPED(x) ({register uint64_t __out, __in = (x); __asm__("bswap %q0" : "=r"(__out) : "0"(__in)); __out;})
+#else
+#include <byteswap.h>
+#define SWAPPED(x) bswap_64(x)
+#endif
+#endif
+
+#else
+
+#define BLOCKSIZE 4
+#define SHIFTBITS 31
+typedef uint32_t subblock;
+#define SWAPPED(x) htonl(x)
+
+#endif
+
+#define HIGHBIT (1<<SHIFTBITS);
+
+
+static void inline XOR(subblock *dst, const subblock *a, const subblock *b) {
+	int i;
+	for (i=0;i<BLOCKSIZE;i++) {
+		dst[i] = a[i] ^ b[i];
+	}
+}
+
+static void inline S2(subblock *block) {
+	subblock carry = SWAPPED(block[0]) >> SHIFTBITS;
+	int i;
+	for (i=0;i<BLOCKSIZE-1;i++)
+		block[i] = SWAPPED((SWAPPED(block[i]) << 1) | (SWAPPED(block[i+1]) >> SHIFTBITS));
+	block[BLOCKSIZE-1] = SWAPPED((SWAPPED(block[BLOCKSIZE-1]) << 1) ^(carry * 0x87));
+}
+
+static void inline S3(subblock *block) {
+	subblock carry = SWAPPED(block[0]) >> SHIFTBITS;
+	int i;
+	for (i=0;i<BLOCKSIZE-1;i++)
+		block[i] ^= SWAPPED((SWAPPED(block[i]) << 1) | (SWAPPED(block[i+1]) >> SHIFTBITS));
+	block[BLOCKSIZE-1] ^= SWAPPED((SWAPPED(block[BLOCKSIZE-1]) << 1) ^(carry * 0x87));
+}
+
+static void inline ZERO(subblock *block) {
+	int i;
+	for (i=0;i<BLOCKSIZE;i++)
+		block[i]=0;
+}
+
+#define AESencrypt(src, dst, cryptstate) AES_encrypt((unsigned char *)(src), (unsigned char *)(dst), &(cryptstate)->encrypt_key);
+#define AESdecrypt(src, dst, cryptstate) AES_decrypt((unsigned char *)(src), (unsigned char *)(dst), &(cryptstate)->decrypt_key);
+
+void CryptState_ocb_encrypt(cryptState_t *cs, const unsigned char *plain, unsigned char *encrypted, unsigned int len, const unsigned char *nonce, unsigned char *tag) {
+	subblock checksum[BLOCKSIZE], delta[BLOCKSIZE], tmp[BLOCKSIZE], pad[BLOCKSIZE];
+
+	// Initialize
+	AESencrypt(nonce, delta, cs);
+	ZERO(checksum);
+
+	while (len > AES_BLOCK_SIZE) {
+		S2(delta);
+		XOR(tmp, delta, (const subblock *)(plain));
+		AESencrypt(tmp, tmp, cs);
+		XOR((subblock *)(encrypted), delta, tmp);
+		XOR(checksum, checksum, (subblock *)(plain));
+		len -= AES_BLOCK_SIZE;
+		plain += AES_BLOCK_SIZE;
+		encrypted += AES_BLOCK_SIZE;
+	}
+
+	S2(delta);
+	ZERO(tmp);
+	tmp[BLOCKSIZE - 1] = SWAPPED(len * 8);
+	XOR(tmp, tmp, delta);
+	AESencrypt(tmp, pad, cs);
+	memcpy(tmp, plain, len);
+	memcpy((unsigned char *)tmp + len, (unsigned char *)pad + len, AES_BLOCK_SIZE - len);
+	XOR(checksum, checksum, tmp);
+	XOR(tmp, pad, tmp);
+	memcpy(encrypted, tmp, len);
+
+	S3(delta);
+	XOR(tmp, delta, checksum);
+	AESencrypt(tmp, tag, cs);
+}
+
+void CryptState_ocb_decrypt(cryptState_t *cs, const unsigned char *encrypted, unsigned char *plain, unsigned int len, const unsigned char *nonce, unsigned char *tag) {
+	subblock checksum[BLOCKSIZE], delta[BLOCKSIZE], tmp[BLOCKSIZE], pad[BLOCKSIZE];
+	// Initialize
+	AESencrypt(nonce, delta, cs);
+	ZERO(checksum);
+
+	while (len > AES_BLOCK_SIZE) {
+		S2(delta);
+		XOR(tmp, delta, (const subblock *)(encrypted));
+		AESdecrypt(tmp, tmp, cs);
+		XOR((subblock *)(plain), delta, tmp);
+		XOR(checksum, checksum, (const subblock *)(plain));
+		len -= AES_BLOCK_SIZE;
+		plain += AES_BLOCK_SIZE;
+		encrypted += AES_BLOCK_SIZE;
+	}
+
+	S2(delta);
+	ZERO(tmp);
+	tmp[BLOCKSIZE - 1] = SWAPPED(len * 8);
+	XOR(tmp, tmp, delta);
+	AESencrypt(tmp, pad, cs);
+	memset(tmp, 0, AES_BLOCK_SIZE);
+	memcpy(tmp, encrypted, len);
+	XOR(tmp, tmp, pad);
+	XOR(checksum, checksum, tmp);
+	memcpy(plain, tmp, len);
+
+	S3(delta);
+	XOR(tmp, delta, checksum);
+	AESencrypt(tmp, tag, cs);
+}
diff --git a/src/crypt.h b/src/crypt.h
new file mode 100644
index 0000000..30c0298
--- /dev/null
+++ b/src/crypt.h
@@ -0,0 +1,80 @@
+/* Copyright (C) 2009-2011, Martin Johansson <martin@fatbob.nu>
+   Copyright (C) 2005-2011, Thorvald Natvig <thorvald@natvig.com>
+
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
+
+   - Redistributions of source code must retain the above copyright notice,
+     this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of the Developers nor the names of its contributors may
+     be used to endorse or promote products derived from this software without
+     specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+   ``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 FOUNDATION 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.
+*/
+#ifndef CRYPTSTATE_H_34564356
+#define CRYPTSTATE_H_34564356
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <openssl/rand.h>
+#include <openssl/aes.h>
+
+#include <stdint.h>
+
+#include <glib.h>
+#define bool_t gboolean
+#define false FALSE
+#define true TRUE
+
+typedef struct CryptState {
+	uint8_t raw_key[AES_BLOCK_SIZE];
+	uint8_t encrypt_iv[AES_BLOCK_SIZE];
+	uint8_t decrypt_iv[AES_BLOCK_SIZE];
+	uint8_t decrypt_history[0x100];
+	
+	unsigned int uiGood;
+	unsigned int uiLate;
+	unsigned int uiLost;
+	unsigned int uiResync;
+	
+	unsigned int uiRemoteGood;
+	unsigned int uiRemoteLate;
+	unsigned int uiRemoteLost;
+	unsigned int uiRemoteResync;
+	AES_KEY	encrypt_key;
+	AES_KEY decrypt_key;
+
+	//etimer_t tLastGood;
+	//etimer_t tLastRequest;
+	bool_t bInit;	
+} cryptState_t;
+
+void CryptState_init(cryptState_t *cs);
+bool_t CryptState_isValid(cryptState_t *cs);
+void CryptState_setKey(cryptState_t *cs, const unsigned char *rkey, const unsigned char *eiv, const unsigned char *div);
+void CryptState_setDecryptIV(cryptState_t *cs, const unsigned char *iv);
+
+bool_t CryptState_decrypt(cryptState_t *cs, const unsigned char *source, unsigned char *dst, unsigned int crypted_length);
+void CryptState_encrypt(cryptState_t *cs, const unsigned char *source, unsigned char *dst, unsigned int plain_length);
+
+
+#endif