sgsnemu: Handle IPv6 SLAAC in tun iface manually

Disable IPv6 automatic SLAAC by linux kernel and handle it manually.
This allows us gaining control on local address acquisition and set
addresses and routing properly. It will also allow us to run in ping
mode without a tun iface.

Related: OS#4434

Change-Id: Iae59cf6ffb181357e10b3080a5c751bd454f4a1f

6 files changed, 307 insertions, 118 deletions

diff --git a/ggsn/ggsn.c b/ggsn/ggsn.c
index 3b10f70..159362f 100644
--- a/ggsn/ggsn.c
+++ b/ggsn/ggsn.c
@@ -636,11 +636,6 @@ static int cb_tun_ind(struct tun_t *tun, void *pack, unsigned len)
 	return 0;
 }
 
-/* RFC3307 link-local scope multicast address */
-static const struct in6_addr all_router_mcast_addr = {
-	.s6_addr = { 0xff,0x02,0,0,  0,0,0,0, 0,0,0,0,  0,0,0,2 }
-};
-
 /* MS-originated GTP1-U packet, needs to be sent via TUN device */
 static int encaps_tun(struct pdp_t *pdp, void *pack, unsigned len)
 {
diff --git a/lib/icmpv6.c b/lib/icmpv6.c
index ae72b4c..1bddf65 100644
--- a/lib/icmpv6.c
+++ b/lib/icmpv6.c
@@ -27,6 +27,7 @@
 #include "../gtp/pdp.h"
 #include "ippool.h"
 #include "syserr.h"
+#include "icmpv6.h"
 #include "config.h"
 
 /* 29.061 11.2.1.3.4 IPv6 Router Configuration Variables in GGSN */
@@ -35,61 +36,11 @@
 #define GGSN_AdvValidLifetime	0xffffffff	/* infinite */
 #define GGSN_AdvPreferredLifetime 0xffffffff	/* infinite */
 
-struct icmpv6_hdr {
-	uint8_t type;
-	uint8_t code;
-	uint16_t csum;
-} __attribute__ ((packed));
-
-/* RFC4861 Section 4.2 */
-struct icmpv6_radv_hdr {
-	struct icmpv6_hdr hdr;
-	uint8_t cur_ho_limit;
-#if BYTE_ORDER == LITTLE_ENDIAN
-	uint8_t res:6,
-		m:1,
-		o:1;
-#elif BYTE_ORDER == BIG_ENDIAN
-	uint8_t m:1,
-		o:1,
-		res:6;
-#else
-# error	"Please fix <bits/endian.h>"
-#endif
-	uint16_t router_lifetime;
-	uint32_t reachable_time;
-	uint32_t retrans_timer;
-	uint8_t options[0];
-} __attribute__ ((packed));
-
-/* RFC4861 Section 4.6 */
-struct icmpv6_opt_hdr {
-	uint8_t type;
-	/* length in units of 8 octets, including type+len! */
-	uint8_t len;
-	uint8_t data[0];
-} __attribute__ ((packed));
-
-/* RFC4861 Section 4.6.2 */
-struct icmpv6_opt_prefix {
-	struct icmpv6_opt_hdr hdr;
-	uint8_t prefix_len;
-#if BYTE_ORDER == LITTLE_ENDIAN
-	uint8_t res:6,
-		a:1,
-		l:1;
-#elif BYTE_ORDER == BIG_ENDIAN
-	uint8_t l:1,
-		a:1,
-		res:6;
-#else
-# error	"Please fix <bits/endian.h>"
-#endif
-	uint32_t valid_lifetime;
-	uint32_t preferred_lifetime;
-	uint32_t res2;
-	uint8_t prefix[16];
-} __attribute__ ((packed));
+/* RFC3307 link-local scope multicast address */
+const struct in6_addr all_router_mcast_addr = {
+	.s6_addr = { 0xff,0x02,0,0,  0,0,0,0, 0,0,0,0,  0,0,0,2 }
+};
+
 /* Prepends the ipv6 header and returns checksum content */
 static uint16_t icmpv6_prepend_ip6hdr(struct msgb *msg, const struct in6_addr *saddr,
 				  const struct in6_addr *daddr)
@@ -115,7 +66,26 @@ static uint16_t icmpv6_prepend_ip6hdr(struct msgb *msg, const struct in6_addr *s
 	return skb_csum;
 }
 
+/*! construct a RFC4861 compliant ICMPv6 router soliciation
+ *  \param[in] saddr Source IPv6 address for router advertisement
+ *  \param[in] daddr Destination IPv6 address for router advertisement IPv6 header
+ *  \param[in] prefix The single prefix to be advertised (/64 implied!)
+ *  \returns callee-allocated message buffer containing router advertisement */
+struct msgb *icmpv6_construct_rs(const struct in6_addr *saddr)
+{
+	struct msgb *msg = msgb_alloc_headroom(512,128, "IPv6 RS");
+	struct icmpv6_rsol_hdr *rs;
+	OSMO_ASSERT(msg);
+	rs = (struct icmpv6_rsol_hdr *) msgb_put(msg, sizeof(*rs));
+	rs->hdr.type = 133;	/* see RFC4861 4.1 */
+	rs->hdr.code = 0;	/* see RFC4861 4.1 */
+	rs->hdr.csum = 0;	/* updated below */
+	rs->reserved = 0;	/* see RFC4861 4.1 */
+
+	rs->hdr.csum = icmpv6_prepend_ip6hdr(msg, saddr, &all_router_mcast_addr);
 
+	return msg;
+}
 /*! construct a 3GPP 29.061 compliant router advertisement for a given prefix
  *  \param[in] saddr Source IPv6 address for router advertisement
  *  \param[in] daddr Destination IPv6 address for router advertisement IPv6 header
@@ -188,6 +158,37 @@ static bool icmpv6_validate_router_solicit(const uint8_t *pack, unsigned len)
 	return true;
 }
 
+/* Validate an ICMPv6 router advertisement according to RFC4861 6.1.2.
+   Returns pointer packet header on success, NULL otherwise. */
+struct icmpv6_radv_hdr *icmpv6_validate_router_adv(const uint8_t *pack, unsigned len)
+{
+	const struct ip6_hdr *ip6h = (struct ip6_hdr *)pack;
+	const struct icmpv6_hdr *ic6h = (struct icmpv6_hdr *) (pack + sizeof(*ip6h));
+
+	/* ICMP length (derived from IP length) is 16 or more octets */
+	if (len < sizeof(*ip6h) + 16)
+		return NULL;
+
+	if (ic6h->type != 134) /* router advertismenet type */
+		return NULL;
+
+	/*Routers must use their link-local address */
+	if (!IN6_IS_ADDR_LINKLOCAL(&ip6h->ip6_src))
+		return NULL;
+	/* Hop limit field must have 255 */
+	if (ip6h->ip6_ctlun.ip6_un1.ip6_un1_hlim != 255)
+		return NULL;
+	/* ICMP Code is 0 */
+	if (ic6h->code != 0)
+		return NULL;
+	/* ICMP length (derived from IP length) is 16 or more octets */
+	if (ip6h->ip6_ctlun.ip6_un1.ip6_un1_plen < 16)
+		return NULL;
+	/* FIXME: All included options have a length > 0 */
+	/* FIXME: If IP source is unspecified, no source link-layer addr option */
+	return (struct icmpv6_radv_hdr *)ic6h;
+}
+
 /* handle incoming packets to the all-routers multicast address */
 int handle_router_mcast(struct gsn_t *gsn, struct pdp_t *pdp,
 			const struct in6_addr *pdp_prefix,
diff --git a/lib/icmpv6.h b/lib/icmpv6.h
index bf91e27..44b9b73 100644
--- a/lib/icmpv6.h
+++ b/lib/icmpv6.h
@@ -1,9 +1,90 @@
 #pragma once
 
+#include <stdbool.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/endian.h>
+
 #include "../gtp/gtp.h"
 #include "../gtp/pdp.h"
 
+#define ICMPv6_OPT_TYPE_PREFIX_INFO 0x03
+
+#define foreach_icmpv6_opt(icmpv6_pkt, icmpv6_len, opt_hdr) \
+		for (opt_hdr = (struct icmpv6_opt_hdr *)(icmpv6_pkt)->options; \
+		     (uint8_t*)(opt_hdr) + sizeof(struct icmpv6_opt_hdr) <= (((uint8_t*)(icmpv6_pkt)) + (icmpv6_len)); \
+		     opt_hdr = (struct icmpv6_opt_hdr*)((uint8_t*)(opt_hdr) + (opt_hdr)->len) \
+		    )
+
+struct icmpv6_hdr {
+	uint8_t type;
+	uint8_t code;
+	uint16_t csum;
+} __attribute__ ((packed));
+
+/* RFC4861 Section 4.1 */
+struct icmpv6_rsol_hdr {
+	struct icmpv6_hdr hdr;
+	uint32_t reserved;
+	uint8_t options[0];
+} __attribute__ ((packed));
+
+/* RFC4861 Section 4.2 */
+struct icmpv6_radv_hdr {
+	struct icmpv6_hdr hdr;
+	uint8_t cur_ho_limit;
+#if OSMO_IS_LITTLE_ENDIAN
+	uint8_t res:6,
+		m:1,
+		o:1;
+#else
+	uint8_t m:1,
+		o:1,
+		res:6;
+#endif
+	uint16_t router_lifetime;
+	uint32_t reachable_time;
+	uint32_t retrans_timer;
+	uint8_t options[0];
+} __attribute__ ((packed));
+
+
+/* RFC4861 Section 4.6 */
+struct icmpv6_opt_hdr {
+	uint8_t type;
+	/* length in units of 8 octets, including type+len! */
+	uint8_t len;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+/* RFC4861 Section 4.6.2 */
+struct icmpv6_opt_prefix {
+	struct icmpv6_opt_hdr hdr;
+	uint8_t prefix_len;
+#if OSMO_IS_LITTLE_ENDIAN
+	uint8_t res:6,
+		a:1,
+		l:1;
+#else
+	uint8_t l:1,
+		a:1,
+		res:6;
+#endif
+	uint32_t valid_lifetime;
+	uint32_t preferred_lifetime;
+	uint32_t res2;
+	uint8_t prefix[16];
+} __attribute__ ((packed));
+
+struct msgb *icmpv6_construct_rs(const struct in6_addr *saddr);
+
 int handle_router_mcast(struct gsn_t *gsn, struct pdp_t *pdp,
 			const struct in6_addr *pdp_prefix,
 			const struct in6_addr *own_ll_addr,
 			const uint8_t *pack, unsigned len);
+
+struct icmpv6_radv_hdr *icmpv6_validate_router_adv(const uint8_t *pack, unsigned len);
+
+
+/* RFC3307 link-local scope multicast address */
+extern const struct in6_addr all_router_mcast_addr;
diff --git a/lib/netdev.c b/lib/netdev.c
index 4d171c9..fd3caff 100644
--- a/lib/netdev.c
+++ b/lib/netdev.c
@@ -643,6 +643,59 @@ static int netdev_route4(struct in_addr *dst, struct in_addr *gateway, struct in
 	return 0;
 }
 
+static int netdev_route6(struct in6_addr *dst, struct in6_addr *gateway, int prefixlen, const char *gw_iface, int delete)
+{
+	int fd;
+#if defined(__linux__)
+	struct in6_rtmsg r;
+	struct ifreq ifr;
+
+	memset(&r, 0, sizeof(r));
+	r.rtmsg_flags = RTF_UP | RTF_GATEWAY; /* RTF_HOST not set */
+	r.rtmsg_metric = 1;
+
+	/* Create a channel to the NET kernel. */
+	if ((fd = socket(AF_INET6, SOCK_DGRAM, 0)) < 0) {
+		SYS_ERR(DTUN, LOGL_ERROR, errno, "socket() failed");
+		return -1;
+	}
+
+	if (gw_iface) {
+		strncpy(ifr.ifr_name, gw_iface, IFNAMSIZ);
+		ifr.ifr_name[IFNAMSIZ - 1] = 0; /* Make sure to terminate */
+		if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+			SYS_ERR(DTUN, LOGL_ERROR, errno,
+				"ioctl(SIOCGIFINDEX) failed");
+			close(fd);
+			return -1;
+		}
+		r.rtmsg_ifindex = ifr.ifr_ifindex;
+	}
+
+	memcpy(&r.rtmsg_dst, dst->s6_addr, sizeof(struct in6_addr));
+	memcpy(&r.rtmsg_gateway, gateway->s6_addr, sizeof(struct in6_addr));
+	r.rtmsg_dst_len = prefixlen;
+
+	if (delete) {
+		if (ioctl(fd, SIOCDELRT, (void *)&r) < 0) {
+			SYS_ERR(DTUN, LOGL_ERROR, errno,
+				"ioctl(SIOCDELRT) failed");
+			close(fd);
+			return -1;
+		}
+	} else {
+		if (ioctl(fd, SIOCADDRT, (void *)&r) < 0) {
+			SYS_ERR(DTUN, LOGL_ERROR, errno,
+				"ioctl(SIOCADDRT) failed");
+			close(fd);
+			return -1;
+		}
+	}
+	close(fd);
+#endif
+	return 0;
+}
+
 int netdev_addroute4(struct in_addr *dst, struct in_addr *gateway, struct in_addr *mask)
 {
 	return netdev_route4(dst, gateway, mask, 0);
@@ -653,6 +706,18 @@ int netdev_delroute4(struct in_addr *dst, struct in_addr *gateway, struct in_add
 	return netdev_route4(dst, gateway, mask, 1);
 }
 
+int netdev_addroute6(struct in6_addr *dst, struct in6_addr *gateway, int prefixlen, const char *gw_iface)
+{
+	return netdev_route6(dst, gateway, prefixlen, gw_iface, 0);
+}
+
+int netdev_delroute6(struct in6_addr *dst, struct in6_addr *gateway, int prefixlen, const char *gw_iface)
+{
+	return netdev_route6(dst, gateway, prefixlen, gw_iface, 1);
+}
+
+
+
 #include <ifaddrs.h>
 
 /*! Obtain the local address of a network device
diff --git a/lib/netdev.h b/lib/netdev.h
index 5dab27f..1bce814 100644
--- a/lib/netdev.h
+++ b/lib/netdev.h
@@ -67,6 +67,8 @@ extern int netdev_addaddr6(const char *devname, struct in6_addr *addr,
 
 extern int netdev_addroute4(struct in_addr *dst, struct in_addr *gateway, struct in_addr *mask);
 extern int netdev_delroute4(struct in_addr *dst, struct in_addr *gateway, struct in_addr *mask);
+extern int netdev_addroute6(struct in6_addr *dst, struct in6_addr *gateway, int prefixlen, const char *gw_iface);
+extern int netdev_delroute6(struct in6_addr *dst, struct in6_addr *gateway, int prefixlen, const char *gw_iface);
 
 extern int netdev_ip_local_get(const char *devname, struct in46_prefix *prefix_list,
 				size_t prefix_size, int flags);
diff --git a/sgsnemu/sgsnemu.c b/sgsnemu/sgsnemu.c
index ea52da6..69879dd 100644
--- a/sgsnemu/sgsnemu.c
+++ b/sgsnemu/sgsnemu.c
@@ -58,6 +58,7 @@
 #include "../lib/ippool.h"
 #include "../lib/syserr.h"
 #include "../lib/netns.h"
+#include "../lib/icmpv6.h"
 #include "../gtp/pdp.h"
 #include "../gtp/gtp.h"
 #include "cmdline.h"
@@ -966,41 +967,6 @@ static int process_options(int argc, char **argv)
 
 }
 
-/* read a single value from a /procc file, up to 255 bytes, callee-allocated */
-static char *proc_read(const char *path)
-{
-	char *ret = NULL;
-	FILE *f;
-
-	f = fopen(path, "r");
-	if (!f)
-		return NULL;
-
-	ret = malloc(256);
-	if (!ret)
-		goto out;
-
-	if (!fgets(ret, 256, f)) {
-		free(ret);
-		ret = NULL;
-		goto out;
-	}
-
-out:
-	fclose(f);
-	return ret;
-}
-
-/* Read value of a /proc/sys/net/ipv6/conf file for given device.
- * Memory is dynamically allocated, caller must free it later. */
-static char *proc_ipv6_conf_read(const char *dev, const char *file)
-{
-	const char *fmt = "/proc/sys/net/ipv6/conf/%s/%s";
-	char path[strlen(fmt) + strlen(dev) + strlen(file)+1];
-	snprintf(path, sizeof(path), fmt, dev, file);
-	return proc_read(path);
-}
-
 /* write a single value to a /proc file */
 static int proc_write(const char *path, const char *value)
 {
@@ -1522,8 +1488,10 @@ static int create_pdp_conf(struct pdp_t *pdp, void *cbp, int cause)
 				if (in46a_is_v4(&addr[i])) {
 					struct in_addr rm;
 					rm.s_addr = 0;
-					netdev_addroute4(&rm, &addr[i].v4, &rm);
-				}
+					if (netdev_addroute4(&rm, &addr[i].v4, &rm) < 0) {
+						SYS_ERR(DSGSN, LOGL_ERROR, 0, "Failed adding default route to %s", in46a_ntoa(&addr[i]));
+					}
+				} /* else: route will be set up once we have a global link address (Router Advertisement) */
 			}
 			if (options.ipup)
 				tun_runscript(tun, options.ipup);
@@ -1532,29 +1500,21 @@ static int create_pdp_conf(struct pdp_t *pdp, void *cbp, int cause)
 		ipset(iph, &addr[i]);
 	}
 
-	/* now that ip-up has been executed, check if we are configured to
-	 * accept router advertisements */
 	if (options.createif && options.pdp_type == PDP_EUA_TYPE_v6) {
-		char *accept_ra, *forwarding;
-
-		accept_ra = proc_ipv6_conf_read(tun->devname, "accept_ra");
-		forwarding = proc_ipv6_conf_read(tun->devname, "forwarding");
-		if (!accept_ra || !forwarding)
-			printf("Could not open proc file for %s ?!?\n", tun->devname);
-		else {
-			if (!strcmp(accept_ra, "0")) {
-				printf("accept_ra=0, i.e. your tun device is not configured to accept "
-					"router advertisements; SLAAC will not succeed, please "
-					"fix your setup!\n");
-			}
-			if (!strcmp(forwarding, "1") && !strcmp(accept_ra, "1")) {
-				printf("forwarding=1 and accept_ra=1, i.e. your tun device is not "
-					"configured to accept router advertisements; SLAAC will not "
-					"succeed, please fix your setup!\n");
-			}
+		struct in6_addr *saddr6;
+		struct msgb *msg;
+		if (in46a_is_v6(&addr[0])) {
+			saddr6 = &addr[0].v6;
+		} else if (num_addr > 1 && in46a_is_v6(&addr[1])) {
+			saddr6 = &addr[1].v6;
+		} else {
+			SYS_ERR(DSGSN, LOGL_ERROR, 0, "Failed to find IPv6 EUA on IPv6 APN");
+			return EOF;	/* Not a valid IP address */
 		}
-		free(accept_ra);
-		free(forwarding);
+		SYS_ERR(DSGSN, LOGL_INFO, 0, "Sending ICMPv6 Router Soliciation to GGSN...");
+		msg = icmpv6_construct_rs(saddr6);
+		gtp_data_req(gsn, iph->pdp, msgb_data(msg), msgb_length(msg));
+		msgb_free(msg);
 	}
 
 #if defined(__linux__)
@@ -1618,8 +1578,83 @@ static int _gtp_cb_conf(int type, int cause, struct pdp_t *pdp, void *cbp)
 	}
 }
 
