Handle non-compressed packets for IPComp in tunnel mode.

RFC3173 says that the IP datagram MUST be sent in the original
non-compressed form, when the total size of a compressed payload
and the IPComp header is not smaller than the size of the original
payload. In tunnel mode for small packets IPComp will send
encapsulated IP datagrams without IPComp header.
Add ip_encap handler for IPPROTO_IPV4 and IPPROTO_IPV6 to handle
these datagrams. The handler does lookup for SA related to IPComp
protocol and given from mbuf source and destination addresses as
tunnel endpoints. It decapsulates packets only when corresponding SA
is found.

Reported by:	gnn
Reviewed by:	gnn
Differential Revision:	https://reviews.freebsd.org/D6062

(cherry picked from commit dcf50398ab66cfcba0ae4484efe3b5ce40fb9824)

3 files changed, 201 insertions, 1 deletions

diff --git a/sys/netipsec/key.c b/sys/netipsec/key.c
index 7705a63..c199741 100644
--- a/sys/netipsec/key.c
+++ b/sys/netipsec/key.c
@@ -1153,6 +1153,66 @@ done:
 	return sav;
 }
 
+struct secasvar *
+key_allocsa_tunnel(union sockaddr_union *src, union sockaddr_union *dst,
+    u_int proto, const char* where, int tag)
+{
+	struct secashead *sah;
+	struct secasvar *sav;
+	u_int stateidx, arraysize, state;
+	const u_int *saorder_state_valid;
+
+	IPSEC_ASSERT(src != NULL, ("null src address"));
+	IPSEC_ASSERT(dst != NULL, ("null dst address"));
+	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
+		printf("DP %s from %s:%u\n", __func__, where, tag));
+
+	SAHTREE_LOCK();
+	if (V_key_preferred_oldsa) {
+		saorder_state_valid = saorder_state_valid_prefer_old;
+		arraysize = _ARRAYLEN(saorder_state_valid_prefer_old);
+	} else {
+		saorder_state_valid = saorder_state_valid_prefer_new;
+		arraysize = _ARRAYLEN(saorder_state_valid_prefer_new);
+	}
+	LIST_FOREACH(sah, &V_sahtree, chain) {
+		/* search valid state */
+		for (stateidx = 0; stateidx < arraysize; stateidx++) {
+			state = saorder_state_valid[stateidx];
+			LIST_FOREACH(sav, &sah->savtree[state], chain) {
+				/* sanity check */
+				KEY_CHKSASTATE(sav->state, state, __func__);
+				/* do not return entries w/ unusable state */
+				if (sav->state != SADB_SASTATE_MATURE &&
+				    sav->state != SADB_SASTATE_DYING)
+					continue;
+				if (IPSEC_MODE_TUNNEL != sav->sah->saidx.mode)
+					continue;
+				if (proto != sav->sah->saidx.proto)
+					continue;
+				/* check src address */
+				if (key_sockaddrcmp(&src->sa,
+				    &sav->sah->saidx.src.sa, 0) != 0)
+					continue;
+				/* check dst address */
+				if (key_sockaddrcmp(&dst->sa,
+				    &sav->sah->saidx.dst.sa, 0) != 0)
+					continue;
+				sa_addref(sav);
+				goto done;
+			}
+		}
+	}
+	sav = NULL;
+done:
+	SAHTREE_UNLOCK();
+
+	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
+		printf("DP %s return SA:%p; refcnt %u\n", __func__,
+			sav, sav ? sav->refcnt : 0));
+	return (sav);
+}
+
 /*
  * Must be called after calling key_allocsp().
  * For both the packet without socket and key_freeso().
diff --git a/sys/netipsec/key.h b/sys/netipsec/key.h
index 39fd1b2..7564de5 100644
--- a/sys/netipsec/key.h
+++ b/sys/netipsec/key.h
@@ -76,11 +76,15 @@ extern void _key_freesp(struct secpolicy **, const char*, int);
 
 extern struct secasvar *key_allocsa(union sockaddr_union *, u_int, u_int32_t,
 	const char*, int);
+extern struct secasvar *key_allocsa_tunnel(union sockaddr_union *,
+    union sockaddr_union *, u_int, const char*, int);
 extern void key_addrefsa(struct secasvar *, const char*, int);
 extern void key_freesav(struct secasvar **, const char*, int);
 
 #define	KEY_ALLOCSA(dst, proto, spi)				\
 	key_allocsa(dst, proto, spi, __FILE__, __LINE__)
+#define	KEY_ALLOCSA_TUNNEL(src, dst, proto)				\
+	key_allocsa_tunnel(src, dst, proto, __FILE__, __LINE__)
 #define	KEY_ADDREFSA(sav)					\
 	key_addrefsa(sav, __FILE__, __LINE__)
 #define	KEY_FREESAV(psav)					\
diff --git a/sys/netipsec/xform_ipcomp.c b/sys/netipsec/xform_ipcomp.c
index a5d1e57..453c9c7 100644
--- a/sys/netipsec/xform_ipcomp.c
+++ b/sys/netipsec/xform_ipcomp.c
@@ -47,7 +47,10 @@
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
 #include <netinet/ip_var.h>
+#include <netinet/ip_encap.h>
 
+#include <net/netisr.h>
+#include <net/route.h>
 #include <net/vnet.h>
 
 #include <netipsec/ipsec.h>
@@ -98,6 +101,75 @@ ipcomp_algorithm_lookup(int alg)
 	return NULL;
 }
 
+#if defined(INET) || defined(INET6)
+/*
+ * RFC 3173 p 2.2. Non-Expansion Policy:
+ * If the total size of a compressed payload and the IPComp header, as
+ * defined in section 3, is not smaller than the size of the original
+ * payload, the IP datagram MUST be sent in the original non-compressed
+ * form.
+ *
+ * When we use IPComp in tunnel mode, for small packets we will receive
+ * encapsulated IP-IP datagrams without any compression and without IPComp
+ * header.
+ */
+static int
+ipcomp_encapcheck(union sockaddr_union *src, union sockaddr_union *dst)
+{
+	struct secasvar *sav;
+
+	sav = KEY_ALLOCSA_TUNNEL(src, dst, IPPROTO_IPCOMP);
+	if (sav == NULL)
+		return (0);
+	KEY_FREESAV(&sav);
+
+	if (src->sa.sa_family == AF_INET)
+		return (sizeof(struct in_addr) << 4);
+	else
+		return (sizeof(struct in6_addr) << 4);
+}
+
+static int
+ipcomp_nonexp_input(struct mbuf **mp, int *offp, int proto)
+{
+	int isr;
+
+	switch (proto) {
+#ifdef INET
+	case IPPROTO_IPV4:
+		isr = NETISR_IP;
+		break;
+#endif
+#ifdef INET6
+	case IPPROTO_IPV6:
+		isr = NETISR_IPV6;
+		break;
+#endif
+	default:
+		IPCOMPSTAT_INC(ipcomps_nopf);
+		m_freem(*mp);
+		return (IPPROTO_DONE);
+	}
+	m_adj(*mp, *offp);
+	IPCOMPSTAT_ADD(ipcomps_ibytes, (*mp)->m_pkthdr.len);
+	IPCOMPSTAT_INC(ipcomps_input);
+	netisr_dispatch(isr, *mp);
+	return (IPPROTO_DONE);
+}
+
+extern struct domain inetdomain;
+static struct protosw ipcomp_protosw = {
+	.pr_type =	SOCK_RAW,
+	.pr_domain =	&inetdomain,
+	.pr_protocol =	0 /* IPPROTO_IPV[46] */,
+	.pr_flags =	PR_ATOMIC | PR_ADDR | PR_LASTHDR,
+	.pr_input =	ipcomp_nonexp_input,
+	.pr_output =	rip_output,
+	.pr_ctloutput =	rip_ctloutput,
+	.pr_usrreqs =	&rip_usrreqs
+};
+#endif /* INET || INET6 */
+
 /*
  * ipcomp_init() is called when an CPI is being set up.
  */
