1 files changed, 617 insertions, 387 deletions

diff --git a/sys/netinet6/ipsec.c b/sys/netinet6/ipsec.c
index b8a24471a94f..3cb835da4e83 100644
--- a/sys/netinet6/ipsec.c
+++ b/sys/netinet6/ipsec.c
@@ -1,5 +1,5 @@
 /*	$FreeBSD$	*/
-/*	$KAME: ipsec.c,v 1.66 2000/06/15 04:08:54 itojun Exp $	*/
+/*	$KAME: ipsec.c,v 1.103 2001/05/24 07:14:18 sakane Exp $	*/
 
 /*
  * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project.
@@ -67,9 +67,11 @@
 #ifdef INET6
 #include <netinet6/ip6_ecn.h>
 #endif
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
 
-#ifdef INET6
 #include <netinet/ip6.h>
+#ifdef INET6
 #include <netinet6/ip6_var.h>
 #endif
 #include <netinet/in_pcb.h>
@@ -97,25 +99,12 @@
 #endif
 #include <netkey/key.h>
 #include <netkey/keydb.h>
-#ifdef IPSEC_DEBUG
 #include <netkey/key_debug.h>
-#else
-#define	KEYDEBUG(lev,arg)
-#endif
 
 #include <machine/in_cksum.h>
 
 #include <net/net_osdep.h>
 
-#ifdef HAVE_NRL_INPCB
-#define in6pcb	inpcb
-#define in6p_sp	inp_sp
-#define in6p_fport	inp_fport
-#define in6p_lport	inp_lport
-#define in6p_socket	inp_socket
-#define sotoin6pcb(so)	((struct inpcb *)(so)->so_pcb)
-#endif
-
 #ifdef IPSEC_DEBUG
 int ipsec_debug = 1;
 #else
@@ -132,11 +121,14 @@ int ip4_ah_trans_deflev = IPSEC_LEVEL_USE;
 int ip4_ah_net_deflev = IPSEC_LEVEL_USE;
 struct secpolicy ip4_def_policy;
 int ip4_ipsec_ecn = 0;		/* ECN ignore(-1)/forbidden(0)/allowed(1) */
+int ip4_esp_randpad = -1;
 
+#ifdef SYSCTL_DECL
 SYSCTL_DECL(_net_inet_ipsec);
 #ifdef INET6
 SYSCTL_DECL(_net_inet6_ipsec6);
 #endif
+#endif
 
 /* net.inet.ipsec */
 SYSCTL_STRUCT(_net_inet_ipsec, IPSECCTL_STATS,
@@ -161,6 +153,8 @@ SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ECN,
 	ecn, CTLFLAG_RW,	&ip4_ipsec_ecn,	0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEBUG,
 	debug, CTLFLAG_RW,	&ipsec_debug,	0, "");
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ESP_RANDPAD,
+	esp_randpad, CTLFLAG_RW,	&ip4_esp_randpad,	0, "");
 
 #ifdef INET6
 struct ipsecstat ipsec6stat;
@@ -170,6 +164,7 @@ int ip6_ah_trans_deflev = IPSEC_LEVEL_USE;
 int ip6_ah_net_deflev = IPSEC_LEVEL_USE;
 struct secpolicy ip6_def_policy;
 int ip6_ipsec_ecn = 0;		/* ECN ignore(-1)/forbidden(0)/allowed(1) */
+int ip6_esp_randpad = -1;
 
 /* net.inet6.ipsec6 */
 SYSCTL_STRUCT(_net_inet6_ipsec6, IPSECCTL_STATS,
@@ -188,16 +183,22 @@ SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ECN,
 	ecn, CTLFLAG_RW,	&ip6_ipsec_ecn,	0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG,
 	debug, CTLFLAG_RW,	&ipsec_debug,	0, "");
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ESP_RANDPAD,
+	esp_randpad, CTLFLAG_RW,	&ip6_esp_randpad,	0, "");
 #endif /* INET6 */
 
 static int ipsec_setspidx_mbuf
-	__P((struct secpolicyindex *, u_int, u_int, struct mbuf *));
-static void ipsec4_setspidx_inpcb __P((struct mbuf *, struct inpcb *pcb));
-static void ipsec4_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+	__P((struct secpolicyindex *, u_int, u_int, struct mbuf *, int));
+static int ipsec4_setspidx_inpcb __P((struct mbuf *, struct inpcb *pcb));
+#ifdef INET6
+static int ipsec6_setspidx_in6pcb __P((struct mbuf *, struct in6pcb *pcb));
+#endif
+static int ipsec_setspidx __P((struct mbuf *, struct secpolicyindex *, int));
+static void ipsec4_get_ulp __P((struct mbuf *m, struct secpolicyindex *, int));
+static int ipsec4_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
 #ifdef INET6
-static void ipsec6_get_ulp __P((struct mbuf *m, struct secpolicyindex *));
-static void ipsec6_setspidx_in6pcb __P((struct mbuf *, struct in6pcb *pcb));
-static void ipsec6_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+static void ipsec6_get_ulp __P((struct mbuf *m, struct secpolicyindex *, int));
+static int ipsec6_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
 #endif
 static struct inpcbpolicy *ipsec_newpcbpolicy __P((void));
 static void ipsec_delpcbpolicy __P((struct inpcbpolicy *));
@@ -208,14 +209,21 @@ static int ipsec_get_policy __P((struct secpolicy *pcb_sp, struct mbuf **mp));
 static void vshiftl __P((unsigned char *, int, int));
 static int ipsec_in_reject __P((struct secpolicy *, struct mbuf *));
 static size_t ipsec_hdrsiz __P((struct secpolicy *));
+#ifdef INET
 static struct mbuf *ipsec4_splithdr __P((struct mbuf *));
+#endif
 #ifdef INET6
 static struct mbuf *ipsec6_splithdr __P((struct mbuf *));
 #endif
+#ifdef INET
 static int ipsec4_encapsulate __P((struct mbuf *, struct secasvar *));
+#endif
 #ifdef INET6
 static int ipsec6_encapsulate __P((struct mbuf *, struct secasvar *));
 #endif
+static struct mbuf *ipsec_addaux __P((struct mbuf *));
+static struct mbuf *ipsec_findaux __P((struct mbuf *));
+static void ipsec_optaux __P((struct mbuf *, struct mbuf *));
 
 /*
  * For OUTBOUND packet having a socket. Searching SPD for packet,
@@ -247,19 +255,29 @@ ipsec4_getpolicybysock(m, dir, so, error)
 	switch (so->so_proto->pr_domain->dom_family) {
 	case AF_INET:
 		/* set spidx in pcb */
-		ipsec4_setspidx_inpcb(m, sotoinpcb(so));
-		pcbsp = sotoinpcb(so)->inp_sp;
+		*error = ipsec4_setspidx_inpcb(m, sotoinpcb(so));
 		break;
 #ifdef INET6
 	case AF_INET6:
 		/* set spidx in pcb */
