MFC r266800 by vanhu:

  IPv4-in-IPv6 and IPv6-in-IPv4 IPsec tunnels.
  For IPv6-in-IPv4, you may need to do the following command
  on the tunnel interface if it is configured as IPv4 only:
  ifconfig <interface> inet6 -ifdisabled

  Code logic inspired from NetBSD.
  PR: kern/169438

MC r266822 by bz:
  Use IPv4 statistics in ipsec4_process_packet() rather than the IPv6
  version.  This also unbreaks the NOINET6 builds after r266800.

MFC r268083 by zec:
  The assumption in ipsec4_process_packet() that the payload may be
  only IPv4 is wrong, so check the IP version before mangling the
  payload header.

MFC r272394:
  Do not strip outer header when operating in transport mode.
  Instead requeue mbuf back to IPv4 protocol handler. If there is one extra IP-IP
  encapsulation, it will be handled with tunneling interface. And thus proper
  interface will be exposed into mbuf's rcvif. Also, tcpdump that listens on tunneling
  interface will see packets in both directions.

PR:		194761

4 files changed, 221 insertions, 342 deletions

diff --git a/sys/netipsec/ipsec6.h b/sys/netipsec/ipsec6.h
index ee6dc6a..38198e1 100644
--- a/sys/netipsec/ipsec6.h
+++ b/sys/netipsec/ipsec6.h
@@ -76,6 +76,7 @@ extern int ipsec6_output_trans __P((struct ipsec_output_state *, u_char *,
 	struct mbuf *, struct secpolicy *, int, int *));
 extern int ipsec6_output_tunnel __P((struct ipsec_output_state *,
 	struct secpolicy *, int));
+extern int ipsec6_process_packet(struct mbuf *, struct ipsecrequest *);
 #endif /*_KERNEL*/
 
 #endif /*_NETIPSEC_IPSEC6_H_*/
diff --git a/sys/netipsec/ipsec_input.c b/sys/netipsec/ipsec_input.c
index d8eaaa1..334b22f 100644
--- a/sys/netipsec/ipsec_input.c
+++ b/sys/netipsec/ipsec_input.c
@@ -295,7 +295,7 @@ int
 ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 			int skip, int protoff, struct m_tag *mt)
 {
-	int prot, af, sproto;
+	int prot, af, sproto, isr_prot;
 	struct ip *ip;
 	struct m_tag *mtag;
 	struct tdb_ident *tdbi;
@@ -349,20 +349,34 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 	}
 	prot = ip->ip_p;
 
-#ifdef notyet
+#ifdef DEV_ENC
+	encif->if_ipackets++;
+	encif->if_ibytes += m->m_pkthdr.len;
+
+	/*
+	 * Pass the mbuf to enc0 for bpf and pfil. We will filter the IPIP
+	 * packet later after it has been decapsulated.
+	 */
+	ipsec_bpf(m, sav, AF_INET, ENC_IN|ENC_BEFORE);
+
+	if (prot != IPPROTO_IPIP)
+		if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_BEFORE)) != 0)
+			return (error);
+#endif /* DEV_ENC */
+
 	/* IP-in-IP encapsulation */
