Move icmpv6 and checksum files from ggsn/ dir to lib/

They will be required by sgsnemu to implement ICMPv6 Router
Soliciations.

Change-Id: Ie878604f0fc0169cc98a1e9eee64b14d76be2c45

5 files changed, 467 insertions, 2 deletions

diff --git a/lib/Makefile.am b/lib/Makefile.am
index f2c5dc9..5bd9443 100644
--- a/lib/Makefile.am
+++ b/lib/Makefile.am
@@ -1,10 +1,10 @@
 noinst_LIBRARIES = libmisc.a
 
-noinst_HEADERS = gnugetopt.h ippool.h lookup.h syserr.h tun.h in46_addr.h netdev.h gtp-kernel.h netns.h util.h
+noinst_HEADERS = gnugetopt.h ippool.h lookup.h syserr.h tun.h in46_addr.h netdev.h gtp-kernel.h netns.h util.h icmpv6.h checksum.h
 
 AM_CFLAGS = -O2 -fno-builtin -Wall -DSBINDIR='"$(sbindir)"' -ggdb $(LIBOSMOCORE_CFLAGS)
 
-libmisc_a_SOURCES = getopt1.c getopt.c ippool.c lookup.c tun.c debug.c in46_addr.c netdev.c netns.c util.c
+libmisc_a_SOURCES = getopt1.c getopt.c ippool.c lookup.c tun.c debug.c in46_addr.c netdev.c netns.c util.c icmpv6.c checksum.c
 
 if ENABLE_GTP_KERNEL
 AM_CFLAGS += -DGTP_KERNEL $(LIBGTPNL_CFLAGS)
