Index: netinet6/ip6_forward.c
===================================================================
RCS file: /cvs/src/sys/netinet6/ip6_forward.c,v
retrieving revision 1.87
diff -u -p -u -r1.87 ip6_forward.c
--- netinet6/ip6_forward.c	29 Mar 2016 11:57:51 -0000	1.87
+++ netinet6/ip6_forward.c	13 Apr 2016 21:47:09 -0000
@@ -93,15 +93,7 @@ ip6_forward(struct mbuf *m, int srcrt)
 	int error = 0, type = 0, code = 0;
 	struct mbuf *mcopy = NULL;
 #ifdef IPSEC
-	u_int8_t sproto = 0;
-	struct m_tag *mtag;
-	union sockaddr_union sdst;
-	struct tdb_ident *tdbi;
-	u_int32_t sspi;
-	struct tdb *tdb;
-#if NPF > 0
-	struct ifnet *encif;
-#endif
+	struct tdb *tdb = NULL;
 #endif /* IPSEC */
 	u_int rtableid = 0;
 	char src6[INET6_ADDRSTRLEN], dst6[INET6_ADDRSTRLEN];
@@ -155,64 +147,21 @@ reroute:
 #endif
 
 #ifdef IPSEC
-	if (!ipsec_in_use)
-		goto done_spd;
-
-	/*
-	 * Check if there was an outgoing SA bound to the flow
-	 * from a transport protocol.
-	 */
-
-	/* Do we have any pending SAs to apply ? */
-	tdb = ipsp_spd_lookup(m, AF_INET6, sizeof(struct ip6_hdr),
-	    &error, IPSP_DIRECTION_OUT, NULL, NULL, 0);
-
-	if (tdb == NULL) {
-		if (error == 0) {
-		        /*
-			 * No IPsec processing required, we'll just send the
-			 * packet out.
-			 */
-		        sproto = 0;
-
-			/* Fall through to routing/multicast handling */
-		} else {
-		        /*
+	if (ipsec_in_use) {
+		tdb = ip6_output_ipsec_lookup(m, &error, NULL);
+		if (error != 0) {
+			/*
 			 * -EINVAL is used to indicate that the packet should
 			 * be silently dropped, typically because we've asked
 			 * key management for an SA.
 			 */
-		        if (error == -EINVAL) /* Should silently drop packet */
+			if (error == -EINVAL) /* Should silently drop packet */
 				error = 0;
 
 			m_freem(m);
 			goto freecopy;
 		}
-	} else {
-		/* Loop detection */
-		for (mtag = m_tag_first(m); mtag != NULL;
-		    mtag = m_tag_next(m, mtag)) {
-			if (mtag->m_tag_id != PACKET_TAG_IPSEC_OUT_DONE)
-				continue;
-			tdbi = (struct tdb_ident *)(mtag + 1);
-			if (tdbi->spi == tdb->tdb_spi &&
-			    tdbi->proto == tdb->tdb_sproto &&
-			    tdbi->rdomain == tdb->tdb_rdomain &&
-			    !bcmp(&tdbi->dst, &tdb->tdb_dst,
-			    sizeof(union sockaddr_union))) {
-				sproto = 0; /* mark as no-IPsec-needed */
-				goto done_spd;
-			}
-		}
-
-	        /* We need to do IPsec */
-	        bcopy(&tdb->tdb_dst, &sdst, sizeof(sdst));
-		sspi = tdb->tdb_spi;
-		sproto = tdb->tdb_sproto;
 	}
-
-	/* Fall through to the routing/multicast handling code */
- done_spd:
 #endif /* IPSEC */
 
 #if NPF > 0
@@ -313,40 +262,12 @@ reroute:
 	 * XXX ipsp_process_packet() calls ip6_output(), and there'll be no
 	 * PMTU notification.  is it okay?
 	 */
