Add ICMPv6 support with acceleration

icmp.c

@@ -6,13 +6,62 @@
 #include <netinet/icmp6.h>
 #include <unistd.h>
 #include <netinet/ip6.h>
+#include <time.h>
+#include <errno.h>
 
 #include "dhcp6.h"
 #include "l2tpns.h"
 #include "ipv6_u.h"
+#include "util.h"
+
+int icmpv6fd;
 
 static uint16_t _checksum(uint8_t *addr, int count);
 
+void icmpv6_init(void)
+{
+	int on = 1, check = 2;
+	struct icmp6_filter filter;
+
+	icmpv6fd = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
+	if (icmpv6fd < 0)
+		LOG(1, 0, 0, "ICMPv6: could not create socket: %s\n", strerror(errno));
+
+#ifdef IPV6_RECVPKTINFO
+	if (setsockopt(icmpv6fd, SOL_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "ICMPv6: could not request pktinfo: %s\n", strerror(errno));
+#else
+	if (setsockopt(icmpv6fd, SOL_IPV6, IPV6_PKTINFO, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "ICMPv6: could not request pktinfo: %s\n", strerror(errno));
+#endif
+
+#ifdef __linux__
+	if (setsockopt(icmpv6fd, SOL_RAW, IPV6_CHECKSUM, &check, sizeof(check)) < 0)
+#else
+	if (setsockopt(icmpv6fd, SOL_IPV6, IPV6_CHECKSUM, &check, sizeof(check)) < 0)
+#endif
+		LOG(1, 0, 0, "ICMPv6: could not request for checking checksums: %s\n", strerror(errno));
+
+	ICMP6_FILTER_SETBLOCKALL(&filter);
+	ICMP6_FILTER_SETPASS(ND_ROUTER_SOLICIT, &filter);
+
+	if (setsockopt(icmpv6fd, SOL_ICMPV6, ICMP6_FILTER, &filter, sizeof(filter)) < 0)
+		LOG(1, 0, 0, "ICMPv6: could not set filter for RS: %s\n", strerror(errno));
+}
+
+//
+// A new ppp interface was created, watch for ICMPv6 RS on it
+void icmpv6_listen(int ifidx)
+{
+	struct ipv6_mreq mreq;
+
+	memset(&mreq, 0, sizeof(mreq));
+	mreq.ipv6mr_interface = ifidx;
+	inet_pton(AF_INET6, "ff02::2", &mreq.ipv6mr_multiaddr);
+	if (setsockopt(icmpv6fd, SOL_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)
+		LOG(2, 0, 0, "ICMPv6: could not join all routers group: %s\n", strerror(errno));
+}
+
 void host_unreachable(in_addr_t destination, uint16_t id, in_addr_t source, uint8_t *packet, int packet_len)
 {
 	char buf[128] = {0};
@@ -203,3 +252,54 @@ void send_ipv6_ra(sessionidt s, tunnelidt t, const struct in6_addr *ip)
 	tunnelsend(b, l + (((uint8_t *) p_ip6_hdr)-b), t); // send it...
 	return;
 }
+
+//
+// An ICMPv6 request was received on a ppp interface, receive it
+void icmpv6_process_from_kernel(uint8_t *p, size_t size_bufp)
+{
+	struct sockaddr_storage fromaddr;
+	struct sockaddr_in6 *sin6;
+	socklen_t fromlen = sizeof(fromaddr);
+	struct in6_addr toaddr;
+	int ifidx;
+	int r, s, t;
+
+	r = recvfromto6(icmpv6fd, p, size_bufp, 0, (struct sockaddr *) &fromaddr, &fromlen, &toaddr, &ifidx);
+	if (r < 0)
+	{
+		static time_t lastwarn;
+		time_t now = time(NULL);
+		if (now > lastwarn)
+		{
+			LOG(5, 0, 0, "ICMPV6: reception error: %s\n", strerror(errno));
+			lastwarn = now;
+		}
+		return;
+	}
+	LOG(5, 0, 0, "Got packet on ICMP socket on if %d\n", ifidx);
+
+	if (fromaddr.ss_family != AF_INET6)
+	{
+		LOG(5, 0, 0, "ICMPV6: got strange family %d\n", fromaddr.ss_family);
+		return;
+	}
+	sin6 = (struct sockaddr_in6 *) &fromaddr;
+
+	if (*p != ND_ROUTER_SOLICIT)
+	{
+		LOG(5, 0, 0, "ICMPV6: not router sollicitation??: %d\n", *p);
+		return;
+	}
+
+	for (s = 1; s < MAXSESSION; s++)
+	{
+		if (sess_local[s].ppp_if_idx != ifidx)
+			continue;
+
+		t = session[s].tunnel;
+
+		send_ipv6_ra(s, t, &sin6->sin6_addr);
+
+		break;
+	}
+}

l2tpns.c

@@ -1229,6 +1229,7 @@ static int create_kernel_accel(sessionidt s)
 	sess_local[s].ppp_if_idx = ifr.ifr_ifindex;
 
 	dhcpv6_listen(ifr.ifr_ifindex);
+	icmpv6_listen(ifr.ifr_ifindex);
 
 	memset(&sess_local[s].last_stats, 0, sizeof(sess_local[s].last_stats));
 
@@ -5449,8 +5450,8 @@ static int still_busy(void)
 	return 0;
 }
 
-// the base set of fds polled: cli, cluster, tun, udp (MAX_UDPFD), control, dae, netlink, udplac, pppoedisc, pppoesess, dhcpv6
-#define BASE_FDS	(10 + MAX_UDPFD)
+// the base set of fds polled: cli, cluster, tun, udp (MAX_UDPFD), control, dae, netlink, udplac, pppoedisc, pppoesess, dhcpv6, icmpv6
+#define BASE_FDS	(11 + MAX_UDPFD)
 
 // additional polled fds
 #ifdef BGP
@@ -5537,6 +5538,10 @@ static void mainloop(void)
 		e.data.ptr = &d[i++];
 		epoll_ctl(epollfd, EPOLL_CTL_ADD, dhcpv6fd, &e);
 
+		d[i].type = FD_TYPE_ICMPV6;
+		e.data.ptr = &d[i++];
+		epoll_ctl(epollfd, EPOLL_CTL_ADD, icmpv6fd, &e);
+
 		for (j = 0; j < config->nbudpfd; j++)
 		{
 			d[i].type = FD_TYPE_UDP;
@@ -5802,6 +5807,12 @@ static void mainloop(void)
 					break;
 				}
 
+				case FD_TYPE_ICMPV6:
+				{
+					icmpv6_process_from_kernel(p, size_bufp);
+					break;
+				}
+
 				default:
 					LOG(0, 0, 0, "Unexpected fd type returned from epoll_wait: %d\n", d->type);
 				}
@@ -6774,6 +6785,7 @@ int main(int argc, char *argv[])
 	initrad();
 	initippool();
 	dhcpv6_init();
+	icmpv6_init();
 
 	// seed prng
 	{

l2tpns.h

@@ -1044,6 +1044,10 @@ int cli_arg_help(struct cli_def *cli, int cr_ok, char *entry, ...);
 
 
 // icmp.c
+extern int icmpv6fd;
+void icmpv6_init(void);
+void icmpv6_listen(int ifidx);
+void icmpv6_process_from_kernel(uint8_t *p, size_t size_bufp);
 void host_unreachable(in_addr_t destination, uint16_t id, in_addr_t source, uint8_t *packet, int packet_len);
 
 
@@ -1087,6 +1091,7 @@ struct event_data {
 		FD_TYPE_PPP_CHAN,
 		FD_TYPE_PPP_IF,
 		FD_TYPE_DHCPV6,
+		FD_TYPE_ICMPV6,
 	} type;
 	int index; // for RADIUS, BGP, UDP
 };

util.c

@@ -140,6 +140,7 @@ pid_t fork_and_close()
 	}
 
 	if (dhcpv6fd != -1)	close(dhcpv6fd);
+	if (icmpv6fd != -1)	close(icmpv6fd);
 	if (snoopfd != -1)	close(snoopfd);
 	if (rand_fd != -1)	close(rand_fd);
 	if (epollfd != -1)	close(epollfd);