-		ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
-		pcbsp = sotoin6pcb(so)->in6p_sp;
+		*error = ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
 		break;
 #endif
 	default:
 		panic("ipsec4_getpolicybysock: unsupported address family\n");
 	}
+	if (*error)
+		return NULL;
+	switch (so->so_proto->pr_domain->dom_family) {
+	case AF_INET:
+		pcbsp = sotoinpcb(so)->inp_sp;
+		break;
+#ifdef INET6
+	case AF_INET6:
+		pcbsp = sotoin6pcb(so)->in6p_sp;
+		break;
+#endif
+	}
 
 	/* sanity check */
 	if (pcbsp == NULL)
@@ -404,7 +422,8 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
 	bzero(&spidx, sizeof(spidx));
 
 	/* make a index to look for a policy */
-	*error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET, m);
+	*error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET, m,
+	    (flag & IP_FORWARDING) ? 0 : 1);
 
 	if (*error != 0)
 		return NULL;
@@ -460,6 +479,11 @@ ipsec6_getpolicybysock(m, dir, so, error)
 	if (m == NULL || so == NULL || error == NULL)
 		panic("ipsec6_getpolicybysock: NULL pointer was passed.\n");
 
+#ifdef DIAGNOSTIC
+	if (so->so_proto->pr_domain->dom_family != AF_INET6)
+		panic("ipsec6_getpolicybysock: socket domain != inet6\n");
+#endif
+
 	/* set spidx in pcb */
 	ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
 
@@ -615,7 +639,8 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
 	bzero(&spidx, sizeof(spidx));
 
 	/* make a index to look for a policy */
-	*error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET6, m);
+	*error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET6, m,
+	    (flag & IP_FORWARDING) ? 0 : 1);
 
 	if (*error != 0)
 		return NULL;
@@ -656,302 +681,286 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
  *	other:	failure, and set errno.
  */
 int
-ipsec_setspidx_mbuf(spidx, dir, family, m)
+ipsec_setspidx_mbuf(spidx, dir, family, m, needport)
 	struct secpolicyindex *spidx;
 	u_int dir, family;
 	struct mbuf *m;
+	int needport;
 {
+	int error;
 
 	/* sanity check */
 	if (spidx == NULL || m == NULL)
 		panic("ipsec_setspidx_mbuf: NULL pointer was passed.\n");
 
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("ipsec_setspidx_mbuf: begin\n"); kdebug_mbuf(m));
-
-	/* initialize */
 	bzero(spidx, sizeof(*spidx));
 
+	error = ipsec_setspidx(m, spidx, needport);
+	if (error)
+		goto bad;
 	spidx->dir = dir;
 
-    {
-	/* sanity check for packet length. */
-	struct mbuf *n;
-	int tlen;
-
-	tlen = 0;
-	for (n = m; n; n = n->m_next)
-		tlen += n->m_len;
-	if (m->m_pkthdr.len != tlen) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-			printf("ipsec_setspidx_mbuf: "
-			       "total of m_len(%d) != pkthdr.len(%d), "
-			       "ignored.\n",
-				tlen, m->m_pkthdr.len));
-		goto bad;
-	}
-    }
+	return 0;
 
-	switch (family) {
-	case AF_INET:
-	{
-		struct ip *ip;
-		struct ip ipbuf;
-		struct sockaddr_in *sin;
+    bad:
+	/* XXX initialize */
+	bzero(spidx, sizeof(*spidx));
+	return EINVAL;
+}
 
-		/* sanity check 1 for minimum ip header length */
-		if (m->m_pkthdr.len < sizeof(struct ip)) {
-			KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-				printf("ipsec_setspidx_mbuf: "
-				       "pkthdr.len(%d) < sizeof(struct ip), "
-				       "ignored.\n",
-					m->m_pkthdr.len));
-			goto bad;
-		}
+static int
+ipsec4_setspidx_inpcb(m, pcb)
+	struct mbuf *m;
+	struct inpcb *pcb;
+{
+	struct secpolicyindex *spidx;
+	int error;
 
-		/*
-		 * get IPv4 header packet.  usually the mbuf is contiguous
-		 * and we need no copies.
-		 */
-		if (m->m_len >= sizeof(*ip))
-			ip = mtod(m, struct ip *);
-		else {
-			m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
-			ip = &ipbuf;
-		}
+	/* sanity check */
+	if (pcb == NULL)
+		panic("ipsec4_setspidx_inpcb: no PCB found.\n");
+	if (pcb->inp_sp == NULL)
+		panic("ipsec4_setspidx_inpcb: no inp_sp found.\n");
+	if (pcb->inp_sp->sp_out == NULL || pcb->inp_sp->sp_in == NULL)
+		panic("ipsec4_setspidx_inpcb: no sp_in/out found.\n");
 
-		/* XXX some more checks on IPv4 header. */
+	bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
+	bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
 
-		sin = (struct sockaddr_in *)&spidx->src;
-		sin->sin_family = AF_INET;
-		sin->sin_len = sizeof(*sin);
-		bcopy(&ip->ip_src, &sin->sin_addr, sizeof(sin->sin_addr));
-		sin->sin_port = IPSEC_PORT_ANY;
+	spidx = &pcb->inp_sp->sp_in->spidx;
+	error = ipsec_setspidx(m, spidx, 1);
+	if (error)
+		goto bad;
+	spidx->dir = IPSEC_DIR_INBOUND;
 
-		sin = (struct sockaddr_in *)&spidx->dst;
-		sin->sin_family = AF_INET;
-		sin->sin_len = sizeof(*sin);
-		bcopy(&ip->ip_dst, &sin->sin_addr, sizeof(sin->sin_addr));
-		sin->sin_port = IPSEC_PORT_ANY;
+	spidx = &pcb->inp_sp->sp_out->spidx;
+	error = ipsec_setspidx(m, spidx, 1);
+	if (error)
+		goto bad;
+	spidx->dir = IPSEC_DIR_OUTBOUND;
 
-		spidx->prefs = spidx->prefd = sizeof(struct in_addr) << 3;
+	return 0;
 
-		spidx->ul_proto = ip->ip_p;
-		break;
-	}
+bad:
+	bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
+	bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
+	return error;
+}
 
 #ifdef INET6
-	case AF_INET6:
-	{
-		struct ip6_hdr *ip6;
-		struct ip6_hdr ip6buf;
-		struct sockaddr_in6 *sin6;
+static int
+ipsec6_setspidx_in6pcb(m, pcb)
+	struct mbuf *m;
+	struct in6pcb *pcb;
+{
+	struct secpolicyindex *spidx;
+	int error;
 
-		/* sanity check 1 for minimum ip header length */
-		if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
-			KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-				printf("ipsec_setspidx_mbuf: "
-				       "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
-				       "ignored.\n",
-					m->m_pkthdr.len));
-			goto bad;
-		}
+	/* sanity check */
+	if (pcb == NULL)
+		panic("ipsec6_setspidx_in6pcb: no PCB found.\n");
+	if (pcb->in6p_sp == NULL)
+		panic("ipsec6_setspidx_in6pcb: no in6p_sp found.\n");
+	if (pcb->in6p_sp->sp_out == NULL || pcb->in6p_sp->sp_in == NULL)
+		panic("ipsec6_setspidx_in6pcb: no sp_in/out found.\n");
 