+static void handle_router_adv(struct ip6_hdr *ip6h, struct icmpv6_radv_hdr *ra, size_t ra_len)
+{
+	struct icmpv6_opt_hdr *opt_hdr;
+	struct icmpv6_opt_prefix *opt_prefix;
+	int rc;
+	sigset_t oldmask;
+	struct in6_addr rm;
+	char ip6strbuf[200];
+	memset(&rm, 0, sizeof(rm));
+
+	SYS_ERR(DSGSN, LOGL_INFO, 0, "Received ICMPv6 Router Advertisement");
+
+	foreach_icmpv6_opt(ra, ra_len, opt_hdr) {
+		if (opt_hdr->type == ICMPv6_OPT_TYPE_PREFIX_INFO) {
+			opt_prefix = (struct icmpv6_opt_prefix *)opt_hdr;
+			size_t prefix_len_bytes = (opt_prefix->prefix_len + 7)/8;
+			SYS_ERR(DSGSN, LOGL_INFO, 0, "Parsing OPT Prefix info (prefix_len=%u): %s",
+				opt_prefix->prefix_len,
+				osmo_hexdump((const unsigned char *)opt_prefix->prefix, prefix_len_bytes));
+			if ((options.createif) && (!options.netaddr.len)) {
+				struct in46_addr addr;
+				addr.len = 16;
+				memcpy(addr.v6.s6_addr, opt_prefix->prefix, prefix_len_bytes);
+				memset(&addr.v6.s6_addr[prefix_len_bytes], 0, 16 - prefix_len_bytes);
+				addr.v6.s6_addr[15] = 0x02;
+				SYS_ERR(DSGSN, LOGL_INFO, 0, "Adding addr %s to tun %s",
+					in46a_ntoa(&addr), tun->devname);
+
+#if defined(__linux__)
+				if ((options.netns)) {
+					if ((rc = switch_ns(netns, &oldmask)) < 0) {
+						SYS_ERR(DSGSN, LOGL_ERROR, 0,
+							"Failed to switch to netns %s: %s",
+							options.netns, strerror(-rc));
+					}
+				}
+#endif
+				rc = tun_addaddr(tun, &addr, NULL, opt_prefix->prefix_len);
+				if (rc < 0) {
+					SYS_ERR(DSGSN, LOGL_ERROR, 0, "Failed to add addr %s to tun %s",
+						in46a_ntoa(&addr), tun->devname);
+				}
+
+				struct in6_addr rm;
+				memset(&rm, 0, sizeof(rm));
+				if (netdev_addroute6(&rm, &ip6h->ip6_src, 0, tun->devname) < 0) {
+					SYS_ERR(DSGSN, LOGL_ERROR, 0, "Failed adding default route to %s", inet_ntop(AF_INET6, &ip6h->ip6_src, ip6strbuf, sizeof(ip6strbuf)));
+				}
+
+#if defined(__linux__)
+				if ((options.netns)) {
+					if ((rc = restore_ns(&oldmask)) < 0) {
+						SYS_ERR(DSGSN, LOGL_ERROR, 0,
+							"Failed to switch to original netns: %s",
+							strerror(-rc));
+					}
+				}
+#endif
+			}
+		}
+	}
+}
+
 static int encaps_tun(struct pdp_t *pdp, void *pack, unsigned len)
 {
+	struct iphdr *iph = (struct iphdr *)pack;
+	struct icmpv6_radv_hdr *ra;
+	switch (iph->version) {
+	case 6:
+		if ((ra = icmpv6_validate_router_adv(pack, len))) {
+			size_t ra_len = (uint8_t*)ra - (uint8_t*)pack;
+			handle_router_adv((struct ip6_hdr *)pack, ra, ra_len);
+			return 0;
+		}
+	break;
+	}
+
 	/*  printf("encaps_tun. Packet received: forwarding to tun\n"); */
 	return tun_encaps((struct tun_t *)pdp->ipif, pack, len);
 }
@@ -1721,6 +1756,12 @@ int main(int argc, char **argv)
 		tun_set_cb_ind(tun, cb_tun_ind);
 		if (tun->fd > maxfd)
 			maxfd = tun->fd;
+
+		if (proc_ipv6_conf_write(options.tun_dev_name, "accept_ra", "0") < 0) {
+			SYS_ERR(DSGSN, LOGL_ERROR, 0,
+				"Failed to disable IPv6 SLAAC on %s\n", options.tun_dev_name);
+			exit(1);
+		}
 	}
 
 	if ((options.createif) && (options.netaddr.len)) {
@@ -1730,6 +1771,10 @@ int main(int argc, char **argv)
 				struct in_addr rm;
 				rm.s_addr = 0;
 				netdev_addroute4(&rm, &options.netaddr.v4, &rm);
+			} else {
+				struct in6_addr rm;
+				memset(&rm, 0, sizeof(rm));
+				netdev_addroute6(&rm, &options.netaddr.v6, 0, tun->devname);
 			}
 		}
 		if (options.ipup)