Only send packets to the right host in ethernet mode

7 files changed, 243 insertions, 59 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3af62b3..f4e3701 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,2 +1,2 @@
-add_executable(fastd fastd.c handshake.c method_null.c queue.c task.c)
+add_executable(fastd fastd.c handshake.c method_null.c peer.c queue.c task.c)
 target_link_libraries(fastd rt)
diff --git a/src/fastd.c b/src/fastd.c
index 9161239..53b8488 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -27,6 +27,7 @@
 
 #include "fastd.h"
 #include "handshake.h"
+#include "peer.h"
 #include "task.h"
 
 #include <arpa/inet.h>
@@ -234,43 +235,12 @@ static void init_peers(fastd_context *ctx) {
 	fastd_peer **current_peer = &ctx->peers;
 	fastd_peer_config *peer_conf;
 	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;
-		(*current_peer)->address = peer_conf->address;
-		(*current_peer)->port = peer_conf->port;
-		(*current_peer)->state = STATE_WAIT;
-		(*current_peer)->last_req_id = 0;
-		(*current_peer)->addresses = NULL;
-
-		fastd_task_schedule_handshake(ctx, *current_peer, 0);
+		*current_peer = fastd_peer_init(ctx, peer_conf);
 
 		current_peer = &(*current_peer)->next;
 	}
 }
 