-		/*
-		 * get IPv6 header packet.  usually the mbuf is contiguous
-		 * and we need no copies.
-		 */
-		if (m->m_len >= sizeof(*ip6))
-			ip6 = mtod(m, struct ip6_hdr *);
-		else {
-			m_copydata(m, 0, sizeof(ip6buf), (caddr_t)&ip6buf);
-			ip6 = &ip6buf;
-		}
+	bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
+	bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
 
-		/* some more checks on IPv4 header. */
-		if ((ip6->ip6_vfc & IPV6_VERSION_MASK) != IPV6_VERSION) {
-			KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-				printf("ipsec_setspidx_mbuf: "
-					"wrong ip version on packet "
-					"(expected IPv6), ignored.\n"));
-			goto bad;
-		}
+	spidx = &pcb->in6p_sp->sp_in->spidx;
+	error = ipsec_setspidx(m, spidx, 1);
+	if (error)
+		goto bad;
+	spidx->dir = IPSEC_DIR_INBOUND;
 
-		sin6 = (struct sockaddr_in6 *)&spidx->src;
-		sin6->sin6_family = AF_INET6;
-		sin6->sin6_len = sizeof(*sin6);
-		bcopy(&ip6->ip6_src, &sin6->sin6_addr, sizeof(sin6->sin6_addr));
-		sin6->sin6_port = IPSEC_PORT_ANY;
-		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
-			/* fix scope id for comparing SPD */
-			sin6->sin6_addr.s6_addr16[1] = 0;
-			sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
-		}
+	spidx = &pcb->in6p_sp->sp_out->spidx;
+	error = ipsec_setspidx(m, spidx, 1);
+	if (error)
+		goto bad;
+	spidx->dir = IPSEC_DIR_OUTBOUND;
 
-		sin6 = (struct sockaddr_in6 *)&spidx->dst;
-		sin6->sin6_family = AF_INET6;
-		sin6->sin6_len = sizeof(*sin6);
-		bcopy(&ip6->ip6_dst, &sin6->sin6_addr, sizeof(sin6->sin6_addr));
-		sin6->sin6_port = IPSEC_PORT_ANY;
-		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
-			/* fix scope id for comparing SPD */
-			sin6->sin6_addr.s6_addr16[1] = 0;
-			sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
-		}
+	return 0;
 
-		spidx->prefs = spidx->prefd = sizeof(struct in6_addr) << 3;
+bad:
+	bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
+	bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
+	return error;
+}
+#endif
 
-		ipsec6_get_ulp(m, spidx);
-		break;
-	}
-#endif /* INET6 */
-	default:
-		panic("ipsec_secsecidx: no supported family passed.\n");
-	}
+/*
+ * configure security policy index (src/dst/proto/sport/dport)
+ * by looking at the content of mbuf.
+ * the caller is responsible for error recovery (like clearing up spidx).
+ */
+static int
+ipsec_setspidx(m, spidx, needport)
+	struct mbuf *m;
+	struct secpolicyindex *spidx;
+	int needport;
+{
+	struct ip *ip = NULL;
+	struct ip ipbuf;
+	u_int v;
+	struct mbuf *n;
+	int len;
+	int error;
 
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("ipsec_setspidx_mbuf: end\n");
-		kdebug_secpolicyindex(spidx));
+	if (m == NULL)
+		panic("ipsec_setspidx: m == 0 passed.\n");
 
-	return 0;
+	/*
+	 * validate m->m_pkthdr.len.  we see incorrect length if we
+	 * mistakenly call this function with inconsistent mbuf chain
+	 * (like 4.4BSD tcp/udp processing).  XXX should we panic here?
+	 */
+	len = 0;
+	for (n = m; n; n = n->m_next)
+		len += n->m_len;
+	if (m->m_pkthdr.len != len) {
+		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+			printf("ipsec_setspidx: "
+			       "total of m_len(%d) != pkthdr.len(%d), "
+			       "ignored.\n",
+				len, m->m_pkthdr.len));
+		return EINVAL;
+	}
 
-    bad:
-	/* XXX initialize */
-	bzero(spidx, sizeof(*spidx));
-	return EINVAL;
-}
+	if (m->m_pkthdr.len < sizeof(struct ip)) {
+		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+			printf("ipsec_setspidx: "
+			    "pkthdr.len(%d) < sizeof(struct ip), ignored.\n",
+			    m->m_pkthdr.len));
+		return EINVAL;
+	}
 
+	if (m->m_len >= sizeof(*ip))
+		ip = mtod(m, struct ip *);
+	else {
+		m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
+		ip = &ipbuf;
+	}
+#ifdef _IP_VHL
+	v = _IP_VHL_V(ip->ip_vhl);
+#else
+	v = ip->ip_v;
+#endif
+	switch (v) {
+	case 4:
+		error = ipsec4_setspidx_ipaddr(m, spidx);
+		if (error)
+			return error;
+		ipsec4_get_ulp(m, spidx, needport);
+		return 0;
 #ifdef INET6
-/*
- * Get upper layer protocol number and port number if there.
- * Assumed all extension headers are in single mbuf.
- */
-#include <netinet/tcp.h>
-#include <netinet/udp.h>
+	case 6:
+		if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
+			KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+				printf("ipsec_setspidx: "
+				    "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
+				    "ignored.\n", m->m_pkthdr.len));
+			return EINVAL;
+		}
+		error = ipsec6_setspidx_ipaddr(m, spidx);
+		if (error)
+			return error;
+		ipsec6_get_ulp(m, spidx, needport);
+		return 0;
+#endif
+	default:
+		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+			printf("ipsec_setspidx: "
+			    "unknown IP version %u, ignored.\n", v));
+		return EINVAL;
+	}
+}
+
 static void
-ipsec6_get_ulp(m, spidx)
+ipsec4_get_ulp(m, spidx, needport)
 	struct mbuf *m;
 	struct secpolicyindex *spidx;
+	int needport;
 {
-	int off, nxt;
+	struct ip ip;
+	struct ip6_ext ip6e;
+	u_int8_t nxt;
+	int off;
+	struct tcphdr th;
+	struct udphdr uh;
 
 	/* sanity check */
 	if (m == NULL)
-		panic("ipsec6_get_ulp: NULL pointer was passed.\n");
-
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("ipsec6_get_ulp:\n"); kdebug_mbuf(m));
+		panic("ipsec4_get_ulp: NULL pointer was passed.\n");
+	if (m->m_pkthdr.len < sizeof(ip))
+		panic("ipsec4_get_ulp: too short\n");
 
 	/* set default */
 	spidx->ul_proto = IPSEC_ULPROTO_ANY;