-	if (sproto != 0) {
-		tdb = gettdb(rtable_l2(m->m_pkthdr.ph_rtableid),
-		    sspi, &sdst, sproto);
-		if (tdb == NULL) {
-			error = EHOSTUNREACH;
-			m_freem(m);
-			goto senderr;	/*XXX*/
-		}
-
-#if NPF > 0
-		if ((encif = enc_getif(tdb->tdb_rdomain,
-		    tdb->tdb_tap)) == NULL ||
-		    pf_test(AF_INET6, PF_FWD, encif, &m) != PF_PASS) {
-			error = EHOSTUNREACH;
-			m_freem(m);
-			goto senderr;
-		}
-		if (m == NULL)
-			goto senderr;
-		/*
-		 * PF_TAG_REROUTE handling or not...
-		 * Packet is entering IPsec so the routing is
-		 * already overruled by the IPsec policy.
-		 * Until now the change was not reconsidered.
-		 * What's the behaviour?
-		 */
-		in6_proto_cksum_out(m, encif);
-#endif
-		m->m_flags &= ~(M_BCAST | M_MCAST);	/* just in case */
-
+	if (tdb != NULL) {
 		/* Callee frees mbuf */
-		error = ipsp_process_packet(m, tdb, AF_INET6, 0);
-		m_freem(mcopy);
-		goto freert;
+		error = ip6_output_ipsec_send(tdb, m, 0, 1);
+		if (error)
+			goto senderr;
+		goto freecopy;
 	}
 #endif /* IPSEC */
 
Index: netinet6/ip6_output.c
===================================================================
RCS file: /cvs/src/sys/netinet6/ip6_output.c,v
retrieving revision 1.204
diff -u -p -u -r1.204 ip6_output.c
--- netinet6/ip6_output.c	21 Jan 2016 11:23:48 -0000	1.204
+++ netinet6/ip6_output.c	13 Apr 2016 21:47:09 -0000
@@ -172,14 +172,7 @@ ip6_output(struct mbuf *m0, struct ip6_p
 	int hdrsplit = 0;
 	u_int8_t sproto = 0;
 #ifdef IPSEC
-	struct m_tag *mtag;
-	union sockaddr_union sdst;
-	struct tdb_ident *tdbi;
-	u_int32_t sspi;
-	struct tdb *tdb;
-#if NPF > 0
-	struct ifnet *encif;
-#endif
+	struct tdb *tdb = NULL;
 #endif /* IPSEC */
 
 #ifdef IPSEC
@@ -215,28 +208,9 @@ ip6_output(struct mbuf *m0, struct ip6_p
 	}
 
 #ifdef IPSEC
-	if (!ipsec_in_use && !inp)
-		goto done_spd;
-
-	/*
-	 * Check if there was an outgoing SA bound to the flow
-	 * from a transport protocol.
-	 */
-
-	/* Do we have any pending SAs to apply ? */
-	tdb = ipsp_spd_lookup(m, AF_INET6, sizeof(struct ip6_hdr),
-	    &error, IPSP_DIRECTION_OUT, NULL, inp, 0);
-
-	if (tdb == NULL) {
-		if (error == 0) {
-		        /*
-			 * No IPsec processing required, we'll just send the
-			 * packet out.
-			 */
-		        sproto = 0;
-
-			/* Fall through to routing/multicast handling */
-		} else {
+	if (ipsec_in_use || inp) {
+		tdb = ip6_output_ipsec_lookup(m, &error, inp);
+		if (error != 0) {
 		        /*
 			 * -EINVAL is used to indicate that the packet should
 			 * be silently dropped, typically because we've asked
@@ -247,31 +221,7 @@ ip6_output(struct mbuf *m0, struct ip6_p
 
 			goto freehdrs;
 		}
-	} else {
-		/* Loop detection */
-		for (mtag = m_tag_first(m); mtag != NULL;
-		    mtag = m_tag_next(m, mtag)) {
-			if (mtag->m_tag_id != PACKET_TAG_IPSEC_OUT_DONE)
-				continue;
-			tdbi = (struct tdb_ident *)(mtag + 1);
-			if (tdbi->spi == tdb->tdb_spi &&
-			    tdbi->proto == tdb->tdb_sproto &&
-			    tdbi->rdomain == tdb->tdb_rdomain &&
-			    !bcmp(&tdbi->dst, &tdb->tdb_dst,
-			    sizeof(union sockaddr_union))) {
-				sproto = 0; /* mark as no-IPsec-needed */
-				goto done_spd;
-			}
-		}
-
-	        /* We need to do IPsec */
-	        bcopy(&tdb->tdb_dst, &sdst, sizeof(sdst));
-		sspi = tdb->tdb_spi;
-		sproto = tdb->tdb_sproto;
 	}
-
-	/* Fall through to the routing/multicast handling code */
- done_spd:
 #endif /* IPSEC */
 
 	/*
@@ -469,55 +419,19 @@ reroute:
 	}
 
 #ifdef IPSEC
-	/*
-	 * Check if the packet needs encapsulation.
-	 * ipsp_process_packet will never come back to here.
-	 */
-	if (sproto != 0) {
+	if (tdb) {
 		/*
 		 * XXX what should we do if ip6_hlim == 0 and the
 		 * packet gets tunneled?
 		 */
-
-		tdb = gettdb(rtable_l2(m->m_pkthdr.ph_rtableid),
-		    sspi, &sdst, sproto);
-		if (tdb == NULL) {
-			error = EHOSTUNREACH;
-			m_freem(m);
-			goto done;
-		}
-
-#if NPF > 0
-		if ((encif = enc_getif(tdb->tdb_rdomain,
-		    tdb->tdb_tap)) == NULL ||
-		    pf_test(AF_INET6, PF_OUT, encif, &m) != PF_PASS) {
-			error = EHOSTUNREACH;
-			m_freem(m);
-			goto done;
-		}
-		if (m == NULL)
-			goto done;
-		/*
-		 * PF_TAG_REROUTE handling or not...
-		 * Packet is entering IPsec so the routing is
-		 * already overruled by the IPsec policy.
-		 * Until now the change was not reconsidered.
-		 * What's the behaviour?
-		 */
-		in6_proto_cksum_out(m, encif);
-#endif
-		m->m_flags &= ~(M_BCAST | M_MCAST);	/* just in case */
-
-		/* Callee frees mbuf */
 		/*
 		 * if we are source-routing, do not attempt to tunnel the
 		 * packet just because ip6_dst is different from what tdb has.
 		 * XXX
 		 */
-		error = ipsp_process_packet(m, tdb, AF_INET6,
-		    exthdrs.ip6e_rthdr ? 1 : 0);
-
-		return error;  /* Nothing more to be done */
+		error = ip6_output_ipsec_send(tdb, m,
+		    exthdrs.ip6e_rthdr ? 1 : 0, 0);
+		goto done;
 	}
 #endif /* IPSEC */
 
@@ -2944,3 +2858,70 @@ in6_proto_cksum_out(struct mbuf *m, stru
 		m->m_pkthdr.csum_flags &= ~M_ICMP_CSUM_OUT; /* Clear */
 	}
 }
+
+#ifdef IPSEC
+struct tdb *
+ip6_output_ipsec_lookup(struct mbuf *m, int *error, struct inpcb *inp)
+{
+	struct tdb *tdb;
+	struct m_tag *mtag;
+	struct tdb_ident *tdbi;
+
+	/*
+	 * Check if there was an outgoing SA bound to the flow
+	 * from a transport protocol.
+	 */
+
+	/* Do we have any pending SAs to apply ? */
+	tdb = ipsp_spd_lookup(m, AF_INET6, sizeof(struct ip6_hdr),
+	    error, IPSP_DIRECTION_OUT, NULL, inp, 0);
+
+	if (tdb != NULL) {
+		/* Loop detection */
+		for (mtag = m_tag_first(m); mtag != NULL;
+		    mtag = m_tag_next(m, mtag)) {
+			if (mtag->m_tag_id != PACKET_TAG_IPSEC_OUT_DONE)
+				continue;
+			tdbi = (struct tdb_ident *)(mtag + 1);
+			if (tdbi->spi == tdb->tdb_spi &&
+			    tdbi->proto == tdb->tdb_sproto &&
+			    tdbi->rdomain == tdb->tdb_rdomain &&
+			    !bcmp(&tdbi->dst, &tdb->tdb_dst,
+			    sizeof(union sockaddr_union)))
+				tdb = NULL;
+		}
+		/* We need to do IPsec */
+	}
+	return tdb;
+}
+
+int
+ip6_output_ipsec_send(struct tdb *tdb, struct mbuf *m, int tunalready, int fwd)
+{
+#if NPF > 0
+	struct ifnet *encif;
+#endif
+
+#if NPF > 0
+	if ((encif = enc_getif(tdb->tdb_rdomain, tdb->tdb_tap)) == NULL ||
+	    pf_test(AF_INET6, fwd ? PF_FWD : PF_OUT, encif, &m) != PF_PASS) {
+		m_freem(m);
+		return EHOSTUNREACH;
+	}
+	if (m == NULL)
+		return 0;
+	/*
+	 * PF_TAG_REROUTE handling or not...
+	 * Packet is entering IPsec so the routing is
+	 * already overruled by the IPsec policy.
+	 * Until now the change was not reconsidered.
+	 * What's the behaviour?
+	 */
+	in6_proto_cksum_out(m, encif);
+#endif
+	m->m_flags &= ~(M_BCAST | M_MCAST);	/* just in case */
+
+	/* Callee frees mbuf */
+	return ipsp_process_packet(m, tdb, AF_INET6, tunalready);
+}
+#endif /* IPSEC */
Index: netinet6/ip6_var.h
===================================================================
RCS file: /cvs/src/sys/netinet6/ip6_var.h,v
retrieving revision 1.57
diff -u -p -u -r1.57 ip6_var.h
--- netinet6/ip6_var.h	3 Dec 2015 21:11:54 -0000	1.57
+++ netinet6/ip6_var.h	13 Apr 2016 21:47:08 -0000
@@ -304,6 +304,14 @@ struct rtentry *in6_selectroute(struct s
 	    struct route_in6 *, unsigned int rtableid);
 
 u_int32_t ip6_randomflowlabel(void);
+
+#ifdef IPSEC
+struct tdb;
+struct tdb *
+	ip6_output_ipsec_lookup(struct mbuf *, int *, struct inpcb *);
+int	ip6_output_ipsec_send(struct tdb *, struct mbuf *, int, int);
+#endif /* IPSEC */
+
 #endif /* _KERNEL */
 
 #endif /* !_NETINET6_IP6_VAR_H_ */