-static void* get_source_address(const fastd_context *ctx, void *buffer) {
-	switch (ctx->conf->protocol) {
-	case PROTOCOL_ETHERNET:
-		return &((struct ethhdr*)buffer)->h_source;
-	case PROTOCOL_IP:
-		return NULL;
-	default:
-		exit_bug(ctx, "invalid protocol");
-	}
-}
-
-static void* get_dest_address(const fastd_context *ctx, void *buffer) {
-	switch (ctx->conf->protocol) {
-	case PROTOCOL_ETHERNET:
-		return &((struct ethhdr*)buffer)->h_dest;
-	case PROTOCOL_IP:
-		return NULL;
-	default:
-		exit_bug(ctx, "invalid protocol");
-	}
-}
-
 static void handle_tasks(fastd_context *ctx) {
 	fastd_task *task;
 	while ((task = fastd_task_get(ctx)) != NULL) {
@@ -303,7 +273,13 @@ static void handle_tasks(fastd_context *ctx) {
 			break;
 
 		case TASK_HANDLE_RECV:
-			// TODO Handle source address
+			if (ctx->conf->protocol == PROTOCOL_ETHERNET) {
+				const fastd_eth_addr *src_addr = fastd_get_source_address(ctx, task->handle_recv.buffer);
+
+				if (fastd_eth_addr_is_unicast(src_addr))
+					fastd_peer_add_eth_addr(ctx, task->handle_recv.peer, src_addr);
+			}
+
 			write(ctx->tunfd, task->handle_recv.buffer.data, task->handle_recv.buffer.len);
 			fastd_buffer_free(task->handle_recv.buffer);
 			break;
@@ -345,25 +321,37 @@ static void handle_input(fastd_context *ctx) {
 		if (len < 0)
 			exit_errno(ctx, "read");
 
-		/*uint8_t *src_addr = get_source_address(ctx, buffer.data);
-		uint8_t *dest_addr = get_dest_address(ctx, buffer.data);
+		fastd_peer *peer = NULL;
 
-		pr_debug(ctx, "A packet with length %u is to be sent from %02x:%02x:%02x:%02x:%02x:%02x to %02x:%02x:%02x:%02x:%02x:%02x",
-			 (unsigned)len, src_addr[0], src_addr[1], src_addr[2], src_addr[3], src_addr[4], src_addr[5],
-			 dest_addr[0], dest_addr[1], dest_addr[2], dest_addr[3], dest_addr[4], dest_addr[5]);*/
+		if (ctx->conf->protocol == PROTOCOL_ETHERNET) {
+			const fastd_eth_addr *dest_addr = fastd_get_dest_address(ctx, buffer);
+			if (fastd_eth_addr_is_unicast(dest_addr)) {
+				peer = fastd_peer_find_by_eth_addr(ctx, dest_addr);
 
-		// TODO find correct peer
+				if (peer == NULL) {
+					fastd_buffer_free(buffer);
+					return;
+				}
 
-		fastd_peer *peer;
-		for (peer = ctx->peers; peer; peer = peer->next) {
-			if (peer->state == STATE_ESTABLISHED) {
-				fastd_buffer send_buffer = fastd_buffer_alloc(len, 0, 0);
-				memcpy(send_buffer.data, buffer.data, len);
-				ctx->conf->method->method_send(ctx, peer, send_buffer);
+				if (peer->state == STATE_ESTABLISHED) {
+					ctx->conf->method->method_send(ctx, peer, buffer);
+				}
+				else {
+					fastd_buffer_free(buffer);
+				}
 			}
 		}
+		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);
+					memcpy(send_buffer.data, buffer.data, len);
+					ctx->conf->method->method_send(ctx, peer, send_buffer);
+				}
+			}
 
-		fastd_buffer_free(buffer);
+			fastd_buffer_free(buffer);
+		}
 	}
 	if (fds[1].revents & POLLIN) {
 		size_t max_len = ctx->conf->method->method_max_packet_size(ctx);
diff --git a/src/fastd.h b/src/fastd.h
index 90df868..acc5a19 100644
--- a/src/fastd.h
+++ b/src/fastd.h
@@ -73,6 +73,10 @@ typedef enum _fastd_peer_state {
 	STATE_ESTABLISHED,
 } fastd_peer_state;
 
+typedef struct _fastd_eth_addr {
+	uint8_t data[ETH_ALEN];
+} fastd_eth_addr;
+
 typedef struct _fastd_peer {
 	struct _fastd_peer *next;
 
@@ -83,10 +87,13 @@ typedef struct _fastd_peer {
 
 	fastd_peer_state state;
 	uint8_t last_req_id;
-
-	void **addresses;
 } fastd_peer;
 
+typedef struct _fastd_peer_eth_addr {
+	fastd_eth_addr addr;
+	fastd_peer *peer;
+} fastd_peer_eth_addr;
+
 typedef struct _fastd_context fastd_context;
 
 typedef struct _fastd_method {
@@ -124,6 +131,10 @@ struct _fastd_context {
 
 	int tunfd;
 	int sockfd;
+
+	size_t eth_addr_size;
+	size_t n_eth_addr;
+	fastd_peer_eth_addr *eth_addr;
 };
 
 
@@ -152,7 +163,6 @@ static inline void fastd_buffer_free(fastd_buffer buffer) {
 	free(buffer.base);
 }
 
-
 static inline size_t fastd_max_packet_size(const fastd_context *ctx) {
 	switch (ctx->conf->protocol) {
 	case PROTOCOL_ETHERNET:
diff --git a/src/peer.c b/src/peer.c
new file mode 100644
index 0000000..f050ebe
--- /dev/null
+++ b/src/peer.c
@@ -0,0 +1,139 @@
+/*
+  Copyright (c) 2012, Matthias Schiffer <mschiffer@universe-factory.net>
+  Partly based on QuickTun Copyright (c) 2010, 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 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 COPYRIGHT HOLDER 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.
+*/
+
+
+#include "peer.h"
+#include "task.h"
+
+
+const fastd_eth_addr* fastd_get_source_address(const fastd_context *ctx, fastd_buffer buffer) {
+	switch (ctx->conf->protocol) {
+	case PROTOCOL_ETHERNET:
+		return (fastd_eth_addr*)&((struct ethhdr*)buffer.data)->h_source;
+	default:
+		exit_bug(ctx, "invalid protocol");
+	}
+}
+
+const fastd_eth_addr* fastd_get_dest_address(const fastd_context *ctx, fastd_buffer buffer) {
+	switch (ctx->conf->protocol) {
+	case PROTOCOL_ETHERNET:
+		return (fastd_eth_addr*)&((struct ethhdr*)buffer.data)->h_dest;
+	default:
+		exit_bug(ctx, "invalid protocol");
+	}
+}
+
+fastd_peer* fastd_peer_init(fastd_context *ctx, fastd_peer_config *peer_conf) {
+	fastd_peer *peer = malloc(sizeof(fastd_peer));
+
+	peer->next = NULL;
+
+	peer->config = peer_conf;
+	peer->address = peer_conf->address;
+	peer->port = peer_conf->port;
+	peer->state = STATE_WAIT;
+	peer->last_req_id = 0;
+
+	fastd_task_schedule_handshake(ctx, peer, 0);
+
+	return peer;
+}
+
+void fastd_peer_free(fastd_context *ctx, fastd_peer *peer) {
+	int i, deleted = 0;
+
+	for (i = 0; i < ctx->n_eth_addr; i++) {
+		if (ctx->eth_addr[i].peer == peer) {
+			deleted++;
+		}
+		else if (deleted) {
+			ctx->eth_addr[i-deleted] = ctx->eth_addr[i];
+		}
+	}
+
+	ctx->n_eth_addr -= deleted;
+
+	free(peer);
+}
+
+static inline int fastd_eth_addr_cmp(const fastd_eth_addr *addr1, const fastd_eth_addr *addr2) {
+	return memcmp(addr1->data, addr2->data, ETH_ALEN);
+}
+
+static inline int fastd_peer_eth_addr_cmp(const fastd_peer_eth_addr *addr1, const fastd_peer_eth_addr *addr2) {
+	return fastd_eth_addr_cmp(&addr1->addr, &addr2->addr);
+}
+
+static inline fastd_peer_eth_addr* peer_get_by_addr(fastd_context *ctx, const fastd_eth_addr *addr) {
+	return bsearch(((uint8_t*)addr)-offsetof(fastd_peer_eth_addr, addr), ctx->eth_addr, ctx->n_eth_addr, sizeof(fastd_peer_eth_addr),
+		       (int (*)(const void *, const void *))fastd_peer_eth_addr_cmp);
+}
+
+void fastd_peer_add_eth_addr(fastd_context *ctx, fastd_peer *peer, const fastd_eth_addr *addr) {
+	int min = 0, max = ctx->n_eth_addr;
+
+	while (max > min) {
+		int cur = (min+max)/2;
+		int cmp = fastd_eth_addr_cmp(addr, &ctx->eth_addr[cur].addr);
+
+		if (cmp == 0) {
+			ctx->eth_addr[cur].peer = peer;
+			return; /* We're done here. */
+		}
+		else if (cmp < 0) {
+			max = cur;
+		}
+		else {
+			min = cur+1;
+		}
+	}
+
+	ctx->n_eth_addr++;
+	if (ctx->n_eth_addr > ctx->eth_addr_size) {
+		if (ctx->eth_addr_size == 0)
+			ctx->eth_addr_size = 16;
+		else
+			ctx->eth_addr_size *= 2;
+
+		ctx->eth_addr = realloc(ctx->eth_addr, ctx->eth_addr_size*sizeof(fastd_peer_eth_addr));
+	}
+
+	int i;
+	for (i = ctx->n_eth_addr-1; i > min+1; i--)
+		ctx->eth_addr[i] = ctx->eth_addr[i-1];
+	
+	ctx->eth_addr[min] = (fastd_peer_eth_addr){ *addr, peer };
+}
+
+fastd_peer *fastd_peer_find_by_eth_addr(fastd_context *ctx, const fastd_eth_addr *addr) {
+	fastd_peer_eth_addr *peer_eth_addr = peer_get_by_addr(ctx, addr);
+
+	if (peer_eth_addr)
+		return peer_eth_addr->peer;
+	else
+		return NULL;
+}
diff --git a/src/peer.h b/src/peer.h
new file mode 100644
index 0000000..e89155e
--- /dev/null
+++ b/src/peer.h
@@ -0,0 +1,47 @@
+/*
+  Copyright (c) 2012, Matthias Schiffer <mschiffer@universe-factory.net>
+  Partly based on QuickTun Copyright (c) 2010, 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 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 COPYRIGHT HOLDER 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 _FASTD_PEER_H_
+#define _FASTD_PEER_H_
+
+#include "fastd.h"
+
+
+const fastd_eth_addr* fastd_get_source_address(const fastd_context *ctx, fastd_buffer buffer);
+const fastd_eth_addr* fastd_get_dest_address(const fastd_context *ctx, fastd_buffer buffer);
+
+fastd_peer* fastd_peer_init(fastd_context *ctx, fastd_peer_config *conf);
+void fastd_peer_free(fastd_context *ctx, fastd_peer *peer);
+
+static inline int fastd_eth_addr_is_unicast(const fastd_eth_addr *addr) {
+	return ((addr->data[0] & 1) == 0);
+}
+
+void fastd_peer_add_eth_addr(fastd_context *ctx, fastd_peer *peer, const fastd_eth_addr *addr);
+fastd_peer* fastd_peer_find_by_eth_addr(fastd_context *ctx, const fastd_eth_addr *addr);
+
+#endif /* _FASTD_PEER_H_ */
diff --git a/src/task.c b/src/task.c
index 2de03b1..ed7f7cc 100644
--- a/src/task.c
+++ b/src/task.c
@@ -33,7 +33,7 @@ fastd_task* fastd_task_get(fastd_context *ctx) {
 	return fastd_queue_get(&ctx->task_queue);
 }
 
-static void fastd_task_put_send_type(fastd_context *ctx, const fastd_peer *peer, uint8_t packet_type, fastd_buffer buffer) {
+static void fastd_task_put_send_type(fastd_context *ctx, fastd_peer *peer, uint8_t packet_type, fastd_buffer buffer) {
 	fastd_task_send *task = malloc(sizeof(fastd_task_send));
 
 	task->type = TASK_SEND;
@@ -44,15 +44,15 @@ static void fastd_task_put_send_type(fastd_context *ctx, const fastd_peer *peer,
 	fastd_queue_put(&ctx->task_queue, task, 0);
 }
 
-void fastd_task_put_send_handshake(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer) {
+void fastd_task_put_send_handshake(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	fastd_task_put_send_type(ctx, peer, 1, buffer);
 }
 
-void fastd_task_put_send(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer) {
+void fastd_task_put_send(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	fastd_task_put_send_type(ctx, peer, 0, buffer);
 }
 
-void fastd_task_put_handle_recv(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer) {
+void fastd_task_put_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	fastd_task_handle_recv *task = malloc(sizeof(fastd_task_handle_recv));
 
 	task->type = TASK_HANDLE_RECV;
diff --git a/src/task.h b/src/task.h
index dcf9cc0..d569176 100644
--- a/src/task.h
+++ b/src/task.h
@@ -41,14 +41,14 @@ typedef enum _fastd_task_type {
 
 typedef struct _fastd_task_send {
 	fastd_task_type type;
-	const fastd_peer *peer;
+	fastd_peer *peer;
 	uint8_t packet_type;
 	fastd_buffer buffer;
 } fastd_task_send;
 
 typedef struct _fastd_task_handle_recv {
 	fastd_task_type type;
-	const fastd_peer *peer;
+	fastd_peer *peer;
 	uint8_t packet_type;
 	fastd_buffer buffer;
 } fastd_task_handle_recv;
@@ -71,10 +71,10 @@ static inline int fastd_task_timeout(fastd_context *ctx) {
 	return fastd_queue_timeout(&ctx->task_queue);
 }
 
-void fastd_task_put_send_handshake(fastd_context *ctx, const fastd_peer *peer, fastd_buffer buffer);
+void fastd_task_put_send_handshake(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
 
-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_put_send(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
+void fastd_task_put_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
 
 void fastd_task_schedule_handshake(fastd_context *ctx, fastd_peer *peer, int timeout);