Merge branch 'kernel' into 'master'

Add L2TP kernel offloading support

Closes #13

See merge request l2tpns/l2tpns!23

cli.c

@@ -18,6 +18,7 @@
 #include <sys/socket.h>
 #include <sys/types.h>
 #include <signal.h>
+#include <dirent.h>
 #include <dlfcn.h>
 #include <netdb.h>
 #include <libcli.h>
@@ -316,6 +317,36 @@ void cli_do(int sockfd)
 	socklen_t l = sizeof(addr);
 
 	if (fork_and_close()) return;
+
+	/* Check that fork_and_close has closed everything but std* and the socket */
+	int fdfd = open("/dev/fd", O_RDONLY|O_DIRECTORY);
+	if (fdfd >= 0)
+	{
+		DIR *fds = fdopendir(fdfd);
+		if (fds)
+		{
+			struct dirent *ent;
+			while ((ent = readdir(fds)))
+			{
+				if (!strcmp(ent->d_name, ".")
+				 || !strcmp(ent->d_name, ".."))
+					continue;
+
+				int fd = atoi(ent->d_name);
+				if (fd <= STDERR_FILENO)
+					continue;
+				if (fd == fdfd || fd == sockfd)
+					continue;
+				if (log_stream && fd == fileno(log_stream))
+					continue;
+
+				LOG(0, 0, 0, "Warning: fd %d is still open within cli. This may interfere with operations.\n", fd);
+			}
+			closedir(fds);
+		}
+		close(fdfd);
+	}
+
 	if (getpeername(sockfd, (struct sockaddr *) &addr, &l) == 0)
 	{
 		require_auth = addr.sin_addr.s_addr != inet_addr("127.0.0.1");
@@ -420,10 +451,23 @@ static int cmd_show_session(struct cli_def *cli, const char *command, char **arg
 		for (i = 0; i < argc; i++)
 		{
 			unsigned int s, b_in, b_out, r;
+			int ifunit = -1;
+
+			if (!strncmp(argv[i], PPP_IF_PREFIX, strlen(PPP_IF_PREFIX)))
+			{
+				char *start = argv[i]+strlen(PPP_IF_PREFIX);
+				char *end;
+				long res = strtol(start, &end, 10);
+				if (end != start && !*end)
+					ifunit = res;
+			}
 
 			for (s = 0; s < MAXSESSION; s++)
-				if (!strcmp(argv[i], session[s].user))
+			{
+				if ((ifunit >= 0 && sess_local[s].ppp_if_unit == ifunit)
+					|| !strcmp(argv[i], session[s].user))
 					break;
+			}
 			if (s >= MAXSESSION)
 			{
 				s = atoi(argv[i]);
@@ -442,6 +486,8 @@ static int cmd_show_session(struct cli_def *cli, const char *command, char **arg
 			cli_print(cli, "\tRemote ID:\t%d", session[s].far);
 			if (session[s].bundle)
 				cli_print(cli, "\tBundle ID:\t%d (%d)", session[s].bundle, bundle[session[s].bundle].num_of_links);
+			if (sess_local[s].ppp_if_unit >= 0)
+				cli_print(cli, "\tInterface:\tppp%d", sess_local[s].ppp_if_unit);
 			cli_print(cli, "\tPPP Phase:\t%s", ppp_phase(session[s].ppp.phase));
 			switch (session[s].ppp.phase)
 			{

cluster.c

@@ -435,8 +435,7 @@ static void send_heartbeat(int seq, uint8_t *data, int size)
 	if (size > sizeof(past_hearts[0].data))
 	{
 		LOG(0, 0, 0, "Tried to heartbeat something larger than the maximum packet!\n");
-		kill(0, SIGTERM);
+		crash();
-		exit(1);
 	}
 	i = seq % HB_HISTORY_SIZE;
 	past_hearts[i].seq = seq;
@@ -903,8 +902,7 @@ static int hb_add_type(uint8_t **p, int type, int id)
 			break;
 		default:
 			LOG(0, 0, 0, "Found an invalid type in heart queue! (%d)\n", type);
-			kill(0, SIGTERM);
+			crash();
-			exit(1);
 	}
 	return 0;
 }
@@ -951,8 +949,7 @@ void cluster_heartbeat()
 
 	if (p > (buff + sizeof(buff))) {	// Did we somehow manage to overun the buffer?
 		LOG(0, 0, 0, "FATAL: Overran the heartbeat buffer! This is fatal. Exiting. (size %d)\n", (int) (p - buff));
-		kill(0, SIGTERM);
+		crash();
-		exit(1);
 	}
 
 		//
@@ -1011,8 +1008,7 @@ void cluster_heartbeat()
 		// Did we do something wrong?
 	if (p > (buff + sizeof(buff))) {	// Did we somehow manage to overun the buffer?
 		LOG(0, 0, 0, "Overran the heartbeat buffer now! This is fatal. Exiting. (size %d)\n", (int) (p - buff));
-		kill(0, SIGTERM);
+		crash();
-		exit(1);
 	}
 
 	LOG(4, 0, 0, "Sending v%d heartbeat #%d, change #%" PRIu64 " with %d changes "
@@ -1364,14 +1360,7 @@ static int cluster_recv_tunnel(int more, uint8_t *p)
 		}
 	}
 
-	memcpy(&tunnel[more], p, sizeof(tunnel[more]) );
+	load_tunnel(more, (tunnelt *) p);	// Copy tunnel into tunnel table..
-
-		//
-		// Clear tunnel control messages. These are dynamically allocated.
-		// If we get unlucky, this may cause the tunnel to drop!
-		//
-	tunnel[more].controls = tunnel[more].controle = NULL;
-	tunnel[more].controlc = 0;
 
 	LOG(5, 0, more, "Received tunnel update\n");
 
@@ -1967,8 +1956,7 @@ static int cluster_process_heartbeat(uint8_t *data, int size, int more, uint8_t
 			LOG(0, 0, 0, "They've seen more state changes (%" PRIu64 " vs my %" PRIu64 ") so I'm gone!\n",
 					h->table_version, config->cluster_table_version);
 
-			kill(0, SIGTERM);
+			crash();
-			exit(1);
 		}
 
 		if (h->table_version < config->cluster_table_version)
@@ -1976,8 +1964,7 @@ static int cluster_process_heartbeat(uint8_t *data, int size, int more, uint8_t
 
 		if (basetime > h->basetime) {
 			LOG(0, 0, 0, "They're an older master than me so I'm gone!\n");
-			kill(0, SIGTERM);
+			crash();
-			exit(1);
 		}
 
 		if (basetime < h->basetime)
@@ -1985,8 +1972,7 @@ static int cluster_process_heartbeat(uint8_t *data, int size, int more, uint8_t
 
 		if (my_address < addr) { // Tie breaker.
 			LOG(0, 0, 0, "They're a higher IP address than me, so I'm gone!\n");
-			kill(0, SIGTERM);
+			crash();
-			exit(1);
 		}
 
 			//
@@ -2373,8 +2359,7 @@ int processcluster(uint8_t *data, int size, in_addr_t addr)
 		}
 
 		LOG(0, 0, 0, "Received a valid C_KILL: I'm going to die now.\n");
-		kill(0, SIGTERM);
+		crash();
-		exit(0);	// Lets be paranoid;
 		return -1;		// Just signalling the compiler.
 
 	case C_HEARTBEAT:

dhcp6.c

@@ -4,13 +4,21 @@
  * GPL licenced
  */
 
+#define _GNU_SOURCE
 #include <netinet/icmp6.h>
 #include <netinet/ip6.h>
 #include <netinet/udp.h>
+#include <netinet/in.h>
+#include <time.h>
+#include <errno.h>
 
 #include "dhcp6.h"
 #include "l2tpns.h"
 #include "ipv6_u.h"
+#include "cluster.h"
+#include "util.h"
+
+int dhcpv6fd;
 
 struct dhcp6_in_option
 {
@@ -29,7 +37,7 @@ static struct dhcp6_in_option list_option;
 
 static int dhcpv6_format_dns_search_name(const char *strdns, uint8_t *buffer);
 
-static void dhcp6_send_reply(sessionidt s, tunnelidt t, struct in6_addr *ip6_src)
+static void dhcp6_send_reply(sessionidt s, tunnelidt t, const struct in6_addr *ip6_src)
 {
 	struct ip6_hdr *p_ip6_hdr;
 	struct udphdr *p_udp;
@@ -309,26 +317,22 @@ static char * get_msg_type(uint8_t type)
 	}
 }
 
-void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
+void dhcpv6_process(sessionidt s, tunnelidt t, const struct in6_addr *addr, uint8_t *p, uint16_t l)
 {
-	struct ip6_hdr *p_ip6_hdr_in;
+	struct dhcp6_mess_hdr *p_mess_hdr = (struct dhcp6_mess_hdr *) p;
-	struct dhcp6_mess_hdr *p_mess_hdr;
 	struct dhcp6_opt_h *p_opt;
 	uint8_t *p_end;
 	uint16_t len;
 
 	CSTAT(dhcpv6_process);
 
-	p_ip6_hdr_in = (struct ip6_hdr *) p;
-	p_mess_hdr = (struct dhcp6_mess_hdr *) (p + 48);
-
 	LOG(3, s, t, "Got DHCPv6 message Type: %s(%d)\n", get_msg_type(p_mess_hdr->type), p_mess_hdr->type);
 
 	if (!session[s].route6[0].ipv6route.s6_addr[0] || !session[s].route6[0].ipv6prefixlen)
 		return;
 
 	p_opt = (struct dhcp6_opt_h *) &p_mess_hdr[1];
-	p_end = ((uint8_t *)p_ip6_hdr_in) + ntohs(p_ip6_hdr_in->ip6_plen) + sizeof(*p_ip6_hdr_in);
+	p_end = p + l;
 	memset(&list_option, 0, sizeof(list_option));
 	list_option.p_mess_hdr = p_mess_hdr;
 	while (((uint8_t *)p_opt) < p_end)
@@ -413,7 +417,7 @@ void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 				return;
 			}
 
-			dhcp6_send_reply(s, t, &p_ip6_hdr_in->ip6_src);
+			dhcp6_send_reply(s, t, addr);
 		}
 		break;
 
@@ -458,8 +462,8 @@ void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 				return;
 			}
 
-			dhcp6_send_reply(s, t, &p_ip6_hdr_in->ip6_src);
+			dhcp6_send_reply(s, t, addr);
-			send_ipv6_ra(s, t, &p_ip6_hdr_in->ip6_src); // send a RA
+			send_ipv6_ra(s, t, addr); // send a RA
 		}
 		break;
 
@@ -489,7 +493,7 @@ void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 				return;
 			}
 