-	if (prot == IPPROTO_IPIP) {
-		struct ip ipn;
+	if (prot == IPPROTO_IPIP &&
+	    saidx->mode != IPSEC_MODE_TRANSPORT) {
 
 		if (m->m_pkthdr.len - skip < sizeof(struct ip)) {
 			IPSEC_ISTAT(sproto, hdrops);
 			error = EINVAL;
 			goto bad;
 		}
-		/* ipn will now contain the inner IPv4 header */
-		m_copydata(m, ip->ip_hl << 2, sizeof(struct ip),
-		    (caddr_t) &ipn);
+		/* enc0: strip outer IPv4 header */
+		m_striphdr(m, 0, ip->ip_hl << 2);
 
+#ifdef notyet
 		/* XXX PROXY address isn't recorded in SAH */
 		/*
 		 * Check that the inner source address is the same as
@@ -388,21 +402,21 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 			error = EACCES;
 			goto bad;
 		}
+#endif /* notyet */
 	}
 #ifdef INET6
 	/* IPv6-in-IP encapsulation. */
-	if (prot == IPPROTO_IPV6) {
-		struct ip6_hdr ip6n;
+	if (prot == IPPROTO_IPV6 &&
+	    saidx->mode != IPSEC_MODE_TRANSPORT) {
 
 		if (m->m_pkthdr.len - skip < sizeof(struct ip6_hdr)) {
 			IPSEC_ISTAT(sproto, hdrops);
 			error = EINVAL;
 			goto bad;
 		}
-		/* ip6n will now contain the inner IPv6 header. */
-		m_copydata(m, ip->ip_hl << 2, sizeof(struct ip6_hdr),
-		    (caddr_t) &ip6n);
-
+		/* enc0: strip IPv4 header, keep IPv6 header only */
+		m_striphdr(m, 0, ip->ip_hl << 2);
+#ifdef notyet 
 		/*
 		 * Check that the inner source address is the same as
 		 * the proxy address, if available.
@@ -426,9 +440,9 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 			error = EACCES;
 			goto bad;
 		}
+#endif /* notyet */
 	}
 #endif /* INET6 */
-#endif /*XXX*/
 
 	/*
 	 * Record what we've done to the packet (under what SA it was
@@ -464,25 +478,50 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 
 	key_sa_recordxfer(sav, m);		/* record data transfer */
 
+	/*
+	 * In transport mode requeue decrypted mbuf back to IPv4 protocol
+	 * handler. This is necessary to correctly expose rcvif.
+	 */
+	if (saidx->mode == IPSEC_MODE_TRANSPORT)
+		prot = IPPROTO_IPIP;
 #ifdef DEV_ENC
-	encif->if_ipackets++;
-	encif->if_ibytes += m->m_pkthdr.len;
-
 	/*
-	 * Pass the mbuf to enc0 for bpf and pfil. We will filter the IPIP
-	 * packet later after it has been decapsulated.
+	 * Pass the mbuf to enc0 for bpf and pfil.
 	 */
-	ipsec_bpf(m, sav, AF_INET, ENC_IN|ENC_BEFORE);
-
-	if (prot != IPPROTO_IPIP)
-		if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_BEFORE)) != 0)
-			return (error);
+	if (prot == IPPROTO_IPIP)
+		ipsec_bpf(m, sav, AF_INET, ENC_IN|ENC_AFTER);
+#ifdef INET6
+	if (prot == IPPROTO_IPV6)
+		ipsec_bpf(m, sav, AF_INET6, ENC_IN|ENC_AFTER);
 #endif
 
+	if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_AFTER)) != 0)
+		return (error);
+#endif /* DEV_ENC */
+
 	/*
 	 * Re-dispatch via software interrupt.
 	 */
-	if ((error = netisr_queue_src(NETISR_IP, (uintptr_t)sav->spi, m))) {
+
+	switch (prot) {
+	case IPPROTO_IPIP:
+		isr_prot = NETISR_IP;
+		break;
+#ifdef INET6
+	case IPPROTO_IPV6:
+		isr_prot = NETISR_IPV6;
+		break;
+#endif
+	default:
+		DPRINTF(("%s: cannot handle inner ip proto %d\n",
+			    __func__, prot));
+		IPSEC_ISTAT(sproto, nopf);
+		error = EPFNOSUPPORT;
+		goto bad;
+	}
+
+	error = netisr_queue_src(isr_prot, (uintptr_t)sav->spi, m);
+	if (error) {
 		IPSEC_ISTAT(sproto, qfull);
 		DPRINTF(("%s: queue full; proto %u packet dropped\n",
 			__func__, sproto));
@@ -605,20 +644,34 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 	prot = 0;
 	m_copydata(m, protoff, 1, (unsigned char *) &prot);
 
-#ifdef notyet
+#ifdef DEV_ENC
+	encif->if_ipackets++;
+	encif->if_ibytes += m->m_pkthdr.len;
+
+	/*
+	 * Pass the mbuf to enc0 for bpf and pfil. We will filter the IPIP
+	 * packet later after it has been decapsulated.
+	 */
+	ipsec_bpf(m, sav, AF_INET6, ENC_IN|ENC_BEFORE);
+
+	/* XXX-BZ does not make sense. */
+	if (prot != IPPROTO_IPIP)
+		if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_BEFORE)) != 0)
+			return (error);
+#endif /* DEV_ENC */
+
 #ifdef INET
 	/* IP-in-IP encapsulation */
 	if (prot == IPPROTO_IPIP) {
-		struct ip ipn;
-
 		if (m->m_pkthdr.len - skip < sizeof(struct ip)) {
 			IPSEC_ISTAT(sproto, hdrops);
 			error = EINVAL;
 			goto bad;
 		}
 		/* ipn will now contain the inner IPv4 header */
-		m_copydata(m, skip, sizeof(struct ip), (caddr_t) &ipn);
-
+	 	m_striphdr(m, 0, skip);
+		skip = 0;
+#ifdef notyet
 		/*
 		 * Check that the inner source address is the same as
 		 * the proxy address, if available.
@@ -641,22 +694,20 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 			error = EACCES;
 			goto bad;
 		}
+#endif /* notyet */
 	}
 #endif /* INET */
