2 files changed, 34 insertions, 28 deletions

diff --git a/src/fastd.c b/src/fastd.c
index 66e8718..e7df62d 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -600,6 +600,29 @@ void fastd_send_handshake(fastd_context_t *ctx, const fastd_socket_t *sock, cons
 	fastd_send_type(ctx, sock, local_addr, remote_addr, PACKET_HANDSHAKE, buffer);
 }
 
+static inline void handle_forward(fastd_context_t *ctx, fastd_peer_t *peer, fastd_buffer_t *buffer) {
+	const fastd_eth_addr_t *dest_addr = fastd_get_dest_address(ctx, *buffer);
+
+	if (fastd_eth_addr_is_unicast(dest_addr)) {
+		fastd_peer_t *dest_peer = fastd_peer_find_by_eth_addr(ctx, dest_addr);
+
+		if (!dest_peer || dest_peer == peer || !fastd_peer_is_established(dest_peer))
+			return;
+
+		ctx->conf->protocol->send(ctx, dest_peer, *buffer);
+		*buffer = FASTD_BUFFER_NULL;
+	}
+	else {
+		fastd_peer_t *dest_peer;
+		for (dest_peer = ctx->peers; dest_peer; dest_peer = dest_peer->next) {
+			if (dest_peer == peer || !fastd_peer_is_established(dest_peer))
+				continue;
+
+			ctx->conf->protocol->send(ctx, dest_peer, fastd_buffer_dup(ctx, *buffer, methods_min_encrypt_head_space(ctx), methods_min_encrypt_tail_space(ctx)));
+		}
+	}
+}
+
 void fastd_handle_receive(fastd_context_t *ctx, fastd_peer_t *peer, fastd_buffer_t buffer) {
 	if (ctx->conf->mode == MODE_TAP) {
 		if (buffer.len < ETH_HLEN) {
@@ -617,35 +640,10 @@ void fastd_handle_receive(fastd_context_t *ctx, fastd_peer_t *peer, fastd_buffer
 	if (write(ctx->tunfd, buffer.data, buffer.len) < 0)
 		pr_warn_errno(ctx, "write");
 
-	if (ctx->conf->mode == MODE_TAP && ctx->conf->forward) {
-		const fastd_eth_addr_t *dest_addr = fastd_get_dest_address(ctx, buffer);
-
-		if (fastd_eth_addr_is_unicast(dest_addr)) {
-			fastd_peer_t *dest_peer = fastd_peer_find_by_eth_addr(ctx, dest_addr);
+	if (ctx->conf->mode == MODE_TAP && ctx->conf->forward)
+		handle_forward(ctx, peer, &buffer);
 
-			if (dest_peer && dest_peer != peer && fastd_peer_is_established(dest_peer)) {
-				ctx->conf->protocol->send(ctx, dest_peer, buffer);
-			}
-			else {
-				fastd_buffer_free(buffer);
-			}
-		}
-		else {
-			fastd_peer_t *dest_peer;
-			for (dest_peer = ctx->peers; dest_peer; dest_peer = dest_peer->next) {
-				if (dest_peer != peer && fastd_peer_is_established(dest_peer)) {
-					fastd_buffer_t send_buffer = fastd_buffer_alloc(ctx, buffer.len, methods_min_encrypt_head_space(ctx), methods_min_encrypt_tail_space(ctx));
-					memcpy(send_buffer.data, buffer.data, buffer.len);
-					ctx->conf->protocol->send(ctx, dest_peer, send_buffer);
-				}
-			}
-
-			fastd_buffer_free(buffer);
-		}
-	}
-	else {
-		fastd_buffer_free(buffer);
-	}
+	fastd_buffer_free(buffer);
 }
 
 static inline void on_up(fastd_context_t *ctx) {
diff --git a/src/fastd.h b/src/fastd.h
index e16cbb8..2a4e9ac 100644
--- a/src/fastd.h
+++ b/src/fastd.h
@@ -378,6 +378,8 @@ static inline size_t alignto(size_t l, size_t a) {
 	return ((l+a-1)/a)*a;
 }
 
+#define FASTD_BUFFER_NULL ((fastd_buffer_t){})
+
 static inline fastd_buffer_t fastd_buffer_alloc(const fastd_context_t *ctx, size_t len, size_t head_space, size_t tail_space) {
 	size_t base_len = head_space+len+tail_space;
 	void *ptr;
@@ -387,6 +389,12 @@ static inline fastd_buffer_t fastd_buffer_alloc(const fastd_context_t *ctx, size
 	return (fastd_buffer_t){ .base = ptr, .base_len = base_len, .data = ptr+head_space, .len = len };
 }
 
+static inline fastd_buffer_t fastd_buffer_dup(const fastd_context_t *ctx, fastd_buffer_t buffer, size_t head_space, size_t tail_space) {
+	fastd_buffer_t new_buffer = fastd_buffer_alloc(ctx, buffer.len, head_space, tail_space);
+	memcpy(new_buffer.data, buffer.data, buffer.len);
+	return new_buffer;
+}
+
 static inline void fastd_buffer_free(fastd_buffer_t buffer) {
 	free(buffer.base);
 }