-	((struct sockaddr_in6 *)&spidx->src)->sin6_port = IPSEC_PORT_ANY;
-	((struct sockaddr_in6 *)&spidx->dst)->sin6_port = IPSEC_PORT_ANY;
+	((struct sockaddr_in *)&spidx->src)->sin_port = IPSEC_PORT_ANY;
+	((struct sockaddr_in *)&spidx->dst)->sin_port = IPSEC_PORT_ANY;
 
-	nxt = -1;
-	off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
-	if (off < 0 || m->m_pkthdr.len < off)
+	m_copydata(m, 0, sizeof(ip), (caddr_t)&ip);
+	/* ip_input() flips it into host endian XXX need more checking */
+	if (ip.ip_off & (IP_MF | IP_OFFMASK))
 		return;
 
-	switch (nxt) {
-	case IPPROTO_TCP:
-		spidx->ul_proto = nxt;
-		if (off + sizeof(struct tcphdr) <= m->m_pkthdr.len) {
-			struct tcphdr th;
+	nxt = ip.ip_p;
+#ifdef _IP_VHL
+	off = _IP_VHL_HL(ip->ip_vhl) << 2;
+#else
+	off = ip.ip_hl << 2;
+#endif
+	while (off < m->m_pkthdr.len) {
+		switch (nxt) {
+		case IPPROTO_TCP:
+			spidx->ul_proto = nxt;
+			if (!needport)
+				return;
+			if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
+				return;
 			m_copydata(m, off, sizeof(th), (caddr_t)&th);
-			((struct sockaddr_in6 *)&spidx->src)->sin6_port =
+			((struct sockaddr_in *)&spidx->src)->sin_port =
 			    th.th_sport;
-			((struct sockaddr_in6 *)&spidx->dst)->sin6_port =
+			((struct sockaddr_in *)&spidx->dst)->sin_port =
 			    th.th_dport;
-		}
-		break;
-	case IPPROTO_UDP:
-		spidx->ul_proto = nxt;
-		if (off + sizeof(struct udphdr) <= m->m_pkthdr.len) {
-			struct udphdr uh;
+			return;
+		case IPPROTO_UDP:
+			spidx->ul_proto = nxt;
+			if (!needport)
+				return;
+			if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
+				return;
 			m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
-			((struct sockaddr_in6 *)&spidx->src)->sin6_port =
+			((struct sockaddr_in *)&spidx->src)->sin_port =
 			    uh.uh_sport;
-			((struct sockaddr_in6 *)&spidx->dst)->sin6_port =
+			((struct sockaddr_in *)&spidx->dst)->sin_port =
 			    uh.uh_dport;
+			return;
+		case IPPROTO_AH:
+			if (m->m_pkthdr.len > off + sizeof(ip6e))
+				return;
+			m_copydata(m, off, sizeof(ip6e), (caddr_t)&ip6e);
+			off += (ip6e.ip6e_len + 2) << 2;
+			nxt = ip6e.ip6e_nxt;
+			break;
+		case IPPROTO_ICMP:
+		default:
+			/* XXX intermediate headers??? */
+			spidx->ul_proto = nxt;
+			return;
 		}
-		break;
-	case IPPROTO_ICMPV6:
-		spidx->ul_proto = nxt;
-		break;
-	default:
-		break;
 	}
 }
-#endif
-
-static void
-ipsec4_setspidx_inpcb(m, pcb)
-	struct mbuf *m;
-	struct inpcb *pcb;
-{
-	struct secpolicyindex *spidx;
-	struct sockaddr_in *sin1, *sin2;
-
-	/* sanity check */
-	if (pcb == NULL)
-		panic("ipsec4_setspidx_inpcb: no PCB found.\n");
-	if (pcb->inp_sp == NULL)
-		panic("ipsec4_setspidx_inpcb: no inp_sp found.\n");
-	if (pcb->inp_sp->sp_out ==NULL || pcb->inp_sp->sp_in == NULL)
-		panic("ipsec4_setspidx_inpcb: no sp_in/out found.\n");
-
-	bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
-	bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
-
-	spidx = &pcb->inp_sp->sp_in->spidx;
-	spidx->dir = IPSEC_DIR_INBOUND;
-	sin1 = (struct sockaddr_in *)&spidx->src;
-	sin2 = (struct sockaddr_in *)&spidx->dst;
-	sin1->sin_len = sin2->sin_len = sizeof(struct sockaddr_in);
-	sin1->sin_family = sin2->sin_family = AF_INET;
-	spidx->prefs = sizeof(struct in_addr) << 3;
-	spidx->prefd = sizeof(struct in_addr) << 3;
-	spidx->ul_proto = pcb->inp_socket->so_proto->pr_protocol;
-	sin1->sin_port = pcb->inp_fport;
-	sin2->sin_port = pcb->inp_lport;
-	ipsec4_setspidx_ipaddr(m, spidx);
-
-	spidx = &pcb->inp_sp->sp_out->spidx;
-	spidx->dir = IPSEC_DIR_OUTBOUND;
-	sin1 = (struct sockaddr_in *)&spidx->src;
-	sin2 = (struct sockaddr_in *)&spidx->dst;
-	sin1->sin_len = sin2->sin_len = sizeof(struct sockaddr_in);
-	sin1->sin_family = sin2->sin_family = AF_INET;
-	spidx->prefs = sizeof(struct in_addr) << 3;
-	spidx->prefd = sizeof(struct in_addr) << 3;
-	spidx->ul_proto = pcb->inp_socket->so_proto->pr_protocol;
-	sin1->sin_port = pcb->inp_lport;
-	sin2->sin_port = pcb->inp_fport;
-	ipsec4_setspidx_ipaddr(m, spidx);
-
-	return;
-}
 
-static void
+/* assumes that m is sane */
+static int
 ipsec4_setspidx_ipaddr(m, spidx)
 	struct mbuf *m;
 	struct secpolicyindex *spidx;
 {
 	struct ip *ip = NULL;
 	struct ip ipbuf;
-
-	/* sanity check 1 for minimum ip header length */
-	if (m == NULL)
-		panic("ipsec4_setspidx_ipaddr: m == 0 passed.\n");
-
-	if (m->m_pkthdr.len < sizeof(struct ip)) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-			printf("ipsec4_setspidx_ipaddr: "
-			       "pkthdr.len(%d) < sizeof(struct ip), "
-			       "ignored.\n",
-				m->m_pkthdr.len));
-		return;
-	}
+	struct sockaddr_in *sin;
 
 	if (m->m_len >= sizeof(*ip))
 		ip = mtod(m, struct ip *);
@@ -960,64 +969,81 @@ ipsec4_setspidx_ipaddr(m, spidx)
 		ip = &ipbuf;
 	}
 
