Fixed 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
Submitted by: emeric.poupon@netasq.com
Reviewed by: fabient, ae
Obtained from: NETASQ

9 files changed, 276 insertions, 558 deletions

diff --git a/sys/netinet6/ip6_forward.c b/sys/netinet6/ip6_forward.c
index 3a0492f..b3061c3 100644
--- a/sys/netinet6/ip6_forward.c
+++ b/sys/netinet6/ip6_forward.c
@@ -252,7 +252,6 @@ ip6_forward(struct mbuf *m, int srcrt)
 
     {
 	struct ipsecrequest *isr = NULL;
-	struct ipsec_output_state state;
 
 	/*
 	 * when the kernel forwards a packet, it is not proper to apply
@@ -285,18 +284,27 @@ ip6_forward(struct mbuf *m, int srcrt)
 	 *
 	 * IPv6 [ESP|AH] IPv6 [extension headers] payload
 	 */
-	bzero(&state, sizeof(state));
-	state.m = m;
-	state.ro = NULL;	/* update at ipsec6_output_tunnel() */
-	state.dst = NULL;	/* update at ipsec6_output_tunnel() */
 
-	error = ipsec6_output_tunnel(&state, sp, 0);
+	/*
+	 * If we need to encapsulate the packet, do it here
+	 * ipsec6_proces_packet will send the packet using ip6_output
+	 */
+	error = ipsec6_process_packet(m, sp->req);
 
-	m = state.m;
 	KEY_FREESP(&sp);
 
+	if (error == EJUSTRETURN) {
+		/*
+		 * We had a SP with a level of 'use' and no SA. We
+		 * will just continue to process the packet without
+		 * IPsec processing.
+		 */
+		error = 0;
+		goto skip_ipsec;
+	}
+
 	if (error) {
-		/* mbuf is already reclaimed in ipsec6_output_tunnel. */
+		/* mbuf is already reclaimed in ipsec6_process_packet. */
 		switch (error) {
 		case EHOSTUNREACH:
 		case ENETUNREACH:
@@ -319,7 +327,6 @@ ip6_forward(struct mbuf *m, int srcrt)
 			m_freem(mcopy);
 #endif
 		}
-		m_freem(m);
 		return;
 	} else {
 		/*
@@ -331,25 +338,7 @@ ip6_forward(struct mbuf *m, int srcrt)
 		m = NULL;
 		goto freecopy;
 	}
-
-	if ((m != NULL) && (ip6 != mtod(m, struct ip6_hdr *)) ){
-		/*
-		 * now tunnel mode headers are added.  we are originating
-		 * packet instead of forwarding the packet.
-		 */
-		ip6_output(m, NULL, NULL, IPV6_FORWARDING/*XXX*/, NULL, NULL,
-		    NULL);
-		goto freecopy;
-	}
-
-	/* adjust pointer */
-	dst = (struct sockaddr_in6 *)state.dst;
-	rt = state.ro ? state.ro->ro_rt : NULL;
-	if (dst != NULL && rt != NULL)
-		ipsecrt = 1;
     }
-	if (ipsecrt)
-		goto skip_routing;
 skip_ipsec:
 #endif
 again:
@@ -372,9 +361,6 @@ again2:
 		goto bad;
 	}
 	rt = rin6.ro_rt;
-#ifdef IPSEC
-skip_routing:
-#endif
 
 	/*
 	 * Source scope check: if a packet can't be delivered to its
diff --git a/sys/netinet6/ip6_ipsec.c b/sys/netinet6/ip6_ipsec.c
index e9d1f5c..accd074 100644
--- a/sys/netinet6/ip6_ipsec.c
+++ b/sys/netinet6/ip6_ipsec.c
@@ -221,23 +221,22 @@ ip6_ipsec_input(struct mbuf *m, int nxt)
 
 int
 ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
-    struct ifnet **ifp, struct secpolicy **sp)
+    struct ifnet **ifp)
 {
 #ifdef IPSEC
+	struct secpolicy *sp = NULL;
 	struct tdb_ident *tdbi;
 	struct m_tag *mtag;
 	/* XXX int s; */
