From 0f2e170ea6edee4df0f90a3821d5c054db6bd6b7 Mon Sep 17 00:00:00 2001 From: Matthias Schiffer Date: Wed, 29 Feb 2012 22:44:40 +0100 Subject: Only send packets to the right host in ethernet mode --- src/CMakeLists.txt | 2 +- src/fastd.c | 80 +++++++++++++----------------- src/fastd.h | 16 ++++-- src/peer.c | 139 +++++++++++++++++++++++++++++++++++++++++++++++++++++ src/peer.h | 47 ++++++++++++++++++ src/task.c | 8 +-- src/task.h | 10 ++-- 7 files changed, 243 insertions(+), 59 deletions(-) create mode 100644 src/peer.c create mode 100644 src/peer.h 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 @@ -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 + Partly based on QuickTun Copyright (c) 2010, Ivo Smits . + 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 + Partly based on QuickTun Copyright (c) 2010, Ivo Smits . + 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); -- cgit v1.2.3