-	bcopy(&ip->ip_src, &((struct sockaddr_in *)&spidx->src)->sin_addr,
-	    sizeof(ip->ip_src));
-	bcopy(&ip->ip_dst, &((struct sockaddr_in *)&spidx->dst)->sin_addr,
-	    sizeof(ip->ip_dst));
+	sin = (struct sockaddr_in *)&spidx->src;
+	bzero(sin, sizeof(*sin));
+	sin->sin_family = AF_INET;
+	sin->sin_len = sizeof(struct sockaddr_in);
+	bcopy(&ip->ip_src, &sin->sin_addr, sizeof(ip->ip_src));
+	spidx->prefs = sizeof(struct in_addr) << 3;
 
-	return;
+	sin = (struct sockaddr_in *)&spidx->dst;
+	bzero(sin, sizeof(*sin));
+	sin->sin_family = AF_INET;
+	sin->sin_len = sizeof(struct sockaddr_in);
+	bcopy(&ip->ip_dst, &sin->sin_addr, sizeof(ip->ip_dst));
+	spidx->prefd = sizeof(struct in_addr) << 3;
+	return 0;
 }
 
 #ifdef INET6
 static void
-ipsec6_setspidx_in6pcb(m, pcb)
+ipsec6_get_ulp(m, spidx, needport)
 	struct mbuf *m;
-	struct in6pcb *pcb;
-{
 	struct secpolicyindex *spidx;
-	struct sockaddr_in6 *sin1, *sin2;
+	int needport;
+{
+	int off, nxt;
+	struct tcphdr th;
+	struct udphdr uh;
 
 	/* sanity check */
-	if (pcb == NULL)
-		panic("ipsec6_setspidx_in6pcb: no PCB found.\n");
-	if (pcb->in6p_sp == NULL)
-		panic("ipsec6_setspidx_in6pcb: no in6p_sp found.\n");
-	if (pcb->in6p_sp->sp_out ==NULL || pcb->in6p_sp->sp_in == NULL)
-		panic("ipsec6_setspidx_in6pcb: no sp_in/out found.\n");
+	if (m == NULL)
+		panic("ipsec6_get_ulp: NULL pointer was passed.\n");
 
-	bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
-	bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
+	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+		printf("ipsec6_get_ulp:\n"); kdebug_mbuf(m));
 
-	spidx = &pcb->in6p_sp->sp_in->spidx;
-	spidx->dir = IPSEC_DIR_INBOUND;
-	sin1 = (struct sockaddr_in6 *)&spidx->src;
-	sin2 = (struct sockaddr_in6 *)&spidx->dst;
-	sin1->sin6_len = sin2->sin6_len = sizeof(struct sockaddr_in6);
-	sin1->sin6_family = sin2->sin6_family = AF_INET6;
-	spidx->prefs = sizeof(struct in6_addr) << 3;
-	spidx->prefd = sizeof(struct in6_addr) << 3;
-	spidx->ul_proto = pcb->in6p_socket->so_proto->pr_protocol;
-	sin1->sin6_port = pcb->in6p_fport;
-	sin2->sin6_port = pcb->in6p_lport;
-	ipsec6_setspidx_ipaddr(m, spidx);
+	/* set default */
+	spidx->ul_proto = IPSEC_ULPROTO_ANY;
+	((struct sockaddr_in6 *)&spidx->src)->sin6_port = IPSEC_PORT_ANY;
+	((struct sockaddr_in6 *)&spidx->dst)->sin6_port = IPSEC_PORT_ANY;
 
-	spidx = &pcb->in6p_sp->sp_out->spidx;
-	spidx->dir = IPSEC_DIR_OUTBOUND;
-	sin1 = (struct sockaddr_in6 *)&spidx->src;
-	sin2 = (struct sockaddr_in6 *)&spidx->dst;
-	sin1->sin6_len = sin2->sin6_len = sizeof(struct sockaddr_in6);
-	sin1->sin6_family = sin2->sin6_family = AF_INET6;
-	spidx->prefs = sizeof(struct in6_addr) << 3;
-	spidx->prefd = sizeof(struct in6_addr) << 3;
-	spidx->ul_proto = pcb->in6p_socket->so_proto->pr_protocol;
-	sin1->sin6_port = pcb->in6p_lport;
-	sin2->sin6_port = pcb->in6p_fport;
-	ipsec6_setspidx_ipaddr(m, spidx);
+	nxt = -1;
+	off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
+	if (off < 0 || m->m_pkthdr.len < off)
+		return;
 
-	return;
+	switch (nxt) {
+	case IPPROTO_TCP:
+		spidx->ul_proto = nxt;
+		if (!needport)
+			break;
+		if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
+			break;
+		m_copydata(m, off, sizeof(th), (caddr_t)&th);
+		((struct sockaddr_in6 *)&spidx->src)->sin6_port = th.th_sport;
+		((struct sockaddr_in6 *)&spidx->dst)->sin6_port = th.th_dport;
+		break;
+	case IPPROTO_UDP:
+		spidx->ul_proto = nxt;
+		if (!needport)
+			break;
+		if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
+			break;
+		m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
+		((struct sockaddr_in6 *)&spidx->src)->sin6_port = uh.uh_sport;
+		((struct sockaddr_in6 *)&spidx->dst)->sin6_port = uh.uh_dport;
+		break;
+	case IPPROTO_ICMPV6:
+	default:
+		/* XXX intermediate headers??? */
+		spidx->ul_proto = nxt;
+		break;
+	}
 }
 
-static void
+/* assumes that m is sane */
+static int
 ipsec6_setspidx_ipaddr(m, spidx)
 	struct mbuf *m;
 	struct secpolicyindex *spidx;
@@ -1026,19 +1052,6 @@ ipsec6_setspidx_ipaddr(m, spidx)
 	struct ip6_hdr ip6buf;
 	struct sockaddr_in6 *sin6;
 
-	/* sanity check 1 for minimum ip header length */
-	if (m == NULL)
-		panic("ipsec6_setspidx_in6pcb: m == 0 passed.\n");
-
-	if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-			printf("ipsec6_setspidx_ipaddr: "
-			       "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
-			       "ignored.\n",
-				m->m_pkthdr.len));
-		return;
-	}
-
 	if (m->m_len >= sizeof(*ip6))
 		ip6 = mtod(m, struct ip6_hdr *);
 	else {
@@ -1046,29 +1059,29 @@ ipsec6_setspidx_ipaddr(m, spidx)
 		ip6 = &ip6buf;
 	}
 
-	if ((ip6->ip6_vfc & IPV6_VERSION_MASK) != IPV6_VERSION) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-			printf("ipsec_setspidx_mbuf: "
-				"wrong ip version on packet "
-				"(expected IPv6), ignored.\n"));
-		return;
-	}
-
 	sin6 = (struct sockaddr_in6 *)&spidx->src;
