Fix bfd ping

[odhcp6c.git] / src / bfd.c

diff --git a/src/bfd.c b/src/bfd.c
index be71361df48c425003d6f7b5272255ab23960946..11fade1d74a57691e315fa70088193dc985526c4 100644 (file)
--- a/src/bfd.c
+++ b/src/bfd.c
@@ -6,6 +6,7 @@
 #include <netinet/icmp6.h>
 
 #include <sys/socket.h>
+#include <net/if.h>
 #include <net/ethernet.h>
 #include <netpacket/packet.h>
 #include <linux/rtnetlink.h>
@@ -22,7 +23,7 @@ static int bfd_failed = 0, bfd_limit = 0, bfd_interval = 0;
 static bool bfd_armed = false;
 
 
-static void bfd_send(int signal)
+static void bfd_send(int signal __attribute__((unused)))
 {
        struct {
                struct ip6_hdr ip6;
@@ -43,6 +44,15 @@ static void bfd_send(int signal)
        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)
@@ -110,13 +120,6 @@ static void bfd_send(int signal)
        ping.ip6.ip6_src = cpd->target;
        ping.ip6.ip6_dst = cpd->target;
 
-       if (bfd_armed) {
-               if (bfd_failed++ > bfd_limit) {
-                       raise(SIGUSR2);
-                       return;
-               }
-       }
-
 /*
        uint16_t sum = cksum(&ping.ip6.ip6_src, sizeof(ping.ip6.ip6_src), 0);
        sum = cksum(&ping.ip6.ip6_dst, sizeof(ping.ip6.ip6_dst), ~sum);
@@ -130,41 +133,47 @@ static void bfd_send(int signal)
 
        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, htonl(8 << 16 | IPPROTO_ICMPV6 << 8 | 254), 0, 13),
+               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, ping.ip6.ip6_dst.s6_addr32[0], 0, 11),
+               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, ping.ip6.ip6_dst.s6_addr32[1], 0, 9),
+               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, ping.ip6.ip6_dst.s6_addr32[2], 0, 7),
+               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, ping.ip6.ip6_dst.s6_addr32[3], 0, 5),
+               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, htonl(ICMP6_ECHO_REQUEST << 24), 0, 3),
+               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, ping.icmp6.icmp6_data32[0], 0, 1),
+               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);
 
-       if (!signal) {
-               bind(sock, (struct sockaddr*)&dest, sizeof(dest));
-               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));
-       alarm(bfd_interval);
 }
 
 
@@ -178,9 +187,9 @@ void bfd_receive(void)
 }
 
 
-int bfd_start(int ifindex, int limit, int interval)
+int bfd_start(const char *ifname, int limit, int interval)
 {
-       if_index = ifindex;
+       if_index = if_nametoindex(ifname);
        bfd_armed = false;
        bfd_failed = 0;
        bfd_limit = limit;
@@ -193,13 +202,8 @@ int bfd_start(int ifindex, int limit, int interval)
        struct sockaddr_nl rtnl_kernel = { .nl_family = AF_NETLINK };
        connect(rtnl, (const struct sockaddr*)&rtnl_kernel, sizeof(rtnl_kernel));
 
-       sock = socket(AF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_IPV6));
-       bfd_send(0);
-
-       fcntl(sock, F_SETOWN, getpid());
-       fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_ASYNC);
-
        signal(SIGALRM, bfd_send);
+       alarm(5);
        return 0;
 }
 
@@ -209,6 +213,9 @@ void bfd_stop(void)
        alarm(0);
        close(sock);
        close(rtnl);
+
+       sock = -1;
+       rtnl = -1;
 }
 
 /*