Make simple handshake work

8 files changed, 98 insertions, 25 deletions

diff --git a/src/Makefile b/src/Makefile
index a10ef63..e36757e 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -10,4 +10,4 @@ task.o : task.c task.h queue.h
 queue.o : queue.c queue.h
 
 clean :
-	rm -f fastd fastd.o method_null.o task.o
+	rm -f fastd fastd.o handshake.o method_null.o task.o queue.o
diff --git a/src/fastd.c b/src/fastd.c
index b5036b4..9798496 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -97,10 +97,12 @@ static void configure(fastd_context *ctx, fastd_config *conf) {
 	conf->peers->port = htons(1337);
 
 	ctx->peers = NULL;
-	fastd_peer **current_peer = &ctx->peers;
+}
 
+static void init_peers(fastd_context *ctx) {
+	fastd_peer **current_peer = &ctx->peers;
 	fastd_peer_config *peer_conf;
-	for (peer_conf = conf->peers; peer_conf; peer_conf = peer_conf->next) {
+	for (peer_conf = ctx->conf->peers; peer_conf; peer_conf = peer_conf->next) {
 		*current_peer = malloc(sizeof(fastd_peer));
 		(*current_peer)->next = NULL;
 		(*current_peer)->config = peer_conf;
@@ -110,6 +112,8 @@ static void configure(fastd_context *ctx, fastd_config *conf) {
 		(*current_peer)->last_req_id = 0;
 		(*current_peer)->addresses = NULL;
 
+		fastd_task_schedule_handshake(ctx, *current_peer, 0);
+
 		current_peer = &(*current_peer)->next;
 	}
 }
@@ -173,6 +177,16 @@ static void handle_tasks(fastd_context *ctx) {
 			fastd_buffer_free(task->handle_recv.buffer);
 			break;
 
+		case TASK_HANDSHAKE:
+			if (task->handshake.peer->state != STATE_WAIT)
+				break;
+
+			pr_debug(ctx, "Sending handshake...");
+			fastd_handshake_send(ctx, task->handshake.peer);
+
+			fastd_task_schedule_handshake(ctx, task->handshake.peer, 20000);
+			break;
+
 		default:
 			exit_bug(ctx, "invalid task type");
 		}
@@ -208,9 +222,15 @@ static void handle_input(fastd_context *ctx) {
 			 dest_addr[0], dest_addr[1], dest_addr[2], dest_addr[3], dest_addr[4], dest_addr[5]);
 
 		// TODO find correct peer
+		fastd_peer *peer = ctx->peers;
 
-		buffer.len = len;
-		ctx->conf->method->method_send(ctx, ctx->peers, buffer);
+		if (peer->state == STATE_ESTABLISHED) {
+			buffer.len = len;
+			ctx->conf->method->method_send(ctx, peer, buffer);
+		}
+		else {
+			fastd_buffer_free(buffer);
+		}
 	}
 	if (fds[1].revents & POLLIN) {
 		size_t max_len = ctx->conf->method->method_max_packet_size(ctx);
@@ -236,17 +256,22 @@ static void handle_input(fastd_context *ctx) {
 		if (len < 0)
 			pr_warn(ctx, "recvfrom: %s", strerror(errno));
 
-		// TODO get peer
+		// TODO get correct peer
+		fastd_peer *peer = ctx->peers;
+
 
 		switch (packet_type) {
 		case 0:
 			buffer.len = len - 1;
-			ctx->conf->method->method_handle_recv(ctx, NULL, buffer);
+			ctx->conf->method->method_handle_recv(ctx, peer, buffer);
 			break;
 
-		default:
-			fastd_handshake_handle(ctx, NULL, packet_type, buffer);
+		case 1:
+			fastd_handshake_handle(ctx, peer, buffer);
 			break;
+
+		default:
+			fastd_buffer_free(buffer);
 		}
 	}
 }
@@ -260,6 +285,8 @@ int main() {
 	configure(&ctx, &conf);
 	ctx.conf = &conf;
 
+	init_peers(&ctx);
+
 	init_tuntap(&ctx);
 	init_socket(&ctx);
 
diff --git a/src/fastd.h b/src/fastd.h
index db0190d..2d372a2 100644
--- a/src/fastd.h
+++ b/src/fastd.h
@@ -70,7 +70,6 @@ typedef struct _fastd_peer_config {
 
 typedef enum _fastd_peer_state {
 	STATE_WAIT,
-	STATE_HANDSHAKE_SENT,
 	STATE_ESTABLISHED,
 } fastd_peer_state;
 
@@ -95,10 +94,10 @@ typedef struct _fastd_method {
 
 	size_t (*method_max_packet_size)(fastd_context *ctx);
 
-	void (*method_init)(fastd_context *ctx, const fastd_peer *peer);
+	void (*method_init)(fastd_context *ctx, fastd_peer *peer);
 
-	void (*method_handle_recv)(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer);
-	void (*method_send)(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer);
+	void (*method_handle_recv)(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
+	void (*method_send)(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
 } fastd_method;
 
 typedef struct _fastd_config {
diff --git a/src/handshake.c b/src/handshake.c
index 5f4645d..350dc5e 100644
--- a/src/handshake.c
+++ b/src/handshake.c
@@ -32,7 +32,7 @@
 #include <string.h>
 
 
-void fastd_handshake_send(fastd_context *ctx, const fastd_peer *peer) {
+void fastd_handshake_send(fastd_context *ctx, fastd_peer *peer) {
 	size_t method_len = strlen(ctx->conf->method->name);
 	size_t len = sizeof(fastd_packet_request)+method_len;
 	fastd_buffer buffer = fastd_buffer_alloc(len, 0);
@@ -40,7 +40,7 @@ void fastd_handshake_send(fastd_context *ctx, const fastd_peer *peer) {
 
 	request->reply = 0;
 	request->cp = 0;
-	request->req_id = 0;
+	request->req_id = ++peer->last_req_id;
 	request->rsv = 0;
 	request->flags = 0;
 	request->proto = ctx->conf->protocol;
@@ -50,10 +50,7 @@ void fastd_handshake_send(fastd_context *ctx, const fastd_peer *peer) {
 	fastd_task_put_send_handshake(ctx, peer, buffer);
 }
 
-void fastd_handshake_handle(fastd_context *ctx, const fastd_peer *peer, uint8_t packet_type, fastd_buffer buffer) {
-	if (packet_type != 1)
-		goto end_free; // TODO
-
+void fastd_handshake_handle(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	if (buffer.len < sizeof(fastd_packet_any))
 		goto end_free;
 
@@ -87,6 +84,30 @@ void fastd_handshake_handle(fastd_context *ctx, const fastd_peer *peer, uint8_t
 
 		fastd_task_put_send_handshake(ctx, peer, reply_buffer);
 	}
+	else if (packet->any.reply) {
+		if (buffer.len < sizeof(fastd_packet_reply))
+			goto end_free;
+
+		if (!packet->reply.cp) {
+			if (packet->reply.req_id != peer->last_req_id)
+				goto end_free;
+		}
+		else {
+			goto end_free; // TODO
+		}
+
+		switch (packet->reply.reply_code) {
+		case REPLY_SUCCESS:
+			pr_info(ctx, "Handshake successful.");
+			pr_info(ctx, "Connection established.");
+			peer->state = STATE_ESTABLISHED;
+			ctx->conf->method->method_init(ctx, peer);
+			break;
+
+		default:
+			pr_warn(ctx, "Handshake failed with code %i.", packet->reply.reply_code);
+		}
+	}
 
  end_free:
 	fastd_buffer_free(buffer);
diff --git a/src/handshake.h b/src/handshake.h
index 503d558..3457a0e 100644
--- a/src/handshake.h
+++ b/src/handshake.h
@@ -30,7 +30,7 @@
 
 #include "fastd.h"
 
-void fastd_handshake_send(fastd_context *ctx, const fastd_peer *peer);
-void fastd_handshake_handle(fastd_context *ctx, const fastd_peer *peer, uint8_t packet_type, fastd_buffer buffer);
+void fastd_handshake_send(fastd_context *ctx, fastd_peer *peer);
+void fastd_handshake_handle(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
 
 #endif /* _FASTD_HANDSHAKE_H_ */
diff --git a/src/method_null.c b/src/method_null.c
index 34d1cce..75e74de 100644
--- a/src/method_null.c
+++ b/src/method_null.c
@@ -32,16 +32,24 @@ static size_t null_max_packet_size(fastd_context *ctx) {
 	return fastd_max_packet_size(ctx);
 }
 
-static void null_init(fastd_context *ctx, const fastd_peer *peer) {
+static void null_init(fastd_context *ctx, fastd_peer *peer) {
 	fastd_buffer buffer = { .base = NULL, .len = 0, .free = NULL, .free_p = NULL };
 	fastd_task_put_send(ctx, peer, buffer);
 }
 
-static void null_handle_recv(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer) {
-	fastd_task_put_handle_recv(ctx, peer, buffer);
+static void null_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
+	if (peer->state != STATE_ESTABLISHED) {
+		pr_info(ctx, "Connection established.");
+		peer->state = STATE_ESTABLISHED;
+	}
+	
+	if (buffer.len)
+		fastd_task_put_handle_recv(ctx, peer, buffer);
+	else
+		fastd_buffer_free(buffer);
 }
 
-static void null_send(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer) {
+static void null_send(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	fastd_task_put_send(ctx, peer, buffer);
 }
 
diff --git a/src/task.c b/src/task.c
index 0367cb4..2de03b1 100644
--- a/src/task.c
+++ b/src/task.c
@@ -61,3 +61,12 @@ void fastd_task_put_handle_recv(fastd_context *ctx, const fastd_peer *peer, fast
 
 	fastd_queue_put(&ctx->task_queue, task, 0);
 }
+
+void fastd_task_schedule_handshake(fastd_context *ctx, fastd_peer *peer, int timeout) {
+	fastd_task_handshake *task = malloc(sizeof(fastd_task_handshake));
+
+	task->type = TASK_HANDSHAKE;
+	task->peer = peer;
+
+	fastd_queue_put(&ctx->task_queue, task, timeout);
+}
diff --git a/src/task.h b/src/task.h
index 731510b..dcf9cc0 100644
--- a/src/task.h
+++ b/src/task.h
@@ -36,6 +36,7 @@
 typedef enum _fastd_task_type {
 	TASK_SEND,
 	TASK_HANDLE_RECV,
+	TASK_HANDSHAKE,
 } fastd_task_type;
 
 typedef struct _fastd_task_send {
@@ -52,10 +53,16 @@ typedef struct _fastd_task_handle_recv {
 	fastd_buffer buffer;
 } fastd_task_handle_recv;
 
+typedef struct _fastd_task_handshake {
+	fastd_task_type type;
+	fastd_peer *peer;
+} fastd_task_handshake;
+
 typedef union _fastd_task {
 	fastd_task_type type;
 	fastd_task_send send;
 	fastd_task_handle_recv handle_recv;
+	fastd_task_handshake handshake;
 } fastd_task;
 
 
@@ -69,4 +76,6 @@ void fastd_task_put_send_handshake(fastd_context *ctx, const fastd_peer *peer, f
 void fastd_task_put_send(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer);
 void fastd_task_put_handle_recv(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer);
 
+void fastd_task_schedule_handshake(fastd_context *ctx, fastd_peer *peer, int timeout);
+
 #endif /* _FASTD_TASK_H_ */