+	bzero(sin6, sizeof(*sin6));
+	sin6->sin6_family = AF_INET6;
+	sin6->sin6_len = sizeof(struct sockaddr_in6);
 	bcopy(&ip6->ip6_src, &sin6->sin6_addr, sizeof(ip6->ip6_src));
 	if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
 		sin6->sin6_addr.s6_addr16[1] = 0;
 		sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
 	}
+	spidx->prefs = sizeof(struct in6_addr) << 3;
 
 	sin6 = (struct sockaddr_in6 *)&spidx->dst;
+	bzero(sin6, sizeof(*sin6));
+	sin6->sin6_family = AF_INET6;
+	sin6->sin6_len = sizeof(struct sockaddr_in6);
 	bcopy(&ip6->ip6_dst, &sin6->sin6_addr, sizeof(ip6->ip6_dst));
 	if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
 		sin6->sin6_addr.s6_addr16[1] = 0;
 		sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
 	}
+	spidx->prefd = sizeof(struct in6_addr) << 3;
 
-	return;
+	return 0;
 }
 #endif
 
@@ -1628,6 +1641,8 @@ ipsec_in_reject(sp, m)
 	need_conf = 0;
 	need_icv = 0;
 
+	/* XXX should compare policy against ipsec header history */
+
 	for (isr = sp->req; isr != NULL; isr = isr->next) {
 
 		/* get current level */
@@ -1653,7 +1668,8 @@ ipsec_in_reject(sp, m)
 		case IPPROTO_IPCOMP:
 			/*
 			 * we don't really care, as IPcomp document says that
-			 * we shouldn't compress small packets
+			 * we shouldn't compress small packets, IPComp policy
+			 * should always be treated as being in "use" level.
 			 */
 			break;
 		}
@@ -1717,12 +1733,10 @@ ipsec4_in_reject(m, inp)
 {
 	if (inp == NULL)
 		return ipsec4_in_reject_so(m, NULL);
-	else {
-		if (inp->inp_socket)
-			return ipsec4_in_reject_so(m, inp->inp_socket);
-		else
-			panic("ipsec4_in_reject: invalid inpcb/socket");
-	}
+	if (inp->inp_socket)
+		return ipsec4_in_reject_so(m, inp->inp_socket);
+	else
+		panic("ipsec4_in_reject: invalid inpcb/socket");
 }
 
 #ifdef INET6
@@ -1771,12 +1785,10 @@ ipsec6_in_reject(m, in6p)
 {
 	if (in6p == NULL)
 		return ipsec6_in_reject_so(m, NULL);
-	else {
-		if (in6p->in6p_socket)
-			return ipsec6_in_reject_so(m, in6p->in6p_socket);
-		else
-			panic("ipsec6_in_reject: invalid in6p/socket");
-	}
+	if (in6p->in6p_socket)
+		return ipsec6_in_reject_so(m, in6p->in6p_socket);
+	else
+		panic("ipsec6_in_reject: invalid in6p/socket");
 }
 #endif
 
@@ -2054,6 +2066,7 @@ ipsec4_encapsulate(m, sav)
 		&ip->ip_src, sizeof(ip->ip_src));
 	bcopy(&((struct sockaddr_in *)&sav->sah->saidx.dst)->sin_addr,
 		&ip->ip_dst, sizeof(ip->ip_dst));
+	ip->ip_ttl = IPDEFTTL;
 
 	/* XXX Should ip_src be updated later ? */
 
@@ -2133,6 +2146,7 @@ ipsec6_encapsulate(m, sav)
 		&ip6->ip6_src, sizeof(ip6->ip6_src));
 	bcopy(&((struct sockaddr_in6 *)&sav->sah->saidx.dst)->sin6_addr,
 		&ip6->ip6_dst, sizeof(ip6->ip6_dst));
+	ip6->ip6_hlim = IPV6_DEFHLIM;
 
 	/* XXX Should ip6_src be updated later ? */
 
@@ -2345,11 +2359,11 @@ ipsec4_logpacketstr(ip, spi)
 	snprintf(buf, sizeof(buf), "packet(SPI=%u ", (u_int32_t)ntohl(spi));
 	while (p && *p)
 		p++;
-	snprintf(p, sizeof(buf) - (p - buf), "src=%d.%d.%d.%d",
+	snprintf(p, sizeof(buf) - (p - buf), "src=%u.%u.%u.%u",
 		s[0], s[1], s[2], s[3]);
 	while (p && *p)
 		p++;
-	snprintf(p, sizeof(buf) - (p - buf), " dst=%d.%d.%d.%d",
+	snprintf(p, sizeof(buf) - (p - buf), " dst=%u.%u.%u.%u",
 		d[0], d[1], d[2], d[3]);
 	while (p && *p)
 		p++;
@@ -2454,6 +2468,7 @@ ipsec_dumpmbuf(m)
 	printf("---\n");
 }
 
+#ifdef INET
 /*
  * IPsec output logic for IPv4.
  */
@@ -2500,6 +2515,8 @@ ipsec4_output(state, sp, flags)
 		/* make SA index for search proper SA */
 		ip = mtod(state->m, struct ip *);
 		bcopy(&isr->saidx, &saidx, sizeof(saidx));
+		saidx.mode = isr->saidx.mode;
+		saidx.reqid = isr->saidx.reqid;
 		sin = (struct sockaddr_in *)&saidx.src;
 		if (sin->sin_len == 0) {
 			sin->sin_len = sizeof(*sin);
@@ -2595,7 +2612,7 @@ ipsec4_output(state, sp, flags)
 			 && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
 			  || dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
 				RTFREE(state->ro->ro_rt);
-				bzero((caddr_t)state->ro, sizeof (*state->ro));
+				state->ro->ro_rt = NULL;
 			}
 			if (state->ro->ro_rt == 0) {
 				dst4->sin_family = AF_INET;
@@ -2672,6 +2689,7 @@ bad:
 	state->m = NULL;
 	return error;
 }
+#endif
 
 #ifdef INET6
 /*
@@ -2720,6 +2738,8 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
 		/* make SA index for search proper SA */
 		ip6 = mtod(state->m, struct ip6_hdr *);
 		bcopy(&isr->saidx, &saidx, sizeof(saidx));
+		saidx.mode = isr->saidx.mode;
+		saidx.reqid = isr->saidx.reqid;
 		sin6 = (struct sockaddr_in6 *)&saidx.src;
 		if (sin6->sin6_len == 0) {
 			sin6->sin6_len = sizeof(*sin6);
@@ -2757,6 +2777,18 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
 			 */
 			ipsec6stat.out_nosa++;
 			error = ENOENT;
+
+			/*
+			 * 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(state->m, ICMP6_DST_UNREACH,
+				    ICMP6_DST_UNREACH_ADMIN, 0);
+			state->m = NULL; /* icmp6_error freed the mbuf */
 			goto bad;
 		}
 
@@ -2951,7 +2983,7 @@ ipsec6_output_tunnel(state, sp, flags)
 			 && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
 			  || !IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ip6->ip6_dst))) {
 				RTFREE(state->ro->ro_rt);
-				bzero((caddr_t)state->ro, sizeof (*state->ro));
+				state->ro->ro_rt = NULL;
 			}
 			if (state->ro->ro_rt == 0) {
 				bzero(dst6, sizeof(*dst6));
@@ -3030,6 +3062,7 @@ bad:
 }
 #endif /*INET6*/
 