-	if (sp == NULL)
-		return 1;
 	mtag = m_tag_find(*m, PACKET_TAG_IPSEC_PENDING_TDB, NULL);
 	if (mtag != NULL) {
 		tdbi = (struct tdb_ident *)(mtag + 1);
-		*sp = ipsec_getpolicy(tdbi, IPSEC_DIR_OUTBOUND);
-		if (*sp == NULL)
+		sp = ipsec_getpolicy(tdbi, IPSEC_DIR_OUTBOUND);
+		if (sp == NULL)
 			*error = -EINVAL;	/* force silent drop */
 		m_tag_delete(*m, mtag);
 	} else {
-		*sp = ipsec4_checkpolicy(*m, IPSEC_DIR_OUTBOUND, *flags,
+		sp = ipsec4_checkpolicy(*m, IPSEC_DIR_OUTBOUND, *flags,
 					error, inp);
 	}
 
@@ -248,9 +247,9 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 	 *    sp == NULL, error == -EINVAL  discard packet w/o error
 	 *    sp == NULL, error != 0	    discard packet, report error
 	 */
-	if (*sp != NULL) {
+	if (sp != NULL) {
 		/* Loop detection, check if ipsec processing already done */
-		KASSERT((*sp)->req != NULL, ("ip_output: no ipsec request"));
+		KASSERT(sp->req != NULL, ("ip_output: no ipsec request"));
 		for (mtag = m_tag_first(*m); mtag != NULL;
 		     mtag = m_tag_next(*m, mtag)) {
 			if (mtag->m_tag_cookie != MTAG_ABI_COMPAT)
@@ -264,12 +263,12 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 			 * an SA; e.g. on first reference.  If it occurs,
 			 * then we let ipsec4_process_packet do its thing.
 			 */
-			if ((*sp)->req->sav == NULL)
+			if (sp->req->sav == NULL)
 				break;
 			tdbi = (struct tdb_ident *)(mtag + 1);
-			if (tdbi->spi == (*sp)->req->sav->spi &&
-			    tdbi->proto == (*sp)->req->sav->sah->saidx.proto &&
-			    bcmp(&tdbi->dst, &(*sp)->req->sav->sah->saidx.dst,
+			if (tdbi->spi == sp->req->sav->spi &&
+			    tdbi->proto == sp->req->sav->sah->saidx.proto &&
+			    bcmp(&tdbi->dst, &sp->req->sav->sah->saidx.dst,
 				 sizeof (union sockaddr_union)) == 0) {
 				/*
 				 * No IPsec processing is needed, free
@@ -278,7 +277,7 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 				 * NB: null pointer to avoid free at
 				 *     done: below.
 				 */
-				KEY_FREESP(sp), *sp = NULL;
+				KEY_FREESP(&sp), sp = NULL;
 				goto done;
 			}
 		}
@@ -286,16 +285,37 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 		/*
 		 * Do delayed checksums now because we send before
 		 * this is done in the normal processing path.
-		 * For IPv6 we do delayed checksums in ip6_output.c.
 		 */
 #ifdef INET
 		if ((*m)->m_pkthdr.csum_flags & CSUM_DELAY_DATA) {
-			ipseclog((LOG_DEBUG,
-			    "%s: we do not support IPv4 over IPv6", __func__));
 			in_delayed_cksum(*m);
 			(*m)->m_pkthdr.csum_flags &= ~CSUM_DELAY_DATA;
 		}
 #endif
+		if ((*m)->m_pkthdr.csum_flags & CSUM_DELAY_DATA_IPV6) {
+			in6_delayed_cksum(*m, (*m)->m_pkthdr.len - sizeof(struct ip6_hdr),
+							sizeof(struct ip6_hdr));
+			(*m)->m_pkthdr.csum_flags &= ~CSUM_DELAY_DATA_IPV6;
+		}
+#ifdef SCTP
+		if ((*m)->m_pkthdr.csum_flags & CSUM_SCTP_IPV6) {
+			sctp_delayed_cksum(*m, sizeof(struct ip6_hdr));
+			(*m)->m_pkthdr.csum_flags &= ~CSUM_SCTP_IPV6;
+		}
+#endif
+
+		/* NB: callee frees mbuf */
+		*error = ipsec6_process_packet(*m, sp->req);
+
+		if (*error == EJUSTRETURN) {
+			/*
+			 * We had a SP with a level of 'use' and no SA. We
+			 * will just continue to process the packet without
+			 * IPsec processing.
+			 */
+			*error = 0;
+			goto done;
+		}
 
 		/*
 		 * Preserve KAME behaviour: ENOENT can be returned
@@ -306,7 +326,7 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 		 */
 		if (*error == ENOENT)
 			*error = 0;
-		goto do_ipsec;
+		goto reinjected;
 	} else {	/* sp == NULL */
 		if (*error != 0) {
 			/*
@@ -323,10 +343,16 @@ ip6_ipsec_output(struct mbuf **m, struct inpcb *inp, int *flags, int *error,
 		}
 	}
 done:
+	if (sp != NULL)
+		KEY_FREESP(&sp);
 	return 0;
-do_ipsec:
+reinjected:
+	if (sp != NULL)
+		KEY_FREESP(&sp);
 	return -1;
 bad:
+	if (sp != NULL)
+		KEY_FREESP(&sp);
 	return 1;
 #endif /* IPSEC */
 	return 0;
diff --git a/sys/netinet6/ip6_ipsec.h b/sys/netinet6/ip6_ipsec.h
index 86d1b00..a65b19a 100644
--- a/sys/netinet6/ip6_ipsec.h
+++ b/sys/netinet6/ip6_ipsec.h
@@ -36,7 +36,7 @@ int	ip6_ipsec_filtertunnel(struct mbuf *);
 int	ip6_ipsec_fwd(struct mbuf *);
 int	ip6_ipsec_input(struct mbuf *, int);
 int	ip6_ipsec_output(struct mbuf **, struct inpcb *, int *, int *,
-	    struct ifnet **, struct secpolicy **sp);
+	    struct ifnet **);
 #if 0
 int	ip6_ipsec_mtu(struct mbuf *);
 #endif
diff --git a/sys/netinet6/ip6_output.c b/sys/netinet6/ip6_output.c
index 559344b..05b4701 100644
--- a/sys/netinet6/ip6_output.c
+++ b/sys/netinet6/ip6_output.c
@@ -185,7 +185,7 @@ static int copypktopts(struct ip6_pktopts *, struct ip6_pktopts *, int);
 	}\
     } while (/*CONSTCOND*/ 0)
 
-static void
+void
 in6_delayed_cksum(struct mbuf *m, uint32_t plen, u_short offset)
 {
 	u_short csum;
@@ -249,15 +249,7 @@ ip6_output(struct mbuf *m0, struct ip6_pktopts *opt,
 	u_int32_t zone;
 	struct route_in6 *ro_pmtu = NULL;
 	int hdrsplit = 0;
-	int needipsec = 0;
 	int sw_csum, tso;
-#ifdef IPSEC
-	struct ipsec_output_state state;
-	struct ip6_rthdr *rh = NULL;
-	int needipsectun = 0;
-	int segleft_org = 0;
-	struct secpolicy *sp = NULL;
-#endif /* IPSEC */
 	struct m_tag *fwd_tag = NULL;
 
 	ip6 = mtod(m, struct ip6_hdr *);
@@ -299,26 +291,12 @@ ip6_output(struct mbuf *m0, struct ip6_pktopts *opt,
 	 * IPSec checking which handles several cases.
 	 * FAST IPSEC: We re-injected the packet.
 	 */
-	switch(ip6_ipsec_output(&m, inp, &flags, &error, &ifp, &sp))
+	switch(ip6_ipsec_output(&m, inp, &flags, &error, &ifp))
 	{
 	case 1:                 /* Bad packet */
 		goto freehdrs;
-	case -1:                /* Do IPSec */
-		needipsec = 1;
-		/*
-		 * Do delayed checksums now, as we may send before returning.
-		 */
-		if (m->m_pkthdr.csum_flags & CSUM_DELAY_DATA_IPV6) {
-			plen = m->m_pkthdr.len - sizeof(*ip6);
-			in6_delayed_cksum(m, plen, sizeof(struct ip6_hdr));
-			m->m_pkthdr.csum_flags &= ~CSUM_DELAY_DATA_IPV6;
-		}
-#ifdef SCTP
-		if (m->m_pkthdr.csum_flags & CSUM_SCTP_IPV6) {
-			sctp_delayed_cksum(m, sizeof(struct ip6_hdr));
-			m->m_pkthdr.csum_flags &= ~CSUM_SCTP_IPV6;
-		}
-#endif
+	case -1:                /* IPSec done */
+		goto done;
 	case 0:                 /* No IPSec */
 	default:
 		break;
@@ -338,15 +316,15 @@ ip6_output(struct mbuf *m0, struct ip6_pktopts *opt,
 		optlen += exthdrs.ip6e_rthdr->m_len;
 	unfragpartlen = optlen + sizeof(struct ip6_hdr);
 
-	/* NOTE: we don't add AH/ESP length here. do that later. */
+	/* NOTE: we don't add AH/ESP length here (done in ip6_ipsec_output) */
 	if (exthdrs.ip6e_dest2)
 		optlen += exthdrs.ip6e_dest2->m_len;
 
 	/*
-	 * If we need IPsec, or there is at least one extension header,
+	 * If there is at least one extension header,
 	 * separate IP6 header from the payload.
 	 */
-	if ((needipsec || optlen) && !hdrsplit) {
+	if (optlen && !hdrsplit) {
 		if ((error = ip6_splithdr(m, &exthdrs)) != 0) {
 			m = NULL;
 			goto freehdrs;
@@ -421,72 +399,6 @@ ip6_output(struct mbuf *m0, struct ip6_pktopts *opt,
 	MAKE_CHAIN(exthdrs.ip6e_rthdr, mprev, nexthdrp,
 		   IPPROTO_ROUTING);
 
-#ifdef IPSEC
-	if (!needipsec)
-		goto skip_ipsec2;
-
-	/*
-	 * pointers after IPsec headers are not valid any more.
-	 * other pointers need a great care too.
-	 * (IPsec routines should not mangle mbufs prior to AH/ESP)
-	 */
-	exthdrs.ip6e_dest2 = NULL;
-
-	if (exthdrs.ip6e_rthdr) {
-		rh = mtod(exthdrs.ip6e_rthdr, struct ip6_rthdr *);
-		segleft_org = rh->ip6r_segleft;
-		rh->ip6r_segleft = 0;
-	}
-
-	bzero(&state, sizeof(state));
-	state.m = m;
-	error = ipsec6_output_trans(&state, nexthdrp, mprev, sp, flags,
-				    &needipsectun);
-	m = state.m;
-	if (error == EJUSTRETURN) {
-		/*
-		 * We had a SP with a level of 'use' and no SA. We
-		 * will just continue to process the packet without
-		 * IPsec processing.
-		 */
-		;
-	} else if (error) {
-		/* mbuf is already reclaimed in ipsec6_output_trans. */
-		m = NULL;
-		switch (error) {
-		case EHOSTUNREACH:
-		case ENETUNREACH:
-		case EMSGSIZE:
-		case ENOBUFS:
-		case ENOMEM:
-			break;
-		default:
-			printf("[%s:%d] (ipsec): error code %d\n",
-			    __func__, __LINE__, error);
-			/* FALLTHROUGH */
-		case ENOENT:
-			/* don't show these error codes to the user */
-			error = 0;
-			break;
-		}
-		goto bad;
-	} else if (!needipsectun) {
-		/*
-		 * In the FAST IPSec case we have already
-		 * re-injected the packet and it has been freed
-		 * by the ipsec_done() function.  So, just clean
-		 * up after ourselves.
-		 */
-		m = NULL;
-		goto done;
-	}
-	if (exthdrs.ip6e_rthdr) {
-		/* ah6_output doesn't modify mbuf chain */
-		rh->ip6r_segleft = segleft_org;
-	}
-skip_ipsec2:;
-#endif /* IPSEC */
-
 	/*
 	 * If there is a routing header, discard the packet.
 	 */
@@ -552,77 +464,6 @@ again:
 			ip6->ip6_hlim = V_ip6_defmcasthlim;
 	}
 
-#ifdef IPSEC
-	/*
-	 * We may re-inject packets into the stack here.
-	 */
-	if (needipsec && needipsectun) {
-		struct ipsec_output_state state;
-
-		/*
-		 * All the extension headers will become inaccessible
-		 * (since they can be encrypted).
-		 * Don't panic, we need no more updates to extension headers
-		 * on inner IPv6 packet (since they are now encapsulated).
-		 *
-		 * IPv6 [ESP|AH] IPv6 [extension headers] payload
-		 */
-		bzero(&exthdrs, sizeof(exthdrs));
-		exthdrs.ip6e_ip6 = m;
-
-		bzero(&state, sizeof(state));
-		state.m = m;
-		state.ro = (struct route *)ro;
-		state.dst = (struct sockaddr *)dst;
-
-		error = ipsec6_output_tunnel(&state, sp, flags);
-
-		m = state.m;
-		ro = (struct route_in6 *)state.ro;
-		dst = (struct sockaddr_in6 *)state.dst;
-		if (error == EJUSTRETURN) {
-			/*
-			 * We had a SP with a level of 'use' and no SA. We
-			 * will just continue to process the packet without
-			 * IPsec processing.
-			 */
-			;
-		} else if (error) {
-			/* mbuf is already reclaimed in ipsec6_output_tunnel. */
-			m0 = m = NULL;
-			m = NULL;
-			switch (error) {
-			case EHOSTUNREACH:
-			case ENETUNREACH:
-			case EMSGSIZE:
-			case ENOBUFS:
-			case ENOMEM:
-				break;
-			default:
-				printf("[%s:%d] (ipsec): error code %d\n",
-				    __func__, __LINE__, error);
-				/* FALLTHROUGH */
-			case ENOENT:
-				/* don't show these error codes to the user */
-				error = 0;
-				break;
-			}
-			goto bad;
-		} else {
-			/*
-			 * In the FAST IPSec case we have already
-			 * re-injected the packet and it has been freed
-			 * by the ipsec_done() function.  So, just clean
-			 * up after ourselves.
-			 */
-			m = NULL;
-			goto done;
-		}
-
-		exthdrs.ip6e_ip6 = m;
-	}
-#endif /* IPSEC */
-
 	/* adjust pointer */
 	ip6 = mtod(m, struct ip6_hdr *);
 
@@ -1185,11 +1026,6 @@ done:
 		RO_RTFREE(ro);
 	if (ro_pmtu == &ip6route)
 		RO_RTFREE(ro_pmtu);
-#ifdef IPSEC
-	if (sp != NULL)
-		KEY_FREESP(&sp);
-#endif
-
 	return (error);
 
 freehdrs:
diff --git a/sys/netinet6/ip6_var.h b/sys/netinet6/ip6_var.h
index 70e487e..a9729f9 100644
--- a/sys/netinet6/ip6_var.h
+++ b/sys/netinet6/ip6_var.h
@@ -456,6 +456,7 @@ int	in6_selectroute_fib(struct sockaddr_in6 *, struct ip6_pktopts *,
 	    struct rtentry **, u_int);
 u_int32_t ip6_randomid(void);
 u_int32_t ip6_randomflowlabel(void);
+void in6_delayed_cksum(struct mbuf *m, uint32_t plen, u_short offset);
 #endif /* _KERNEL */
 
 #endif /* !_NETINET6_IP6_VAR_H_ */
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 f019581..40ac8d9 100644
--- a/sys/netipsec/ipsec_input.c
+++ b/sys/netipsec/ipsec_input.c
@@ -296,7 +296,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;
@@ -350,20 +350,33 @@ 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 (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
@@ -389,21 +402,20 @@ 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 (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.
@@ -427,9 +439,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
@@ -466,24 +478,43 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 	key_sa_recordxfer(sav, m);		/* record data transfer */
 
 #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));
@@ -606,20 +637,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.
@@ -642,22 +687,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.
@@ -681,8 +724,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
@@ -720,23 +763,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 5babb9a..13caf24 100644
--- a/sys/netipsec/ipsec_output.c
+++ b/sys/netipsec/ipsec_output.c
@@ -177,8 +177,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
@@ -543,7 +542,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;
@@ -560,9 +559,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;
+			IPSEC6STAT_INC(ips_out_inval);
+			goto bad;
+		}
 		error = (*sav->tdb_xform->xf_output)(m, isr, NULL, i, off);
 	} else {
 		error = ipsec_process_done(m, isr);
@@ -578,224 +594,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));
-
-
-	/*
-	 * 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;
+	struct in6_addr ia2;
 
-	/* XXX Should ip6_src be updated later ? */
+	memcpy(&ia2, &sa->sin6_addr, sizeof(ia2));
+	if (IN6_IS_SCOPE_LINKLOCAL(&sa->sin6_addr))
+		ia2.s6_addr16[1] = htons(sa->sin6_scope_id);
 
-	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;
@@ -805,95 +647,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*/
+#endif /*INET6*/
\ No newline at end of file
diff --git a/sys/netipsec/xform_ipip.c b/sys/netipsec/xform_ipip.c
index 853f428..07a6391 100644
--- a/sys/netipsec/xform_ipip.c
+++ b/sys/netipsec/xform_ipip.c
@@ -309,26 +309,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
@@ -508,10 +488,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:
@@ -542,7 +525,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 */