Fix bfd ping

4 files changed, 75 insertions, 30 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8112859..47537e9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,7 +11,12 @@ if(${EXT_PREFIX_CLASS})
 	add_definitions(-DEXT_PREFIX_CLASS=${EXT_PREFIX_CLASS})
 endif(${EXT_PREFIX_CLASS})
 
-add_executable(odhcp6c src/odhcp6c.c src/dhcpv6.c src/ra.c src/script.c src/md5.c)
+if(${EXT_BFD_PING})
+	add_definitions(-DEXT_BFD_PING)
+	set(BFD_SOURCE src/bfd.c)
+endif(${EXT_BFD_PING})
+
+add_executable(odhcp6c src/odhcp6c.c src/dhcpv6.c src/ra.c src/script.c src/md5.c ${BFD_SOURCE})
 target_link_libraries(odhcp6c resolv)
 
 # Installation
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;
 }
 
 /*
diff --git a/src/bfd.h b/src/bfd.h
new file mode 100644
index 0000000..0d3616a
--- /dev/null
+++ b/src/bfd.h
@@ -0,0 +1,18 @@
+/**
+ * Copyright (C) 2012-2013 Steven Barth <steven@midlink.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License v2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#pragma once
+
+int bfd_start(const char *ifname, int limit, int interval);
+void bfd_receive(void);
+void bfd_stop(void);
diff --git a/src/odhcp6c.c b/src/odhcp6c.c
index 1f75c50..2c5bfad 100644
--- a/src/odhcp6c.c
+++ b/src/odhcp6c.c
@@ -32,6 +32,10 @@
 #include "odhcp6c.h"
 #include "ra.h"
 
+#ifdef EXT_BFD_PING
+#include "bfd.h"
+#endif
+
 
 static void sighandler(int signal);
 static int usage(void);
@@ -262,6 +266,9 @@ int main(_unused int argc, char* const argv[])
 		script_call("bound");
 		bound = true;
 		syslog(LOG_NOTICE, "entering stateful-mode on %s", ifname);
+#ifdef EXT_BFD_PING
+		bfd_start(ifname, 3, 10);
+#endif
 
 		while (do_signal == 0 || do_signal == SIGUSR1) {
 			// Renew Cycle
@@ -311,6 +318,10 @@ int main(_unused int argc, char* const argv[])
 				script_call("rebound");
 		}
 
+#ifdef EXT_BFD_PING
+		bfd_stop();
+#endif
+
 
 		size_t ia_pd_len, ia_na_len, server_id_len;
 		odhcp6c_get_state(STATE_IA_PD, &ia_pd_len);
@@ -398,6 +409,10 @@ bool odhcp6c_signal_process(void)
 			script_call("ra-updated"); // Immediate process urgent events
 		else if (ra_updated && !bound && allow_slaac_only > 0)
 			script_delay_call("ra-updated", allow_slaac_only);
+
+#ifdef EXT_BFD_PING
+		bfd_receive();
+#endif
 	}
 
 	return do_signal != 0;