+#ifdef INET
 /*
  * Chop IP header and option off from the payload.
  */
@@ -3071,6 +3104,7 @@ ipsec4_splithdr(m)
 	}
 	return m;
 }
+#endif
 
 #ifdef INET6
 static struct mbuf *
@@ -3111,38 +3145,89 @@ ipsec6_splithdr(m)
 
 /* validate inbound IPsec tunnel packet. */
 int
-ipsec4_tunnel_validate(ip, nxt0, sav)
-	struct ip *ip;
+ipsec4_tunnel_validate(m, off, nxt0, sav)
+	struct mbuf *m;		/* no pullup permitted, m->m_len >= ip */
+	int off;
 	u_int nxt0;
 	struct secasvar *sav;
 {
 	u_int8_t nxt = nxt0 & 0xff;
 	struct sockaddr_in *sin;
+	struct sockaddr_in osrc, odst, isrc, idst;
 	int hlen;
+	struct secpolicy *sp;
+	struct ip *oip;
 
+#ifdef DIAGNOSTIC
+	if (m->m_len < sizeof(struct ip))
+		panic("too short mbuf on ipsec4_tunnel_validate");
+#endif
 	if (nxt != IPPROTO_IPV4)
 		return 0;
+	if (m->m_pkthdr.len < off + sizeof(struct ip))
+		return 0;
+	/* do not decapsulate if the SA is for transport mode only */
+	if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
+		return 0;
+
+	oip = mtod(m, struct ip *);
 #ifdef _IP_VHL
-	hlen = _IP_VHL_HL(ip->ip_vhl) << 2;
+	hlen = _IP_VHL_HL(oip->ip_vhl) << 2;
 #else
-	hlen = ip->ip_hl << 2;
+	hlen = oip->ip_hl << 2;
 #endif
 	if (hlen != sizeof(struct ip))
 		return 0;
-	switch (((struct sockaddr *)&sav->sah->saidx.dst)->sa_family) {
-	case AF_INET:
-		sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
-		if (bcmp(&ip->ip_dst, &sin->sin_addr, sizeof(ip->ip_dst)) != 0)
-			return 0;
-		break;
-#ifdef INET6
-	case AF_INET6:
-		/* should be supported, but at this moment we don't. */
-		/*FALLTHROUGH*/
-#endif
-	default:
+
+	/* AF_INET6 should be supported, but at this moment we don't. */
+	sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
+	if (sin->sin_family != AF_INET)
 		return 0;
-	}
+	if (bcmp(&oip->ip_dst, &sin->sin_addr, sizeof(oip->ip_dst)) != 0)
+		return 0;
+
+	/* XXX slow */
+	bzero(&osrc, sizeof(osrc));
+	bzero(&odst, sizeof(odst));
+	bzero(&isrc, sizeof(isrc));
+	bzero(&idst, sizeof(idst));
+	osrc.sin_family = odst.sin_family = isrc.sin_family = idst.sin_family = 
+	    AF_INET;
+	osrc.sin_len = odst.sin_len = isrc.sin_len = idst.sin_len = 
+	    sizeof(struct sockaddr_in);
+	osrc.sin_addr = oip->ip_src;
+	odst.sin_addr = oip->ip_dst;
+	m_copydata(m, off + offsetof(struct ip, ip_src), sizeof(isrc.sin_addr),
+	    (caddr_t)&isrc.sin_addr);
+	m_copydata(m, off + offsetof(struct ip, ip_dst), sizeof(idst.sin_addr),
+	    (caddr_t)&idst.sin_addr);
+
+	/*
+	 * RFC2401 5.2.1 (b): (assume that we are using tunnel mode)
+	 * - if the inner destination is multicast address, there can be
+	 *   multiple permissible inner source address.  implementation
+	 *   may want to skip verification of inner source address against
+	 *   SPD selector.
+	 * - if the inner protocol is ICMP, the packet may be an error report
+	 *   from routers on the other side of the VPN cloud (R in the
+	 *   following diagram).  in this case, we cannot verify inner source
+	 *   address against SPD selector.
+	 *	me -- gw === gw -- R -- you
+	 *
+	 * we consider the first bullet to be users responsibility on SPD entry
+	 * configuration (if you need to encrypt multicast traffic, set
+	 * the source range of SPD selector to 0.0.0.0/0, or have explicit
+	 * address ranges for possible senders).
+	 * the second bullet is not taken care of (yet).
+	 *
+	 * therefore, we do not do anything special about inner source.
+	 */
+
+	sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
+	    (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
+	if (!sp)
+		return 0;
+	key_freesp(sp);
 
 	return 1;
 }
@@ -3150,28 +3235,64 @@ ipsec4_tunnel_validate(ip, nxt0, sav)
 #ifdef INET6
 /* validate inbound IPsec tunnel packet. */
 int
-ipsec6_tunnel_validate(ip6, nxt0, sav)
-	struct ip6_hdr *ip6;
+ipsec6_tunnel_validate(m, off, nxt0, sav)
+	struct mbuf *m;		/* no pullup permitted, m->m_len >= ip */
+	int off;
 	u_int nxt0;
 	struct secasvar *sav;
 {
 	u_int8_t nxt = nxt0 & 0xff;
 	struct sockaddr_in6 *sin6;
+	struct sockaddr_in6 osrc, odst, isrc, idst;
+	struct secpolicy *sp;
+	struct ip6_hdr *oip6;
 
+#ifdef DIAGNOSTIC
+	if (m->m_len < sizeof(struct ip6_hdr))
+		panic("too short mbuf on ipsec6_tunnel_validate");
+#endif
 	if (nxt != IPPROTO_IPV6)
 		return 0;
-	switch (((struct sockaddr *)&sav->sah->saidx.dst)->sa_family) {
-	case AF_INET6:
-		sin6 = ((struct sockaddr_in6 *)&sav->sah->saidx.dst);
-		if (!IN6_ARE_ADDR_EQUAL(&ip6->ip6_dst, &sin6->sin6_addr))
-			return 0;
-		break;
-	case AF_INET:
-		/* should be supported, but at this moment we don't. */
-		/*FALLTHROUGH*/
-	default:
+	if (m->m_pkthdr.len < off + sizeof(struct ip6_hdr))
+		return 0;
+	/* do not decapsulate if the SA is for transport mode only */
+	if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
+		return 0;
+
+	oip6 = mtod(m, struct ip6_hdr *);
+	/* AF_INET should be supported, but at this moment we don't. */
+	sin6 = (struct sockaddr_in6 *)&sav->sah->saidx.dst;
+	if (sin6->sin6_family != AF_INET6)
+		return 0;
+	if (!IN6_ARE_ADDR_EQUAL(&oip6->ip6_dst, &sin6->sin6_addr))
 		return 0;
-	}
+
+	/* XXX slow */
+	bzero(&osrc, sizeof(osrc));
+	bzero(&odst, sizeof(odst));
+	bzero(&isrc, sizeof(isrc));
+	bzero(&idst, sizeof(idst));
+	osrc.sin6_family = odst.sin6_family = isrc.sin6_family =
+	    idst.sin6_family = AF_INET6;
+	osrc.sin6_len = odst.sin6_len = isrc.sin6_len = idst.sin6_len = 
+	    sizeof(struct sockaddr_in6);
+	osrc.sin6_addr = oip6->ip6_src;
+	odst.sin6_addr = oip6->ip6_dst;
+	m_copydata(m, off + offsetof(struct ip6_hdr, ip6_src),
+	    sizeof(isrc.sin6_addr), (caddr_t)&isrc.sin6_addr);
+	m_copydata(m, off + offsetof(struct ip6_hdr, ip6_dst),
+	    sizeof(idst.sin6_addr), (caddr_t)&idst.sin6_addr);
+
+	/*
+	 * regarding to inner source address validation, see a long comment
+	 * in ipsec4_tunnel_validate.
+	 */
+
+	sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
+	    (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
+	if (!sp)
+		return 0;
+	key_freesp(sp);
 
 	return 1;
 }
@@ -3235,7 +3356,7 @@ ipsec_copypkt(m)
 				 */
 				remain = n->m_len;
 				copied = 0;
-				while(1) {
+				while (1) {
 					int len;
 					struct mbuf *mn;
 
@@ -3265,6 +3386,7 @@ ipsec_copypkt(m)
 					MGETHDR(mn, M_DONTWAIT, MT_HEADER);
 					if (mn == NULL)
 						goto fail;
+					mn->m_pkthdr.rcvif = NULL;
 					mm->m_next = mn;
 					mm = mn;
 				}
@@ -3288,27 +3410,78 @@ ipsec_copypkt(m)
 	return(NULL);
 }
 
+static struct mbuf *
+ipsec_addaux(m)
+	struct mbuf *m;
+{
+	struct mbuf *n;
+
+	n = m_aux_find(m, AF_INET, IPPROTO_ESP);
+	if (!n)
+		n = m_aux_add(m, AF_INET, IPPROTO_ESP);
+	if (!n)
+		return n;	/* ENOBUFS */
+	n->m_len = sizeof(struct socket *);
+	bzero(mtod(n, void *), n->m_len);
+	return n;
+}
+
+static struct mbuf *
+ipsec_findaux(m)
+	struct mbuf *m;
+{
+	struct mbuf *n;
+
+	n = m_aux_find(m, AF_INET, IPPROTO_ESP);
+#ifdef DIAGNOSTIC
+	if (n && n->m_len < sizeof(struct socket *))
+		panic("invalid ipsec m_aux");
+#endif
+	return n;
+}
+
 void
+ipsec_delaux(m)
+	struct mbuf *m;
+{
+	struct mbuf *n;
+
+	n = m_aux_find(m, AF_INET, IPPROTO_ESP);
+	if (n)
+		m_aux_delete(m, n);
+}
+
+/* if the aux buffer is unnecessary, nuke it. */
+static void
+ipsec_optaux(m, n)
+	struct mbuf *m;
+	struct mbuf *n;
+{
+
+	if (!n)
+		return;
+	if (n->m_len == sizeof(struct socket *) && !*mtod(n, struct socket **))
+		ipsec_delaux(m);
+}
+
+int
 ipsec_setsocket(m, so)
 	struct mbuf *m;
 	struct socket *so;
 {
 	struct mbuf *n;
 
-	n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-	if (so && !n)
-		n = m_aux_add(m, AF_INET, IPPROTO_ESP);
-	if (n) {
-		if (so) {
-			*mtod(n, struct socket **) = so;
-			/*
-			 * XXX think again about it when we put decryption
-			 * histrory into aux mbuf
-			 */
-			n->m_len = sizeof(struct socket *);
-		} else
-			m_aux_delete(m, n);
-	}
+	/* if so == NULL, don't insist on getting the aux mbuf */
+	if (so) {
+		n = ipsec_addaux(m);
+		if (!n)
+			return ENOBUFS;
+	} else
+		n = ipsec_findaux(m);
+	if (n && n->m_len >= sizeof(struct socket *))
+		*mtod(n, struct socket **) = so;
+	ipsec_optaux(m, n);
+	return 0;
 }
 
 struct socket *
@@ -3317,9 +3490,66 @@ ipsec_getsocket(m)
 {
 	struct mbuf *n;
 
-	n = m_aux_find(m, AF_INET, IPPROTO_ESP);
+	n = ipsec_findaux(m);
 	if (n && n->m_len >= sizeof(struct socket *))
 		return *mtod(n, struct socket **);
 	else
 		return NULL;
 }
+
+int
+ipsec_addhist(m, proto, spi)
+	struct mbuf *m;
+	int proto;
+	u_int32_t spi;
+{
+	struct mbuf *n;
+	struct ipsec_history *p;
+
+	n = ipsec_addaux(m);
+	if (!n)
+		return ENOBUFS;
+	if (M_TRAILINGSPACE(n) < sizeof(*p))
+		return ENOSPC;	/*XXX*/
+	p = (struct ipsec_history *)(mtod(n, caddr_t) + n->m_len);
+	n->m_len += sizeof(*p);
+	bzero(p, sizeof(*p));
+	p->ih_proto = proto;
+	p->ih_spi = spi;
+	return 0;
+}
+
+struct ipsec_history *
+ipsec_gethist(m, lenp)
+	struct mbuf *m;
+	int *lenp;
+{
+	struct mbuf *n;
+	int l;
+
+	n = ipsec_findaux(m);
+	if (!n)
+		return NULL;
+	l = n->m_len;
+	if (sizeof(struct socket *) > l)
+		return NULL;
+	if ((l - sizeof(struct socket *)) % sizeof(struct ipsec_history))
+		return NULL;
+	/* XXX does it make more sense to divide by sizeof(ipsec_history)? */
+	if (lenp)
+		*lenp = l - sizeof(struct socket *);
+	return (struct ipsec_history *)
+	    (mtod(n, caddr_t) + sizeof(struct socket *));
+}
+
+void
+ipsec_clearhist(m)
+	struct mbuf *m;
+{
+	struct mbuf *n;
+
+	n = ipsec_findaux(m);
+	if ((n) && n->m_len > sizeof(struct socket *))
+		n->m_len = sizeof(struct socket *);
+	ipsec_optaux(m, n);
+}