From 09eb0b81799380b302ed0e8c8252202525f1a14d Mon Sep 17 00:00:00 2001 From: Matthias Schiffer Date: Wed, 29 Aug 2012 02:18:07 +0200 Subject: Initialize more stuff --- ffvisd/ffvisd.c | 274 +++++++++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 250 insertions(+), 24 deletions(-) diff --git a/ffvisd/ffvisd.c b/ffvisd/ffvisd.c index a169e89..851ab3c 100644 --- a/ffvisd/ffvisd.c +++ b/ffvisd/ffvisd.c @@ -26,6 +26,7 @@ #define _GNU_SOURCE +#include #include #include #include @@ -33,18 +34,25 @@ #include #include #include +#include #include +#include + +#include + #include #include #include +#include + #include #include -#define ANNOUNCE_INTERVAL 20 -#define ANNOUNCE_TIMEOUT 60 +#define ANNOUNCE_INTERVAL 10000 +#define ANNOUNCE_TIMEOUT 30000 #define SYSFS_PATH_MAX 256 #define SYSFS_CLASS_NET "/sys/class/net" @@ -52,32 +60,33 @@ #define FFD_MAGIC 0xffd #define FFVISD_VERSION 0 +#define FFD_PORT 0xffd + -//static char *mesh = "bat0"; +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 _ffvisd_eth_address_t { +typedef struct __attribute__((__packed__)) _ffvisd_eth_address_t { uint8_t address[ETH_ALEN]; } ffvisd_eth_address_t; -typedef struct _ffvisd_ipv6_host_address_t { +typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_address_t { uint8_t address[8]; } ffvisd_ipv6_host_address_t; -typedef struct _ffvisd_server_announce_t { +typedef struct __attribute__((__packed__)) _ffvisd_packet_server_announce_t { + uint32_t seqno; ffvisd_eth_address_t eth_address; - uint16_t reserved; ffvisd_ipv6_host_address_t address; - uint32_t age; -} ffvisd_server_announce_t; +} ffvisd_packet_server_announce_t; -typedef struct _ffvisd_announce_t { +typedef struct __attribute__((__packed__)) _ffvisd_packet_announce_t { uint16_t magic; uint16_t version; uint32_t n_servers; - uint32_t reserved; - ffvisd_server_announce_t servers[]; -} ffvisd_announce_t; + ffvisd_packet_server_announce_t servers[]; +} ffvisd_packet_announce_t; typedef enum _ffvisd_iftype_t { IF_UNKNOWN = 0, @@ -97,6 +106,19 @@ typedef struct _ffvisd_iface_t { 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; + + struct timeval received; + ffvisd_packet_server_announce_t announce; +} ffvisd_announce_t; + static const char *ffvisd_iftype_names[IF_MAX] = { [IF_UNKNOWN] = "unknown", [IF_WIRED] = "wired", @@ -106,13 +128,28 @@ static const char *ffvisd_iftype_names[IF_MAX] = { [IF_MESH] = "mesh", }; + typedef void (*iface_cb)(const ffvisd_iface_t *iface, void *arg); -void ffvisd_iface_free(ffvisd_iface_t *iface); +static int sockfd; +static char *mesh = "bat0"; + + +ffvisd_announce_t *announcements = NULL; + +static void ffvisd_iface_free(ffvisd_iface_t *iface); -bool file_readv(const char *file, const char *format, va_list ap) { + + +/* returns (tp1 - tp2) in milliseconds */ +static inline int timespec_diff(const struct timespec *tp1, const struct timespec *tp2) { + return ((tp1->tv_sec - tp2->tv_sec))*1000 + (tp1->tv_nsec - tp2->tv_nsec)/1e6; +} + + +static bool file_readv(const char *file, const char *format, va_list ap) { FILE *f = fopen(file, "r"); if (!f) return false; @@ -124,7 +161,7 @@ bool file_readv(const char *file, const char *format, va_list ap) { return (ret > 0); } -bool iface_file_read(const char *ifname, const char *file, const char *format, ...) { +static bool iface_file_read(const char *ifname, const char *file, const char *format, ...) { char filename[SYSFS_PATH_MAX]; snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/%s", ifname, file); @@ -137,7 +174,7 @@ bool iface_file_read(const char *ifname, const char *file, const char *format, . return ret; } -bool iface_file_exists(const char *ifname, const char *file) { +static bool iface_file_exists(const char *ifname, const char *file) { char filename[SYSFS_PATH_MAX]; snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/%s", ifname, file); @@ -146,7 +183,11 @@ bool iface_file_exists(const char *ifname, const char *file) { return (stat(filename, &st) == 0); } -char* get_bridge(const char *ifname) { +static bool iface_is_mesh(const char *ifname) { + return iface_file_exists(ifname, "mesh"); +} + +static char* iface_get_bridge(const char *ifname) { char filename[SYSFS_PATH_MAX], filename2[SYSFS_PATH_MAX] = {0}; snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/brport/bridge", ifname); @@ -156,7 +197,74 @@ char* get_bridge(const char *ifname) { return strdup(basename(filename2)); } -ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) { +static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) { + struct msg { + struct nlmsghdr nh; + struct ifaddrmsg addr; + uint8_t attrbuf[16384]; + } msg; + + memset(&msg, 0, sizeof(msg)); + + int rtnetlink_sk = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); + + msg.nh.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifaddrmsg)); + msg.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT; + msg.nh.nlmsg_type = RTM_GETADDR; + + msg.addr.ifa_family = AF_INET6; + + if(write(rtnetlink_sk, &msg, msg.nh.nlmsg_len) <= 0) { + fprintf(stderr, "error: write: %m\n"); + close(rtnetlink_sk); + return NULL; + } + + int readlen = read(rtnetlink_sk, &msg, sizeof(msg)); + + if (readlen <= 0) { + fprintf(stderr, "error: read: %m\n"); + close(rtnetlink_sk); + return NULL; + } + + close(rtnetlink_sk); + + struct msg *chunk; + + for (chunk = &msg; readlen > sizeof(sizeof(struct nlmsghdr));) { + int len = chunk->nh.nlmsg_len - sizeof(struct nlmsghdr); + + if (len < sizeof(struct ifaddrmsg) || readlen < len) + return NULL; + + if (!NLMSG_OK(&chunk->nh, readlen)) + return NULL; + + if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == iface->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); + return ret; + } + } + } + + readlen -= NLMSG_ALIGN(chunk->nh.nlmsg_len); + 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); @@ -167,7 +275,7 @@ ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) { iface->mesh_iface = NULL; } - iface->bridge_iface = get_bridge(ifname); + iface->bridge_iface = iface_get_bridge(ifname); int type; if (iface_file_read(ifname, "type", "%i", &type) && type == ARPHRD_ETHER) { @@ -186,7 +294,7 @@ ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) { return iface; } -void ffvisd_iface_free(ffvisd_iface_t *iface) { +static void ffvisd_iface_free(ffvisd_iface_t *iface) { if (iface) { free(iface->ifname); free(iface->bridge_iface); @@ -195,11 +303,11 @@ void ffvisd_iface_free(ffvisd_iface_t *iface) { } } -void ffvisd_iface_foreach(iface_cb cb, void *arg) { +static void ffvisd_iface_foreach(iface_cb cb, void *arg) { struct if_nameindex *ifaces = if_nameindex(); if (!ifaces) { - fprintf(stderr, "error: if_nameindex: %m"); + fprintf(stderr, "error: if_nameindex: %m\n"); return; } @@ -217,13 +325,131 @@ void ffvisd_iface_foreach(iface_cb cb, void *arg) { } -void pr_iface_info(const ffvisd_iface_t *iface, void *arg) { +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 ffvisd_iface_t *iface, void *arg) { + if (!iface->mesh_iface || strcmp(iface->mesh_iface, mesh)) + return; + + 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"); +} + +static bool check_config() { + if (!iface_is_mesh(mesh)) { + fprintf(stderr, "error: configured interface is no mesh\n"); + return false; + } + + return true; +} + +static bool init_socket() { + sockfd = socket(AF_INET6, SOCK_DGRAM, 0); + 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 (!iface) + 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])) + return false; + + struct in6_addr *addr = iface_get_address(iface, RT_SCOPE_LINK); + if (!addr) { + ffvisd_iface_free(iface); + return false; + } + + char buf[INET6_ADDRSTRLEN]; + inet_ntop(AF_INET6, addr, buf, sizeof(buf)); + puts(buf); + + memcpy(announce->announce.address.address, addr->s6_addr+8, 8); + free(addr); + + ffvisd_iface_free(iface); + + return true; +} + +static void maintenance() { + ffvisd_announce_t **announce; + + for(announce = &announcements; *announce; announce = &(*announce)->next) { + if (!(*announce)->received.tv_sec) /* announcement is local */ + (*announce)->announce.seqno++; + } +} + int main() { + if (!check_config()) + return 1; + ffvisd_iface_foreach(pr_iface_info, NULL); + if (!init_socket()) + return 1; + + if (!init_announcements()) + return 1; + + while (true) { + ffvisd_iface_foreach(join_mcast, NULL); + + maintenance(); + + usleep(ANNOUNCE_INTERVAL*1000); + } + return 0; } -- cgit v1.2.3