-
 	/* IPv6-in-IP encapsulation */
 	if (prot == IPPROTO_IPV6) {
-		struct ip6_hdr ip6n;
-
 		if (m->m_pkthdr.len - skip < sizeof(struct ip6_hdr)) {
 			IPSEC_ISTAT(sproto, hdrops);
 			error = EINVAL;
 			goto bad;
 		}
 		/* ip6n will now contain the inner IPv6 header. */
-		m_copydata(m, skip, sizeof(struct ip6_hdr),
-		    (caddr_t) &ip6n);
-
+		m_striphdr(m, 0, skip);
+		skip = 0;
+#ifdef notyet
 		/*
 		 * Check that the inner source address is the same as
 		 * the proxy address, if available.
@@ -680,8 +731,8 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 			error = EACCES;
 			goto bad;
 		}
+#endif /* notyet */
 	}
-#endif /*XXX*/
 
 	/*
 	 * Record what we've done to the packet (under what SA it was
@@ -719,23 +770,22 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 	key_sa_recordxfer(sav, m);
 
 #ifdef DEV_ENC
-	encif->if_ipackets++;
-	encif->if_ibytes += m->m_pkthdr.len;
-
 	/*
-	 * Pass the mbuf to enc0 for bpf and pfil. We will filter the IPIP
-	 * packet later after it has been decapsulated.
+	 * Pass the mbuf to enc0 for bpf and pfil.
 	 */
-	ipsec_bpf(m, sav, AF_INET6, ENC_IN|ENC_BEFORE);
-
-	/* XXX-BZ does not make sense. */
-	if (prot != IPPROTO_IPIP)
-		if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_BEFORE)) != 0)
-			return (error);
+#ifdef INET
+	if (prot == IPPROTO_IPIP)
+		ipsec_bpf(m, sav, AF_INET, ENC_IN|ENC_AFTER);
 #endif
+	if (prot == IPPROTO_IPV6)
+		ipsec_bpf(m, sav, AF_INET6, ENC_IN|ENC_AFTER);
 
+	if ((error = ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_AFTER)) != 0)
+		return (error);
+#endif /* DEV_ENC */
 	/* Retrieve new protocol */
-	m_copydata(m, protoff, sizeof(u_int8_t), (caddr_t) &nxt8);
+	/* We have stripped the IP6 header from the mbuf, we have to use the backuped proto value instead */
+	nxt8 = prot;
 
 	/*
 	 * See the end of ip6_input for this logic.
diff --git a/sys/netipsec/ipsec_output.c b/sys/netipsec/ipsec_output.c
index a394590..77dbe03 100644
--- a/sys/netipsec/ipsec_output.c
+++ b/sys/netipsec/ipsec_output.c
@@ -176,8 +176,7 @@ ipsec_process_done(struct mbuf *m, struct ipsecrequest *isr)
 #ifdef INET6
 		case AF_INET6:
 			/* XXX */
-			ipsec6_output_trans()
-			ipsec6_output_tunnel()
+			return ipsec6_process_packet(m, isr->next);
 			/* NOTREACHED */
 #endif /* INET6 */
 #endif
@@ -498,9 +497,11 @@ ipsec4_process_packet(
 				goto bad;
 			}
 			ip = mtod(m, struct ip *);
