1 files changed, 36 insertions, 29 deletions

diff --git a/src/bfd.c b/src/bfd.c
index be71361..11fade1 100644
--- 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;
 }
 
 /*