Implement encrypted send/receive

4 files changed, 83 insertions, 3 deletions

diff --git a/src/fastd.c b/src/fastd.c
index 40f0046..9194301 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -198,7 +198,7 @@ static void handle_tasks(fastd_context *ctx) {
 
 static void handle_tun(fastd_context *ctx) {
 	size_t max_len = fastd_max_packet_size(ctx);
-	fastd_buffer buffer = fastd_buffer_alloc(max_len, 0, 0);
+	fastd_buffer buffer = fastd_buffer_alloc(max_len, ctx->conf->protocol->min_encrypt_head_space(ctx), 0);
 
 	ssize_t len = read(ctx->tunfd, buffer.data, max_len);
 	if (len < 0)
@@ -229,7 +229,7 @@ static void handle_tun(fastd_context *ctx) {
 	if (peer == NULL) {
 		for (peer = ctx->peers; peer; peer = peer->next) {
 			if (peer->state == STATE_ESTABLISHED) {
-				fastd_buffer send_buffer = fastd_buffer_alloc(len, 0, 0);
+				fastd_buffer send_buffer = fastd_buffer_alloc(len, ctx->conf->protocol->min_encrypt_head_space(ctx), 0);
 				memcpy(send_buffer.data, buffer.data, len);
 				ctx->conf->protocol->send(ctx, peer, send_buffer);
 			}
@@ -241,7 +241,7 @@ static void handle_tun(fastd_context *ctx) {
 
 static void handle_socket(fastd_context *ctx, int sockfd) {
 	size_t max_len = ctx->conf->protocol->max_packet_size(ctx);
-	fastd_buffer buffer = fastd_buffer_alloc(max_len, 0, 0);
+	fastd_buffer buffer = fastd_buffer_alloc(max_len, ctx->conf->protocol->min_decrypt_head_space(ctx), 0);
 
 	uint8_t packet_type;
 
diff --git a/src/fastd.h b/src/fastd.h
index 8bd04ec..873da61 100644
--- a/src/fastd.h
+++ b/src/fastd.h
@@ -62,6 +62,8 @@ struct _fastd_protocol {
 	void (*init)(fastd_context *ctx);
 
 	size_t (*max_packet_size)(fastd_context *ctx);
+	size_t (*min_encrypt_head_space)(fastd_context *ctx);
+	size_t (*min_decrypt_head_space)(fastd_context *ctx);
 
 	char* (*peer_str)(const fastd_context *ctx, const fastd_peer *peer);
 
@@ -161,6 +163,22 @@ static inline void fastd_buffer_free(fastd_buffer buffer) {
 	free(buffer.base);
 }
 
+static inline void fastd_buffer_pull_head(fastd_buffer *buffer, size_t len) {
+	buffer->data -= len;
+	buffer->len += len;
+
+	if (buffer->data < buffer->base)
+		abort();
+}
+
+static inline void fastd_buffer_push_head(fastd_buffer *buffer, size_t len) {
+	if (buffer->len < len)
+		abort();
+
+	buffer->data += len;
+	buffer->len -= len;
+}
+
 static inline size_t fastd_max_packet_size(const fastd_context *ctx) {
 	switch (ctx->conf->mode) {
 	case MODE_TAP:
diff --git a/src/protocol_ec25519_fhmqvc_xsalsa20_poly1305.c b/src/protocol_ec25519_fhmqvc_xsalsa20_poly1305.c
index c729428..d743d9c 100644
--- a/src/protocol_ec25519_fhmqvc_xsalsa20_poly1305.c
+++ b/src/protocol_ec25519_fhmqvc_xsalsa20_poly1305.c
@@ -58,6 +58,10 @@
 #error bug: HASHBYTES != SECRETKEYBYTES
 #endif
 
+#if crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES > NONCEBYTES
+#error bug: crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES > NONCEBYTES
+#endif
+
 
 struct _fastd_protocol_context {
 	ecc_secret_key_256 secret_key;
@@ -190,6 +194,14 @@ static size_t protocol_max_packet_size(fastd_context *ctx) {
 	return (fastd_max_packet_size(ctx) - NONCEBYTES);
 }
 
+static size_t protocol_min_encrypt_head_space(fastd_context *ctx) {
+	return crypto_secretbox_xsalsa20poly1305_ZEROBYTES;
+}
+
+static size_t protocol_min_decrypt_head_space(fastd_context *ctx) {
+	return 0;
+}
+
 static char* protocol_peer_str(const fastd_context *ctx, const fastd_peer *peer) {
 	char addr_buf[INET6_ADDRSTRLEN] = "";
 	char *ret;
@@ -574,6 +586,29 @@ static void protocol_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buf
 			pr_debug(ctx, "received unexpected non-handshake packet from %P", peer);
 			goto end;
 		}
+
+		if (!is_nonce_valid(buffer.data, peer->protocol_state->receive_nonce)) {
+			pr_debug(ctx, "received packet with invalid nonce from %P", peer);
+			goto end;
+		}
+
+		uint8_t nonce[NONCEBYTES];
+		memcpy(nonce, buffer.data, NONCEBYTES);
+
+		fastd_buffer_push_head(&buffer, NONCEBYTES-crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES);
+		memset(buffer.data, 0, crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES);
+
+		fastd_buffer recv_buffer = fastd_buffer_alloc(buffer.len, 0, 0);
+
+		if (crypto_secretbox_xsalsa20poly1305_open(recv_buffer.data, buffer.data, buffer.len, nonce, peer->protocol_state->shared_session_key) != 0) {
+			pr_debug(ctx, "varification failed for packet received from %P", peer);
+			goto end;
+		}
+
+		fastd_buffer_push_head(&recv_buffer, crypto_secretbox_xsalsa20poly1305_ZEROBYTES);
+		fastd_task_put_handle_recv(ctx, peer, recv_buffer);
+
+		memcpy(peer->protocol_state->receive_nonce, nonce, NONCEBYTES);
 	}
 
  end:
@@ -581,7 +616,26 @@ static void protocol_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buf
 }
 
 static void protocol_send(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
+	if (!peer->protocol_state || peer->protocol_state->state != HANDSHAKE_STATE_ESTABLISHED) {
+		fastd_buffer_free(buffer);
+		return;
+	}
+
+	fastd_buffer_pull_head(&buffer, crypto_secretbox_xsalsa20poly1305_ZEROBYTES);
+	memset(buffer.data, 0, crypto_secretbox_xsalsa20poly1305_ZEROBYTES);
+
+	fastd_buffer send_buffer = fastd_buffer_alloc(buffer.len, NONCEBYTES-crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES, 0);
+
+	crypto_secretbox_xsalsa20poly1305(send_buffer.data, buffer.data, buffer.len, peer->protocol_state->send_nonce, peer->protocol_state->shared_session_key);
+
 	fastd_buffer_free(buffer);
+
+	fastd_buffer_pull_head(&send_buffer, NONCEBYTES-crypto_secretbox_xsalsa20poly1305_BOXZEROBYTES);
+	memcpy(send_buffer.data, peer->protocol_state->send_nonce, NONCEBYTES);
+
+	fastd_task_put_send(ctx, peer, send_buffer);
+
+	increment_nonce(peer->protocol_state->send_nonce);
 }
 
 static void protocol_free_peer_state(fastd_context *ctx, fastd_peer *peer) {
@@ -595,6 +649,8 @@ const fastd_protocol fastd_protocol_ec25519_fhmqvc_xsalsa20_poly1305 = {
 	.init = protocol_init,
 
 	.max_packet_size = protocol_max_packet_size,
+	.min_encrypt_head_space = protocol_min_encrypt_head_space,
+	.min_decrypt_head_space = protocol_min_decrypt_head_space,
 
 	.peer_str = protocol_peer_str,
 
diff --git a/src/protocol_null.c b/src/protocol_null.c
index ffe6ad1..9bbe667 100644
--- a/src/protocol_null.c
+++ b/src/protocol_null.c
@@ -43,6 +43,10 @@ static size_t protocol_max_packet_size(fastd_context *ctx) {
 	return fastd_max_packet_size(ctx);
 }
 
+static size_t protocol_min_head_space(fastd_context *ctx) {
+	return 0;
+}
+
 static char* protocol_peer_str(const fastd_context *ctx, const fastd_peer *peer) {
 	char addr_buf[INET6_ADDRSTRLEN] = "";
 	char *ret;
@@ -124,6 +128,8 @@ const fastd_protocol fastd_protocol_null = {
 	.init = protocol_init,
 
 	.max_packet_size = protocol_max_packet_size,
+	.min_encrypt_head_space = protocol_min_head_space,
+	.min_decrypt_head_space = protocol_min_head_space,
 
 	.peer_str = protocol_peer_str,