-			ip->ip_len = htons(m->m_pkthdr.len);
-			ip->ip_sum = 0;
-			ip->ip_sum = in_cksum(m, ip->ip_hl << 2);
+			if (ip->ip_v == IPVERSION) {
+				ip->ip_len = htons(m->m_pkthdr.len);
+				ip->ip_sum = 0;
+				ip->ip_sum = in_cksum(m, ip->ip_hl << 2);
+			}
 
 			/* Encapsulate the packet */
 			error = ipip_output(m, isr, &mp, 0, 0);
@@ -542,7 +543,7 @@ ipsec4_process_packet(
 
 #ifdef DEV_ENC
 	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, sav, AF_INET, ENC_OUT|ENC_AFTER);
+	ipsec_bpf(m, sav, sav->sah->saidx.dst.sa.sa_family, ENC_OUT|ENC_AFTER);
 	/* pass the mbuf to enc0 for packet filtering */
 	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_AFTER)) != 0)
 		goto bad;
@@ -559,9 +560,26 @@ ipsec4_process_packet(
 	 *     for reclaiming their resources.
 	 */
 	if (sav->tdb_xform->xf_type != XF_IP4) {
-		ip = mtod(m, struct ip *);
-		i = ip->ip_hl << 2;
-		off = offsetof(struct ip, ip_p);
+		union sockaddr_union *dst = &sav->sah->saidx.dst;
+		switch(dst->sa.sa_family) {
+		case AF_INET:
+			ip = mtod(m, struct ip *);
+			i = ip->ip_hl << 2;
+			off = offsetof(struct ip, ip_p);
+			break;
+#ifdef INET6
+		case AF_INET6:
+			i = sizeof(struct ip6_hdr);
+			off = offsetof(struct ip6_hdr, ip6_nxt);
+			break;
+#endif /* INET6 */
+		default:
+		DPRINTF(("%s: unsupported protocol family %u\n",
+				 __func__, dst->sa.sa_family));
+			error = EPFNOSUPPORT;
+			IPSECSTAT_INC(ips_out_inval);
+			goto bad;
+		}
 		error = (*sav->tdb_xform->xf_output)(m, isr, NULL, i, off);
 	} else {
 		error = ipsec_process_done(m, isr);
@@ -577,224 +595,50 @@ bad:
 }
 #endif
 
-#ifdef INET6
-/*
- * Chop IP6 header from the payload.
- */
-static struct mbuf *
-ipsec6_splithdr(struct mbuf *m)
-{
-	struct mbuf *mh;
-	struct ip6_hdr *ip6;
-	int hlen;
-
-	IPSEC_ASSERT(m->m_len >= sizeof (struct ip6_hdr),
-		("first mbuf too short, len %u", m->m_len));
-	ip6 = mtod(m, struct ip6_hdr *);
-	hlen = sizeof(struct ip6_hdr);
-	if (m->m_len > hlen) {
-		MGETHDR(mh, M_NOWAIT, MT_DATA);
-		if (!mh) {
-			m_freem(m);
-			return NULL;
-		}
-		M_MOVE_PKTHDR(mh, m);
-		MH_ALIGN(mh, hlen);
-		m->m_len -= hlen;
-		m->m_data += hlen;
-		mh->m_next = m;
-		m = mh;
-		m->m_len = hlen;
-		bcopy((caddr_t)ip6, mtod(m, caddr_t), hlen);
-	} else if (m->m_len < hlen) {
-		m = m_pullup(m, hlen);
-		if (!m)
-			return NULL;
-	}
-	return m;
-}
-
-/*
- * IPsec output logic for IPv6, transport mode.
- */
-int
-ipsec6_output_trans(
-	struct ipsec_output_state *state,
-	u_char *nexthdrp,
-	struct mbuf *mprev,
-	struct secpolicy *sp,
-	int flags,
-	int *tun)
-{
-	struct ipsecrequest *isr;
-	struct secasindex saidx;
-	int error = 0;
-	struct mbuf *m;
-
-	IPSEC_ASSERT(state != NULL, ("null state"));
-	IPSEC_ASSERT(state->m != NULL, ("null m"));
-	IPSEC_ASSERT(nexthdrp != NULL, ("null nexthdrp"));
-	IPSEC_ASSERT(mprev != NULL, ("null mprev"));
-	IPSEC_ASSERT(sp != NULL, ("null sp"));
-	IPSEC_ASSERT(tun != NULL, ("null tun"));
-
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("%s: applied SP\n", __func__);
-		kdebug_secpolicy(sp));
-
-	isr = sp->req;
-	if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
-		/* the rest will be handled by ipsec6_output_tunnel() */
-		*tun = 1;		/* need tunnel-mode processing */
-		return 0;
-	}
-
-	*tun = 0;
-	m = state->m;
-
-	IPSECREQUEST_LOCK(isr);		/* insure SA contents don't change */
-	isr = ipsec_nextisr(m, isr, AF_INET6, &saidx, &error);
-	if (isr == NULL) {
-		if (error != 0) {
-#ifdef notdef
-			/* XXX should notification be done for all errors ? */
-			/*
-			 * Notify the fact that the packet is discarded
-			 * to ourselves. I believe this is better than
-			 * just silently discarding. (jinmei@kame.net)
-			 * XXX: should we restrict the error to TCP packets?
-			 * XXX: should we directly notify sockets via
-			 *      pfctlinputs?
-			 */
-			icmp6_error(m, ICMP6_DST_UNREACH,
-				    ICMP6_DST_UNREACH_ADMIN, 0);
-			m = NULL;	/* NB: icmp6_error frees mbuf */
-#endif
-			goto bad;
-		}
-		return EJUSTRETURN;
-	}
-
-	error = (*isr->sav->tdb_xform->xf_output)(m, isr, NULL,
-						  sizeof (struct ip6_hdr),
-						  offsetof(struct ip6_hdr, 
-							   ip6_nxt));
-	IPSECREQUEST_UNLOCK(isr);
-	return error;
-bad:
-	if (isr)
-		IPSECREQUEST_UNLOCK(isr);
-	if (m)
-		m_freem(m);
-	state->m = NULL;
-	return error;
-}
 
