diff options
| -rw-r--r-- | ffvisd/ffvisd.c | 352 |
1 files changed, 196 insertions, 156 deletions
diff --git a/ffvisd/ffvisd.c b/ffvisd/ffvisd.c index fab2f39..1dc0a32 100644 --- a/ffvisd/ffvisd.c +++ b/ffvisd/ffvisd.c @@ -26,8 +26,10 @@ #define _GNU_SOURCE +#include <alloca.h> #include <errno.h> #include <libgen.h> +#include <poll.h> #include <stdarg.h> #include <stdbool.h> #include <stdio.h> @@ -47,12 +49,15 @@ #include <netinet/in.h> +#include <netpacket/packet.h> + #include <sys/types.h> #include <sys/stat.h> -#define ANNOUNCE_INTERVAL 10000 -#define ANNOUNCE_TIMEOUT 30000 +#define ANNOUNCE_INTERVAL 10 +#define ANNOUNCE_TIMEOUT (3*ANNOUNCE_INTERVAL) +#define ANNOUNCE_PRUNE (3*ANNOUNCE_TIMEOUT) #define SYSFS_PATH_MAX 256 #define SYSFS_CLASS_NET "/sys/class/net" @@ -60,31 +65,29 @@ #define FFD_MAGIC 0xffd #define FFVISD_VERSION 0 +#define FFD_PROTO 0xffd #define FFD_PORT 0xffd -static const struct in6_addr ffd_addr = { .s6_addr = {0xff, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xfd} }; - - -typedef struct __attribute__((__packed__)) _ffvisd_eth_address_t { +typedef struct __attribute__((__packed__)) _eth_addr_t { uint8_t address[ETH_ALEN]; -} ffvisd_eth_address_t; +} eth_addr_t; -typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_address_t { +typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_addr_t { uint8_t address[8]; -} ffvisd_ipv6_host_address_t; +} ffvisd_ipv6_host_addr_t; typedef struct __attribute__((__packed__)) _ffvisd_packet_server_announce_t { - uint32_t seqno; - ffvisd_eth_address_t eth_address; - ffvisd_ipv6_host_address_t address; + uint16_t seqno; + eth_addr_t eth_address; + ffvisd_ipv6_host_addr_t address; } ffvisd_packet_server_announce_t; typedef struct __attribute__((__packed__)) _ffvisd_packet_announce_t { uint16_t magic; uint16_t version; - uint32_t n_servers; + uint16_t csum; + uint16_t n_servers; ffvisd_packet_server_announce_t servers[]; } ffvisd_packet_announce_t; @@ -98,20 +101,6 @@ typedef enum _ffvisd_iftype_t { IF_MAX } ffvisd_iftype_t; -typedef struct _ffvisd_iface_t { - unsigned ifindex; - ffvisd_iftype_t iftype; - char *ifname; - char *bridge_iface; - char *mesh_iface; -} ffvisd_iface_t; - -typedef struct _ffvisd_orig_t { - struct _ffvisd_orig_t *next; - - ffvisd_eth_address_t eth_address; -} ffvisd_orig_t; - typedef struct _ffvisd_announce_t { struct _ffvisd_announce_t *next; @@ -119,28 +108,42 @@ typedef struct _ffvisd_announce_t { ffvisd_packet_server_announce_t announce; } ffvisd_announce_t; -static const char *ffvisd_iftype_names[IF_MAX] = { +/*static const char *ffvisd_iftype_names[IF_MAX] = { [IF_UNKNOWN] = "unknown", [IF_WIRED] = "wired", [IF_WIRELESS] = "wireless", [IF_VIRTUAL] = "virtual", [IF_BRIDGE] = "bridge", [IF_MESH] = "mesh", -}; + };*/ -typedef void (*iface_cb)(const ffvisd_iface_t *iface, void *arg); +typedef void (*iface_cb)(const char *ifname, unsigned ifindex, void *arg); -static int sockfd; -static char *mesh = "bat0"; +static const eth_addr_t eth_addr_unspec = {{0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}; +static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}}; + + +static inline bool is_eth_addr_unspec(const eth_addr_t *address) { + const uint8_t *a = address->address; + + if (a[0]||a[1]||a[2]||a[3]||a[4]||a[5]) + return false; + else + return true; +} -ffvisd_announce_t *announcements = NULL; +#define ETH_ADDR_IS_UNSPEC(a) (!((a).address[0]||(a).address[0]||(a).address[0]||(a).address[0]||(a).address[0]||(a).address[0])) -static void ffvisd_iface_free(ffvisd_iface_t *iface); +static int sockfd; +static char *mesh = "bat0"; + + +ffvisd_announce_t *announcements = NULL; /* returns (tp1 - tp2) in milliseconds */ @@ -187,6 +190,17 @@ static bool iface_is_mesh(const char *ifname) { return iface_file_exists(ifname, "mesh"); } +static char* iface_get_mesh(const char *ifname) { + char *mesh = NULL; + + if (!iface_file_read(ifname, "batman_adv/mesh_iface", "%as", &mesh) || !strcmp(mesh, "none")) { + free(mesh); + return NULL; + } + + return mesh; +} + static char* iface_get_bridge(const char *ifname) { char filename[SYSFS_PATH_MAX], filename2[SYSFS_PATH_MAX] = {0}; @@ -197,7 +211,17 @@ static char* iface_get_bridge(const char *ifname) { return strdup(basename(filename2)); } -static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { +static eth_addr_t iface_get_eth_address(const char *ifname) { + eth_addr_t ret; + uint8_t *a = ret.address; + + if (!iface_file_read(ifname, "address", "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3], &a[4], &a[5])) + return eth_addr_unspec; + + return ret; +} + +static struct in6_addr iface_get_address(unsigned ifindex, int scope) { struct msg { struct nlmsghdr nh; struct ifaddrmsg addr; @@ -217,7 +241,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { if(write(rtnetlink_sk, &msg, msg.nh.nlmsg_len) <= 0) { fprintf(stderr, "error: write: %m\n"); close(rtnetlink_sk); - return NULL; + return in6addr_any; } int readlen = read(rtnetlink_sk, &msg, sizeof(msg)); @@ -225,7 +249,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { if (readlen <= 0) { fprintf(stderr, "error: read: %m\n"); close(rtnetlink_sk); - return NULL; + return in6addr_any; } close(rtnetlink_sk); @@ -236,19 +260,19 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { int len = chunk->nh.nlmsg_len - sizeof(struct nlmsghdr); if (len < sizeof(struct ifaddrmsg) || readlen < len) - return NULL; + return in6addr_any; if (!NLMSG_OK(&chunk->nh, readlen)) - return NULL; + return in6addr_any; - if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == iface->ifindex) { + if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == ifindex) { struct rtattr *rta = (struct rtattr *)IFA_RTA(&chunk->addr); int rtattrlen = IFA_PAYLOAD(&chunk->nh); for (; RTA_OK(rta, rtattrlen); rta = RTA_NEXT(rta, rtattrlen)) { if(rta->rta_type == IFA_ADDRESS && rta->rta_len == RTA_LENGTH(16)) { - struct in6_addr *ret = malloc(sizeof(struct in6_addr)); - memcpy(ret->s6_addr, RTA_DATA(rta), 16); + struct in6_addr ret; + memcpy(ret.s6_addr, RTA_DATA(rta), 16); return ret; } } @@ -258,49 +282,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { chunk = (struct msg*)((uint8_t*)chunk + NLMSG_ALIGN(chunk->nh.nlmsg_len)); } - return NULL; -} - -static ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) { - if (!ifname || !ifindex) - return false; - - ffvisd_iface_t *iface = calloc(1, sizeof(ffvisd_iface_t)); - - iface->ifname = strdup(ifname); - iface->ifindex = ifindex; - - if (!iface_file_read(ifname, "batman_adv/mesh_iface", "%as", &iface->mesh_iface) || !strcmp(iface->mesh_iface, "none")) { - free(iface->mesh_iface); - iface->mesh_iface = NULL; - } - - iface->bridge_iface = iface_get_bridge(ifname); - - int type; - if (iface_file_read(ifname, "type", "%i", &type) && type == ARPHRD_ETHER) { - if(iface_file_exists(ifname, "mesh")) - iface->iftype = IF_MESH; - else if(iface_file_exists(ifname, "bridge")) - iface->iftype = IF_BRIDGE; - else if(iface_file_exists(ifname, "tun_flags")) - iface->iftype = IF_VIRTUAL; - else if(iface_file_exists(ifname, "wireless")) - iface->iftype = IF_WIRELESS; - else - iface->iftype = IF_WIRED; - } - - return iface; -} - -static void ffvisd_iface_free(ffvisd_iface_t *iface) { - if (iface) { - free(iface->ifname); - free(iface->bridge_iface); - free(iface->mesh_iface); - free(iface); - } + return in6addr_any; } static void ffvisd_iface_foreach(iface_cb cb, void *arg) { @@ -312,33 +294,37 @@ static void ffvisd_iface_foreach(iface_cb cb, void *arg) { } int i; - for (i = 0; ifaces[i].if_name; i++) { - ffvisd_iface_t *iface = ffvisd_iface_new(ifaces[i].if_name, ifaces[i].if_index); - - if (iface) - cb(iface, arg); - - ffvisd_iface_free(iface); - } + for (i = 0; ifaces[i].if_name; i++) + cb(ifaces[i].if_name, ifaces[i].if_index, arg); if_freenameindex(ifaces); } -static void pr_iface_info(const ffvisd_iface_t *iface, void *arg) { - printf("Interface %s: ifindex=%i bridge_iface=%s mesh_iface=%s iftype=%s\n", iface->ifname, iface->ifindex, iface->bridge_iface, iface->mesh_iface, ffvisd_iftype_names[iface->iftype]); -} +static void join_mcast(const char *ifname, unsigned ifindex, void *arg) { + char *if_mesh = iface_get_mesh(ifname); + + if (if_mesh && !strcmp(if_mesh, mesh)) { + struct packet_mreq mr; + memset(&mr, 0, sizeof(mr)); + mr.mr_ifindex = ifindex; + mr.mr_type = PACKET_MR_MULTICAST; + mr.mr_alen = ETH_ALEN; + memcpy(mr.mr_address, ffd_addr.address, ETH_ALEN); + if (setsockopt(sockfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) && errno != EADDRINUSE) + fprintf(stderr, "warning: setsockopt: %m\n"); + } + free(if_mesh); +} -static void join_mcast(const ffvisd_iface_t *iface, void *arg) { - if (!iface->mesh_iface || strcmp(iface->mesh_iface, mesh)) - return; +static void pr_announcement_packet(const ffvisd_packet_server_announce_t *announce) { + const uint8_t *e = announce->eth_address.address, *a = announce->address.address; - struct ipv6_mreq imr; - imr.ipv6mr_multiaddr = ffd_addr; - imr.ipv6mr_interface = iface->ifindex; - if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &imr, sizeof(struct ipv6_mreq)) && errno != EADDRINUSE) - fprintf(stderr, "warning: setsockopt: %m\n"); + printf("seqno: %u, ether: %02x:%02x:%02x:%02x:%02x:%02x, address: fe80::%02x%02x:%02x%02x:%02x%02x:%02x%02x\n", + announce->seqno, + e[0], e[1], e[2], e[3], e[4], e[5], + a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7]); } static void pr_announcement(const ffvisd_announce_t *announce) { @@ -351,12 +337,7 @@ static void pr_announcement(const ffvisd_announce_t *announce) { printf("age: %.1fs, ", timespec_diff(&tv, &announce->received)/1000.0); } - const uint8_t *e = announce->announce.eth_address.address, *a = announce->announce.address.address; - - printf("seqno: %u, ether: %02x:%02x:%02x:%02x:%02x:%02x, address: fe80::%02x%02x:%02x%02x:%02x%02x:%02x%02x\n", - announce->announce.seqno, - e[0], e[1], e[2], e[3], e[4], e[5], - a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7]); + pr_announcement_packet(&announce->announce); } static bool check_config() { @@ -369,69 +350,45 @@ static bool check_config() { } static bool init_socket() { - sockfd = socket(AF_INET6, SOCK_DGRAM, 0); + sockfd = socket(AF_PACKET, SOCK_DGRAM, htons(FFD_PROTO)); if (sockfd < 0) { fprintf(stderr, "error: socket: %m\n"); return false; } - int val = 1; - if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_V6ONLY, &val, sizeof(val))) { - fprintf(stderr, "error: setsockopt: %m\n"); - return false; - } - - val = 0; - if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &val, sizeof(val))) { - fprintf(stderr, "error: setsockopt: %m\n"); - return false; - } - - struct sockaddr_in6 sa6; - memset(&sa6, 0, sizeof(sa6)); - sa6.sin6_family = AF_INET6; - sa6.sin6_port = htons(FFD_PORT); - - if (bind(sockfd, (struct sockaddr*)&sa6, sizeof(struct sockaddr_in6))) { - fprintf(stderr, "error: bind: %m\n"); - return false; - } - return true; } static bool init_announcements() { - ffvisd_iface_t *iface; - char *ifname = iface_get_bridge(mesh); - if (ifname) { - iface = ffvisd_iface_new(ifname, if_nametoindex(ifname)); - free(ifname); - } - else { - iface = ffvisd_iface_new(mesh, if_nametoindex(mesh)); - } + if (!ifname) + ifname = strdup(mesh); + + unsigned ifindex = if_nametoindex(ifname); - if (!iface) + if (!ifindex) { + free(ifname); return false; + } ffvisd_announce_t *announce = announcements = calloc(1, sizeof(ffvisd_announce_t)); - uint8_t *a = announce->announce.eth_address.address; - if (!iface_file_read(iface->ifname, "address", "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3], &a[4], &a[5])) + announce->announce.eth_address = iface_get_eth_address(ifname); + if (is_eth_addr_unspec(&announce->announce.eth_address)) { + free(ifname); return false; + } - struct in6_addr *addr = iface_get_address(iface, RT_SCOPE_LINK); - if (!addr) { - ffvisd_iface_free(iface); + struct in6_addr addr = iface_get_address(ifindex, RT_SCOPE_LINK); + if (IN6_IS_ADDR_UNSPECIFIED(&addr)) { + free(ifname); return false; } - memcpy(announce->announce.address.address, addr->s6_addr+8, 8); - free(addr); + memcpy(announce->announce.address.address, addr.s6_addr+8, 8); - ffvisd_iface_free(iface); + free(ifname); return true; } @@ -445,19 +402,81 @@ static void maintenance() { } } +static void send_announcement(const char *ifname, unsigned ifindex, void *arg) { + ffvisd_packet_announce_t *packet = arg; + + struct sockaddr_ll sa; + memset(&sa, 0, sizeof(sa)); + + sa.sll_family = AF_PACKET; + sa.sll_protocol = htons(FFD_PROTO); + sa.sll_ifindex = ifindex; + sa.sll_halen = ETH_ALEN; + memcpy(sa.sll_addr, ffd_addr.address, ETH_ALEN); + + sendto(sockfd, packet, sizeof(ffvisd_packet_announce_t) + ntohs(packet->n_servers)*sizeof(ffvisd_packet_server_announce_t), 0, (struct sockaddr*)&sa, sizeof(sa)); +} + +static void send_announcements() { + ffvisd_packet_announce_t *packet = alloca(sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t)); + + packet->magic = htons(FFD_MAGIC); + packet->version = htons(FFVISD_VERSION); + packet->csum = 0; + + uint16_t n_servers = 0; + ffvisd_announce_t *announce; + for(announce = announcements; announce; announce = announce->next) { + packet->servers[n_servers++] = announce->announce; + } + + packet->n_servers = htons(n_servers); + + ffvisd_iface_foreach(send_announcement, packet); +} + +static void receive_announcement() { + ffvisd_packet_announce_t *packet = alloca(sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t)); + + int readlen = read(sockfd, packet, sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t)); + + if (readlen < 0) { + fprintf(stderr, "error: read: %m\n"); + return; + } + + if (readlen < sizeof(ffvisd_packet_announce_t)) { + fprintf(stderr, "warning: short read"); + return; + } + + int n_servers = ntohs(packet->n_servers); + + if (readlen < sizeof(ffvisd_packet_announce_t) + n_servers*sizeof(ffvisd_packet_server_announce_t)) { + fprintf(stderr, "warning: short read"); + return; + } + + puts("Received announcements:"); + int i; + for(i = 0; i < n_servers; i++) + pr_announcement_packet(&packet->servers[i]); + puts(""); +} + int main() { if (!check_config()) return 1; - ffvisd_iface_foreach(pr_iface_info, NULL); - puts(""); - if (!init_socket()) return 1; if (!init_announcements()) return 1; + struct timespec next_announce; + clock_gettime(CLOCK_MONOTONIC, &next_announce); + while (true) { ffvisd_iface_foreach(join_mcast, NULL); @@ -469,7 +488,28 @@ int main() { pr_announcement(announce); puts(""); - usleep(ANNOUNCE_INTERVAL*1000); + send_announcements(); + + next_announce.tv_sec += ANNOUNCE_INTERVAL; + + while (true) { + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + int timeout = timespec_diff(&next_announce, &now); + + if (timeout <= 0) + break; + + struct pollfd fds[1]; + + fds[0].fd = sockfd; + fds[0].events = POLLIN; + + poll(fds, 1, timeout); + + if (fds[0].revents & POLLIN) + receive_announcement(); + } } return 0; |