diff --git a/lib/checksum.c b/lib/checksum.c
new file mode 100644
index 0000000..4b23897
--- /dev/null
+++ b/lib/checksum.c
@@ -0,0 +1,211 @@
+/*
+ *
+ * INET		An implementation of the TCP/IP protocol suite for the LINUX
+ *		operating system.  INET is implemented using the  BSD Socket
+ *		interface as the means of communication with the user level.
+ *
+ *		IP/TCP/UDP checksumming routines
+ *
+ * Authors:	Jorge Cwik, <jorge@laser.satlink.net>
+ *		Arnt Gulbrandsen, <agulbra@nvg.unit.no>
+ *		Tom May, <ftom@netcom.com>
+ *		Andreas Schwab, <schwab@issan.informatik.uni-dortmund.de>
+ *		Lots of code moved from tcp.c and ip.c; see those files
+ *		for more names.
+ *
+ * 03/02/96	Jes Sorensen, Andreas Schwab, Roman Hodek:
+ *		Fixed some nasty bugs, causing some horrible crashes.
+ *		A: At some points, the sum (%0) was used as
+ *		length-counter instead of the length counter
+ *		(%1). Thanks to Roman Hodek for pointing this out.
+ *		B: GCC seems to mess up if one uses too many
+ *		data-registers to hold input values and one tries to
+ *		specify d0 and d1 as scratch registers. Letting gcc
+ *		choose these registers itself solves the problem.
+ *
+ *		This program is free software; you can redistribute it and/or
+ *		modify it under the terms of the GNU General Public License
+ *		as published by the Free Software Foundation; either version
+ *		2 of the License, or (at your option) any later version.
+ */
+
+/* Revised by Kenneth Albanowski for m68knommu. Basic problem: unaligned access
+ kills, so most of the assembly has to go. */
+
+#if defined(__FreeBSD__)
+#define _KERNEL	/* needed on FreeBSD 10.x for s6_addr32 */
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <sys/endian.h>
+#endif
+
+#include "checksum.h"
+#include <arpa/inet.h>
+
+static inline unsigned short from32to16(unsigned int x)
+{
+	/* add up 16-bit and 16-bit for 16+c bit */
+	x = (x & 0xffff) + (x >> 16);
+	/* add up carry.. */
+	x = (x & 0xffff) + (x >> 16);
+	return x;
+}
+
+static unsigned int do_csum(const unsigned char *buff, int len)
+{
+	int odd;
+	unsigned int result = 0;
+
+	if (len <= 0)
+		goto out;
+	odd = 1 & (unsigned long) buff;
+	if (odd) {
+#if BYTE_ORDER == LITTLE_ENDIAN
+		result += (*buff << 8);
+#else
+		result = *buff;
+#endif
+		len--;
+		buff++;
+	}
+	if (len >= 2) {
+		if (2 & (unsigned long) buff) {
+			result += *(unsigned short *) buff;
+			len -= 2;
+			buff += 2;
+		}
+		if (len >= 4) {
+			const unsigned char *end = buff + ((unsigned)len & ~3);
+			unsigned int carry = 0;
+			do {
+				unsigned int w = *(unsigned int *) buff;
+				buff += 4;
+				result += carry;
+				result += w;
+				carry = (w > result);
+			} while (buff < end);
+			result += carry;
+			result = (result & 0xffff) + (result >> 16);
+		}
+		if (len & 2) {
+			result += *(unsigned short *) buff;
+			buff += 2;
+		}
+	}
+	if (len & 1)
+#if BYTE_ORDER == LITTLE_ENDIAN
+		result += *buff;
+#else
+		result += (*buff << 8);
+#endif
+	result = from32to16(result);
+	if (odd)
+		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
+out:
+	return result;
+}
+
+/*
+ *	This is a version of ip_compute_csum() optimized for IP headers,
+ *	which always checksum on 4 octet boundaries.
+ */
+uint16_t ip_fast_csum(const void *iph, unsigned int ihl)
+{
+	return (uint16_t)~do_csum(iph, ihl*4);
+}
+
+/*
+ * computes the checksum of a memory block at buff, length len,
+ * and adds in "sum" (32-bit)
+ *
+ * returns a 32-bit number suitable for feeding into itself
+ * or csum_tcpudp_magic
+ *
+ * this function must be called with even lengths, except
+ * for the last fragment, which may be odd
+ *
+ * it's best to have buff aligned on a 32-bit boundary
+ */
+uint32_t csum_partial(const void *buff, int len, uint32_t wsum)
+{
+	unsigned int sum = (unsigned int)wsum;
+	unsigned int result = do_csum(buff, len);
+
+	/* add in old sum, and carry.. */
+	result += sum;
+	if (sum > result)
+		result += 1;
+	return (uint32_t)result;
+}
+
+/*
+ * this routine is used for miscellaneous IP-like checksums, mainly
+ * in icmp.c
+ */
+uint16_t ip_compute_csum(const void *buff, int len)
+{
+	return (uint16_t)~do_csum(buff, len);
+}
+
+uint16_t csum_ipv6_magic(const struct in6_addr *saddr,
+			const struct in6_addr *daddr,
+			uint32_t len, uint8_t proto, uint32_t csum)
+{
+	int carry;
+	uint32_t ulen;
+	uint32_t uproto;
+	uint32_t sum = (uint32_t)csum;
+
+	sum += (uint32_t)saddr->s6_addr32[0];
+	carry = (sum < (uint32_t)saddr->s6_addr32[0]);
+	sum += carry;
+
+	sum += (uint32_t)saddr->s6_addr32[1];
+	carry = (sum < (uint32_t)saddr->s6_addr32[1]);
+	sum += carry;
+
+	sum += (uint32_t)saddr->s6_addr32[2];
+	carry = (sum < (uint32_t)saddr->s6_addr32[2]);
+	sum += carry;
+
+	sum += (uint32_t)saddr->s6_addr32[3];
+	carry = (sum < (uint32_t)saddr->s6_addr32[3]);
+	sum += carry;
+
+	sum += (uint32_t)daddr->s6_addr32[0];
+	carry = (sum < (uint32_t)daddr->s6_addr32[0]);
+	sum += carry;
+
+	sum += (uint32_t)daddr->s6_addr32[1];
+	carry = (sum < (uint32_t)daddr->s6_addr32[1]);
+	sum += carry;
+
+	sum += (uint32_t)daddr->s6_addr32[2];
+	carry = (sum < (uint32_t)daddr->s6_addr32[2]);
+	sum += carry;
+
+	sum += (uint32_t)daddr->s6_addr32[3];
+	carry = (sum < (uint32_t)daddr->s6_addr32[3]);
+	sum += carry;
+
+	ulen = (uint32_t)htonl((uint32_t) len);
+	sum += ulen;
+	carry = (sum < ulen);
+	sum += carry;
+
+	uproto = (uint32_t)htonl(proto);
+	sum += uproto;
+	carry = (sum < uproto);
+	sum += carry;
+
+	return csum_fold((uint32_t)sum);
+}
+
+/* fold a partial checksum */
+uint16_t csum_fold(uint32_t csum)
+{
+	uint32_t sum = (uint32_t)csum;
+	sum = (sum & 0xffff) + (sum >> 16);
+	sum = (sum & 0xffff) + (sum >> 16);
+	return (uint16_t)~sum;
+}
diff --git a/lib/checksum.h b/lib/checksum.h
new file mode 100644
index 0000000..4b22431
--- /dev/null
+++ b/lib/checksum.h
@@ -0,0 +1,13 @@
+#pragma once
+#include <stdint.h>
+#include <netinet/in.h>
+
+uint16_t ip_fast_csum(const void *iph, unsigned int ihl);
+uint32_t csum_partial(const void *buff, int len, uint32_t wsum);
+uint16_t ip_compute_csum(const void *buff, int len);
+
+uint16_t csum_ipv6_magic(const struct in6_addr *saddr,
+			const struct in6_addr *daddr,
+			uint32_t len, uint8_t proto, uint32_t csum);
+
+uint16_t csum_fold(uint32_t csum);
diff --git a/lib/icmpv6.c b/lib/icmpv6.c
new file mode 100644
index 0000000..a6545fd
--- /dev/null
+++ b/lib/icmpv6.c
@@ -0,0 +1,232 @@
+/* Minimal ICMPv6 code for generating router advertisements as required by
+ * relevant 3GPP specs for a GGSN with IPv6 PDP contexts */
+
+/* (C) 2017 by Harald Welte <laforge@gnumonks.org>
+ *
+ * The contents of this file may be used under the terms of the GNU
+ * General Public License Version 2, provided that the above copyright
+ * notice and this permission notice is included in all copies or
+ * substantial portions of the software.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <netinet/in.h>
+#if defined(__FreeBSD__)
+#include <sys/types.h>	/* FreeBSD 10.x needs this before ip6.h */
+#include <sys/endian.h>
+#endif
+#include <netinet/ip6.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/utils.h>
+#include "checksum.h"
+
+#include "../gtp/gtp.h"
+#include "../gtp/pdp.h"
+#include "ippool.h"
+#include "syserr.h"
+#include "config.h"
+
+/* 29.061 11.2.1.3.4 IPv6 Router Configuration Variables in GGSN */
+#define GGSN_MaxRtrAdvInterval	21600		/* 6 hours */
+#define GGSN_MinRtrAdvInterval 16200		/* 4.5 hours */
+#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));
+
+
+/*! 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
+ *  \param[in] prefix The single prefix to be advertised (/64 implied!)
+ *  \returns callee-allocated message buffer containing router advertisement */
+struct msgb *icmpv6_construct_ra(const struct in6_addr *saddr,
+				 const struct in6_addr *daddr,
+				 const struct in6_addr *prefix)
+{
+	struct msgb *msg = msgb_alloc_headroom(512,128, "IPv6 RA");
+	struct icmpv6_radv_hdr *ra;
+	struct icmpv6_opt_prefix *ra_opt_pref;
+	struct ip6_hdr *i6h;
+	uint32_t len;
+	uint16_t skb_csum;
+
+	OSMO_ASSERT(msg);
+
+	ra = (struct icmpv6_radv_hdr *) msgb_put(msg, sizeof(*ra));
+	ra->hdr.type = 134;	/* see RFC4861 4.2 */
+	ra->hdr.code = 0;	/* see RFC4861 4.2 */
+	ra->hdr.csum = 0;	/* updated below */
+	ra->cur_ho_limit = 64;	/* seems reasonable? */
+	/* the GGSN shall leave the M-flag cleared in the Router
+	 * Advertisement messages */
+	ra->m = 0;
+	/* The GGSN may set the O-flag if there are additional
+	 * configuration parameters that need to be fetched by the MS */
+	ra->o = 0;		/* no DHCPv6 */
+	ra->res = 0;
+	/* RFC4861 Default: 3 * MaxRtrAdvInterval */
+	ra->router_lifetime = htons(3*GGSN_MaxRtrAdvInterval);
+	ra->reachable_time = 0;	/* Unspecified */
+
+	/* RFC4861 Section 4.6.2 */
+	ra_opt_pref = (struct icmpv6_opt_prefix *) msgb_put(msg, sizeof(*ra_opt_pref));
+	ra_opt_pref->hdr.type = 3;	/* RFC4861 4.6.2 */
+	ra_opt_pref->hdr.len = 4;	/* RFC4861 4.6.2 */
+	ra_opt_pref->prefix_len = 64;	/* only prefix length as per 3GPP */
+	/* The Prefix is contained in the Prefix Information Option of
+	 * the Router Advertisements and shall have the A-flag set
+	 * and the L-flag cleared */
+	ra_opt_pref->a = 1;
+	ra_opt_pref->l = 0;
+	ra_opt_pref->res = 0;
+	/*  The lifetime of the prefix shall be set to infinity */
+	ra_opt_pref->valid_lifetime = htonl(GGSN_AdvValidLifetime);
+	ra_opt_pref->preferred_lifetime = htonl(GGSN_AdvPreferredLifetime);
+	ra_opt_pref->res2 = 0;
+	memcpy(ra_opt_pref->prefix, prefix, sizeof(ra_opt_pref->prefix));
+
+	/* checksum */
+	skb_csum = csum_partial(msgb_data(msg), msgb_length(msg), 0);
+	len = msgb_length(msg);
+	ra->hdr.csum = csum_ipv6_magic(saddr, daddr, len, IPPROTO_ICMPV6, skb_csum);
+
+	/* Push IPv6 header in front of ICMPv6 packet */
+	i6h = (struct ip6_hdr *) msgb_push(msg, sizeof(*i6h));
+	/* 4 bits version, 8 bits TC, 20 bits flow-ID */
+	i6h->ip6_ctlun.ip6_un1.ip6_un1_flow = htonl(0x60000000);
+	i6h->ip6_ctlun.ip6_un1.ip6_un1_plen = htons(len);
+	i6h->ip6_ctlun.ip6_un1.ip6_un1_nxt = IPPROTO_ICMPV6;
+	i6h->ip6_ctlun.ip6_un1.ip6_un1_hlim = 255;
+	i6h->ip6_src = *saddr;
+	i6h->ip6_dst = *daddr;
+
+	return msg;
+}
+
+/* Walidate an ICMPv6 router solicitation according to RFC4861 6.1.1 */
+static bool icmpv6_validate_router_solicit(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));
+
+	/* Hop limit field must have 255 */
+	if (ip6h->ip6_ctlun.ip6_un1.ip6_un1_hlim != 255)
+		return false;
+	/* FIXME: ICMP checksum is valid */
+	/* ICMP length (derived from IP length) is 8 or more octets */
+	if (ip6h->ip6_ctlun.ip6_un1.ip6_un1_plen < 8)
+		return false;
+	/* FIXME: All included options have a length > 0 */
+	/* FIXME: If IP source is unspecified, no source link-layer addr option */
+	return true;
+}
+
+/* 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,
+			const struct in6_addr *own_ll_addr,
+			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));
+	struct msgb *msg;
+
+	if (len < sizeof(*ip6h)) {
+		LOGP(DICMP6, LOGL_NOTICE, "Packet too short: %u bytes\n", len);
+		return -1;
+	}
+
+	/* we only treat ICMPv6 here */
+	if (ip6h->ip6_ctlun.ip6_un1.ip6_un1_nxt != IPPROTO_ICMPV6) {
+		LOGP(DICMP6, LOGL_DEBUG, "Ignoring non-ICMP to all-routers mcast\n");
+		return 0;
+	}
+
+	if (len < sizeof(*ip6h) + sizeof(*ic6h)) {
+		LOGP(DICMP6, LOGL_NOTICE, "Short ICMPv6 packet: %s\n", osmo_hexdump(pack, len));
+		return -1;
+	}
+
+	switch (ic6h->type) {
+	case 133:	/* router solicitation */
+		if (ic6h->code != 0) {
+			LOGP(DICMP6, LOGL_NOTICE, "ICMPv6 type 133 but code %d\n", ic6h->code);
+			return -1;
+		}
+		if (!icmpv6_validate_router_solicit(pack, len)) {
+			LOGP(DICMP6, LOGL_NOTICE, "Invalid Router Solicitation: %s\n",
+				osmo_hexdump(pack, len));
+			return -1;
+		}
+		/* Send router advertisement from GGSN link-local
+		 * address to MS link-local address, including prefix
+		 * allocated to this PDP context */
+		msg = icmpv6_construct_ra(own_ll_addr, &ip6h->ip6_src, pdp_prefix);
+		/* Send the constructed RA to the MS */
+		gtp_data_req(gsn, pdp, msgb_data(msg), msgb_length(msg));
+		msgb_free(msg);
+		break;
+	default:
+		LOGP(DICMP6, LOGL_DEBUG, "Unknown ICMPv6 type %u\n", ic6h->type);
+		break;
+	}
+	return 0;
+}
diff --git a/lib/icmpv6.h b/lib/icmpv6.h
new file mode 100644
index 0000000..bf91e27
--- /dev/null
+++ b/lib/icmpv6.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include "../gtp/gtp.h"
+#include "../gtp/pdp.h"
+
+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);