+#ifdef INET6
 static int
-ipsec6_encapsulate(struct mbuf *m, struct secasvar *sav)
+in6_sa_equal_addrwithscope(const struct sockaddr_in6 *sa, const struct in6_addr *ia)
 {
-	struct ip6_hdr *oip6;
-	struct ip6_hdr *ip6;
-	size_t plen;
-
-	/* can't tunnel between different AFs */
-	if (sav->sah->saidx.src.sa.sa_family != AF_INET6 ||
-	    sav->sah->saidx.dst.sa.sa_family != AF_INET6) {
-		m_freem(m);
-		return EINVAL;
-	}
-	IPSEC_ASSERT(m->m_len == sizeof (struct ip6_hdr),
-		("mbuf wrong size; len %u", m->m_len));
-
+	struct in6_addr ia2;
 
-	/*
-	 * grow the mbuf to accomodate the new IPv6 header.
-	 */
-	plen = m->m_pkthdr.len;
-	if (M_LEADINGSPACE(m->m_next) < sizeof(struct ip6_hdr)) {
-		struct mbuf *n;
-		MGET(n, M_NOWAIT, MT_DATA);
-		if (!n) {
-			m_freem(m);
-			return ENOBUFS;
-		}
-		n->m_len = sizeof(struct ip6_hdr);
-		n->m_next = m->m_next;
-		m->m_next = n;
-		m->m_pkthdr.len += sizeof(struct ip6_hdr);
-		oip6 = mtod(n, struct ip6_hdr *);
-	} else {
-		m->m_next->m_len += sizeof(struct ip6_hdr);
-		m->m_next->m_data -= sizeof(struct ip6_hdr);
-		m->m_pkthdr.len += sizeof(struct ip6_hdr);
-		oip6 = mtod(m->m_next, struct ip6_hdr *);
-	}
-	ip6 = mtod(m, struct ip6_hdr *);
-	bcopy((caddr_t)ip6, (caddr_t)oip6, sizeof(struct ip6_hdr));
-
-	/* Fake link-local scope-class addresses */
-	if (IN6_IS_SCOPE_LINKLOCAL(&oip6->ip6_src))
-		oip6->ip6_src.s6_addr16[1] = 0;
-	if (IN6_IS_SCOPE_LINKLOCAL(&oip6->ip6_dst))
-		oip6->ip6_dst.s6_addr16[1] = 0;
-
-	/* construct new IPv6 header. see RFC 2401 5.1.2.2 */
-	/* ECN consideration. */
-	ip6_ecn_ingress(V_ip6_ipsec_ecn, &ip6->ip6_flow, &oip6->ip6_flow);
-	if (plen < IPV6_MAXPACKET - sizeof(struct ip6_hdr))
-		ip6->ip6_plen = htons(plen);
-	else {
-		/* ip6->ip6_plen will be updated in ip6_output() */
-	}
-	ip6->ip6_nxt = IPPROTO_IPV6;
-	ip6->ip6_src = sav->sah->saidx.src.sin6.sin6_addr;
-	ip6->ip6_dst = sav->sah->saidx.dst.sin6.sin6_addr;
-	ip6->ip6_hlim = IPV6_DEFHLIM;
+	memcpy(&ia2, &sa->sin6_addr, sizeof(ia2));
+	if (IN6_IS_SCOPE_LINKLOCAL(&sa->sin6_addr))
+		ia2.s6_addr16[1] = htons(sa->sin6_scope_id);
 
-	/* XXX Should ip6_src be updated later ? */
-
-	return 0;
+	return IN6_ARE_ADDR_EQUAL(ia, &ia2);
 }
 
 /*
- * IPsec output logic for IPv6, tunnel mode.
+ * IPsec output logic for IPv6.
  */
 int
