diff options
Diffstat (limited to 'src/bfd.c')
| -rw-r--r-- | src/bfd.c | 211 |
1 files changed, 0 insertions, 211 deletions
diff --git a/src/bfd.c b/src/bfd.c deleted file mode 100644 index 1822555..0000000 --- a/src/bfd.c +++ /dev/null @@ -1,211 +0,0 @@ -#include <syslog.h> -#include <signal.h> -#include <stddef.h> -#include <stdlib.h> -#include <netinet/ip6.h> -#include <netinet/icmp6.h> - -#include <sys/socket.h> -#include <net/if.h> -#include <net/ethernet.h> -#include <netpacket/packet.h> -#include <linux/rtnetlink.h> -#include <linux/filter.h> - -#include <errno.h> -#include <fcntl.h> -#include <unistd.h> - -#include "odhcp6c.h" - -static int sock = -1, rtnl = -1; -static int if_index = -1; -static int bfd_failed = 0, bfd_limit = 0, bfd_interval = 0; -static bool bfd_armed = false; - - -static void bfd_send(int signal __attribute__((unused))) -{ - struct { - struct ip6_hdr ip6; - struct icmp6_hdr icmp6; - } ping; - memset(&ping, 0, sizeof(ping)); - - ping.ip6.ip6_vfc = 6 << 4; - ping.ip6.ip6_plen = htons(8); - ping.ip6.ip6_nxt = IPPROTO_ICMPV6; - ping.ip6.ip6_hlim = 255; - - ping.icmp6.icmp6_type = ICMP6_ECHO_REQUEST; - ping.icmp6.icmp6_data32[0] = htonl(0xbfd0bfd); - - size_t pdlen, rtlen; - struct odhcp6c_entry *pd = odhcp6c_get_state(STATE_IA_PD, &pdlen), *cpd = NULL; - struct odhcp6c_entry *rt = odhcp6c_get_state(STATE_RA_ROUTE, &rtlen), *crt = NULL; - bool crt_found = false; - - alarm(bfd_interval); - - if (bfd_armed) { - if (++bfd_failed > bfd_limit) { - raise(SIGUSR2); - return; - } - } - - // Detect PD-Prefix - for (size_t i = 0; i < pdlen / sizeof(*pd); ++i) - if (!cpd || ((cpd->target.s6_addr[0] & 7) == 0xfc) > ((pd[i].target.s6_addr[0] & 7) == 0xfc) - || cpd->preferred < pd[i].preferred) - cpd = &pd[i]; - - // Detect default router - for (size_t i = 0; i < rtlen / sizeof(*rt); ++i) - if (IN6_IS_ADDR_UNSPECIFIED(&rt[i].target) && (!crt || crt->priority > rt[i].priority)) - crt = &rt[i]; - - struct sockaddr_ll dest = { - .sll_family = AF_PACKET, - .sll_protocol = htons(ETH_P_IPV6), - .sll_ifindex = if_index, - .sll_halen = ETH_ALEN, - }; - - if (crt) { - struct { - struct nlmsghdr hdr; - struct ndmsg ndm; - } req = { - .hdr = {sizeof(req), RTM_GETNEIGH, NLM_F_REQUEST | NLM_F_DUMP, 1, 0}, - .ndm = {.ndm_family = AF_INET6, .ndm_ifindex = if_index} - }; - send(rtnl, &req, sizeof(req), 0); - - uint8_t buf[8192]; - struct nlmsghdr *nhm; - do { - ssize_t read = recv(rtnl, buf, sizeof(buf), 0); - nhm = (struct nlmsghdr*)buf; - if ((read < 0 && errno == EINTR) || !NLMSG_OK(nhm, (size_t)read)) - continue; - else if (read < 0) - break; - - for (; read > 0 && NLMSG_OK(nhm, (size_t)read); nhm = NLMSG_NEXT(nhm, read)) { - ssize_t attrlen = NLMSG_PAYLOAD(nhm, sizeof(struct ndmsg)); - if (nhm->nlmsg_type != RTM_NEWNEIGH || attrlen <= 0) { - nhm = NULL; - break; - } - - // Already have our MAC - if (crt_found) - continue; - - struct ndmsg *ndm = NLMSG_DATA(nhm); - for (struct rtattr *rta = (struct rtattr*)&ndm[1]; - attrlen > 0 && RTA_OK(rta, (size_t)attrlen); - rta = RTA_NEXT(rta, attrlen)) { - if (rta->rta_type == NDA_DST) { - crt_found = IN6_ARE_ADDR_EQUAL(RTA_DATA(rta), &crt->router); - } else if (rta->rta_type == NDA_LLADDR) { - memcpy(dest.sll_addr, RTA_DATA(rta), ETH_ALEN); - } - } - } - } while (nhm); - } - - if (!crt_found || !cpd) - return; - - ping.ip6.ip6_src = cpd->target; - ping.ip6.ip6_dst = cpd->target; - - struct sock_filter bpf[] = { - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_plen)), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 8 << 16 | IPPROTO_ICMPV6 << 8 | 254, 0, 13), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst)), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[0]), 0, 11), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 4), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[1]), 0, 9), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 8), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[2]), 0, 7), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 12), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[3]), 0, 5), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, sizeof(struct ip6_hdr) + - offsetof(struct icmp6_hdr, icmp6_type)), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ICMP6_ECHO_REQUEST << 24, 0, 3), - BPF_STMT(BPF_LD | BPF_W | BPF_ABS, sizeof(struct ip6_hdr) + - offsetof(struct icmp6_hdr, icmp6_data32)), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.icmp6.icmp6_data32[0]), 0, 1), - BPF_STMT(BPF_RET | BPF_K, 0xffffffff), - BPF_STMT(BPF_RET | BPF_K, 0), - }; - struct sock_fprog bpf_prog = {sizeof(bpf) / sizeof(*bpf), bpf}; - - - if (sock < 0) { - sock = socket(AF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_IPV6)); - bind(sock, (struct sockaddr*)&dest, sizeof(dest)); - - fcntl(sock, F_SETOWN, getpid()); - fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_ASYNC); - } - - setsockopt(sock, SOL_SOCKET, SO_DETACH_FILTER, &bpf_prog, sizeof(bpf_prog)); - if (setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &bpf_prog, sizeof(bpf_prog))) { - close(sock); - sock = -1; - return; - } - - uint8_t dummy[8]; - while (recv(sock, dummy, sizeof(dummy), MSG_DONTWAIT | MSG_TRUNC) > 0); - - sendto(sock, &ping, sizeof(ping), MSG_DONTWAIT, - (struct sockaddr*)&dest, sizeof(dest)); -} - - -void bfd_receive(void) -{ - uint8_t dummy[8]; - while (recv(sock, dummy, sizeof(dummy), MSG_DONTWAIT | MSG_TRUNC) > 0) { - bfd_failed = 0; - bfd_armed = true; - } -} - - -int bfd_start(const char *ifname, int limit, int interval) -{ - if_index = if_nametoindex(ifname); - bfd_armed = false; - bfd_failed = 0; - bfd_limit = limit; - bfd_interval = interval; - - if (limit < 1 || interval < 1) - return 0; - - rtnl = socket(AF_NETLINK, SOCK_DGRAM | SOCK_CLOEXEC, NETLINK_ROUTE); - struct sockaddr_nl rtnl_kernel = { .nl_family = AF_NETLINK }; - connect(rtnl, (const struct sockaddr*)&rtnl_kernel, sizeof(rtnl_kernel)); - - signal(SIGALRM, bfd_send); - alarm(5); - return 0; -} - - -void bfd_stop(void) -{ - alarm(0); - close(sock); - close(rtnl); - - sock = -1; - rtnl = -1; -} |