@@ -627,11 +699,75 @@ static struct xformsw ipcomp_xformsw = {
 	ipcomp_output
 };
 
+#ifdef INET
+static const struct encaptab *ipe4_cookie = NULL;
+static int
+ipcomp4_nonexp_encapcheck(const struct mbuf *m, int off, int proto,
+    void *arg __unused)
+{
+	union sockaddr_union src, dst;
+	const struct ip *ip;
+
+	if (V_ipcomp_enable == 0)
+		return (0);
+	bzero(&src, sizeof(src));
+	bzero(&dst, sizeof(dst));
+	src.sa.sa_family = dst.sa.sa_family = AF_INET;
+	src.sin.sin_len = dst.sin.sin_len = sizeof(struct sockaddr_in);
+	ip = mtod(m, const struct ip *);
+	src.sin.sin_addr = ip->ip_src;
+	dst.sin.sin_addr = ip->ip_dst;
+	return (ipcomp_encapcheck(&src, &dst));
+}
+#endif
+#ifdef INET6
+static const struct encaptab *ipe6_cookie = NULL;
+static int
+ipcomp6_nonexp_encapcheck(const struct mbuf *m, int off, int proto,
+    void *arg __unused)
+{
+	union sockaddr_union src, dst;
+	const struct ip6_hdr *ip6;
+
+	if (V_ipcomp_enable == 0)
+		return (0);
+	bzero(&src, sizeof(src));
+	bzero(&dst, sizeof(dst));
+	src.sa.sa_family = dst.sa.sa_family = AF_INET;
+	src.sin6.sin6_len = dst.sin6.sin6_len = sizeof(struct sockaddr_in6);
+	ip6 = mtod(m, const struct ip6_hdr *);
+	src.sin6.sin6_addr = ip6->ip6_src;
+	dst.sin6.sin6_addr = ip6->ip6_dst;
+	if (IN6_IS_SCOPE_LINKLOCAL(&src.sin6.sin6_addr)) {
+		/* XXX: sa6_recoverscope() */
+		src.sin6.sin6_scope_id =
+		    ntohs(src.sin6.sin6_addr.s6_addr16[1]);
+		src.sin6.sin6_addr.s6_addr16[1] = 0;
+	}
+	if (IN6_IS_SCOPE_LINKLOCAL(&dst.sin6.sin6_addr)) {
+		/* XXX: sa6_recoverscope() */
+		dst.sin6.sin6_scope_id =
+		    ntohs(dst.sin6.sin6_addr.s6_addr16[1]);
+		dst.sin6.sin6_addr.s6_addr16[1] = 0;
+	}
+	return (ipcomp_encapcheck(&src, &dst));
+}
+#endif
+
 static void
 ipcomp_attach(void)
 {
 
+#ifdef INET
+	ipe4_cookie = encap_attach_func(AF_INET, IPPROTO_IPV4,
+	    ipcomp4_nonexp_encapcheck, &ipcomp_protosw, NULL);
+#endif
+#ifdef INET6
+	ipe6_cookie = encap_attach_func(AF_INET6, IPPROTO_IPV6,
+	    ipcomp6_nonexp_encapcheck, &ipcomp_protosw, NULL);
+#endif
 	xform_register(&ipcomp_xformsw);
 }
 
-SYSINIT(ipcomp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE, ipcomp_attach, NULL);
+SYSINIT(ipcomp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    ipcomp_attach, NULL);