-ipsec6_output_tunnel(struct ipsec_output_state *state, struct secpolicy *sp, int flags)
+ipsec6_process_packet(
+	struct mbuf *m,
+ 	struct ipsecrequest *isr
+    )
 {
-	struct ip6_hdr *ip6;
-	struct ipsecrequest *isr;
 	struct secasindex saidx;
-	int error;
-	struct sockaddr_in6 *dst6;
-	struct mbuf *m;
-
-	IPSEC_ASSERT(state != NULL, ("null state"));
-	IPSEC_ASSERT(state->m != NULL, ("null m"));
-	IPSEC_ASSERT(sp != NULL, ("null sp"));
-
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("%s: applied SP\n", __func__);
-		kdebug_secpolicy(sp));
+	struct secasvar *sav;
+	struct ip6_hdr *ip6;
+	int error, i, off;
+	union sockaddr_union *dst;
 
-	m = state->m;
-	/*
-	 * transport mode ipsec (before the 1st tunnel mode) is already
-	 * processed by ipsec6_output_trans().
-	 */
-	for (isr = sp->req; isr; isr = isr->next) {
-		if (isr->saidx.mode == IPSEC_MODE_TUNNEL)
-			break;
-	}
+	IPSEC_ASSERT(m != NULL, ("ipsec6_process_packet: null mbuf"));
+	IPSEC_ASSERT(isr != NULL, ("ipsec6_process_packet: null isr"));
 
 	IPSECREQUEST_LOCK(isr);		/* insure SA contents don't change */
+
 	isr = ipsec_nextisr(m, isr, AF_INET6, &saidx, &error);
 	if (isr == NULL) {
 		if (error != 0)
 			goto bad;
-		return EJUSTRETURN; 
+		return EJUSTRETURN;
 	}
 
+	sav = isr->sav;
+	dst = &sav->sah->saidx.dst;
+
 #ifdef DEV_ENC
 	encif->if_opackets++;
 	encif->if_obytes += m->m_pkthdr.len;
@@ -804,95 +648,96 @@ ipsec6_output_tunnel(struct ipsec_output_state *state, struct secpolicy *sp, int
 	/* pass the mbuf to enc0 for packet filtering */
 	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_BEFORE)) != 0)
 		goto bad;
