diff options
| -rw-r--r-- | ffd/ffd.c | 136 | ||||
| -rw-r--r-- | ffd/ffd.h | 7 | ||||
| -rw-r--r-- | ffd/tlv.c | 2 |
3 files changed, 133 insertions, 12 deletions
@@ -52,7 +52,6 @@ static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}}; static char *mesh = "bat0"; -static unsigned mtu = 1528; static int sockfd; static struct timespec now; @@ -114,6 +113,16 @@ static bool init_socket() { return true; } +static inline ffd_iface_t* get_iface(unsigned ifindex) { + ffd_iface_t *iface; + for (iface = iface_list; iface; iface = iface->next) { + if (iface->ifindex == ifindex) + return iface; + } + + return NULL; +} + static void update_netif(const char *ifname, unsigned ifindex, void *arg) { if (!use_netif(ifname)) return; @@ -128,11 +137,7 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) { if (setsockopt(sockfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) && errno != EADDRINUSE) fprintf(stderr, "warning: setsockopt: %m\n"); - ffd_iface_t *iface; - for (iface = iface_list; iface; iface = iface->next) { - if (iface->ifindex == ifindex) - break; - } + ffd_iface_t *iface = get_iface(ifindex); if (!iface) { /* new iface */ @@ -141,7 +146,7 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) { iface_list = iface; iface->seqno = 0; - iface->neighs = NULL; + iface->neigh_list = NULL; } iface->ifindex = ifindex; @@ -149,6 +154,41 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) { iface->type = netif_get_type(ifname); } +static inline ffd_neigh_t* get_neigh(const ffd_iface_t *iface, const eth_addr_t *addr) { + ffd_neigh_t *neigh; + for (neigh = iface->neigh_list; neigh; neigh = neigh->next) { + if (are_eth_addrs_equal(addr, &neigh->addr)) + return neigh; + } + + return neigh; +} + +static ffd_neigh_t* get_neigh_create(ffd_iface_t *iface, const eth_addr_t *addr) { + ffd_neigh_t *neigh = get_neigh(iface, addr); + if (!neigh) { + neigh = malloc(sizeof(ffd_neigh_t)); + neigh->next = iface->neigh_list; + iface->neigh_list = neigh; + + neigh->addr = *addr; + neigh->hello_log = 0; + neigh->hello_interval = 0; + neigh->last_seqno = 0; + neigh->last_hello = (struct timespec){}; + } + + return neigh; +} + +static void free_neighs(ffd_neigh_t *neigh) { + ffd_neigh_t *next; + for (; neigh; neigh = next) { + next = neigh->next; + free(neigh); + } +} + static void update_netifs() { ffd_iface_t *iface, **cur; @@ -163,6 +203,8 @@ static void update_netifs() { if (iface->type == IF_UNSPEC) { *cur = iface->next; + + free_neighs(iface->neigh_list); free(iface); } else { @@ -226,19 +268,93 @@ static void send_hellos() { } } +static void handle_tlv_hello(const ffd_tlv_hello_t *tlv_hello, size_t len, const eth_addr_t *addr, ffd_iface_t *iface) { + if (len < sizeof(ffd_tlv_hello_t)) { + fprintf(stderr, "warn: received short hello TLV.\n"); + return; + } + + fprintf(stderr, "debug: received hello with seqno %u.\n", ntohs(tlv_hello->seqno)); + + ffd_neigh_t *neigh = get_neigh_create(iface, addr); + + uint16_t seqno = ntohs(tlv_hello->seqno); + + if (neigh->last_hello.tv_sec) { + int16_t seqdiff = seqno - neigh->last_seqno; + if (seqdiff <= 0) { + fprintf(stderr, "debug: seqno already seen, ignorin.\n"); + return; + } + + int timediff = timespec_diff(&now, &neigh->last_hello)/10; + uint16_t seqexp = neigh->last_seqno + (timediff - neigh->hello_interval/2)/neigh->hello_interval; + + /* cast to int16_t to ensure correct handling of seqno wrapping */ + if (abs((int16_t)(seqno - seqexp)) > 16) { + fprintf(stderr, "info: neighbor was reset.\n"); + neigh->hello_log = 0; + } + else { + neigh->hello_log <<= seqdiff; + } + } + else { + fprintf(stderr, "info: received hello from new neighbor.\n"); + } + + neigh->hello_log |= 1; + neigh->hello_interval = ntohs(tlv_hello->interval); + neigh->last_seqno = seqno; + neigh->last_hello = now; + + fprintf(stderr, "debug: accepted hello, log %04x.\n", neigh->hello_log); +} + +static void handle_tlv(ffd_tlv_type_t type, const void *data, size_t len, void *arg) { + const struct sockaddr_ll *from = arg; + const eth_addr_t *addr = (const eth_addr_t*)from->sll_addr; + ffd_iface_t *iface = get_iface(from->sll_ifindex); + + if (!iface) + return; + + switch (type) { + case TLV_HELLO: + handle_tlv_hello(data, len, addr, iface); + return; + + default: + fprintf(stderr, "debug: received unknown TLV %u on %s.\n", type, iface->name); + return; + } +} + static void receive_packet() { - uint8_t buf[mtu]; + ffd_packet_t *packet = alloca(sizeof(ffd_packet_t)+PACKET_MAX); struct sockaddr_ll from; socklen_t fromlen = sizeof(from); - ssize_t readlen = recvfrom(sockfd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &fromlen); + ssize_t readlen = recvfrom(sockfd, packet, sizeof(ffd_packet_t)+PACKET_MAX, 0, (struct sockaddr*)&from, &fromlen); if (readlen < 0) { fprintf(stderr, "error: recvfrom: %m\n"); return; } - fprintf(stderr, "debug: read %zi bytes.\n", readlen); + if (from.sll_halen != ETH_ALEN) { + fprintf(stderr, "error: receive_packet: strange address length\n"); + return; + } + + if (readlen < sizeof(ffd_packet_t) || readlen < sizeof(ffd_packet_t)+ntohs(packet->len)) { + fprintf(stderr, "debug: received short packet.\n"); + return; + } + + if (!ffd_tlv_parse(packet, handle_tlv, &from)) { + fprintf(stderr, "debug: received invalid packet.\n"); + } } int main() { @@ -45,6 +45,11 @@ typedef struct _ffd_neigh_t { struct _ffd_neigh_t *next; eth_addr_t addr; + + uint16_t hello_log; + uint16_t hello_interval; + uint16_t last_seqno; + struct timespec last_hello; } ffd_neigh_t; typedef struct _ffd_iface_t { @@ -56,7 +61,7 @@ typedef struct _ffd_iface_t { netif_type_t type; uint16_t seqno; - ffd_neigh_t *neighs; + ffd_neigh_t *neigh_list; } ffd_iface_t; #endif /* _FFD_FFD_H_ */ @@ -36,7 +36,7 @@ bool ffd_tlv_parse(const ffd_packet_t *packet, ffd_tlv_cb cb, void *arg) { if (packet->version_magic != htons(FFD_VERSION_MAGIC)) return false; - const uint8_t *data = packet->tlv + 2, *end = packet->tlv + packet->len; + const uint8_t *data = packet->tlv + 2, *end = packet->tlv + ntohs(packet->len); while (data <= end) { if (data[-2] == TLV_PAD1) { |