-			dhcp6_send_reply(s, t, &p_ip6_hdr_in->ip6_src);
+			dhcp6_send_reply(s, t, addr);
 		}
 		break;
 
@@ -501,7 +505,7 @@ void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 				return;
 			}
 
-			dhcp6_send_reply(s, t, &p_ip6_hdr_in->ip6_src);
+			dhcp6_send_reply(s, t, addr);
 		}
 		break;
 
@@ -527,6 +531,37 @@ void dhcpv6_process(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 	return;
 }
 
+void dhcpv6_process_from_ipv6(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
+{
+	struct ip6_hdr *p_ip6_hdr_in = (struct ip6_hdr *) p;
+	struct in6_addr *addr = &p_ip6_hdr_in->ip6_src;
+	uint16_t ipv6_len = ntohs(p_ip6_hdr_in->ip6_plen);
+
+	l -= sizeof(*p_ip6_hdr_in);
+	p += sizeof(*p_ip6_hdr_in);
+
+	if (ipv6_len > l)
+	{
+		LOG(5, 0, 0, "bogus IPv6 packet size??\n");
+		return;
+	}
+
+	if (p_ip6_hdr_in->ip6_nxt != IPPROTO_UDP)
+	{
+		LOG(5, 0, 0, "not UDP DHCP packet??\n");
+		return;
+	}
+	if (ipv6_len < sizeof(struct udphdr))
+	{
+		LOG(5, 0, 0, "bogus IPv6 packet size for UDP??\n");
+		return;
+	}
+	ipv6_len -= sizeof(struct udphdr);
+	p += sizeof(struct udphdr);
+
+	dhcpv6_process(s, t, addr, p, ipv6_len);
+}
+
 static int dhcpv6_format_dns_search_name(const char *strdns, uint8_t *buffer)
 {
 	int n = strlen(strdns);
@@ -570,6 +605,8 @@ static int dhcpv6_format_dns_search_name(const char *strdns, uint8_t *buffer)
 void dhcpv6_init(void)
 {
 	uint32_t id;
+	int on = 1;
+	struct sockaddr_in6 addr;
 
 	dhcp6_local_serverid.opt_hdr.code = htons(D6_OPT_SERVERID);
 	dhcp6_local_serverid.opt_hdr.len = htons(4 + sizeof(id));
@@ -582,4 +619,129 @@ void dhcpv6_init(void)
 		id = htobe32(0xFDFDFAFA);
 
 	memcpy(dhcp6_local_serverid.duid.u.ll.addr, &id, sizeof(id));
+
+	dhcpv6fd = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
+	if (dhcpv6fd < 0)
+		LOG(1, 0, 0, "DHCPv6: could not create UDP socket: %s\n", strerror(errno));
+
+#ifdef SO_REUSEPORT
+	if (setsockopt(dhcpv6fd, SOL_SOCKET, SO_REUSEPORT, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not set reusing port: %s\n", strerror(errno));
+#endif
+
+	if (setsockopt(dhcpv6fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not set reusing address: %s\n", strerror(errno));
+
+#ifdef IPV6_RECVPKTINFO
+	if (setsockopt(dhcpv6fd, SOL_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not request pktinfo: %s\n", strerror(errno));
+#else
+	if (setsockopt(dhcpv6fd, SOL_IPV6, IPV6_PKTINFO, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not request pktinfo: %s\n", strerror(errno));
+#endif
+#ifdef IPV6_V6ONLY
+	if (setsockopt(dhcpv6fd, SOL_IPV6, IPV6_V6ONLY, &on, sizeof(on)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not set v6only: %s\n", strerror(errno));
+#endif
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sin6_family = AF_INET6;
+	addr.sin6_port = htons(DHCP6_SERVER_PORT);
+	if (bind(dhcpv6fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
+		LOG(1, 0, 0, "DHCPv6: could not bind to DHCPv6 server port\n");
+}
+
+//
+// A new ppp interface was created, watch for DHCPv6 on it
+void dhcpv6_listen(int ifidx)
+{
+	struct ipv6_mreq mreq;
+
+	memset(&mreq, 0, sizeof(mreq));
+	mreq.ipv6mr_interface = ifidx;
+	inet_pton(AF_INET6, DHCP6_SERVER_ADDRESS, &mreq.ipv6mr_multiaddr);
+	if (setsockopt(dhcpv6fd, SOL_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)
+		LOG(2, 0, 0, "DHCPv6: could not join DHCPv6 group: %s\n", strerror(errno));
+}
+
+//
+// A DHCPv6 request was received on a ppp interface, receive it
+void dhcpv6_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(dhcpv6fd, 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, "DHCPV6: reception error: %s\n", strerror(errno));
+			lastwarn = now;
+		}
+		return;
+	}
+	LOG(5, 0, 0, "Got packet on DHCP socket on if %d\n", ifidx);
+
+	if (fromaddr.ss_family != AF_INET6)
+	{
+		LOG(5, 0, 0, "DHCPV6: got strange family %d\n", fromaddr.ss_family);
+		return;
+	}
+	sin6 = (struct sockaddr_in6 *) &fromaddr;
+
+	if (ntohs(sin6->sin6_port) != DHCP6_CLIENT_PORT)
+	{
+		LOG(5, 0, 0, "DHCPV6: got strange client port %d\n", ntohs(sin6->sin6_port));
+		return;
+	}
+
+	for (s = 1; s < MAXSESSION; s++)
+	{
+		if (sess_local[s].ppp_if_idx != ifidx)
+			continue;
+
+		t = session[s].tunnel;
+
+		if (config->cluster_iam_master)
+			dhcpv6_process(s, t, &sin6->sin6_addr, p, r);
+		else
+		{
+			// DHCPV6 must be managed by the Master.
+
+			// Fake UDPv6 header
+			struct udphdr *udp = (struct udphdr *)p - 1;
+			udp->source = sin6->sin6_port;
+			udp->dest = htons(DHCP6_SERVER_PORT);
+			udp->len = sizeof(*udp) + r;
+			// udp->check is not checked by Master anyway
+			r += sizeof(*udp);
+
+			struct ip6_hdr *ip6 = (struct ip6_hdr *)udp - 1;
+			ip6->ip6_flow = htonl(6);
+			ip6->ip6_plen = htons(r);
+			ip6->ip6_nxt = IPPROTO_UDP;
+			ip6->ip6_hlim = 255;
+			memcpy(&ip6->ip6_src, &sin6->sin6_addr, sizeof(sin6->sin6_addr));
+			memcpy(&ip6->ip6_dst, &toaddr, sizeof(toaddr));
+			r += sizeof(*ip6);
+
+			uint16_t *w = (uint16_t *)ip6 - 4;
+			w[0] = htons(0x0002); /* L2TP data*/
+			w[1] = htons(t);
+			w[2] = htons(s);
+			w[3] = htons(PPPIPV6); /* PPP protocol */
+			r += 8;
+
+			master_forward_packet((uint8_t *) w, r, htonl(tunnel[t].ip), htons(tunnel[t].port), tunnel[t].indexudp);
+		}
+
+		break;
+	}
 }

dhcp6.h

@@ -7,6 +7,10 @@
 #ifndef __DHCP6_H__
 #define __DHCP6_H__
 
+#define DHCP6_CLIENT_PORT             546
+#define DHCP6_SERVER_PORT             547
+#define DHCP6_SERVER_ADDRESS          "ff02::1:2"
+
 #define DHCP6_SOLICIT                 1
 #define DHCP6_ADVERTISE               2
 #define DHCP6_REQUEST                 3
@@ -212,7 +216,10 @@ struct dhcp6_opt_ia_prefix {
 } __attribute__((packed));
 
 // dhcp6.c
-void dhcpv6_process(uint16_t s, uint16_t t, uint8_t *p, uint16_t l);
+extern int dhcpv6fd;
+void dhcpv6_process_from_ipv6(uint16_t s, uint16_t t, uint8_t *p, uint16_t l);
 void dhcpv6_init(void);
+void dhcpv6_listen(int ifidx);
+void dhcpv6_process_from_kernel(uint8_t *p, size_t size_bufp);
 
 #endif /* __DHCP6_H__ */

docs/manpages/startup-config.5

@@ -412,6 +412,20 @@ connected users.
 .PP
 Number of token buckets to allocate for throttling.
 Each throttled session requires two buckets (in and out).
+.PP
+\f[B]kernel_accel\f[R] (boolean)
+.PP
+Determines whether or not to enable kernel acceleration.
+Note that only one l2tpns instance can use it per network namespace,
+otherwise they will step on each other.
+Also, if you have a lot of clients (e.g.\ at least a hundred), listening
+for DHCPv6 and RS requires a lot of igmp6 subscriptions, tuning sysctl
+may be needed, such as
+.PP
+sysctl net.core.optmem_max=1048576
+.PP
+otherwise the logs will mention failures to subscribe due to lack of
+memory.
 .SS DHCPv6 And IPv6 SETTINGS
 .PP
 \f[B]dhcp6_preferred_lifetime\f[R] (int)

docs/src/man/startup-config.5.md

@@ -251,6 +251,14 @@ The following `variables` may be set:
 
   Number of token buckets to allocate for throttling. Each throttled session requires two buckets (in and out).
 
+**kernel\_accel** (boolean)
+
+  Determines whether or not to enable kernel acceleration. Note that only one l2tpns instance can use it per network namespace, otherwise they will step on each other. Also, if you have a lot of clients (e.g. at least a hundred), listening for DHCPv6 and RS requires a lot of igmp6 subscriptions, tuning sysctl may be needed, such as
+
+sysctl net.core.optmem\_max=1048576
+
+otherwise the logs will mention failures to subscribe due to lack of memory.
+
 ## DHCPv6 And IPv6 SETTINGS
 
 **dhcp6\_preferred\_lifetime** (int)

etc/startup-config.default

@@ -146,3 +146,18 @@ set ppp_keepalive yes
 
 # Walled garden
 #load plugin "garden"
+
+# Kernel acceleration, enable on no more than one instance!
+#set kernel_accel yes
+#
+# You will probably want to also enable MSS clamping, which l2tpns won't be able to do any more:
+# iptables -A FORWARD -p tcp --tcp-flags SYN,RST SYN -j TCPMSS --clamp-mss-to-pmtu
+# ip6tables -A FORWARD -p tcp --tcp-flags SYN,RST SYN -j TCPMSS --clamp-mss-to-pmtu
+# or
+# nft add rule inet filter forward tcp flags syn tcp option maxseg size set rt mtu
+#
+# and allow dhcpv6 traffic:
+# iptables -A INPUT -i ppp+ -p udp --sport 546 --dport 547 -j ACCEPT
+#
+# and increase the memory available for igmp6 for DHCPv6 and RS:
+# sysctl net.core.optmem_max=10485760

garden.c

@@ -238,6 +238,8 @@ int garden_session(sessiont *s, int flag, char *newuser)
 
 	s->walled_garden = 0;
 
+	// TODO: try to enable kernel acceleration with switch_kernel_accel(s);
+
 	if (flag != F_CLEANUP)
 	{
 	    /* OK, we're up! */

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};
@@ -96,7 +145,7 @@ struct nd_opt_rdnss_info_l2tpns
 	struct in6_addr nd_opt_rdnssi[0];
 };
 
-void send_ipv6_ra(sessionidt s, tunnelidt t, struct in6_addr *ip)
+void send_ipv6_ra(sessionidt s, tunnelidt t, const struct in6_addr *ip)
 {
 	struct nd_opt_prefix_info *pinfo;
 	struct ip6_hdr *p_ip6_hdr;
@@ -203,3 +252,54 @@ void send_ipv6_ra(sessionidt s, tunnelidt t, 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;
+	}
+}

l2tplac.c

@@ -532,20 +532,8 @@ int lac_session_forward(uint8_t *buf, int len, sessionidt sess, uint16_t proto,
 	if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
 	{
 		session[sess].last_packet = session[sess].last_data = time_now;
-		// Update STAT IN
+		update_session_in_stat(sess, 1, len);
-		increment_counter(&session[sess].cin, &session[sess].cin_wrap, len);
+		update_session_out_stat(s, 1, len);
-		session[sess].cin_delta += len;
-		session[sess].pin++;
-		sess_local[sess].cin += len;
-		sess_local[sess].pin++;
-
-		session[s].last_data = time_now;
-		// Update STAT OUT
-		increment_counter(&session[s].cout, &session[s].cout_wrap, len); // byte count
-		session[s].cout_delta += len;
-		session[s].pout++;
-		sess_local[s].cout += len;
-		sess_local[s].pout++;
 	}
 	else
 		session[sess].last_packet = time_now;

l2tpns.h

@@ -13,6 +13,9 @@
 #include <sys/socket.h>
 #include <arpa/inet.h>
 #include <sys/types.h>
+#include <net/if.h>
+#include <linux/ppp_defs.h>
+#include <linux/if_ppp.h>
 #include <libcli.h>
 
 #define VERSION        "2.4.1"
@@ -28,6 +31,8 @@
 // Tunnel Id reserved for pppoe
 #define TUNNEL_ID_PPPOE	1
 
+#define PPP_IF_PREFIX "ppp"
+
 #define RADIUS_SHIFT	6
 #define RADIUS_FDS	(1 << RADIUS_SHIFT)
 #define RADIUS_MASK	((1 << RADIUS_SHIFT) - 1)
@@ -270,6 +275,7 @@ typedef struct controls		// control message
 {
 	struct controls *next;	// next in queue
 	uint16_t length;	// length
+	uint16_t ns;		// sequence number
 	uint8_t buf[MAXCONTROL];
 }
 controlt;
@@ -436,6 +442,9 @@ typedef struct
 	// last LCP Echo
 	time_t last_echo;
 
+	// Whether we tried to suggest the IPV6CP identifier option.
+	int tried_identifier;
+
 	// last unsolicited RA sent to user
 	time_t last_ra;
 
@@ -446,6 +455,23 @@ typedef struct
 	uint32_t jitteravg;
 	// time in milliseconds of the last fragment.
 	uint64_t prev_time;
+
+	// Pending kernel switch
+	int needs_switch;
+
+	// l2tp PPPoL2TP socket
+	int pppox_fd;
+	struct pppol2tp_ioc_stats last_stats;
+
+	// ppp channel
+	int ppp_chan_fd;
+
+	// ppp interface
+	int ppp_if_fd;
+	// ppp interface number (ppp%d)
+	int ppp_if_unit;
+	// ppp interface index (for rtnetlink etc.)
+	int ppp_if_idx;
 } sessionlocalt;
 
 // session flags
@@ -479,6 +505,13 @@ typedef struct
 }
 tunnelt;
 
+typedef struct
+{
+	controlt *controlr;	// queue of OoO-received messages
+	int l2tp_fd;		// kernel acceleration UDP socket
+}
+tunnellocalt;
+
 // 164 bytes per radius session
 typedef struct			// outstanding RADIUS requests
 {
@@ -821,6 +854,7 @@ typedef struct
 	uint32_t dhcp6_server_duid;		// DUID of dhcpv6 server (see rfc3315)
 	uint32_t dns6_lifetime;		// RDNSS lifetime default 1200 (see rfc6106, rfc4861) (MaxRtrAdvInterval <= Lifetime <= 2*MaxRtrAdvInterval)
 	char default_ipv6_domain_list[255];
+	int kernel_accel;	// Enable kernel-accelerated support
 } configt;
 
 enum config_typet { INT, STRING, UNSIGNED_LONG, SHORT, BOOL, IPv4, IPv6 };
@@ -960,8 +994,9 @@ int rad_tunnel_pwdecode(uint8_t *pl2tpsecret, size_t *pl2tpsecretlen, const char
 
 // l2tpns.c
 clockt backoff(uint8_t try);
-void send_ipv6_ra(sessionidt s, tunnelidt t, struct in6_addr *ip);
+void send_ipv6_ra(sessionidt s, tunnelidt t, const struct in6_addr *ip);
 void route6set(sessionidt s, struct in6_addr ip, int prefixlen, int add);
+void routes6set(sessionidt s, sessiont *sp, int add);
 sessionidt sessionbyip(in_addr_t ip);
 sessionidt sessionbyipv6(struct in6_addr ip);
 sessionidt sessionbyipv6new(struct in6_addr ip);
@@ -979,6 +1014,8 @@ void adjust_tcp6_mss(sessionidt s, tunnelidt t, uint8_t *buf, int len, uint8_t *
 void sendipcp(sessionidt s, tunnelidt t);
 void sendipv6cp(sessionidt s, tunnelidt t);
 void processudp(uint8_t *buf, int len, struct sockaddr_in *addr, uint16_t indexudpfd);
+void update_session_in_stat(sessionidt s, int packets, size_t len);
+void update_session_out_stat(sessionidt s, int packets, size_t len);
 void processipout(uint8_t *buf, int len);
 void snoop_send_packet(uint8_t *packet, uint16_t size, in_addr_t destination, uint16_t port);
 int find_filter(char const *name, size_t len);
@@ -1005,8 +1042,11 @@ int sessionsetup(sessionidt s, tunnelidt t);
 int run_plugins(int plugin_type, void *data);
 void rebuild_address_pool(void);
 void throttle_session(sessionidt s, int rate_in, int rate_out);
+int load_tunnel(tunnelidt, tunnelt *);
 int load_session(sessionidt, sessiont *);
+int create_kernel_bridge(sessionidt s, sessionidt fwds);
 void become_master(void);	// We're the master; kick off any required master initializations.
+void crash(void);		// We messed up. Die.
 
 
 // cli.c
@@ -1018,6 +1058,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);
 
 
@@ -1025,6 +1069,7 @@ extern tunnelt *tunnel;
 extern bundlet *bundle;
 extern sessiont *session;
 extern sessionlocalt *sess_local;
+extern tunnellocalt *tunn_local;
 extern ippoolt *ip_address_pool;
 #define sessionfree (session[0].next)
 
@@ -1037,7 +1082,10 @@ extern uint32_t last_id;
 extern struct Tstats *_statistics;
 extern in_addr_t my_address;
 extern int clifd;
+extern int rtnlfd;
+extern int genlfd;
 extern int epollfd;
+extern FILE *log_stream;
 
 struct event_data {
 	enum {
@@ -1049,9 +1097,15 @@ struct event_data {
 		FD_TYPE_DAE,
 		FD_TYPE_RADIUS,
 		FD_TYPE_BGP,
-		FD_TYPE_NETLINK,
+		FD_TYPE_RTNETLINK,
 		FD_TYPE_PPPOEDISC,
-		FD_TYPE_PPPOESESS
+		FD_TYPE_PPPOESESS,
+		FD_TYPE_L2TP,
+		FD_TYPE_PPPOX,
+		FD_TYPE_PPP_CHAN,
+		FD_TYPE_PPP_IF,
+		FD_TYPE_DHCPV6,
+		FD_TYPE_ICMPV6,
 	} type;
 	int index; // for RADIUS, BGP, UDP
 };

ppp.c

@@ -1479,21 +1479,10 @@ void processipcp(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 
 static void ipv6cp_open(sessionidt s, tunnelidt t)
 {
-	int i;
 	LOG(3, s, t, "IPV6CP: Opened\n");
 
 	change_state(s, ipv6cp, Opened);
-	for (i = 0; i < MAXROUTE6 && session[s].route6[i].ipv6prefixlen; i++)
+	routes6set(s, &session[s], 1);
-	{
-		route6set(s, session[s].route6[i].ipv6route, session[s].route6[i].ipv6prefixlen, 1);
-	}
-
-	if (session[s].ipv6address.s6_addr[0])
-	{
-		// Check if included in prefix
-		if (sessionbyipv6(session[s].ipv6address) != s)
-			route6set(s, session[s].ipv6address, 128, 1);
-	}
 
 	// Send an initial RA
 	send_ipv6_ra(s, t, NULL);
@@ -1571,6 +1560,17 @@ void processipv6cp(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 		int gotip = 0;
 		uint32_t ident[2];
 
+		if (session[s].ipv6address.s6_addr[0])
+		{
+			// LSB 64bits of assigned IPv6 address to user (see radius attribut Framed-IPv6-Address)
+			memcpy(&ident[0], &session[s].ipv6address.s6_addr[8], 8);
+		}
+		else
+		{
+			ident[0] = htonl(session[s].ip);
+			ident[1] = 0;
+		}
+
 		while (length > 2)
 		{
 			if (!o[1] || o[1] > length) return;
@@ -1581,17 +1581,6 @@ void processipv6cp(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 				gotip++; // seen address
 				if (o[1] != 10) return;
 
-				if (session[s].ipv6address.s6_addr[0])
-				{
-					// LSB 64bits of assigned IPv6 address to user (see radius attribut Framed-IPv6-Address)
-					memcpy(&ident[0], &session[s].ipv6address.s6_addr[8], 8);
-				}
-				else
-				{
-					ident[0] = htonl(session[s].ip);
-					ident[1] = 0;
-				}
-
 				if (memcmp(o + 2, ident, sizeof(ident)))
 				{
 					q = ppp_conf_nak(s, b, sizeof(b), PPPIPV6CP, &response, q, p, o, (uint8_t *)ident, sizeof(ident));
@@ -1610,24 +1599,30 @@ void processipv6cp(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 			o += o[1];
 		}
 
+		if (!response && !gotip && sess_local[s].tried_identifier++ < 2)
+		{
+			uint8_t identifier_option[6] = { 1, 6 };
+
+			// No interface identifier option, try to suggest one
+			q = ppp_conf_nak(s, b, sizeof(b), PPPIPV6CP, &response, q, p, identifier_option, (uint8_t *)ident, sizeof(ident));
+			if (!q) return;
+		}
+
 		if (response)
 		{
 			l = q - response; // IPV6CP packet length
 			*((uint16_t *) (response + 2)) = htons(l); // update header
 		}
-		else if (gotip)
+		else
 		{
+			if (!gotip)
+				LOG(2, s, t, "No interface identifier in IPV6CP request, hoping for the best\n");
+
 			// Send packet back as ConfigAck
 			response = makeppp(b, sizeof(b), p, l, s, t, PPPIPV6CP, 0, 0, 0);
 			if (!response) return;
 			*response = ConfigAck;
 		}
-		else
-		{
-			LOG(3, s, t, "No interface identifier in IPV6CP request\n");
-			STAT(tunnel_rx_errors);
-			return;
-		}
 
 		switch (session[s].ppp.ipv6cp)
 		{
@@ -1722,12 +1717,7 @@ static void update_sessions_in_stat(sessionidt s, uint16_t l)
 	bundleidt b = session[s].bundle;
 	if (!b)
 	{
-		increment_counter(&session[s].cin, &session[s].cin_wrap, l);
+		update_session_in_stat(s, 1, l);
-        	session[s].cin_delta += l;
-       		session[s].pin++;
-
-        	sess_local[s].cin += l;
-        	sess_local[s].pin++;
 	}
 	else
 	{
@@ -1737,12 +1727,8 @@ static void update_sessions_in_stat(sessionidt s, uint16_t l)
 		{
 			l = frag[b].fragment[i].length;
 			s = frag[b].fragment[i].sid;
-			increment_counter(&session[s].cin, &session[s].cin_wrap, l);
+			update_session_in_stat(s, 1, l);
-	                session[s].cin_delta += l;
-       		        session[s].pin++;
 
-                	sess_local[s].cin += l;
-                	sess_local[s].pin++;
 			if (i == end)
 				return;
 			i = (i + 1) & MAXFRAGNUM_MASK;
@@ -2306,7 +2292,7 @@ void processipv6in(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
 			*(uint16_t *)(p + 34) == 0 && *(p + 36) == 0 && *(p + 37) == 1 && *(p + 38) == 0 && *(p + 39) == 2 &&
 			*(p + 40) == 2 && *(p + 41) == 0x22 && *(p + 42) == 2 && *(p + 43) == 0x23)
 	{
-		dhcpv6_process(s, t, p, l);
+		dhcpv6_process_from_ipv6(s, t, p, l);
 		return;
 	}
 
@@ -2383,12 +2369,7 @@ void send_ipin(sessionidt s, uint8_t *buf, int len)
 	}
 
 	// Increment packet counters
-	increment_counter(&session[s].cin, &session[s].cin_wrap, len);
+	update_session_in_stat(s, 1, len);
-	session[s].cin_delta += len;
-	session[s].pin++;
-
-	sess_local[s].cin += len;
-	sess_local[s].pin++;
 
 	eth_tx += len;
 

pppoe.c

@@ -955,21 +955,9 @@ static void pppoe_forwardto_session_rmlns(uint8_t *pack, int size, sessionidt se
 
 	if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
 	{
-		session[sess].last_packet = session[sess].last_data = time_now;
+		session[sess].last_packet = time_now;
-		// Update STAT IN
+		update_session_in_stat(s, 1, ll2tp);
-		increment_counter(&session[sess].cin, &session[sess].cin_wrap, ll2tp);
+		update_session_out_stat(s, 1, ll2tp);
-		session[sess].cin_delta += ll2tp;
-		session[sess].pin++;
-		sess_local[sess].cin += ll2tp;
-		sess_local[sess].pin++;
-
-		session[s].last_data = time_now;
-		// Update STAT OUT
-		increment_counter(&session[s].cout, &session[s].cout_wrap, ll2tp); // byte count
-		session[s].cout_delta += ll2tp;
-		session[s].pout++;
-		sess_local[s].cout += ll2tp;
-		sess_local[s].pout++;
 	}
 	else
 		session[sess].last_packet = time_now;
@@ -1026,21 +1014,9 @@ void pppoe_forwardto_session_pppoe(uint8_t *pack, int size, sessionidt sess, uin
 
 	if ((proto == PPPIP) || (proto == PPPMP) ||(proto == PPPIPV6 && config->ipv6_prefix.s6_addr[0]))
 	{
-		session[sess].last_packet = session[sess].last_data = time_now;
+		session[sess].last_packet = time_now;
-		// Update STAT IN
+		update_session_in_stat(s, 1, lpppoe);
-		increment_counter(&session[sess].cin, &session[sess].cin_wrap, lpppoe);
+		update_session_out_stat(s, 1, lpppoe);
-		session[sess].cin_delta += lpppoe;
-		session[sess].pin++;
-		sess_local[sess].cin += lpppoe;
-		sess_local[sess].pin++;
-
-		session[s].last_data = time_now;
-		// Update STAT OUT
-		increment_counter(&session[s].cout, &session[s].cout_wrap, lpppoe); // byte count
-		session[s].cout_delta += lpppoe;
-		session[s].pout++;
-		sess_local[s].cout += lpppoe;
-		sess_local[s].pout++;
 	}
 	else
 		session[sess].last_packet = time_now;

util.c

@@ -1,9 +1,11 @@
 /* Misc util functions */
 
+#define _GNU_SOURCE
 #include <unistd.h>
 #include <errno.h>
 #include <sched.h>
 #include <sys/socket.h>
+#include <netinet/ip6.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include <string.h>
@@ -108,12 +110,8 @@ pid_t fork_and_close()
 		if (udpfd[i] != -1)	close(udpfd[i]);
 	}
 
-	if (pppoediscfd != -1)	close(pppoediscfd);
 	if (controlfd != -1)	close(controlfd);
 	if (daefd != -1)	close(daefd);
-	if (snoopfd != -1)	close(snoopfd);
-	if (rand_fd != -1)	close(rand_fd);
-	if (epollfd != -1)	close(epollfd);
 
 	for (i = 0; radfds && i < RADIUS_FDS; i++)
 		close(radfds[i]);
@@ -124,44 +122,116 @@ pid_t fork_and_close()
 			close(bgp_peers[i].sock);
 #endif /* BGP */
 
+	if (rtnlfd != -1)	close(rtnlfd);
+	if (genlfd != -1)	close(genlfd);
+	if (pppoediscfd != -1)	close(pppoediscfd);
+	if (pppoesessfd != -1)	close(pppoesessfd);
+
+	for (i = 0; i <= config->cluster_highest_tunnelid; i++)
+	{
+		if (tunn_local[i].l2tp_fd >= 0)		close(tunn_local[i].l2tp_fd);
+	}
+
+	for (i = 0; i <= config->cluster_highest_sessionid; i++)
+	{
+		if (sess_local[i].pppox_fd >= 0)	close(sess_local[i].pppox_fd);
+		if (sess_local[i].ppp_chan_fd >= 0)	close(sess_local[i].ppp_chan_fd);
+		if (sess_local[i].ppp_if_fd >= 0)	close(sess_local[i].ppp_if_fd);
+	}
+
+	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);
+
     return pid;
 }
 
+static ssize_t recvfromtox(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, char *cbuf, size_t cbuflen, struct msghdr *msg)
+{
+	ssize_t r;
+	struct iovec vec;
+
+	memset(msg, 0, sizeof(*msg));
+	msg->msg_name = from;
+	msg->msg_namelen = *fromlen;
+
+	vec.iov_base = buf;
+	vec.iov_len = len;
+	msg->msg_iov = &vec;
+	msg->msg_iovlen = 1;
+	msg->msg_flags = 0;
+
+	msg->msg_control = cbuf;
+	msg->msg_controllen = cbuflen;
+
+	if ((r = recvmsg(s, msg, flags)) < 0)
+		return r;
+
+	if (fromlen)
+		*fromlen = msg->msg_namelen;
+
+	return r;
+}
+
 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
-	struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
+    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr, int *ifidx)
 {
 	ssize_t r;
 	struct msghdr msg;
 	struct cmsghdr *cmsg;
-	struct iovec vec;
+	char cbuf[BUFSIZ];
-	char cbuf[128];
 
-	memset(&msg, 0, sizeof(msg));
+	if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
-	msg.msg_name = from;
-	msg.msg_namelen = *fromlen;
-
-	vec.iov_base = buf;
-	vec.iov_len = len;
-	msg.msg_iov = &vec;
-	msg.msg_iovlen = 1;
-	msg.msg_flags = 0;
-
-	msg.msg_control = cbuf;
-	msg.msg_controllen = sizeof(cbuf);
-
-	if ((r = recvmsg(s, &msg, flags)) < 0)
 		return r;
 
-	if (fromlen)
-		*fromlen = msg.msg_namelen;
 
-	memset(toaddr, 0, sizeof(*toaddr));
+	if (toaddr)
+		memset(toaddr, 0, sizeof(*toaddr));
+	if (ifidx)
+		*ifidx = -1;
 	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
 	{
 		if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
 		{
 			struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
-			memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
+			if (toaddr)
+				memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
+			if (ifidx)
+				*ifidx = i->ipi_ifindex;
+			break;
+		}
+	}
+
+	return r;
+}
+
+ssize_t recvfromto6(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, struct in6_addr *toaddr, int *ifidx)
+{
+	ssize_t r;
+	struct msghdr msg;
+	struct cmsghdr *cmsg;
+	char cbuf[BUFSIZ];
+
+	if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
+		return r;
+
+	if (toaddr)
+		memset(toaddr, 0, sizeof(*toaddr));
+	if (ifidx)
+		*ifidx = -1;
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
+	{
+		if (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO)
+		{
+			struct in6_pktinfo *i = (struct in6_pktinfo *) CMSG_DATA(cmsg);
+			if (toaddr)
+				memcpy(toaddr, &i->ipi6_addr, sizeof(*toaddr));
+			if (ifidx)
+				*ifidx = i->ipi6_ifindex;
 			break;
 		}
 	}

util.h

@@ -10,6 +10,9 @@ ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
     struct sockaddr const *to, socklen_t tolen, struct in_addr const *from);
 
 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
-    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr);
+    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr, int *ifidx);
+
+ssize_t recvfromto6(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, struct in6_addr *toaddr, int *ifidx);
 
 #endif /* __UTIL_H__ */