-#endif
+#endif /* DEV_ENC */
+
+	ip6 = mtod(m, struct ip6_hdr *); /* XXX */
+
+	/* Do the appropriate encapsulation, if necessary */
+	if (isr->saidx.mode == IPSEC_MODE_TUNNEL || /* Tunnel requ'd */
+	    dst->sa.sa_family != AF_INET6 ||        /* PF mismatch */
+	    ((dst->sa.sa_family == AF_INET6) &&
+	     (!IN6_IS_ADDR_UNSPECIFIED(&dst->sin6.sin6_addr)) &&
+	     (!in6_sa_equal_addrwithscope(&dst->sin6,
+				  &ip6->ip6_dst)))) {
+		struct mbuf *mp;
+
+		/* Fix IPv6 header payload length. */
+		if (m->m_len < sizeof(struct ip6_hdr))
+			if ((m = m_pullup(m,sizeof(struct ip6_hdr))) == NULL) {
+				error = ENOBUFS;
+				goto bad;
+			}
 
-	/*
-	 * There may be the case that SA status will be changed when
-	 * we are refering to one. So calling splsoftnet().
-	 */
-	if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
-		/*
-		 * build IPsec tunnel.
-		 */
-		/* XXX should be processed with other familiy */
-		if (isr->sav->sah->saidx.src.sa.sa_family != AF_INET6) {
-			ipseclog((LOG_ERR, "%s: family mismatched between "
-			    "inner and outer, spi=%u\n", __func__,
-			    ntohl(isr->sav->spi)));
-			IPSEC6STAT_INC(ips_out_inval);
-			error = EAFNOSUPPORT;
+		if (m->m_pkthdr.len - sizeof(*ip6) > IPV6_MAXPACKET) {
+			/* No jumbogram support. */
+			error = ENXIO;   /*XXX*/
 			goto bad;
 		}
 
-		m = ipsec6_splithdr(m);
-		if (!m) {
-			IPSEC6STAT_INC(ips_out_nomem);
-			error = ENOMEM;
-			goto bad;
-		}
-		error = ipsec6_encapsulate(m, isr->sav);
-		if (error) {
-			m = NULL;
+		ip6 = mtod(m, struct ip6_hdr *);
+		ip6->ip6_plen = htons(m->m_pkthdr.len - sizeof(*ip6));
+
+		/* Encapsulate the packet */
+		error = ipip_output(m, isr, &mp, 0, 0);
+		if (mp == NULL && !error) {
+			/* Should never happen. */
+			DPRINTF(("ipsec6_process_packet: ipip_output "
+				 "returns no mbuf and no error!"));
+			error = EFAULT;
 			goto bad;
 		}
-		ip6 = mtod(m, struct ip6_hdr *);
 
-		state->ro =
-		    (struct route *)&isr->sav->sah->route_cache.sin6_route;
-		state->dst = (struct sockaddr *)&state->ro->ro_dst;
-		dst6 = (struct sockaddr_in6 *)state->dst;
-		if (state->ro->ro_rt
-		 && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
-		  || !IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ip6->ip6_dst))) {
-			RTFREE(state->ro->ro_rt);
-			state->ro->ro_rt = NULL;
-		}
-		if (state->ro->ro_rt == NULL) {
-			bzero(dst6, sizeof(*dst6));
-			dst6->sin6_family = AF_INET6;
-			dst6->sin6_len = sizeof(*dst6);
-			dst6->sin6_addr = ip6->ip6_dst;
-			rtalloc_ign_fib(state->ro, 0UL, M_GETFIB(m));
-		}
-		if (state->ro->ro_rt == NULL) {
-			IP6STAT_INC(ip6s_noroute);
-			IPSEC6STAT_INC(ips_out_noroute);
-			error = EHOSTUNREACH;
+		if (error) {
+			if (mp) {
+				/* XXX: Should never happen! */
+				m_freem(mp);
+			}
+			m = NULL; /* ipip_output() already freed it */
 			goto bad;
 		}
 
-		/* adjust state->dst if tunnel endpoint is offlink */
-		if (state->ro->ro_rt->rt_flags & RTF_GATEWAY)
-			state->dst = (struct sockaddr *)state->ro->ro_rt->rt_gateway;
-	}
-
-	m = ipsec6_splithdr(m);
-	if (!m) {
-		IPSEC6STAT_INC(ips_out_nomem);
-		error = ENOMEM;
-		goto bad;
+		m = mp;
+		mp = NULL;
 	}
-	ip6 = mtod(m, struct ip6_hdr *);
 
 #ifdef DEV_ENC
-	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, isr->sav, AF_INET6, ENC_OUT|ENC_AFTER);
+	ipsec_bpf(m, isr->sav, dst->sa.sa_family, ENC_OUT|ENC_AFTER);
 	/* pass the mbuf to enc0 for packet filtering */
 	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_AFTER)) != 0)
 		goto bad;
-#endif
+#endif /* DEV_ENC */
 
-	error = (*isr->sav->tdb_xform->xf_output)(m, isr, NULL,
-		sizeof (struct ip6_hdr),
-		offsetof(struct ip6_hdr, ip6_nxt));
+	switch(dst->sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		{
+		struct ip *ip;
+		ip = mtod(m, struct ip *);
+		i = ip->ip_hl << 2;
+		off = offsetof(struct ip, ip_p);
+		}
+		break;
+#endif /* AF_INET */
+	case AF_INET6:
+		i = sizeof(struct ip6_hdr);
+		off = offsetof(struct ip6_hdr, ip6_nxt);
+		break;
+	default:
+		DPRINTF(("%s: unsupported protocol family %u\n",
+				 __func__, dst->sa.sa_family));
+		error = EPFNOSUPPORT;
+		IPSEC6STAT_INC(ips_out_inval);
+		goto bad;
+	}
+	error = (*sav->tdb_xform->xf_output)(m, isr, NULL, i, off);
 	IPSECREQUEST_UNLOCK(isr);
 	return error;
 bad:
+
 	if (isr)
 		IPSECREQUEST_UNLOCK(isr);
 	if (m)
 		m_freem(m);
-	state->m = NULL;
 	return error;
 }
 #endif /*INET6*/
diff --git a/sys/netipsec/xform_ipip.c b/sys/netipsec/xform_ipip.c
index 3e1fc1f..01a6af8 100644
--- a/sys/netipsec/xform_ipip.c
+++ b/sys/netipsec/xform_ipip.c
@@ -308,26 +308,6 @@ _ipip_input(struct mbuf *m, int iphlen, struct ifnet *gifp)
 	/* Statistics */
 	IPIPSTAT_ADD(ipips_ibytes, m->m_pkthdr.len - iphlen);
 
-#ifdef DEV_ENC
-	switch (v >> 4) {
-#ifdef INET
-	case 4:
-		ipsec_bpf(m, NULL, AF_INET, ENC_IN|ENC_AFTER);
-		break;
-#endif
-#ifdef INET6
-	case 6:
-		ipsec_bpf(m, NULL, AF_INET6, ENC_IN|ENC_AFTER);
-		break;
-#endif
-	default:
-		panic("%s: bogus ip version %u", __func__, v>>4);
-	}
-	/* pass the mbuf to enc0 for packet filtering */
-	if (ipsec_filter(&m, PFIL_IN, ENC_IN|ENC_AFTER) != 0)
-		return;
-#endif
-
 	/*
 	 * Interface pointer stays the same; if no IPsec processing has
 	 * been done (or will be done), this will point to a normal
@@ -507,10 +487,13 @@ ipip_output(
 		ip6o->ip6_vfc &= ~IPV6_VERSION_MASK;
 		ip6o->ip6_vfc |= IPV6_VERSION;
 		ip6o->ip6_plen = htons(m->m_pkthdr.len);
-		ip6o->ip6_hlim = V_ip_defttl;
+		ip6o->ip6_hlim = IPV6_DEFHLIM;
 		ip6o->ip6_dst = saidx->dst.sin6.sin6_addr;
 		ip6o->ip6_src = saidx->src.sin6.sin6_addr;
 
+		/* Fix payload length */
+		ip6o->ip6_plen = htons(m->m_pkthdr.len - sizeof(*ip6));
+
 		switch (tp) {
 #ifdef INET
 		case IPVERSION:
@@ -541,7 +524,7 @@ ipip_output(
 		}
 
 		otos = 0;
-		ip_ecn_ingress(ECN_ALLOWED, &otos, &itos);
+		ip_ecn_ingress(V_ip6_ipsec_ecn, &otos, &itos);
 		ip6o->ip6_flow |= htonl((u_int32_t) otos << 20);
 		break;
 #endif /* INET6 */