Update to FreeBSD head 2017-04-04

Git mirror commit 642b174daddbd0efd9bb5f242c43f4ab4db6869f.

30 files changed, 8877 insertions, 8150 deletions

diff --git a/freebsd/sys/netipsec/ah_var.h b/freebsd/sys/netipsec/ah_var.h
index 812fe2dc..9b992c07 100644
--- a/freebsd/sys/netipsec/ah_var.h
+++ b/freebsd/sys/netipsec/ah_var.h
@@ -48,37 +48,39 @@
 #define	AH_ALG_MAX	16
 
 struct ahstat {
-	u_int32_t	ahs_hdrops;	/* Packet shorter than header shows */
-	u_int32_t	ahs_nopf;	/* Protocol family not supported */
-	u_int32_t	ahs_notdb;
-	u_int32_t	ahs_badkcr;
-	u_int32_t	ahs_badauth;
-	u_int32_t	ahs_noxform;
-	u_int32_t	ahs_qfull;
-	u_int32_t	ahs_wrap;
-	u_int32_t	ahs_replay;
-	u_int32_t	ahs_badauthl;	/* Bad authenticator length */
-	u_int32_t	ahs_input;	/* Input AH packets */
-	u_int32_t	ahs_output;	/* Output AH packets */
-	u_int32_t	ahs_invalid;	/* Trying to use an invalid TDB */
-	u_int64_t	ahs_ibytes;	/* Input bytes */
-	u_int64_t	ahs_obytes;	/* Output bytes */
-	u_int32_t	ahs_toobig;	/* Packet got larger than IP_MAXPACKET */
-	u_int32_t	ahs_pdrops;	/* Packet blocked due to policy */
-	u_int32_t	ahs_crypto;	/* Crypto processing failure */
-	u_int32_t	ahs_tunnel;	/* Tunnel sanity check failure */
-	u_int32_t	ahs_hist[AH_ALG_MAX];	/* Per-algorithm op count */
+	uint64_t	ahs_hdrops;	/* Packet shorter than header shows */
+	uint64_t	ahs_nopf;	/* Protocol family not supported */
+	uint64_t	ahs_notdb;
+	uint64_t	ahs_badkcr;
+	uint64_t	ahs_badauth;
+	uint64_t	ahs_noxform;
+	uint64_t	ahs_qfull;
+	uint64_t	ahs_wrap;
+	uint64_t	ahs_replay;
+	uint64_t	ahs_badauthl;	/* Bad authenticator length */
+	uint64_t	ahs_input;	/* Input AH packets */
+	uint64_t	ahs_output;	/* Output AH packets */
+	uint64_t	ahs_invalid;	/* Trying to use an invalid TDB */
+	uint64_t	ahs_ibytes;	/* Input bytes */
+	uint64_t	ahs_obytes;	/* Output bytes */
+	uint64_t	ahs_toobig;	/* Packet got larger than IP_MAXPACKET */
+	uint64_t	ahs_pdrops;	/* Packet blocked due to policy */
+	uint64_t	ahs_crypto;	/* Crypto processing failure */
+	uint64_t	ahs_tunnel;	/* Tunnel sanity check failure */
+	uint64_t	ahs_hist[AH_ALG_MAX];	/* Per-algorithm op count */
 };
 
 #ifdef _KERNEL
+#include <sys/counter.h>
+
 VNET_DECLARE(int, ah_enable);
 VNET_DECLARE(int, ah_cleartos);
-VNET_DECLARE(struct ahstat, ahstat);
+VNET_PCPUSTAT_DECLARE(struct ahstat, ahstat);
 
-#define	AHSTAT_ADD(name, val)	V_ahstat.name += (val)
+#define	AHSTAT_ADD(name, val)	\
+    VNET_PCPUSTAT_ADD(struct ahstat, ahstat, name , (val))
 #define	AHSTAT_INC(name)	AHSTAT_ADD(name, 1)
 #define	V_ah_enable		VNET(ah_enable)
 #define	V_ah_cleartos		VNET(ah_cleartos)
-#define	V_ahstat		VNET(ahstat)
 #endif /* _KERNEL */
 #endif /*_NETIPSEC_AH_VAR_H_*/
diff --git a/freebsd/sys/netipsec/esp.h b/freebsd/sys/netipsec/esp.h
index eb373970..8eb09630 100644
--- a/freebsd/sys/netipsec/esp.h
+++ b/freebsd/sys/netipsec/esp.h
@@ -42,8 +42,7 @@ struct esp {
 	/*variable size, 32bit bound*/	/* Initialization Vector */
 	/*variable size*/		/* Payload data */
 	/*variable size*/		/* padding */
-	/*8bit*/			/* pad size */
-	/*8bit*/			/* next header */
+	/*8bit*/			/* pad length */
 	/*8bit*/			/* next header */
 	/*variable size, 32bit bound*/	/* Authentication data (new IPsec) */
 };
@@ -53,8 +52,7 @@ struct newesp {
 	u_int32_t	esp_seq;	/* Sequence number */
 	/*variable size*/		/* (IV and) Payload data */
 	/*variable size*/		/* padding */
-	/*8bit*/			/* pad size */
-	/*8bit*/			/* next header */
+	/*8bit*/			/* pad length */
 	/*8bit*/			/* next header */
 	/*variable size, 32bit bound*/	/* Authentication data */
 };
diff --git a/freebsd/sys/netipsec/esp_var.h b/freebsd/sys/netipsec/esp_var.h
index c6133614..48240418 100644
--- a/freebsd/sys/netipsec/esp_var.h
+++ b/freebsd/sys/netipsec/esp_var.h
@@ -48,36 +48,38 @@
 #define	ESP_ALG_MAX	256		/* NB: could be < but skipjack is 249 */
 
 struct espstat {
-	u_int32_t	esps_hdrops;	/* Packet shorter than header shows */
-	u_int32_t	esps_nopf;	/* Protocol family not supported */
-	u_int32_t	esps_notdb;
-	u_int32_t	esps_badkcr;
-	u_int32_t	esps_qfull;
-	u_int32_t	esps_noxform;
-	u_int32_t	esps_badilen;
-	u_int32_t	esps_wrap;	/* Replay counter wrapped around */
-	u_int32_t	esps_badenc;	/* Bad encryption detected */
-	u_int32_t	esps_badauth;	/* Only valid for transforms with auth */
-	u_int32_t	esps_replay;	/* Possible packet replay detected */
-	u_int32_t	esps_input;	/* Input ESP packets */
-	u_int32_t	esps_output;	/* Output ESP packets */
-	u_int32_t	esps_invalid;	/* Trying to use an invalid TDB */
-	u_int64_t	esps_ibytes;	/* Input bytes */
-	u_int64_t	esps_obytes;	/* Output bytes */
-	u_int32_t	esps_toobig;	/* Packet got larger than IP_MAXPACKET */
-	u_int32_t	esps_pdrops;	/* Packet blocked due to policy */
-	u_int32_t	esps_crypto;	/* Crypto processing failure */
-	u_int32_t	esps_tunnel;	/* Tunnel sanity check failure */
-	u_int32_t	esps_hist[ESP_ALG_MAX];	/* Per-algorithm op count */
+	uint64_t	esps_hdrops;	/* Packet shorter than header shows */
+	uint64_t	esps_nopf;	/* Protocol family not supported */
+	uint64_t	esps_notdb;
+	uint64_t	esps_badkcr;
+	uint64_t	esps_qfull;
+	uint64_t	esps_noxform;
+	uint64_t	esps_badilen;
+	uint64_t	esps_wrap;	/* Replay counter wrapped around */
+	uint64_t	esps_badenc;	/* Bad encryption detected */
+	uint64_t	esps_badauth;	/* Only valid for transforms with auth */
+	uint64_t	esps_replay;	/* Possible packet replay detected */
+	uint64_t	esps_input;	/* Input ESP packets */
+	uint64_t	esps_output;	/* Output ESP packets */
+	uint64_t	esps_invalid;	/* Trying to use an invalid TDB */
+	uint64_t	esps_ibytes;	/* Input bytes */
+	uint64_t	esps_obytes;	/* Output bytes */
+	uint64_t	esps_toobig;	/* Packet got larger than IP_MAXPACKET */
+	uint64_t	esps_pdrops;	/* Packet blocked due to policy */
+	uint64_t	esps_crypto;	/* Crypto processing failure */
+	uint64_t	esps_tunnel;	/* Tunnel sanity check failure */
+	uint64_t	esps_hist[ESP_ALG_MAX];	/* Per-algorithm op count */
 };
 
 #ifdef _KERNEL
+#include <sys/counter.h>
+
 VNET_DECLARE(int, esp_enable);
-VNET_DECLARE(struct espstat, espstat);
+VNET_PCPUSTAT_DECLARE(struct espstat, espstat);
 
-#define	ESPSTAT_ADD(name, val)	V_espstat.name += (val)
+#define	ESPSTAT_ADD(name, val)	\
+    VNET_PCPUSTAT_ADD(struct espstat, espstat, name, (val))
 #define	ESPSTAT_INC(name)	ESPSTAT_ADD(name, 1)
 #define	V_esp_enable	VNET(esp_enable)
-#define	V_espstat	VNET(espstat)
 #endif /* _KERNEL */
 #endif /*_NETIPSEC_ESP_VAR_H_*/
diff --git a/freebsd/sys/netipsec/ipcomp_var.h b/freebsd/sys/netipsec/ipcomp_var.h
index ee15598f..5062c9dd 100644
--- a/freebsd/sys/netipsec/ipcomp_var.h
+++ b/freebsd/sys/netipsec/ipcomp_var.h
@@ -41,36 +41,37 @@
  */
 #define	IPCOMP_ALG_MAX	8
 
-#define	IPCOMPSTAT_VERSION	1
+#define	IPCOMPSTAT_VERSION	2
 struct ipcompstat {
-	u_int32_t	ipcomps_hdrops;	/* Packet shorter than header shows */
-	u_int32_t	ipcomps_nopf;	/* Protocol family not supported */
-	u_int32_t	ipcomps_notdb;
-	u_int32_t	ipcomps_badkcr;
-	u_int32_t	ipcomps_qfull;
-	u_int32_t	ipcomps_noxform;
-	u_int32_t	ipcomps_wrap;
-	u_int32_t	ipcomps_input;	/* Input IPcomp packets */
-	u_int32_t	ipcomps_output;	/* Output IPcomp packets */
-	u_int32_t	ipcomps_invalid;/* Trying to use an invalid TDB */
-	u_int64_t	ipcomps_ibytes;	/* Input bytes */
-	u_int64_t	ipcomps_obytes;	/* Output bytes */
-	u_int32_t	ipcomps_toobig;	/* Packet got > IP_MAXPACKET */
-	u_int32_t	ipcomps_pdrops;	/* Packet blocked due to policy */
-	u_int32_t	ipcomps_crypto;	/* "Crypto" processing failure */
-	u_int32_t	ipcomps_hist[IPCOMP_ALG_MAX];/* Per-algorithm op count */
-	u_int32_t	version;	/* Version of this structure. */
-	u_int32_t	ipcomps_threshold; /* Packet < comp. algo. threshold. */
-	u_int32_t	ipcomps_uncompr; /* Compression was useles. */
+	uint64_t	ipcomps_hdrops;	/* Packet shorter than header shows */
+	uint64_t	ipcomps_nopf;	/* Protocol family not supported */
+	uint64_t	ipcomps_notdb;
+	uint64_t	ipcomps_badkcr;
+	uint64_t	ipcomps_qfull;
+	uint64_t	ipcomps_noxform;
+	uint64_t	ipcomps_wrap;
+	uint64_t	ipcomps_input;	/* Input IPcomp packets */
+	uint64_t	ipcomps_output;	/* Output IPcomp packets */
+	uint64_t	ipcomps_invalid;/* Trying to use an invalid TDB */
+	uint64_t	ipcomps_ibytes;	/* Input bytes */
+	uint64_t	ipcomps_obytes;	/* Output bytes */
+	uint64_t	ipcomps_toobig;	/* Packet got > IP_MAXPACKET */
+	uint64_t	ipcomps_pdrops;	/* Packet blocked due to policy */
+	uint64_t	ipcomps_crypto;	/* "Crypto" processing failure */
+	uint64_t	ipcomps_hist[IPCOMP_ALG_MAX];/* Per-algorithm op count */
+	uint64_t	ipcomps_threshold; /* Packet < comp. algo. threshold. */
+	uint64_t	ipcomps_uncompr; /* Compression was useles. */
 };
 
 #ifdef _KERNEL
+#include <sys/counter.h>
+
 VNET_DECLARE(int, ipcomp_enable);
-VNET_DECLARE(struct ipcompstat, ipcompstat);
+VNET_PCPUSTAT_DECLARE(struct ipcompstat, ipcompstat);
 
-#define	IPCOMPSTAT_ADD(name, val)	V_ipcompstat.name += (val)
+#define	IPCOMPSTAT_ADD(name, val)	\
+    VNET_PCPUSTAT_ADD(struct ipcompstat, ipcompstat, name, (val))
 #define	IPCOMPSTAT_INC(name)		IPCOMPSTAT_ADD(name, 1)
 #define	V_ipcomp_enable		VNET(ipcomp_enable)
-#define	V_ipcompstat		VNET(ipcompstat)
 #endif /* _KERNEL */
 #endif /*_NETIPSEC_IPCOMP_VAR_H_*/
diff --git a/freebsd/sys/netipsec/ipip_var.h b/freebsd/sys/netipsec/ipip_var.h
deleted file mode 100644
index 415d5c10..00000000
--- a/freebsd/sys/netipsec/ipip_var.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*	$FreeBSD$	*/
-/*	$OpenBSD: ip_ipip.h,v 1.5 2002/06/09 16:26:10 itojun Exp $ */
-/*-
- * The authors of this code are John Ioannidis (ji@tla.org),
- * Angelos D. Keromytis (kermit@csd.uch.gr) and
- * Niels Provos (provos@physnet.uni-hamburg.de).
- *
- * The original version of this code was written by John Ioannidis
- * for BSD/OS in Athens, Greece, in November 1995.
- *
- * Ported to OpenBSD and NetBSD, with additional transforms, in December 1996,
- * by Angelos D. Keromytis.
- *
- * Additional transforms and features in 1997 and 1998 by Angelos D. Keromytis
- * and Niels Provos.
- *
- * Additional features in 1999 by Angelos D. Keromytis.
- *
- * Copyright (C) 1995, 1996, 1997, 1998, 1999 by John Ioannidis,
- * Angelos D. Keromytis and Niels Provos.
- * Copyright (c) 2001, Angelos D. Keromytis.
- *
- * Permission to use, copy, and modify this software with or without fee
- * is hereby granted, provided that this entire notice is included in
- * all copies of any software which is or includes a copy or
- * modification of this software.
- * You may use this code under the GNU public license if you so wish. Please
- * contribute changes back to the authors under this freer than GPL license
- * so that we may further the use of strong encryption without limitations to
- * all.
- *
- * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR
- * IMPLIED WARRANTY. IN PARTICULAR, NONE OF THE AUTHORS MAKES ANY
- * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE
- * MERCHANTABILITY OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR
- * PURPOSE.
- */
-
-#ifndef _NETINET_IPIP_H_
-#define _NETINET_IPIP_H_
-
-/*
- * IP-inside-IP processing.
- * Not quite all the functionality of RFC-1853, but the main idea is there.
- */
-
-struct ipipstat
-{
-    u_int32_t	ipips_ipackets;		/* total input packets */
-    u_int32_t	ipips_opackets;		/* total output packets */
-    u_int32_t	ipips_hdrops;		/* packet shorter than header shows */
-    u_int32_t	ipips_qfull;
-    u_int64_t   ipips_ibytes;
-    u_int64_t   ipips_obytes;
-    u_int32_t	ipips_pdrops;		/* packet dropped due to policy */
-    u_int32_t	ipips_spoof;		/* IP spoofing attempts */
-    u_int32_t   ipips_family;		/* Protocol family mismatch */
-    u_int32_t   ipips_unspec;            /* Missing tunnel endpoint address */
-};
-
-#ifdef _KERNEL
-VNET_DECLARE(int, ipip_allow);
-VNET_DECLARE(struct ipipstat, ipipstat);
-
-#define	IPIPSTAT_ADD(name, val)	V_ipipstat.name += (val)
-#define	IPIPSTAT_INC(name)	IPIPSTAT_ADD(name, 1)
-#define	V_ipip_allow		VNET(ipip_allow)
-#define	V_ipipstat		VNET(ipipstat)
-#endif /* _KERNEL */
-#endif /* _NETINET_IPIP_H_ */
diff --git a/freebsd/sys/netipsec/ipsec.c b/freebsd/sys/netipsec/ipsec.c
index c1eed678..20aad0b6 100644
--- a/freebsd/sys/netipsec/ipsec.c
+++ b/freebsd/sys/netipsec/ipsec.c
@@ -50,6 +50,7 @@
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <rtems/bsd/sys/errno.h>
+#include <sys/hhook.h>
 #include <sys/time.h>
 #include <sys/kernel.h>
 #include <sys/syslog.h>
@@ -57,7 +58,8 @@
 #include <sys/proc.h>
 
 #include <net/if.h>
-#include <net/route.h>
+#include <net/if_enc.h>
+#include <net/if_var.h>
 #include <net/vnet.h>
 
 #include <netinet/in.h>
@@ -88,6 +90,7 @@
 #include <netipsec/esp_var.h>
 #include <netipsec/ipcomp.h>		/*XXX*/
 #include <netipsec/ipcomp_var.h>
+#include <netipsec/ipsec_support.h>
 
 #include <netipsec/key.h>
 #include <netipsec/keydb.h>
@@ -99,14 +102,14 @@
 
 #include <opencrypto/cryptodev.h>
 
-#ifdef IPSEC_DEBUG
-VNET_DEFINE(int, ipsec_debug) = 1;
-#else
-VNET_DEFINE(int, ipsec_debug) = 0;
-#endif
-
 /* NB: name changed so netstat doesn't use it. */
-VNET_DEFINE(struct ipsecstat, ipsec4stat);
+VNET_PCPUSTAT_DEFINE(struct ipsecstat, ipsec4stat);
+VNET_PCPUSTAT_SYSINIT(ipsec4stat);
+
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(ipsec4stat);
+#endif /* VIMAGE */
+
 VNET_DEFINE(int, ip4_ah_offsetmask) = 0;	/* maybe IP_DF? */
 /* DF bit on encap. 0: clear 1: set 2: copy */
 VNET_DEFINE(int, ip4_ipsec_dfbit) = 0;
@@ -114,11 +117,32 @@ VNET_DEFINE(int, ip4_esp_trans_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip4_esp_net_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip4_ah_trans_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip4_ah_net_deflev) = IPSEC_LEVEL_USE;
-VNET_DEFINE(struct secpolicy, ip4_def_policy);
 /* ECN ignore(-1)/forbidden(0)/allowed(1) */
 VNET_DEFINE(int, ip4_ipsec_ecn) = 0;
 VNET_DEFINE(int, ip4_esp_randpad) = -1;
 
+static VNET_DEFINE(int, ip4_filtertunnel) = 0;
+#define	V_ip4_filtertunnel VNET(ip4_filtertunnel)
+static VNET_DEFINE(int, check_policy_history) = 0;
+#define	V_check_policy_history	VNET(check_policy_history)
+static VNET_DEFINE(struct secpolicy *, def_policy) = NULL;
+#define	V_def_policy	VNET(def_policy)
+static int
+sysctl_def_policy(SYSCTL_HANDLER_ARGS)
+{
+	int error, value;
+
+	value = V_def_policy->policy;
+	error = sysctl_handle_int(oidp, &value, 0, req);
+	if (error == 0) {
+		if (value != IPSEC_POLICY_DISCARD &&
+		    value != IPSEC_POLICY_NONE)
+			return (EINVAL);
+		V_def_policy->policy = value;
+	}
+	return (error);
+}
+
 /*
  * Crypto support requirements:
  *
@@ -127,51 +151,63 @@ VNET_DEFINE(int, ip4_esp_randpad) = -1;
  *  0	take anything
  */
 VNET_DEFINE(int, crypto_support) = CRYPTOCAP_F_HARDWARE | CRYPTOCAP_F_SOFTWARE;
+/*
+ * TCP/UDP checksum handling policy for transport mode NAT-T (RFC3948)
+ *
+ * 0 - auto: incrementally recompute, when checksum delta is known;
+ *     if checksum delta isn't known, reset checksum to zero for UDP,
+ *     and mark csum_flags as valid for TCP.
+ * 1 - fully recompute TCP/UDP checksum.
+ */
+VNET_DEFINE(int, natt_cksum_policy) = 0;
 
 FEATURE(ipsec, "Internet Protocol Security (IPsec)");
-#ifdef IPSEC_NAT_T
 FEATURE(ipsec_natt, "UDP Encapsulation of IPsec ESP Packets ('NAT-T')");
-#endif
 
 SYSCTL_DECL(_net_inet_ipsec);
 
 /* net.inet.ipsec */
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEF_POLICY, def_policy,
-	CTLFLAG_RW, &VNET_NAME(ip4_def_policy).policy, 0,
+SYSCTL_PROC(_net_inet_ipsec, IPSECCTL_DEF_POLICY, def_policy,
+	CTLTYPE_INT | CTLFLAG_VNET | CTLFLAG_RW, 0, 0, sysctl_def_policy, "I",
 	"IPsec default policy.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
-	CTLFLAG_RW, &VNET_NAME(ip4_esp_trans_deflev), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_esp_trans_deflev), 0,
 	"Default ESP transport mode level");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
-	CTLFLAG_RW, &VNET_NAME(ip4_esp_net_deflev), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_esp_net_deflev), 0,
 	"Default ESP tunnel mode level.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
-	CTLFLAG_RW, &VNET_NAME(ip4_ah_trans_deflev), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_ah_trans_deflev), 0,
 	"AH transfer mode default level.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
-	CTLFLAG_RW, &VNET_NAME(ip4_ah_net_deflev), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_ah_net_deflev), 0,
 	"AH tunnel mode default level.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_AH_CLEARTOS, ah_cleartos,
-	CTLFLAG_RW, &VNET_NAME(ah_cleartos), 0,
-	"If set clear type-of-service field when doing AH computation.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_AH_OFFSETMASK, ah_offsetmask,
-	CTLFLAG_RW, &VNET_NAME(ip4_ah_offsetmask), 0,
-	"If not set clear offset field mask when doing AH computation.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DFBIT, dfbit,
-	CTLFLAG_RW, &VNET_NAME(ip4_ipsec_dfbit), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_CLEARTOS, ah_cleartos,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ah_cleartos), 0,
+	"If set, clear type-of-service field when doing AH computation.");
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_OFFSETMASK, ah_offsetmask,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_ah_offsetmask), 0,
+	"If not set, clear offset field mask when doing AH computation.");
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DFBIT, dfbit,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_ipsec_dfbit), 0,
 	"Do not fragment bit on encap.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_ECN, ecn,
-	CTLFLAG_RW, &VNET_NAME(ip4_ipsec_ecn), 0,
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ECN, ecn,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_ipsec_ecn), 0,
 	"Explicit Congestion Notification handling.");
-SYSCTL_VNET_INT(_net_inet_ipsec, IPSECCTL_DEBUG, debug,
-	CTLFLAG_RW, &VNET_NAME(ipsec_debug), 0,
-	"Enable IPsec debugging output when set.");
-SYSCTL_VNET_INT(_net_inet_ipsec, OID_AUTO, crypto_support,
-	CTLFLAG_RW, &VNET_NAME(crypto_support), 0,
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, crypto_support,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(crypto_support), 0,
 	"Crypto driver selection.");
-SYSCTL_VNET_STRUCT(_net_inet_ipsec, OID_AUTO, ipsecstats,
-	CTLFLAG_RD, &VNET_NAME(ipsec4stat), ipsecstat,	
-	"IPsec IPv4 statistics.");
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, check_policy_history,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(check_policy_history), 0,
+	"Use strict check of inbound packets to security policy compliance.");
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, natt_cksum_policy,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(natt_cksum_policy), 0,
+	"Method to fix TCP/UDP checksum for transport mode IPsec after NAT.");
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, filtertunnel,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip4_filtertunnel), 0,
+	"If set, filter packets from an IPsec tunnel.");
+SYSCTL_VNET_PCPUSTAT(_net_inet_ipsec, OID_AUTO, ipsecstats, struct ipsecstat,
+    ipsec4stat, "IPsec IPv4 statistics.");
 
 #ifdef REGRESSION
 /*
@@ -179,448 +215,293 @@ SYSCTL_VNET_STRUCT(_net_inet_ipsec, OID_AUTO, ipsecstats,
  * This allows to verify if the other side has proper replay attacks detection.
  */
 VNET_DEFINE(int, ipsec_replay) = 0;
-SYSCTL_VNET_INT(_net_inet_ipsec, OID_AUTO, test_replay,
-	CTLFLAG_RW, &VNET_NAME(ipsec_replay), 0,
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, test_replay,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_replay), 0,
 	"Emulate replay attack");
 /*
  * When set 1, IPsec will send packets with corrupted HMAC.
  * This allows to verify if the other side properly detects modified packets.
  */
 VNET_DEFINE(int, ipsec_integrity) = 0;
-SYSCTL_VNET_INT(_net_inet_ipsec, OID_AUTO, test_integrity,
-	CTLFLAG_RW, &VNET_NAME(ipsec_integrity), 0,
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, test_integrity,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_integrity), 0,
 	"Emulate man-in-the-middle attack");
 #endif
 
 #ifdef INET6 
-VNET_DEFINE(struct ipsecstat, ipsec6stat);
+VNET_PCPUSTAT_DEFINE(struct ipsecstat, ipsec6stat);
+VNET_PCPUSTAT_SYSINIT(ipsec6stat);
+
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(ipsec6stat);
+#endif /* VIMAGE */
+
 VNET_DEFINE(int, ip6_esp_trans_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip6_esp_net_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip6_ah_trans_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip6_ah_net_deflev) = IPSEC_LEVEL_USE;
 VNET_DEFINE(int, ip6_ipsec_ecn) = 0;	/* ECN ignore(-1)/forbidden(0)/allowed(1) */
 
+static VNET_DEFINE(int, ip6_filtertunnel) = 0;
+#define	V_ip6_filtertunnel	VNET(ip6_filtertunnel)
+
 SYSCTL_DECL(_net_inet6_ipsec6);
 
 /* net.inet6.ipsec6 */
-#ifdef COMPAT_KAME
-SYSCTL_OID(_net_inet6_ipsec6, IPSECCTL_STATS, stats, CTLFLAG_RD,
-    0, 0, compat_ipsecstats_sysctl, "S", "IPsec IPv6 statistics.");
-#endif /* COMPAT_KAME */
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEF_POLICY, def_policy, CTLFLAG_RW,
-	&VNET_NAME(ip4_def_policy).policy, 0,
+SYSCTL_PROC(_net_inet6_ipsec6, IPSECCTL_DEF_POLICY, def_policy,
+	CTLTYPE_INT | CTLFLAG_VNET | CTLFLAG_RW, 0, 0, sysctl_def_policy, "I",
 	"IPsec default policy.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_TRANSLEV, 
-	esp_trans_deflev, CTLFLAG_RW, &VNET_NAME(ip6_esp_trans_deflev),	0,
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_esp_trans_deflev), 0,
 	"Default ESP transport mode level.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_NETLEV, 
-	esp_net_deflev, CTLFLAG_RW, &VNET_NAME(ip6_esp_net_deflev),	0,
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_esp_net_deflev), 0,
 	"Default ESP tunnel mode level.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_TRANSLEV, 
-	ah_trans_deflev, CTLFLAG_RW, &VNET_NAME(ip6_ah_trans_deflev),	0,
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_ah_trans_deflev), 0,
 	"AH transfer mode default level.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_NETLEV, 
-	ah_net_deflev, CTLFLAG_RW, &VNET_NAME(ip6_ah_net_deflev),	0,
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_ah_net_deflev), 0,
 	"AH tunnel mode default level.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_ECN,
-	ecn, CTLFLAG_RW, &VNET_NAME(ip6_ipsec_ecn),	0,
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ECN, ecn,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_ipsec_ecn), 0,
 	"Explicit Congestion Notification handling.");
-SYSCTL_VNET_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG, debug, CTLFLAG_RW,
-	&VNET_NAME(ipsec_debug), 0,
-	"Enable IPsec debugging output when set.");
-SYSCTL_VNET_STRUCT(_net_inet6_ipsec6, IPSECCTL_STATS,
-	ipsecstats, CTLFLAG_RD, &VNET_NAME(ipsec6stat), ipsecstat,
-	"IPsec IPv6 statistics.");
+SYSCTL_INT(_net_inet6_ipsec6, OID_AUTO, filtertunnel,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ip6_filtertunnel),  0,
+	"If set, filter packets from an IPsec tunnel.");
+SYSCTL_VNET_PCPUSTAT(_net_inet6_ipsec6, IPSECCTL_STATS, ipsecstats,
+    struct ipsecstat, ipsec6stat, "IPsec IPv6 statistics.");
 #endif /* INET6 */
 
-static int ipsec_setspidx_inpcb __P((struct mbuf *, struct inpcb *));
-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 *));
+static int ipsec_in_reject(struct secpolicy *, struct inpcb *,
+    const struct mbuf *);
+
+#ifdef INET
+static void ipsec4_get_ulp(const struct mbuf *, struct secpolicyindex *, int);
+static void ipsec4_setspidx_ipaddr(const struct mbuf *,
+    struct secpolicyindex *);
+#endif
 #ifdef INET6
-static void ipsec6_get_ulp __P((struct mbuf *m, struct secpolicyindex *, int));
-static int ipsec6_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+static void ipsec6_get_ulp(const struct mbuf *m, struct secpolicyindex *, int);
+static void ipsec6_setspidx_ipaddr(const struct mbuf *,
+    struct secpolicyindex *);
 #endif
-static void ipsec_delpcbpolicy __P((struct inpcbpolicy *));
-static struct secpolicy *ipsec_deepcopy_policy __P((struct secpolicy *src));
-static void vshiftl __P((unsigned char *, int, int));
-
-MALLOC_DEFINE(M_IPSEC_INPCB, "inpcbpolicy", "inpcb-resident ipsec policy");
 
 /*
  * Return a held reference to the default SP.
  */
 static struct secpolicy *
-key_allocsp_default(const char* where, int tag)
+key_allocsp_default(void)
 {
-	struct secpolicy *sp;
-
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP key_allocsp_default from %s:%u\n", where, tag));
-
-	sp = &V_ip4_def_policy;
-	if (sp->policy != IPSEC_POLICY_DISCARD &&
-	    sp->policy != IPSEC_POLICY_NONE) {
-		ipseclog((LOG_INFO, "fixed system default policy: %d->%d\n",
-		    sp->policy, IPSEC_POLICY_NONE));
-		sp->policy = IPSEC_POLICY_NONE;
-	}
-	key_addref(sp);
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP key_allocsp_default returns SP:%p (%u)\n",
-			sp, sp->refcnt));
-	return (sp);
+	key_addref(V_def_policy);
+	return (V_def_policy);
 }
-#define	KEY_ALLOCSP_DEFAULT() \
-	key_allocsp_default(__FILE__, __LINE__)
 
-/*
- * For OUTBOUND packet having a socket. Searching SPD for packet,
- * and return a pointer to SP.
- * OUT:	NULL:	no apropreate SP found, the following value is set to error.
- *		0	: bypass
- *		EACCES	: discard packet.
- *		ENOENT	: ipsec_acquire() in progress, maybe.
- *		others	: error occured.
- *	others:	a pointer to SP
- *
- * NOTE: IPv6 mapped adddress concern is implemented here.
- */
-struct secpolicy *
-ipsec_getpolicy(struct tdb_ident *tdbi, u_int dir)
+static void
+ipsec_invalidate_cache(struct inpcb *inp, u_int dir)
 {
 	struct secpolicy *sp;
 
-	IPSEC_ASSERT(tdbi != NULL, ("null tdbi"));
-	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
-		("invalid direction %u", dir));
-
-	sp = KEY_ALLOCSP2(tdbi->spi, &tdbi->dst, tdbi->proto, dir);
-	if (sp == NULL)			/*XXX????*/
-		sp = KEY_ALLOCSP_DEFAULT();
-	IPSEC_ASSERT(sp != NULL, ("null SP"));
-	return (sp);
+	INP_WLOCK_ASSERT(inp);
+	if (dir == IPSEC_DIR_OUTBOUND) {
+		if (inp->inp_sp->flags & INP_INBOUND_POLICY)
+			return;
+		sp = inp->inp_sp->sp_in;
+		inp->inp_sp->sp_in = NULL;
+	} else {
+		if (inp->inp_sp->flags & INP_OUTBOUND_POLICY)
+			return;
+		sp = inp->inp_sp->sp_out;
+		inp->inp_sp->sp_out = NULL;
+	}
+	if (sp != NULL)
+		key_freesp(&sp); /* release extra reference */
 }
 
-/*
- * For OUTBOUND packet having a socket. Searching SPD for packet,
- * and return a pointer to SP.
- * OUT:	NULL:	no apropreate SP found, the following value is set to error.
- *		0	: bypass
- *		EACCES	: discard packet.
- *		ENOENT	: ipsec_acquire() in progress, maybe.
- *		others	: error occured.
- *	others:	a pointer to SP
- *
- * NOTE: IPv6 mapped adddress concern is implemented here.
- */
-static struct secpolicy *
-ipsec_getpolicybysock(struct mbuf *m, u_int dir, struct inpcb *inp, int *error)
+static void
+ipsec_cachepolicy(struct inpcb *inp, struct secpolicy *sp, u_int dir)
 {
-	struct inpcbpolicy *pcbsp;
-	struct secpolicy *currsp = NULL;	/* Policy on socket. */
-	struct secpolicy *sp;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-	IPSEC_ASSERT(inp != NULL, ("null inpcb"));
-	IPSEC_ASSERT(error != NULL, ("null error"));
-	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
-		("invalid direction %u", dir));
+	uint32_t genid;
+	int downgrade;
 
-	/* Set spidx in pcb. */
-	*error = ipsec_setspidx_inpcb(m, inp);
-	if (*error)
-		return (NULL);
+	INP_LOCK_ASSERT(inp);
 
-	pcbsp = inp->inp_sp;
-	IPSEC_ASSERT(pcbsp != NULL, ("null pcbsp"));
-	switch (dir) {
-	case IPSEC_DIR_INBOUND:
-		currsp = pcbsp->sp_in;
-		break;
-	case IPSEC_DIR_OUTBOUND:
-		currsp = pcbsp->sp_out;
-		break;
+	if (dir == IPSEC_DIR_OUTBOUND) {
+		/* Do we have configured PCB policy? */
+		if (inp->inp_sp->flags & INP_OUTBOUND_POLICY)
+			return;
+		/* Another thread has already set cached policy */
+		if (inp->inp_sp->sp_out != NULL)
+			return;
+		/*
+		 * Do not cache OUTBOUND policy if PCB isn't connected,
+		 * i.e. foreign address is INADDR_ANY/UNSPECIFIED.
+		 */
+#ifdef INET
+		if ((inp->inp_vflag & INP_IPV4) != 0 &&
+		    inp->inp_faddr.s_addr == INADDR_ANY)
+			return;
+#endif
+#ifdef INET6
+		if ((inp->inp_vflag & INP_IPV6) != 0 &&
+		    IN6_IS_ADDR_UNSPECIFIED(&inp->in6p_faddr))
+			return;
+#endif
+	} else {
+		/* Do we have configured PCB policy? */
+		if (inp->inp_sp->flags & INP_INBOUND_POLICY)
+			return;
+		/* Another thread has already set cached policy */
+		if (inp->inp_sp->sp_in != NULL)
+			return;
+		/*
+		 * Do not cache INBOUND policy for listen socket,
+		 * that is bound to INADDR_ANY/UNSPECIFIED address.
+		 */
+#ifdef INET
+		if ((inp->inp_vflag & INP_IPV4) != 0 &&
+		    inp->inp_faddr.s_addr == INADDR_ANY)
+			return;
+#endif
+#ifdef INET6
+		if ((inp->inp_vflag & INP_IPV6) != 0 &&
+		    IN6_IS_ADDR_UNSPECIFIED(&inp->in6p_faddr))
+			return;
+#endif
 	}
-	IPSEC_ASSERT(currsp != NULL, ("null currsp"));
-
-	if (pcbsp->priv) {			/* When privilieged socket. */
-		switch (currsp->policy) {
-		case IPSEC_POLICY_BYPASS:
-		case IPSEC_POLICY_IPSEC:
-			key_addref(currsp);
-			sp = currsp;
-			break;
-
-		case IPSEC_POLICY_ENTRUST:
-			/* Look for a policy in SPD. */
-			sp = KEY_ALLOCSP(&currsp->spidx, dir);
-			if (sp == NULL)		/* No SP found. */
-				sp = KEY_ALLOCSP_DEFAULT();
-			break;
-
-		default:
-			ipseclog((LOG_ERR, "%s: Invalid policy for PCB %d\n",
-				__func__, currsp->policy));
-			*error = EINVAL;
-			return (NULL);
-		}
-	} else {				/* Unpriv, SPD has policy. */
-		sp = KEY_ALLOCSP(&currsp->spidx, dir);
-		if (sp == NULL) {		/* No SP found. */
-			switch (currsp->policy) {
-			case IPSEC_POLICY_BYPASS:
-				ipseclog((LOG_ERR, "%s: Illegal policy for "
-					"non-priviliged defined %d\n",
-					__func__, currsp->policy));
-				*error = EINVAL;
-				return (NULL);
-
-			case IPSEC_POLICY_ENTRUST:
-				sp = KEY_ALLOCSP_DEFAULT();
-				break;
-
-			case IPSEC_POLICY_IPSEC:
-				key_addref(currsp);
-				sp = currsp;
-				break;
-
-			default:
-				ipseclog((LOG_ERR, "%s: Invalid policy for "
-					"PCB %d\n", __func__, currsp->policy));
-				*error = EINVAL;
-				return (NULL);
-			}
-		}
+	downgrade = 0;
+	if (!INP_WLOCKED(inp)) {
+		if ((downgrade = INP_TRY_UPGRADE(inp)) == 0)
+			return;
 	}
-	IPSEC_ASSERT(sp != NULL,
-		("null SP (priv %u policy %u", pcbsp->priv, currsp->policy));
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s (priv %u policy %u) allocate SP:%p (refcnt %u)\n",
-			__func__, pcbsp->priv, currsp->policy, sp, sp->refcnt));
-	return (sp);
-}
-
-/*
- * For FORWADING packet or OUTBOUND without a socket. Searching SPD for packet,
- * and return a pointer to SP.
- * OUT:	positive: a pointer to the entry for security policy leaf matched.
- *	NULL:	no apropreate SP found, the following value is set to error.
- *		0	: bypass
- *		EACCES	: discard packet.
- *		ENOENT	: ipsec_acquire() in progress, maybe.
- *		others	: error occured.
- */
-struct secpolicy *
-ipsec_getpolicybyaddr(struct mbuf *m, u_int dir, int flag, int *error)
-{
-	struct secpolicyindex spidx;
-	struct secpolicy *sp;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-	IPSEC_ASSERT(error != NULL, ("null error"));
-	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
-		("invalid direction %u", dir));
-
-	sp = NULL;
-	if (key_havesp(dir)) {
-		/* Make an index to look for a policy. */
-		*error = ipsec_setspidx(m, &spidx,
-					(flag & IP_FORWARDING) ? 0 : 1);
-		if (*error != 0) {
-			DPRINTF(("%s: setpidx failed, dir %u flag %u\n",
-				__func__, dir, flag));
-			return (NULL);
-		}
-		spidx.dir = dir;
-
-		sp = KEY_ALLOCSP(&spidx, dir);
+	if (dir == IPSEC_DIR_OUTBOUND)
+		inp->inp_sp->sp_out = sp;
+	else
+		inp->inp_sp->sp_in = sp;
+	/*
+	 * SP is already referenced by the lookup code.
+	 * We take extra reference here to avoid race in the
+	 * ipsec_getpcbpolicy() function - SP will not be freed in the
+	 * time between we take SP pointer from the cache and key_addref()
+	 * call.
+	 */
+	key_addref(sp);
+	genid = key_getspgen();
+	if (genid != inp->inp_sp->genid) {
+		ipsec_invalidate_cache(inp, dir);
+		inp->inp_sp->genid = genid;
 	}
-	if (sp == NULL)			/* No SP found, use system default. */
-		sp = KEY_ALLOCSP_DEFAULT();
-	IPSEC_ASSERT(sp != NULL, ("null SP"));
-	return (sp);
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: PCB(%p): cached %s SP(%p)\n",
+	    __func__, inp, dir == IPSEC_DIR_OUTBOUND ? "OUTBOUND":
+	    "INBOUND", sp));
+	if (downgrade != 0)
+		INP_DOWNGRADE(inp);
 }
 
-struct secpolicy *
-ipsec4_checkpolicy(struct mbuf *m, u_int dir, u_int flag, int *error,
-    struct inpcb *inp)
+static struct secpolicy *
+ipsec_checkpolicy(struct secpolicy *sp, struct inpcb *inp, int *error)
 {
-	struct secpolicy *sp;
 
-	*error = 0;
-	if (inp == NULL)
-		sp = ipsec_getpolicybyaddr(m, dir, flag, error);
-	else
-		sp = ipsec_getpolicybysock(m, dir, inp, error);
-	if (sp == NULL) {
-		IPSEC_ASSERT(*error != 0, ("getpolicy failed w/o error"));
-		IPSECSTAT_INC(ips_out_inval);
-		return (NULL);
-	}
-	IPSEC_ASSERT(*error == 0, ("sp w/ error set to %u", *error));
+	/* Save found OUTBOUND policy into PCB SP cache. */
+	if (inp != NULL && inp->inp_sp != NULL && inp->inp_sp->sp_out == NULL)
+		ipsec_cachepolicy(inp, sp, IPSEC_DIR_OUTBOUND);
+
 	switch (sp->policy) {
-	case IPSEC_POLICY_ENTRUST:
 	default:
 		printf("%s: invalid policy %u\n", __func__, sp->policy);
 		/* FALLTHROUGH */
 	case IPSEC_POLICY_DISCARD:
-		IPSECSTAT_INC(ips_out_polvio);
 		*error = -EINVAL;	/* Packet is discarded by caller. */
-		break;
+		/* FALLTHROUGH */
 	case IPSEC_POLICY_BYPASS:
 	case IPSEC_POLICY_NONE:
-		KEY_FREESP(&sp);
+		key_freesp(&sp);
 		sp = NULL;		/* NB: force NULL result. */
 		break;
 	case IPSEC_POLICY_IPSEC:
-		if (sp->req == NULL)	/* Acquire a SA. */
-			*error = key_spdacquire(sp);
+		/* XXXAE: handle LARVAL SP */
 		break;
 	}
-	if (*error != 0) {
-		KEY_FREESP(&sp);
-		sp = NULL;
-	}
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: get SP(%p), error %d\n", __func__, sp, *error));
 	return (sp);
 }
 
-static int
-ipsec_setspidx_inpcb(struct mbuf *m, struct inpcb *inp)
+static struct secpolicy *
+ipsec_getpcbpolicy(struct inpcb *inp, u_int dir)
 {
-	int error;
+	struct secpolicy *sp;
+	int flags, downgrade;
+
+	if (inp == NULL || inp->inp_sp == NULL)
+		return (NULL);
 
-	IPSEC_ASSERT(inp != NULL, ("null inp"));
-	IPSEC_ASSERT(inp->inp_sp != NULL, ("null inp_sp"));
-	IPSEC_ASSERT(inp->inp_sp->sp_out != NULL && inp->inp_sp->sp_in != NULL,
-		("null sp_in || sp_out"));
+	INP_LOCK_ASSERT(inp);
 
-	error = ipsec_setspidx(m, &inp->inp_sp->sp_in->spidx, 1);
-	if (error == 0) {
-		inp->inp_sp->sp_in->spidx.dir = IPSEC_DIR_INBOUND;
-		inp->inp_sp->sp_out->spidx = inp->inp_sp->sp_in->spidx;
-		inp->inp_sp->sp_out->spidx.dir = IPSEC_DIR_OUTBOUND;
+	flags = inp->inp_sp->flags;
+	if (dir == IPSEC_DIR_OUTBOUND) {
+		sp = inp->inp_sp->sp_out;
+		flags &= INP_OUTBOUND_POLICY;
 	} else {
-		bzero(&inp->inp_sp->sp_in->spidx,
-			sizeof (inp->inp_sp->sp_in->spidx));
-		bzero(&inp->inp_sp->sp_out->spidx,
-			sizeof (inp->inp_sp->sp_in->spidx));
+		sp = inp->inp_sp->sp_in;
+		flags &= INP_INBOUND_POLICY;
 	}
-	return (error);
-}
-
-/*
- * 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(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;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-
 	/*
-	 * 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?
+	 * Check flags. If we have PCB SP, just return it.
+	 * Otherwise we need to check that cached SP entry isn't stale.
 	 */
-	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("%s: pkthdr len(%d) mismatch (%d), ignored.\n",
-				__func__, len, m->m_pkthdr.len));
-		return (EINVAL);
-	}
-
-	if (m->m_pkthdr.len < sizeof(struct ip)) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-			printf("%s: pkthdr len(%d) too small (v4), ignored.\n",
-			    __func__, 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
-	case 6:
-		if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
-			KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-				printf("%s: pkthdr len(%d) too small (v6), "
-				"ignored\n", __func__, m->m_pkthdr.len));
-			return (EINVAL);
+	if (flags == 0) {
+		if (sp == NULL)
+			return (NULL);
+		if (inp->inp_sp->genid != key_getspgen()) {
+			/* Invalidate the cache. */
+			downgrade = 0;
+			if (!INP_WLOCKED(inp)) {
+				if ((downgrade = INP_TRY_UPGRADE(inp)) == 0)
+					return (NULL);
+			}
+			ipsec_invalidate_cache(inp, IPSEC_DIR_OUTBOUND);
+			ipsec_invalidate_cache(inp, IPSEC_DIR_INBOUND);
+			if (downgrade != 0)
+				INP_DOWNGRADE(inp);
+			return (NULL);
 		}
-		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("%s: " "unknown IP version %u, ignored.\n",
-				__func__, v));
-		return (EINVAL);
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: PCB(%p): cache hit SP(%p)\n",
+		    __func__, inp, sp));
+		/* Return referenced cached policy */
 	}
+	key_addref(sp);
+	return (sp);
 }
 
+#ifdef INET
 static void
-ipsec4_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
+ipsec4_get_ulp(const struct mbuf *m, struct secpolicyindex *spidx,
+    int needport)
 {
-	u_int8_t nxt;
+	uint8_t nxt;
 	int off;
 
 	/* Sanity check. */
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-	IPSEC_ASSERT(m->m_pkthdr.len >= sizeof(struct ip),("packet too short"));
+	IPSEC_ASSERT(m->m_pkthdr.len >= sizeof(struct ip),
+	    ("packet too short"));
 
-	/* NB: ip_input() flips it into host endian. XXX Need more checking. */
 	if (m->m_len >= sizeof (struct ip)) {
-		struct ip *ip = mtod(m, struct ip *);
-		if (ip->ip_off & (IP_MF | IP_OFFMASK))
+		const struct ip *ip = mtod(m, const struct ip *);
+		if (ip->ip_off & htons(IP_MF | IP_OFFMASK))
 			goto done;
-#ifdef _IP_VHL
-		off = _IP_VHL_HL(ip->ip_vhl) << 2;
-#else
 		off = ip->ip_hl << 2;
-#endif
 		nxt = ip->ip_p;
 	} else {
 		struct ip ih;
 
 		m_copydata(m, 0, sizeof (struct ip), (caddr_t) &ih);
-		if (ih.ip_off & (IP_MF | IP_OFFMASK))
+		if (ih.ip_off & htons(IP_MF | IP_OFFMASK))
 			goto done;
-#ifdef _IP_VHL
-		off = _IP_VHL_HL(ih.ip_vhl) << 2;
-#else
 		off = ih.ip_hl << 2;
-#endif
 		nxt = ih.ip_p;
 	}
 
@@ -670,60 +551,134 @@ done:
 done_proto:
 	spidx->src.sin.sin_port = IPSEC_PORT_ANY;
 	spidx->dst.sin.sin_port = IPSEC_PORT_ANY;
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: ", __func__); kdebug_secpolicyindex(spidx, NULL));
 }
 
-/* Assumes that m is sane. */
-static int
-ipsec4_setspidx_ipaddr(struct mbuf *m, struct secpolicyindex *spidx)
+static void
+ipsec4_setspidx_ipaddr(const struct mbuf *m, struct secpolicyindex *spidx)
 {
-	static const struct sockaddr_in template = {
-		sizeof (struct sockaddr_in),
-		AF_INET,
-		0, { 0 }, { 0, 0, 0, 0, 0, 0, 0, 0 }
-	};
 
-	spidx->src.sin = template;
-	spidx->dst.sin = template;
+	ipsec4_setsockaddrs(m, &spidx->src, &spidx->dst);
+	spidx->prefs = sizeof(struct in_addr) << 3;
+	spidx->prefd = sizeof(struct in_addr) << 3;
+}
 
-	if (m->m_len < sizeof (struct ip)) {
-		m_copydata(m, offsetof(struct ip, ip_src),
-			   sizeof (struct  in_addr),
-			   (caddr_t) &spidx->src.sin.sin_addr);
-		m_copydata(m, offsetof(struct ip, ip_dst),
-			   sizeof (struct  in_addr),
-			   (caddr_t) &spidx->dst.sin.sin_addr);
-	} else {
-		struct ip *ip = mtod(m, struct ip *);
-		spidx->src.sin.sin_addr = ip->ip_src;
-		spidx->dst.sin.sin_addr = ip->ip_dst;
+static struct secpolicy *
+ipsec4_getpolicy(const struct mbuf *m, struct inpcb *inp, u_int dir)
+{
+	struct secpolicyindex spidx;
+	struct secpolicy *sp;
+
+	sp = ipsec_getpcbpolicy(inp, dir);
+	if (sp == NULL && key_havesp(dir)) {
+		/* Make an index to look for a policy. */
+		ipsec4_setspidx_ipaddr(m, &spidx);
+		/* Fill ports in spidx if we have inpcb. */
+		ipsec4_get_ulp(m, &spidx, inp != NULL);
+		spidx.dir = dir;
+		sp = key_allocsp(&spidx, dir);
 	}
+	if (sp == NULL)		/* No SP found, use system default. */
+		sp = key_allocsp_default();
+	return (sp);
+}
 
-	spidx->prefs = sizeof(struct in_addr) << 3;
-	spidx->prefd = sizeof(struct in_addr) << 3;
+/*
+ * Check security policy for *OUTBOUND* IPv4 packet.
+ */
+struct secpolicy *
+ipsec4_checkpolicy(const struct mbuf *m, struct inpcb *inp, int *error)
+{
+	struct secpolicy *sp;
 
-	return (0);
+	*error = 0;
+	sp = ipsec4_getpolicy(m, inp, IPSEC_DIR_OUTBOUND);
+	if (sp != NULL)
+		sp = ipsec_checkpolicy(sp, inp, error);
+	if (sp == NULL) {
+		switch (*error) {
+		case 0: /* No IPsec required: BYPASS or NONE */
+			break;
+		case -EINVAL:
+			IPSECSTAT_INC(ips_out_polvio);
+			break;
+		default:
+			IPSECSTAT_INC(ips_out_inval);
+		}
+	}
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: using SP(%p), error %d\n", __func__, sp, *error));
+	if (sp != NULL)
+		KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
+	return (sp);
 }
 
+/*
+ * Check IPv4 packet against *INBOUND* security policy.
+ * This function is called from tcp_input(), udp_input(),
+ * rip_input() and sctp_input().
+ */
+int
+ipsec4_in_reject(const struct mbuf *m, struct inpcb *inp)
+{
+	struct secpolicy *sp;
+	int result;
+
+	sp = ipsec4_getpolicy(m, inp, IPSEC_DIR_INBOUND);
+	result = ipsec_in_reject(sp, inp, m);
+	key_freesp(&sp);
+	if (result != 0)
+		IPSECSTAT_INC(ips_in_polvio);
+	return (result);
+}
+
+/*
+ * IPSEC_CAP() method implementation for IPv4.
+ */
+int
+ipsec4_capability(struct mbuf *m, u_int cap)
+{
+
+	switch (cap) {
+	case IPSEC_CAP_BYPASS_FILTER:
+		/*
+		 * Bypass packet filtering for packets previously handled
+		 * by IPsec.
+		 */
+		if (!V_ip4_filtertunnel &&
+		    m_tag_find(m, PACKET_TAG_IPSEC_IN_DONE, NULL) != NULL)
+			return (1);
+		return (0);
+	case IPSEC_CAP_OPERABLE:
+		/* Do we have active security policies? */
+		if (key_havesp(IPSEC_DIR_INBOUND) != 0 ||
+		    key_havesp(IPSEC_DIR_OUTBOUND) != 0)
+			return (1);
+		return (0);
+	};
+	return (EOPNOTSUPP);
+}
+
+#endif /* INET */
+
 #ifdef INET6
 static void
-ipsec6_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
+ipsec6_get_ulp(const struct mbuf *m, struct secpolicyindex *spidx,
+    int needport)
 {
-	int off, nxt;
 	struct tcphdr th;
 	struct udphdr uh;
 	struct icmp6_hdr ih;
+	int off, nxt;
 
-	/* Sanity check. */
-	if (m == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
-
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("%s:\n", __func__); kdebug_mbuf(m));
+	IPSEC_ASSERT(m->m_pkthdr.len >= sizeof(struct ip6_hdr),
+	    ("packet too short"));
 
 	/* 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->src.sin6.sin6_port = IPSEC_PORT_ANY;
+	spidx->dst.sin6.sin6_port = IPSEC_PORT_ANY;
 
 	nxt = -1;
 	off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
@@ -738,8 +693,8 @@ ipsec6_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
 		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;
+		spidx->src.sin6.sin6_port = th.th_sport;
+		spidx->dst.sin6.sin6_port = th.th_dport;
 		break;
 	case IPPROTO_UDP:
 		spidx->ul_proto = nxt;
@@ -748,355 +703,157 @@ ipsec6_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
 		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;
+		spidx->src.sin6.sin6_port = uh.uh_sport;
+		spidx->dst.sin6.sin6_port = uh.uh_dport;
 		break;
 	case IPPROTO_ICMPV6:
 		spidx->ul_proto = nxt;
 		if (off + sizeof(struct icmp6_hdr) > m->m_pkthdr.len)
 			break;
 		m_copydata(m, off, sizeof(ih), (caddr_t)&ih);
-		((struct sockaddr_in6 *)&spidx->src)->sin6_port =
-		    htons((uint16_t)ih.icmp6_type);
-		((struct sockaddr_in6 *)&spidx->dst)->sin6_port =
-		    htons((uint16_t)ih.icmp6_code);
+		spidx->src.sin6.sin6_port = htons((uint16_t)ih.icmp6_type);
+		spidx->dst.sin6.sin6_port = htons((uint16_t)ih.icmp6_code);
 		break;
 	default:
 		/* XXX Intermediate headers??? */
 		spidx->ul_proto = nxt;
 		break;
 	}
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: ", __func__); kdebug_secpolicyindex(spidx, NULL));
 }
 
-/* Assumes that m is sane. */
-static int
-ipsec6_setspidx_ipaddr(struct mbuf *m, struct secpolicyindex *spidx)
+static void
+ipsec6_setspidx_ipaddr(const struct mbuf *m, struct secpolicyindex *spidx)
 {
-	struct ip6_hdr *ip6 = NULL;
-	struct ip6_hdr ip6buf;
-	struct sockaddr_in6 *sin6;
-
-	if (m->m_len >= sizeof(*ip6))
-		ip6 = mtod(m, struct ip6_hdr *);
-	else {
-		m_copydata(m, 0, sizeof(ip6buf), (caddr_t)&ip6buf);
-		ip6 = &ip6buf;
-	}
 
-	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]);
-	}
+	ipsec6_setsockaddrs(m, &spidx->src, &spidx->dst);
 	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 (0);
 }
-#endif
 
-static void
-ipsec_delpcbpolicy(struct inpcbpolicy *p)
-{
-
-	free(p, M_IPSEC_INPCB);
-}
-
-/* Initialize policy in PCB. */
-int
-ipsec_init_policy(struct socket *so, struct inpcbpolicy **pcb_sp)
+static struct secpolicy *
+ipsec6_getpolicy(const struct mbuf *m, struct inpcb *inp, u_int dir)
 {
-	struct inpcbpolicy *new;
-
-	/* Sanity check. */
-	if (so == NULL || pcb_sp == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
-
-	new = (struct inpcbpolicy *) malloc(sizeof(struct inpcbpolicy),
-					    M_IPSEC_INPCB, M_NOWAIT|M_ZERO);
-	if (new == NULL) {
-		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-		return (ENOBUFS);
-	}
-
-	new->priv = IPSEC_IS_PRIVILEGED_SO(so);
-
-	if ((new->sp_in = KEY_NEWSP()) == NULL) {
-		ipsec_delpcbpolicy(new);
-		return (ENOBUFS);
-	}
-	new->sp_in->state = IPSEC_SPSTATE_ALIVE;
-	new->sp_in->policy = IPSEC_POLICY_ENTRUST;
+	struct secpolicyindex spidx;
+	struct secpolicy *sp;
 
-	if ((new->sp_out = KEY_NEWSP()) == NULL) {
-		KEY_FREESP(&new->sp_in);
-		ipsec_delpcbpolicy(new);
-		return (ENOBUFS);
+	sp = ipsec_getpcbpolicy(inp, dir);
+	if (sp == NULL && key_havesp(dir)) {
+		/* Make an index to look for a policy. */
+		ipsec6_setspidx_ipaddr(m, &spidx);
+		/* Fill ports in spidx if we have inpcb. */
+		ipsec6_get_ulp(m, &spidx, inp != NULL);
+		spidx.dir = dir;
+		sp = key_allocsp(&spidx, dir);
 	}
-	new->sp_out->state = IPSEC_SPSTATE_ALIVE;
-	new->sp_out->policy = IPSEC_POLICY_ENTRUST;
-
-	*pcb_sp = new;
-
-	return (0);
+	if (sp == NULL)		/* No SP found, use system default. */
+		sp = key_allocsp_default();
+	return (sp);
 }
 
-/* Copy old IPsec policy into new. */
-int
-ipsec_copy_policy(struct inpcbpolicy *old, struct inpcbpolicy *new)
+/*
+ * Check security policy for *OUTBOUND* IPv6 packet.
+ */
+struct secpolicy *
+ipsec6_checkpolicy(const struct mbuf *m, struct inpcb *inp, int *error)
 {
 	struct secpolicy *sp;
 
-	sp = ipsec_deepcopy_policy(old->sp_in);
-	if (sp) {
-		KEY_FREESP(&new->sp_in);
-		new->sp_in = sp;
-	} else
-		return (ENOBUFS);
-
-	sp = ipsec_deepcopy_policy(old->sp_out);
-	if (sp) {
-		KEY_FREESP(&new->sp_out);
-		new->sp_out = sp;
-	} else
-		return (ENOBUFS);
-
-	new->priv = old->priv;
-
-	return (0);
-}
-
-struct ipsecrequest *
-ipsec_newisr(void)
-{
-	struct ipsecrequest *p;
-
-	p = malloc(sizeof(struct ipsecrequest), M_IPSEC_SR, M_NOWAIT|M_ZERO);
-	if (p != NULL)
-		IPSECREQUEST_LOCK_INIT(p);
-	return (p);
-}
-
-void
-ipsec_delisr(struct ipsecrequest *p)
-{
-
-	IPSECREQUEST_LOCK_DESTROY(p);
-	free(p, M_IPSEC_SR);
-}
-
-/* Deep-copy a policy in PCB. */
-static struct secpolicy *
-ipsec_deepcopy_policy(struct secpolicy *src)
-{
-	struct ipsecrequest *newchain = NULL;
-	struct ipsecrequest *p;
-	struct ipsecrequest **q;
-	struct ipsecrequest *r;
-	struct secpolicy *dst;
-
-	if (src == NULL)
-		return (NULL);
-	dst = KEY_NEWSP();
-	if (dst == NULL)
-		return (NULL);
-
-	/*
-	 * Deep-copy IPsec request chain.  This is required since struct
-	 * ipsecrequest is not reference counted.
-	 */
-	q = &newchain;
-	for (p = src->req; p; p = p->next) {
-		*q = ipsec_newisr();
-		if (*q == NULL)
-			goto fail;
-		(*q)->saidx.proto = p->saidx.proto;
-		(*q)->saidx.mode = p->saidx.mode;
-		(*q)->level = p->level;
-		(*q)->saidx.reqid = p->saidx.reqid;
-
-		bcopy(&p->saidx.src, &(*q)->saidx.src, sizeof((*q)->saidx.src));
-		bcopy(&p->saidx.dst, &(*q)->saidx.dst, sizeof((*q)->saidx.dst));
-
-		(*q)->sp = dst;
-
-		q = &((*q)->next);
-	}
-
-	dst->req = newchain;
-	dst->state = src->state;
-	dst->policy = src->policy;
-	/* Do not touch the refcnt fields. */
-
-	return (dst);
-
-fail:
-	for (p = newchain; p; p = r) {
-		r = p->next;
-		ipsec_delisr(p);
-		p = NULL;
+	*error = 0;
+	sp = ipsec6_getpolicy(m, inp, IPSEC_DIR_OUTBOUND);
+	if (sp != NULL)
+		sp = ipsec_checkpolicy(sp, inp, error);
+	if (sp == NULL) {
+		switch (*error) {
+		case 0: /* No IPsec required: BYPASS or NONE */
+			break;
+		case -EINVAL:
+			IPSEC6STAT_INC(ips_out_polvio);
+			break;
+		default:
+			IPSEC6STAT_INC(ips_out_inval);
+		}
 	}
-	return (NULL);
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: using SP(%p), error %d\n", __func__, sp, *error));
+	if (sp != NULL)
+		KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
+	return (sp);
 }
 
-/* Set policy and IPsec request if present. */
-static int
-ipsec_set_policy_internal(struct secpolicy **pcb_sp, int optname,
-    caddr_t request, size_t len, struct ucred *cred)
+/*
+ * Check IPv6 packet against inbound security policy.
+ * This function is called from tcp6_input(), udp6_input(),
+ * rip6_input() and sctp_input().
+ */
+int
+ipsec6_in_reject(const struct mbuf *m, struct inpcb *inp)
 {
-	struct sadb_x_policy *xpl;
-	struct secpolicy *newsp = NULL;
-	int error;
-
-	/* Sanity check. */
-	if (pcb_sp == NULL || *pcb_sp == NULL || request == NULL)
-		return (EINVAL);
-	if (len < sizeof(*xpl))
-		return (EINVAL);
-	xpl = (struct sadb_x_policy *)request;
-
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("%s: passed policy\n", __func__);
-		kdebug_sadb_x_policy((struct sadb_ext *)xpl));
-
-	/* Check policy type. */
-	/* ipsec_set_policy_internal() accepts IPSEC, ENTRUST and BYPASS. */
-	if (xpl->sadb_x_policy_type == IPSEC_POLICY_DISCARD
-	 || xpl->sadb_x_policy_type == IPSEC_POLICY_NONE)
-		return (EINVAL);
-
-	/* Check privileged socket. */
-	if (cred != NULL && xpl->sadb_x_policy_type == IPSEC_POLICY_BYPASS) {
-		error = priv_check_cred(cred, PRIV_NETINET_IPSEC, 0);
-		if (error)
-			return (EACCES);
-	}
-
-	/* Allocating new SP entry. */
-	if ((newsp = key_msg2sp(xpl, len, &error)) == NULL)
-		return (error);
-
-	newsp->state = IPSEC_SPSTATE_ALIVE;
-
-	/* Clear old SP and set new SP. */
-	KEY_FREESP(pcb_sp);
-	*pcb_sp = newsp;
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("%s: new policy\n", __func__);
-		kdebug_secpolicy(newsp));
+	struct secpolicy *sp;
+	int result;
 
-	return (0);
+	sp = ipsec6_getpolicy(m, inp, IPSEC_DIR_INBOUND);
+	result = ipsec_in_reject(sp, inp, m);
+	key_freesp(&sp);
+	if (result)
+		IPSEC6STAT_INC(ips_in_polvio);
+	return (result);
 }
 
+/*
+ * IPSEC_CAP() method implementation for IPv6.
+ */
 int
-ipsec_set_policy(struct inpcb *inp, int optname, caddr_t request,
-    size_t len, struct ucred *cred)
+ipsec6_capability(struct mbuf *m, u_int cap)
 {
-	struct sadb_x_policy *xpl;
-	struct secpolicy **pcb_sp;
-
-	/* Sanity check. */
-	if (inp == NULL || request == NULL)
-		return (EINVAL);
-	if (len < sizeof(*xpl))
-		return (EINVAL);
-	xpl = (struct sadb_x_policy *)request;
-
-	/* Select direction. */
-	switch (xpl->sadb_x_policy_dir) {
-	case IPSEC_DIR_INBOUND:
-		pcb_sp = &inp->inp_sp->sp_in;
-		break;
-	case IPSEC_DIR_OUTBOUND:
-		pcb_sp = &inp->inp_sp->sp_out;
-		break;
-	default:
-		ipseclog((LOG_ERR, "%s: invalid direction=%u\n", __func__,
-			xpl->sadb_x_policy_dir));
-		return (EINVAL);
-	}
 
-	return (ipsec_set_policy_internal(pcb_sp, optname, request, len, cred));
+	switch (cap) {
+	case IPSEC_CAP_BYPASS_FILTER:
+		/*
+		 * Bypass packet filtering for packets previously handled
+		 * by IPsec.
+		 */
+		if (!V_ip6_filtertunnel &&
+		    m_tag_find(m, PACKET_TAG_IPSEC_IN_DONE, NULL) != NULL)
+			return (1);
+		return (0);
+	case IPSEC_CAP_OPERABLE:
+		/* Do we have active security policies? */
+		if (key_havesp(IPSEC_DIR_INBOUND) != 0 ||
+		    key_havesp(IPSEC_DIR_OUTBOUND) != 0)
+			return (1);
+		return (0);
+	};
+	return (EOPNOTSUPP);
 }
+#endif /* INET6 */
 
 int
-ipsec_get_policy(struct inpcb *inp, caddr_t request, size_t len,
-    struct mbuf **mp)
+ipsec_run_hhooks(struct ipsec_ctx_data *ctx, int type)
 {
-	struct sadb_x_policy *xpl;
-	struct secpolicy *pcb_sp;
+	int idx;
 
-	/* Sanity check. */
-	if (inp == NULL || request == NULL || mp == NULL)
-		return (EINVAL);
-	IPSEC_ASSERT(inp->inp_sp != NULL, ("null inp_sp"));
-	if (len < sizeof(*xpl))
-		return (EINVAL);
-	xpl = (struct sadb_x_policy *)request;
-
-	/* Select direction. */
-	switch (xpl->sadb_x_policy_dir) {
-	case IPSEC_DIR_INBOUND:
-		pcb_sp = inp->inp_sp->sp_in;
+	switch (ctx->af) {
+#ifdef INET
+	case AF_INET:
+		idx = HHOOK_IPSEC_INET;
 		break;
-	case IPSEC_DIR_OUTBOUND:
-		pcb_sp = inp->inp_sp->sp_out;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		idx = HHOOK_IPSEC_INET6;
 		break;
+#endif
 	default:
-		ipseclog((LOG_ERR, "%s: invalid direction=%u\n", __func__,
-			xpl->sadb_x_policy_dir));
-		return (EINVAL);
-	}
-
-	/* Sanity check. Should be an IPSEC_ASSERT. */
-	if (pcb_sp == NULL)
-		return (EINVAL);
-
-	*mp = key_sp2msg(pcb_sp);
-	if (!*mp) {
-		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-		return (ENOBUFS);
+		return (EPFNOSUPPORT);
 	}
-
-	(*mp)->m_type = MT_DATA;
-	KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-		printf("%s:\n", __func__); kdebug_mbuf(*mp));
-
-	return (0);
-}
-
-/* Delete policy in PCB. */
-int
-ipsec_delete_pcbpolicy(struct inpcb *inp)
-{
-	IPSEC_ASSERT(inp != NULL, ("null inp"));
-
-	if (inp->inp_sp == NULL)
-		return (0);
-
-	if (inp->inp_sp->sp_in != NULL)
-		KEY_FREESP(&inp->inp_sp->sp_in);
-
-	if (inp->inp_sp->sp_out != NULL)
-		KEY_FREESP(&inp->inp_sp->sp_out);
-
-	ipsec_delpcbpolicy(inp->inp_sp);
-	inp->inp_sp = NULL;
-
+	if (type == HHOOK_TYPE_IPSEC_IN)
+		HHOOKS_RUN_IF(V_ipsec_hhh_in[idx], ctx, NULL);
+	else
+		HHOOKS_RUN_IF(V_ipsec_hhh_out[idx], ctx, NULL);
+	if (*ctx->mp == NULL)
+		return (EACCES);
 	return (0);
 }
 
@@ -1105,32 +862,36 @@ ipsec_delete_pcbpolicy(struct inpcb *inp)
  * Either IPSEC_LEVEL_USE or IPSEC_LEVEL_REQUIRE are always returned.
  */
 u_int
-ipsec_get_reqlevel(struct ipsecrequest *isr)
+ipsec_get_reqlevel(struct secpolicy *sp, u_int idx)
 {
-	u_int level = 0;
+	struct ipsecrequest *isr;
 	u_int esp_trans_deflev, esp_net_deflev;
 	u_int ah_trans_deflev, ah_net_deflev;
+	u_int level = 0;
 
-	IPSEC_ASSERT(isr != NULL && isr->sp != NULL, ("null argument"));
-	IPSEC_ASSERT(isr->sp->spidx.src.sa.sa_family == isr->sp->spidx.dst.sa.sa_family,
-		("af family mismatch, src %u, dst %u",
-		 isr->sp->spidx.src.sa.sa_family,
-		 isr->sp->spidx.dst.sa.sa_family));
-
+	IPSEC_ASSERT(idx < sp->tcount, ("Wrong IPsec request index %d", idx));
 /* XXX Note that we have ipseclog() expanded here - code sync issue. */
 #define IPSEC_CHECK_DEFAULT(lev) \
-	(((lev) != IPSEC_LEVEL_USE && (lev) != IPSEC_LEVEL_REQUIRE	      \
-			&& (lev) != IPSEC_LEVEL_UNIQUE)			      \
-		? (V_ipsec_debug						      \
-			? log(LOG_INFO, "fixed system default level " #lev ":%d->%d\n",\
-				(lev), IPSEC_LEVEL_REQUIRE)		      \
-			: 0),						      \
-			(lev) = IPSEC_LEVEL_REQUIRE,			      \
-			(lev)						      \
-		: (lev))
+	(((lev) != IPSEC_LEVEL_USE && (lev) != IPSEC_LEVEL_REQUIRE &&	\
+	  (lev) != IPSEC_LEVEL_UNIQUE)					\
+		? (V_ipsec_debug  ?					\
+		log(LOG_INFO, "fixed system default level " #lev ":%d->%d\n",\
+		(lev), IPSEC_LEVEL_REQUIRE) : 0),			\
+		(lev) = IPSEC_LEVEL_REQUIRE, (lev) : (lev))
+
+	/*
+	 * IPsec VTI uses unique security policy with fake spidx filled
+	 * with zeroes. Just return IPSEC_LEVEL_REQUIRE instead of doing
+	 * full level lookup for such policies.
+	 */
+	if (sp->state == IPSEC_SPSTATE_IFNET) {
+		IPSEC_ASSERT(sp->req[idx]->level == IPSEC_LEVEL_UNIQUE,
+		    ("Wrong IPsec request level %d", sp->req[idx]->level));
+		return (IPSEC_LEVEL_REQUIRE);
+	}
 
 	/* Set default level. */
-	switch (((struct sockaddr *)&isr->sp->spidx.src)->sa_family) {
+	switch (sp->spidx.src.sa.sa_family) {
 #ifdef INET
 	case AF_INET:
 		esp_trans_deflev = IPSEC_CHECK_DEFAULT(V_ip4_esp_trans_deflev);
@@ -1149,11 +910,12 @@ ipsec_get_reqlevel(struct ipsecrequest *isr)
 #endif /* INET6 */
 	default:
 		panic("%s: unknown af %u",
-			__func__, isr->sp->spidx.src.sa.sa_family);
+			__func__, sp->spidx.src.sa.sa_family);
 	}
 
 #undef IPSEC_CHECK_DEFAULT
 
+	isr = sp->req[idx];
 	/* Set level. */
 	switch (isr->level) {
 	case IPSEC_LEVEL_DEFAULT:
@@ -1198,6 +960,45 @@ ipsec_get_reqlevel(struct ipsecrequest *isr)
 	return (level);
 }
 
+static int
+ipsec_check_history(const struct mbuf *m, struct secpolicy *sp, u_int idx)
+{
+	struct xform_history *xh;
+	struct m_tag *mtag;
+
+	mtag = NULL;
+	while ((mtag = m_tag_find(__DECONST(struct mbuf *, m),
+	    PACKET_TAG_IPSEC_IN_DONE, mtag)) != NULL) {
+		xh = (struct xform_history *)(mtag + 1);
+		KEYDBG(IPSEC_DATA,
+		    char buf[IPSEC_ADDRSTRLEN];
+		    printf("%s: mode %s proto %u dst %s\n", __func__,
+			kdebug_secasindex_mode(xh->mode), xh->proto,
+			ipsec_address(&xh->dst, buf, sizeof(buf))));
+		if (xh->proto != sp->req[idx]->saidx.proto)
+			continue;
+		/* If SA had IPSEC_MODE_ANY, consider this as match. */
+		if (xh->mode != sp->req[idx]->saidx.mode &&
+		    xh->mode != IPSEC_MODE_ANY)
+			continue;
+		/*
+		 * For transport mode IPsec request doesn't contain
+		 * addresses. We need to use address from spidx.
+		 */
+		if (sp->req[idx]->saidx.mode == IPSEC_MODE_TRANSPORT) {
+			if (key_sockaddrcmp_withmask(&xh->dst.sa,
+			    &sp->spidx.dst.sa, sp->spidx.prefd) != 0)
+				continue;
+		} else {
+			if (key_sockaddrcmp(&xh->dst.sa,
+			    &sp->req[idx]->saidx.dst.sa, 0) != 0)
+				continue;
+		}
+		return (0); /* matched */
+	}
+	return (1);
+}
+
 /*
  * Check security policy requirements against the actual
  * packet contents.  Return one if the packet should be
@@ -1208,14 +1009,17 @@ ipsec_get_reqlevel(struct ipsecrequest *isr)
  *	0: valid
  *	1: invalid
  */
-int
-ipsec_in_reject(struct secpolicy *sp, struct mbuf *m)
+static int
+ipsec_in_reject(struct secpolicy *sp, struct inpcb *inp, const struct mbuf *m)
 {
-	struct ipsecrequest *isr;
-	int need_auth;
+	int i;
+
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: PCB(%p): using SP(%p)\n", __func__, inp, sp));
+	KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
 
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("%s: using SP\n", __func__); kdebug_secpolicy(sp));
+	if (inp != NULL && inp->inp_sp != NULL && inp->inp_sp->sp_in == NULL)
+		ipsec_cachepolicy(inp, sp, IPSEC_DIR_INBOUND);
 
 	/* Check policy. */
 	switch (sp->policy) {
@@ -1229,131 +1033,59 @@ ipsec_in_reject(struct secpolicy *sp, struct mbuf *m)
 	IPSEC_ASSERT(sp->policy == IPSEC_POLICY_IPSEC,
 		("invalid policy %u", sp->policy));
 
-	/* XXX Should compare policy against IPsec header history. */
-
-	need_auth = 0;
-	for (isr = sp->req; isr != NULL; isr = isr->next) {
-		if (ipsec_get_reqlevel(isr) != IPSEC_LEVEL_REQUIRE)
+	/*
+	 * ipsec[46]_common_input_cb after each transform adds
+	 * PACKET_TAG_IPSEC_IN_DONE mbuf tag. It contains SPI, proto, mode
+	 * and destination address from saidx. We can compare info from
+	 * these tags with requirements in SP.
+	 */
+	for (i = 0; i < sp->tcount; i++) {
+		/*
+		 * Do not check IPcomp, since IPcomp document
+		 * says that we shouldn't compress small packets.
+		 * IPComp policy should always be treated as being
+		 * in "use" level.
+		 */
+		if (sp->req[i]->saidx.proto == IPPROTO_IPCOMP ||
+		    ipsec_get_reqlevel(sp, i) != IPSEC_LEVEL_REQUIRE)
 			continue;
-		switch (isr->saidx.proto) {
+		if (V_check_policy_history != 0 &&
+		    ipsec_check_history(m, sp, i) != 0)
+			return (1);
+		else switch (sp->req[i]->saidx.proto) {
 		case IPPROTO_ESP:
 			if ((m->m_flags & M_DECRYPTED) == 0) {
-				KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+				KEYDBG(IPSEC_DUMP,
 				    printf("%s: ESP m_flags:%x\n", __func__,
 					    m->m_flags));
 				return (1);
 			}
-
-			if (!need_auth &&
-			    isr->sav != NULL &&
-			    isr->sav->tdb_authalgxform != NULL &&
-			    (m->m_flags & M_AUTHIPDGM) == 0) {
-				KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-				    printf("%s: ESP/AH m_flags:%x\n", __func__,
-					    m->m_flags));
-				return (1);
-			}
 			break;
 		case IPPROTO_AH:
-			need_auth = 1;
 			if ((m->m_flags & M_AUTHIPHDR) == 0) {
-				KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+				KEYDBG(IPSEC_DUMP,
 				    printf("%s: AH m_flags:%x\n", __func__,
 					    m->m_flags));
 				return (1);
 			}
 			break;
-		case IPPROTO_IPCOMP:
-			/*
-			 * We don't really care, as IPcomp document
-			 * says that we shouldn't compress small
-			 * packets.  IPComp policy should always be
-			 * treated as being in "use" level.
-			 */
-			break;
 		}
 	}
 	return (0);		/* Valid. */
 }
 
-static int
-ipsec46_in_reject(struct mbuf *m, struct inpcb *inp)
-{
-	struct secpolicy *sp;
-	int error;
-	int result;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-
-	/*
-	 * Get SP for this packet.
-	 * When we are called from ip_forward(), we call
-	 * ipsec_getpolicybyaddr() with IP_FORWARDING flag.
-	 */
-	if (inp == NULL)
-		sp = ipsec_getpolicybyaddr(m, IPSEC_DIR_INBOUND, IP_FORWARDING, &error);
-	else
-		sp = ipsec_getpolicybysock(m, IPSEC_DIR_INBOUND, inp, &error);
-
-	if (sp != NULL) {
-		result = ipsec_in_reject(sp, m);
-		KEY_FREESP(&sp);
-	} else {
-		result = 0;	/* XXX Should be panic?
-				 * -> No, there may be error. */
-	}
-	return (result);
-}
-
-/*
- * Check AH/ESP integrity.
- * This function is called from tcp_input(), udp_input(),
- * and {ah,esp}4_input for tunnel mode.
- */
-int
-ipsec4_in_reject(struct mbuf *m, struct inpcb *inp)
-{
-	int result;
-
-	result = ipsec46_in_reject(m, inp);
-	if (result)
-		IPSECSTAT_INC(ips_in_polvio);
-
-	return (result);
-}
-
-#ifdef INET6
-/*
- * Check AH/ESP integrity.
- * This function is called from tcp6_input(), udp6_input(),
- * and {ah,esp}6_input for tunnel mode.
- */
-int
-ipsec6_in_reject(struct mbuf *m, struct inpcb *inp)
-{
-	int result;
-
-	result = ipsec46_in_reject(m, inp);
-	if (result)
-		IPSEC6STAT_INC(ips_in_polvio);
-
-	return (result);
-}
-#endif
-
 /*
  * Compute the byte size to be occupied by IPsec header.
  * In case it is tunnelled, it includes the size of outer IP header.
- * NOTE: SP passed is freed in this function.
  */
 static size_t
 ipsec_hdrsiz_internal(struct secpolicy *sp)
 {
-	struct ipsecrequest *isr;
 	size_t size;
+	int i;
 
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("%s: using SP\n", __func__); kdebug_secpolicy(sp));
+	KEYDBG(IPSEC_STAMP, printf("%s: using SP(%p)\n", __func__, sp));
+	KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
 
 	switch (sp->policy) {
 	case IPSEC_POLICY_DISCARD:
@@ -1365,80 +1097,69 @@ ipsec_hdrsiz_internal(struct secpolicy *sp)
 	IPSEC_ASSERT(sp->policy == IPSEC_POLICY_IPSEC,
 		("invalid policy %u", sp->policy));
 
+	/*
+	 * XXX: for each transform we need to lookup suitable SA
+	 * and use info from SA to calculate headers size.
+	 * XXX: for NAT-T we need to cosider UDP header size.
+	 */
 	size = 0;
-	for (isr = sp->req; isr != NULL; isr = isr->next) {
-		size_t clen = 0;
-
-		switch (isr->saidx.proto) {
+	for (i = 0; i < sp->tcount; i++) {
+		switch (sp->req[i]->saidx.proto) {
 		case IPPROTO_ESP:
-			clen = esp_hdrsiz(isr->sav);
+			size += esp_hdrsiz(NULL);
 			break;
 		case IPPROTO_AH:
-			clen = ah_hdrsiz(isr->sav);
+			size += ah_hdrsiz(NULL);
 			break;
 		case IPPROTO_IPCOMP:
-			clen = sizeof(struct ipcomp);
+			size += sizeof(struct ipcomp);
 			break;
 		}
 
-		if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
-			switch (isr->saidx.dst.sa.sa_family) {
+		if (sp->req[i]->saidx.mode == IPSEC_MODE_TUNNEL) {
+			switch (sp->req[i]->saidx.dst.sa.sa_family) {
+#ifdef INET
 			case AF_INET:
-				clen += sizeof(struct ip);
+				size += sizeof(struct ip);
 				break;
+#endif
 #ifdef INET6
 			case AF_INET6:
-				clen += sizeof(struct ip6_hdr);
+				size += sizeof(struct ip6_hdr);
 				break;
 #endif
 			default:
 				ipseclog((LOG_ERR, "%s: unknown AF %d in "
 				    "IPsec tunnel SA\n", __func__,
-				    ((struct sockaddr *)&isr->saidx.dst)->sa_family));
+				    sp->req[i]->saidx.dst.sa.sa_family));
 				break;
 			}
 		}
-		size += clen;
 	}
-
 	return (size);
 }
 
-/* 
- * This function is called from ipsec_hdrsiz_tcp(), ip_ipsec_mtu(),
- * disabled ip6_ipsec_mtu() and ip6_forward().
+/*
+ * Compute ESP/AH header size for protocols with PCB, including
+ * outer IP header. Currently only tcp_output() uses it.
  */
 size_t
-ipsec_hdrsiz(struct mbuf *m, u_int dir, struct inpcb *inp)
+ipsec_hdrsiz_inpcb(struct inpcb *inp)
 {
+	struct secpolicyindex spidx;
 	struct secpolicy *sp;
-	int error;
-	size_t size;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-
-	/* Get SP for this packet.
-	 * When we are called from ip_forward(), we call
-	 * ipsec_getpolicybyaddr() with IP_FORWARDING flag.
-	 */
-	if (inp == NULL)
-		sp = ipsec_getpolicybyaddr(m, dir, IP_FORWARDING, &error);
-	else
-		sp = ipsec_getpolicybysock(m, dir, inp, &error);
+	size_t sz;
 
-	if (sp != NULL) {
-		size = ipsec_hdrsiz_internal(sp);
-		KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-			printf("%s: size:%lu.\n", __func__,
-				(unsigned long)size));
-
-		KEY_FREESP(&sp);
-	} else {
-		size = 0;	/* XXX Should be panic?
-				 * -> No, we are called w/o knowing if
-				 *    IPsec processing is needed. */
+	sp = ipsec_getpcbpolicy(inp, IPSEC_DIR_OUTBOUND);
+	if (sp == NULL && key_havesp(IPSEC_DIR_OUTBOUND)) {
+		ipsec_setspidx_inpcb(inp, &spidx, IPSEC_DIR_OUTBOUND);
+		sp = key_allocsp(&spidx, IPSEC_DIR_OUTBOUND);
 	}
-	return (size);
+	if (sp == NULL)
+		sp = key_allocsp_default();
+	sz = ipsec_hdrsiz_internal(sp);
+	key_freesp(&sp);
+	return (sz);
 }
 
 /*
@@ -1449,27 +1170,31 @@ ipsec_hdrsiz(struct mbuf *m, u_int dir, struct inpcb *inp)
  * beforehand).
  * 0 (zero) is returned if packet disallowed, 1 if packet permitted.
  *
- * Based on RFC 2401.
+ * Based on RFC 6479. Blocks are 32 bits unsigned integers
  */
+
+#define IPSEC_BITMAP_INDEX_MASK(w)	(w - 1)
+#define IPSEC_REDUNDANT_BIT_SHIFTS	5
+#define IPSEC_REDUNDANT_BITS		(1 << IPSEC_REDUNDANT_BIT_SHIFTS)
+#define IPSEC_BITMAP_LOC_MASK		(IPSEC_REDUNDANT_BITS - 1)
+
 int
-ipsec_chkreplay(u_int32_t seq, struct secasvar *sav)
+ipsec_chkreplay(uint32_t seq, struct secasvar *sav)
 {
 	const struct secreplay *replay;
-	u_int32_t diff;
-	int fr;
-	u_int32_t wsizeb;	/* Constant: bits of window size. */
-	int frlast;		/* Constant: last frame. */
+	uint32_t wsizeb;		/* Constant: window size. */
+	int index, bit_location;
 
 	IPSEC_ASSERT(sav != NULL, ("Null SA"));
 	IPSEC_ASSERT(sav->replay != NULL, ("Null replay state"));
 
 	replay = sav->replay;
 
+	/* No need to check replay if disabled. */
 	if (replay->wsize == 0)
-		return (1);	/* No need to check replay. */
+		return (1);
 
 	/* Constant. */
-	frlast = replay->wsize - 1;
 	wsizeb = replay->wsize << 3;
 
 	/* Sequence number of 0 is invalid. */
@@ -1480,26 +1205,26 @@ ipsec_chkreplay(u_int32_t seq, struct secasvar *sav)
 	if (replay->count == 0)
 		return (1);
 
-	if (seq > replay->lastseq) {
-		/* Larger sequences are okay. */
+	/* Larger sequences are okay. */
+	if (seq > replay->lastseq)
 		return (1);
-	} else {
-		/* seq is equal or less than lastseq. */
-		diff = replay->lastseq - seq;
 
-		/* Over range to check, i.e. too old or wrapped. */
-		if (diff >= wsizeb)
-			return (0);
-
-		fr = frlast - diff / 8;
+	/* Over range to check, i.e. too old or wrapped. */
+	if (replay->lastseq - seq >= wsizeb)
+		return (0);
 
-		/* This packet already seen? */
-		if ((replay->bitmap)[fr] & (1 << (diff % 8)))
-			return (0);
+	/* The sequence is inside the sliding window
+	 * now check the bit in the bitmap
+	 * bit location only depends on the sequence number
+	 */
+	bit_location = seq & IPSEC_BITMAP_LOC_MASK;
+	index = (seq >> IPSEC_REDUNDANT_BIT_SHIFTS)
+		& IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size);
 
-		/* Out of order but good. */
-		return (1);
-	}
+	/* This packet already seen? */
+	if ((replay->bitmap)[index] & (1 << bit_location))
+		return (0);
+	return (1);
 }
 
 /*
@@ -1508,13 +1233,12 @@ ipsec_chkreplay(u_int32_t seq, struct secasvar *sav)
  *	1:	NG
  */
 int
-ipsec_updatereplay(u_int32_t seq, struct secasvar *sav)
+ipsec_updatereplay(uint32_t seq, struct secasvar *sav)
 {
+	char buf[128];
 	struct secreplay *replay;
-	u_int32_t diff;
-	int fr;
-	u_int32_t wsizeb;	/* Constant: bits of window size. */
-	int frlast;		/* Constant: last frame. */
+	uint32_t wsizeb;		/* Constant: window size. */
+	int diff, index, bit_location;
 
 	IPSEC_ASSERT(sav != NULL, ("Null SA"));
 	IPSEC_ASSERT(sav->replay != NULL, ("Null replay state"));
@@ -1525,58 +1249,46 @@ ipsec_updatereplay(u_int32_t seq, struct secasvar *sav)
 		goto ok;	/* No need to check replay. */
 
 	/* Constant. */
-	frlast = replay->wsize - 1;
 	wsizeb = replay->wsize << 3;
 
 	/* Sequence number of 0 is invalid. */
 	if (seq == 0)
 		return (1);
 
-	/* First time. */
-	if (replay->count == 0) {
-		replay->lastseq = seq;
-		bzero(replay->bitmap, replay->wsize);
-		(replay->bitmap)[frlast] = 1;
+	/* The packet is too old, no need to update */
+	if (wsizeb + seq < replay->lastseq)
 		goto ok;
-	}
 
+	/* Now update the bit */
+	index = (seq >> IPSEC_REDUNDANT_BIT_SHIFTS);
+
+	/* First check if the sequence number is in the range */
 	if (seq > replay->lastseq) {
-		/* seq is larger than lastseq. */
-		diff = seq - replay->lastseq;
-
-		/* New larger sequence number. */
-		if (diff < wsizeb) {
-			/* In window. */
-			/* Set bit for this packet. */
-			vshiftl(replay->bitmap, diff, replay->wsize);
-			(replay->bitmap)[frlast] |= 1;
-		} else {
-			/* This packet has a "way larger". */
-			bzero(replay->bitmap, replay->wsize);
-			(replay->bitmap)[frlast] = 1;
-		}
-		replay->lastseq = seq;
+		int id;
+		int index_cur = replay->lastseq >> IPSEC_REDUNDANT_BIT_SHIFTS;
 
-		/* Larger is good. */
-	} else {
-		/* seq is equal or less than lastseq. */
-		diff = replay->lastseq - seq;
+		diff = index - index_cur;
+		if (diff > replay->bitmap_size) {
+			/* something unusual in this case */
+			diff = replay->bitmap_size;
+		}
 
-		/* Over range to check, i.e. too old or wrapped. */
-		if (diff >= wsizeb)
-			return (1);
+		for (id = 0; id < diff; ++id) {
+			replay->bitmap[(id + index_cur + 1)
+			& IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size)] = 0;
+		}
 
-		fr = frlast - diff / 8;
+		replay->lastseq = seq;
+	}
 
-		/* This packet already seen? */
-		if ((replay->bitmap)[fr] & (1 << (diff % 8)))
-			return (1);
+	index &= IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size);
+	bit_location = seq & IPSEC_BITMAP_LOC_MASK;
 
-		/* Mark as seen. */
-		(replay->bitmap)[fr] |= (1 << (diff % 8));
+	/* this packet has already been received */
+	if (replay->bitmap[index] & (1 << bit_location))
+		return (1);
 
-		/* Out of order but good. */
-	}
+	replay->bitmap[index] |= (1 << bit_location);
 
 ok:
 	if (replay->count == ~0) {
@@ -1585,167 +1297,99 @@ ok:
 		replay->overflow++;
 
 		/* Don't increment, no more packets accepted. */
-		if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0)
+		if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0) {
+			if (sav->sah->saidx.proto == IPPROTO_AH)
+				AHSTAT_INC(ahs_wrap);
+			else if (sav->sah->saidx.proto == IPPROTO_ESP)
+				ESPSTAT_INC(esps_wrap);
 			return (1);
+		}
 
 		ipseclog((LOG_WARNING, "%s: replay counter made %d cycle. %s\n",
-		    __func__, replay->overflow, ipsec_logsastr(sav)));
+		    __func__, replay->overflow,
+		    ipsec_sa2str(sav, buf, sizeof(buf))));
 	}
-
-	replay->count++;
-
 	return (0);
 }
 
-/*
- * Shift variable length buffer to left.
- * IN:	bitmap: pointer to the buffer
- * 	nbit:	the number of to shift.
- *	wsize:	buffer size (bytes).
- */
-static void
-vshiftl(unsigned char *bitmap, int nbit, int wsize)
-{
-	int s, j, i;
-	unsigned char over;
-
-	for (j = 0; j < nbit; j += 8) {
-		s = (nbit - j < 8) ? (nbit - j): 8;
-		bitmap[0] <<= s;
-		for (i = 1; i < wsize; i++) {
-			over = (bitmap[i] >> (8 - s));
-			bitmap[i] <<= s;
-			bitmap[i-1] |= over;
-		}
-	}
-}
-
-#ifdef INET
-/* Return a printable string for the IPv4 address. */
-static char *
-inet_ntoa4(struct in_addr ina)
-{
-	static char buf[4][4 * sizeof "123" + 4];
-	unsigned char *ucp = (unsigned char *) &ina;
-	static int i = 3;
-
-	/* XXX-BZ Returns static buffer. */
-	i = (i + 1) % 4;
-	sprintf(buf[i], "%d.%d.%d.%d", ucp[0] & 0xff, ucp[1] & 0xff,
-	    ucp[2] & 0xff, ucp[3] & 0xff);
-	return (buf[i]);
-}
-#endif
-
-/* Return a printable string for the address. */
-char *
-ipsec_address(union sockaddr_union* sa)
+int
+ipsec_updateid(struct secasvar *sav, uint64_t *new, uint64_t *old)
 {
-#ifdef INET6
-	char ip6buf[INET6_ADDRSTRLEN];
-#endif
+	uint64_t tmp;
 
-	switch (sa->sa.sa_family) {
-#ifdef INET
-	case AF_INET:
-		return (inet_ntoa4(sa->sin.sin_addr));
-#endif /* INET */
-#ifdef INET6
-	case AF_INET6:
-		return (ip6_sprintf(ip6buf, &sa->sin6.sin6_addr));
-#endif /* INET6 */
-	default:
-		return ("(unknown address family)");
+	/*
+	 * tdb_cryptoid is initialized by xform_init().
+	 * Then it can be changed only when some crypto error occurred or
+	 * when SA is deleted. We stored used cryptoid in the xform_data
+	 * structure. In case when crypto error occurred and crypto
+	 * subsystem has reinited the session, it returns new cryptoid
+	 * and EAGAIN error code.
+	 *
+	 * This function will be called when we got EAGAIN from crypto
+	 * subsystem.
+	 * *new is cryptoid that was returned by crypto subsystem in
+	 * the crp_sid.
+	 * *old is the original cryptoid that we stored in xform_data.
+	 *
+	 * For first failed request *old == sav->tdb_cryptoid, then
+	 * we update sav->tdb_cryptoid and redo crypto_dispatch().
+	 * For next failed request *old != sav->tdb_cryptoid, then
+	 * we store cryptoid from first request into the *new variable
+	 * and crp_sid from this second session will be returned via
+	 * *old pointer, so caller can release second session.
+	 *
+	 * XXXAE: check this more carefully.
+	 */
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: SA(%p) moves cryptoid %jd -> %jd\n",
+		__func__, sav, (uintmax_t)(*old), (uintmax_t)(*new)));
+	KEYDBG(IPSEC_DATA, kdebug_secasv(sav));
+	SECASVAR_LOCK(sav);
+	if (sav->tdb_cryptoid != *old) {
+		/* cryptoid was already updated */
+		tmp = *new;
+		*new = sav->tdb_cryptoid;
+		*old = tmp;
+		SECASVAR_UNLOCK(sav);
+		return (1);
 	}
+	sav->tdb_cryptoid = *new;
+	SECASVAR_UNLOCK(sav);
+	return (0);
 }
 
-const char *
-ipsec_logsastr(struct secasvar *sav)
+int
+ipsec_initialized(void)
 {
-	static char buf[256];
-	char *p;
-	struct secasindex *saidx = &sav->sah->saidx;
-
-	IPSEC_ASSERT(saidx->src.sa.sa_family == saidx->dst.sa.sa_family,
-		("address family mismatch"));
-
-	p = buf;
-	snprintf(buf, sizeof(buf), "SA(SPI=%u ", (u_int32_t)ntohl(sav->spi));
-	while (p && *p)
-		p++;
-	/* NB: only use ipsec_address on one address at a time. */
-	snprintf(p, sizeof (buf) - (p - buf), "src=%s ",
-		ipsec_address(&saidx->src));
-	while (p && *p)
-		p++;
-	snprintf(p, sizeof (buf) - (p - buf), "dst=%s)",
-		ipsec_address(&saidx->dst));
-
-	return (buf);
-}
 
-void
-ipsec_dumpmbuf(struct mbuf *m)
-{
-	int totlen;
-	int i;
-	u_char *p;
-
-	totlen = 0;
-	printf("---\n");
-	while (m) {
-		p = mtod(m, u_char *);
-		for (i = 0; i < m->m_len; i++) {
-			printf("%02x ", p[i]);
-			totlen++;
-			if (totlen % 16 == 0)
-				printf("\n");
-		}
-		m = m->m_next;
-	}
-	if (totlen % 16 != 0)
-		printf("\n");
-	printf("---\n");
+	return (V_def_policy != NULL);
 }
 
 static void
-ipsec_init(const void *unused __unused)
+def_policy_init(const void *unused __unused)
 {
 
-	SECPOLICY_LOCK_INIT(&V_ip4_def_policy);
-	V_ip4_def_policy.refcnt = 1;			/* NB: disallow free. */
+	V_def_policy = key_newsp();
+	if (V_def_policy != NULL) {
+		V_def_policy->policy = IPSEC_POLICY_NONE;
+		/* Force INPCB SP cache invalidation */
+		key_bumpspgen();
+	} else
+		printf("%s: failed to initialize default policy\n", __func__);
 }
-VNET_SYSINIT(ipsec_init, SI_SUB_PROTO_DOMAININIT, SI_ORDER_ANY, ipsec_init,
-    NULL);
-
-
-/* XXX This stuff doesn't belong here... */
 
-static	struct xformsw* xforms = NULL;
 
-/*
- * Register a transform; typically at system startup.
- */
-void
-xform_register(struct xformsw* xsp)
+static void
+def_policy_uninit(const void *unused __unused)
 {
 
-	xsp->xf_next = xforms;
-	xforms = xsp;
+	if (V_def_policy != NULL) {
+		key_freesp(&V_def_policy);
+		key_bumpspgen();
+	}
 }
 
-/*
- * Initialize transform support in an sav.
- */
-int
-xform_init(struct secasvar *sav, int xftype)
-{
-	struct xformsw *xsp;
-
-	if (sav->tdb_xform != NULL)	/* Previously initialized. */
-		return (0);
-	for (xsp = xforms; xsp; xsp = xsp->xf_next)
-		if (xsp->xf_type == xftype)
-			return ((*xsp->xf_init)(sav, xsp));
-	return (EINVAL);
-}
+VNET_SYSINIT(def_policy_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_FIRST,
+    def_policy_init, NULL);
+VNET_SYSUNINIT(def_policy_uninit, SI_SUB_PROTO_DOMAIN, SI_ORDER_FIRST,
+    def_policy_uninit, NULL);
diff --git a/freebsd/sys/netipsec/ipsec.h b/freebsd/sys/netipsec/ipsec.h
index f3415872..7653e4d4 100644
--- a/freebsd/sys/netipsec/ipsec.h
+++ b/freebsd/sys/netipsec/ipsec.h
@@ -47,12 +47,11 @@
 
 #ifdef _KERNEL
 
-#define	IPSEC_ASSERT(_c,_m) KASSERT(_c, _m)
+#include <sys/_lock.h>
+#include <sys/_mutex.h>
+#include <sys/_rwlock.h>
 
-#define	IPSEC_IS_PRIVILEGED_SO(_so) \
-	((_so)->so_cred != NULL && \
-	 priv_check_cred((_so)->so_cred, PRIV_NETINET_IPSEC, 0) \
-	 == 0)
+#define	IPSEC_ASSERT(_c,_m) KASSERT(_c, _m)
 
 /*
  * Security Policy Index
@@ -61,37 +60,41 @@
  * specifies ICMPv6 type, and the port field in "dst" specifies ICMPv6 code.
  */
 struct secpolicyindex {
-	u_int8_t dir;			/* direction of packet flow, see below */
 	union sockaddr_union src;	/* IP src address for SP */
 	union sockaddr_union dst;	/* IP dst address for SP */
-	u_int8_t prefs;			/* prefix length in bits for src */
-	u_int8_t prefd;			/* prefix length in bits for dst */
-	u_int16_t ul_proto;		/* upper layer Protocol */
-#ifdef notyet
-	uid_t uids;
-	uid_t uidd;
-	gid_t gids;
-	gid_t gidd;
-#endif
+	uint8_t ul_proto;		/* upper layer Protocol */
+	uint8_t dir;			/* direction of packet flow */
+	uint8_t prefs;			/* prefix length in bits for src */
+	uint8_t prefd;			/* prefix length in bits for dst */
+};
+
+/* Request for IPsec */
+struct ipsecrequest {
+	struct secasindex saidx;/* hint for search proper SA */
+				/* if __ss_len == 0 then no address specified.*/
+	u_int level;		/* IPsec level defined below. */
 };
 
 /* Security Policy Data Base */
 struct secpolicy {
-	LIST_ENTRY(secpolicy) chain;
-	struct mtx lock;
+	TAILQ_ENTRY(secpolicy) chain;
+	LIST_ENTRY(secpolicy) idhash;
+	LIST_ENTRY(secpolicy) drainq;
 
-	u_int refcnt;			/* reference count */
 	struct secpolicyindex spidx;	/* selector */
-	u_int32_t id;			/* It's unique number on the system. */
-	u_int state;			/* 0: dead, others: alive */
-#define IPSEC_SPSTATE_DEAD	0
-#define IPSEC_SPSTATE_ALIVE	1
-	u_int16_t policy;		/* policy_type per pfkeyv2.h */
-	u_int16_t scangen;		/* scan generation # */
-	struct ipsecrequest *req;
-				/* pointer to the ipsec request tree, */
-				/* if policy == IPSEC else this value == NULL.*/
-
+#define	IPSEC_MAXREQ		4
+	struct ipsecrequest *req[IPSEC_MAXREQ];
+	u_int tcount;			/* IPsec transforms count */
+	volatile u_int refcnt;		/* reference count */
+	u_int policy;			/* policy_type per pfkeyv2.h */
+	u_int state;
+#define	IPSEC_SPSTATE_DEAD	0
+#define	IPSEC_SPSTATE_LARVAL	1
+#define	IPSEC_SPSTATE_ALIVE	2
+#define	IPSEC_SPSTATE_PCB	3
+#define	IPSEC_SPSTATE_IFNET	4
+	uint32_t priority;		/* priority of this policy */
+	uint32_t id;			/* It's unique number on the system. */
 	/*
 	 * lifetime handler.
 	 * the policy can be used without limitiation if both lifetime and
@@ -105,48 +108,25 @@ struct secpolicy {
 	long validtime;		/* duration this policy is valid without use */
 };
 
-#define	SECPOLICY_LOCK_INIT(_sp) \
-	mtx_init(&(_sp)->lock, "ipsec policy", NULL, MTX_DEF)
-#define	SECPOLICY_LOCK(_sp)		mtx_lock(&(_sp)->lock)
-#define	SECPOLICY_UNLOCK(_sp)		mtx_unlock(&(_sp)->lock)
-#define	SECPOLICY_LOCK_DESTROY(_sp)	mtx_destroy(&(_sp)->lock)
-#define	SECPOLICY_LOCK_ASSERT(_sp)	mtx_assert(&(_sp)->lock, MA_OWNED)
-
-/* Request for IPsec */
-struct ipsecrequest {
-	struct ipsecrequest *next;
-				/* pointer to next structure */
-				/* If NULL, it means the end of chain. */
-	struct secasindex saidx;/* hint for search proper SA */
-				/* if __ss_len == 0 then no address specified.*/
-	u_int level;		/* IPsec level defined below. */
-
-	struct secasvar *sav;	/* place holder of SA for use */
-	struct secpolicy *sp;	/* back pointer to SP */
-	struct rwlock lock;	/* to interlock updates */
-};
-
 /*
- * Need recursion for when crypto callbacks happen directly,
- * as in the case of software crypto.  Need to look at how
- * hard it is to remove this...
+ * PCB security policies.
+ * Application can setup private security policies for socket.
+ * Such policies can have IPSEC, BYPASS and ENTRUST type.
+ * By default, policies are set to NULL. This means that they have ENTRUST type.
+ * When application sets BYPASS or IPSEC type policy, the flags field
+ * is also updated. When flags is not set, the system could store
+ * used security policy into the sp_in/sp_out pointer to speed up further
+ * lookups.
  */
-#define	IPSECREQUEST_LOCK_INIT(_isr) \
-	rw_init_flags(&(_isr)->lock, "ipsec request", RW_RECURSE)
-#define	IPSECREQUEST_LOCK(_isr)		rw_rlock(&(_isr)->lock)
-#define	IPSECREQUEST_UNLOCK(_isr)	rw_runlock(&(_isr)->lock)
-#define	IPSECREQUEST_WLOCK(_isr)	rw_wlock(&(_isr)->lock)
-#define	IPSECREQUEST_WUNLOCK(_isr)	rw_wunlock(&(_isr)->lock)
-#define	IPSECREQUEST_UPGRADE(_isr)	rw_try_upgrade(&(_isr)->lock)
-#define	IPSECREQUEST_DOWNGRADE(_isr)	rw_downgrade(&(_isr)->lock)
-#define	IPSECREQUEST_LOCK_DESTROY(_isr)	rw_destroy(&(_isr)->lock)
-#define	IPSECREQUEST_LOCK_ASSERT(_isr)	rw_assert(&(_isr)->lock, RA_LOCKED)
-
-/* security policy in PCB */
 struct inpcbpolicy {
-	struct secpolicy *sp_in;
-	struct secpolicy *sp_out;
-	int priv;			/* privileged socket ? */
+	struct secpolicy	*sp_in;
+	struct secpolicy	*sp_out;
+
+	uint32_t		genid;
+	uint16_t		flags;
+#define	INP_INBOUND_POLICY	0x0001
+#define	INP_OUTBOUND_POLICY	0x0002
+	uint16_t		hdrsz;
 };
 
 /* SP acquiring list table. */
@@ -161,6 +141,9 @@ struct secspacq {
 };
 #endif /* _KERNEL */
 
+/* buffer size for formatted output of ipsec address */
+#define	IPSEC_ADDRSTRLEN	(INET6_ADDRSTRLEN + 11)
+
 /* according to IANA assignment, port 0x0000 and proto 0xff are reserved. */
 #define IPSEC_PORT_ANY		0
 #define IPSEC_ULPROTO_ANY	255
@@ -196,6 +179,12 @@ struct secspacq {
 #define IPSEC_POLICY_ENTRUST	3	/* consulting SPD if present. */
 #define IPSEC_POLICY_BYPASS	4	/* only for privileged socket. */
 
+/* Policy scope */
+#define	IPSEC_POLICYSCOPE_ANY		0x00	/* unspecified */
+#define	IPSEC_POLICYSCOPE_GLOBAL	0x01	/* global scope */
+#define	IPSEC_POLICYSCOPE_IFNET		0x02	/* if_ipsec(4) scope */
+#define	IPSEC_POLICYSCOPE_PCB		0x04	/* PCB scope */
+
 /* Security protocol level */
 #define	IPSEC_LEVEL_DEFAULT	0	/* reference to system default */
 #define	IPSEC_LEVEL_USE		1	/* use SA if present. */
@@ -217,62 +206,33 @@ struct secspacq {
 
 /* statistics for ipsec processing */
 struct ipsecstat {
-	u_quad_t in_success;  /* succeeded inbound process */
-	u_quad_t in_polvio;
-			/* security policy violation for inbound process */
-	u_quad_t in_nosa;     /* inbound SA is unavailable */
-	u_quad_t in_inval;    /* inbound processing failed due to EINVAL */
-	u_quad_t in_nomem;    /* inbound processing failed due to ENOBUFS */
-	u_quad_t in_badspi;   /* failed getting a SPI */
-	u_quad_t in_ahreplay; /* AH replay check failed */
-	u_quad_t in_espreplay; /* ESP replay check failed */
-	u_quad_t in_ahauthsucc; /* AH authentication success */
-	u_quad_t in_ahauthfail; /* AH authentication failure */
-	u_quad_t in_espauthsucc; /* ESP authentication success */
-	u_quad_t in_espauthfail; /* ESP authentication failure */
-	u_quad_t in_esphist[256];
-	u_quad_t in_ahhist[256];
-	u_quad_t in_comphist[256];
-	u_quad_t out_success; /* succeeded outbound process */
-	u_quad_t out_polvio;
-			/* security policy violation for outbound process */
-	u_quad_t out_nosa;    /* outbound SA is unavailable */
-	u_quad_t out_inval;   /* outbound process failed due to EINVAL */
-	u_quad_t out_nomem;    /* inbound processing failed due to ENOBUFS */
-	u_quad_t out_noroute; /* there is no route */
-	u_quad_t out_esphist[256];
-	u_quad_t out_ahhist[256];
-	u_quad_t out_comphist[256];
-
-	u_quad_t spdcachelookup;
-	u_quad_t spdcachemiss;
-
-	u_int32_t ips_in_polvio;	/* input: sec policy violation */
-	u_int32_t ips_out_polvio;	/* output: sec policy violation */
-	u_int32_t ips_out_nosa;		/* output: SA unavailable  */
-	u_int32_t ips_out_nomem;	/* output: no memory available */
-	u_int32_t ips_out_noroute;	/* output: no route available */
-	u_int32_t ips_out_inval;	/* output: generic error */
-	u_int32_t ips_out_bundlesa;	/* output: bundled SA processed */
-	u_int32_t ips_mbcoalesced;	/* mbufs coalesced during clone */
-	u_int32_t ips_clcoalesced;	/* clusters coalesced during clone */
-	u_int32_t ips_clcopied;		/* clusters copied during clone */
-	u_int32_t ips_mbinserted;	/* mbufs inserted during makespace */
+	uint64_t ips_in_polvio;		/* input: sec policy violation */
+	uint64_t ips_in_nomem;		/* input: no memory available */
+	uint64_t ips_in_inval;		/* input: generic error */
+
+	uint64_t ips_out_polvio;	/* output: sec policy violation */
+	uint64_t ips_out_nosa;		/* output: SA unavailable  */
+	uint64_t ips_out_nomem;		/* output: no memory available */
+	uint64_t ips_out_noroute;	/* output: no route available */
+	uint64_t ips_out_inval;		/* output: generic error */
+	uint64_t ips_out_bundlesa;	/* output: bundled SA processed */
+
+	uint64_t ips_mbcoalesced;	/* mbufs coalesced during clone */
+	uint64_t ips_clcoalesced;	/* clusters coalesced during clone */
+	uint64_t ips_clcopied;		/* clusters copied during clone */
+	uint64_t ips_mbinserted;	/* mbufs inserted during makespace */
 	/* 
 	 * Temporary statistics for performance analysis.
 	 */
 	/* See where ESP/AH/IPCOMP header land in mbuf on input */
-	u_int32_t ips_input_front;
-	u_int32_t ips_input_middle;
-	u_int32_t ips_input_end;
+	uint64_t ips_input_front;
+	uint64_t ips_input_middle;
+	uint64_t ips_input_end;
 };
 
 /*
  * Definitions for IPsec & Key sysctl operations.
  */
-/*
- * Names for IPsec & Key sysctl objects
- */
 #define IPSECCTL_STATS			1	/* stats */
 #define IPSECCTL_DEF_POLICY		2
 #define IPSECCTL_DEF_ESP_TRANSLEV	3	/* int; ESP transport mode */
@@ -288,53 +248,18 @@ struct ipsecstat {
 #define	IPSECCTL_ECN			11
 #define	IPSECCTL_DEBUG			12
 #define	IPSECCTL_ESP_RANDPAD		13
-#define IPSECCTL_MAXID			14
-
-#define IPSECCTL_NAMES { \
-	{ 0, 0 }, \
-	{ 0, 0 }, \
-	{ "def_policy", CTLTYPE_INT }, \
-	{ "esp_trans_deflev", CTLTYPE_INT }, \
-	{ "esp_net_deflev", CTLTYPE_INT }, \
-	{ "ah_trans_deflev", CTLTYPE_INT }, \
-	{ "ah_net_deflev", CTLTYPE_INT }, \
-	{ 0, 0 }, \
-	{ "ah_cleartos", CTLTYPE_INT }, \
-	{ "ah_offsetmask", CTLTYPE_INT }, \
-	{ "dfbit", CTLTYPE_INT }, \
-	{ "ecn", CTLTYPE_INT }, \
-	{ "debug", CTLTYPE_INT }, \
-	{ "esp_randpad", CTLTYPE_INT }, \
-}
-
-#define IPSEC6CTL_NAMES { \
-	{ 0, 0 }, \
-	{ 0, 0 }, \
-	{ "def_policy", CTLTYPE_INT }, \
-	{ "esp_trans_deflev", CTLTYPE_INT }, \
-	{ "esp_net_deflev", CTLTYPE_INT }, \
-	{ "ah_trans_deflev", CTLTYPE_INT }, \
-	{ "ah_net_deflev", CTLTYPE_INT }, \
-	{ 0, 0 }, \
-	{ 0, 0 }, \
-	{ 0, 0 }, \
-	{ 0, 0 }, \
-	{ "ecn", CTLTYPE_INT }, \
-	{ "debug", CTLTYPE_INT }, \
-	{ "esp_randpad", CTLTYPE_INT }, \
-}
 
 #ifdef _KERNEL
-struct ipsec_output_state {
-	struct mbuf *m;
-	struct route *ro;
-	struct sockaddr *dst;
-};
+#include <sys/counter.h>
 
-struct ipsec_history {
-	int ih_proto;
-	u_int32_t ih_spi;
-};
+struct ipsec_ctx_data;
+#define	IPSEC_INIT_CTX(_ctx, _mp, _sav, _af, _enc) do {	\
+	(_ctx)->mp = (_mp);				\
+	(_ctx)->sav = (_sav);				\
+	(_ctx)->af = (_af);				\
+	(_ctx)->enc = (_enc);				\
+} while(0)
+int	ipsec_run_hhooks(struct ipsec_ctx_data *ctx, int direction);
 
 VNET_DECLARE(int, ipsec_debug);
 #define	V_ipsec_debug		VNET(ipsec_debug)
@@ -347,8 +272,7 @@ VNET_DECLARE(int, ipsec_integrity);
 #define	V_ipsec_integrity	VNET(ipsec_integrity)
 #endif
 
-VNET_DECLARE(struct ipsecstat, ipsec4stat);
-VNET_DECLARE(struct secpolicy, ip4_def_policy);
+VNET_PCPUSTAT_DECLARE(struct ipsecstat, ipsec4stat);
 VNET_DECLARE(int, ip4_esp_trans_deflev);
 VNET_DECLARE(int, ip4_esp_net_deflev);
 VNET_DECLARE(int, ip4_ah_trans_deflev);
@@ -358,10 +282,10 @@ VNET_DECLARE(int, ip4_ipsec_dfbit);
 VNET_DECLARE(int, ip4_ipsec_ecn);
 VNET_DECLARE(int, ip4_esp_randpad);
 VNET_DECLARE(int, crypto_support);
+VNET_DECLARE(int, natt_cksum_policy);
 
-#define	IPSECSTAT_INC(name)	V_ipsec4stat.name += 1
-#define	V_ipsec4stat		VNET(ipsec4stat)
-#define	V_ip4_def_policy	VNET(ip4_def_policy)
+#define	IPSECSTAT_INC(name)	\
+    VNET_PCPUSTAT_ADD(struct ipsecstat, ipsec4stat, name, 1)
 #define	V_ip4_esp_trans_deflev	VNET(ip4_esp_trans_deflev)
 #define	V_ip4_esp_net_deflev	VNET(ip4_esp_net_deflev)
 #define	V_ip4_ah_trans_deflev	VNET(ip4_ah_trans_deflev)
@@ -371,64 +295,52 @@ VNET_DECLARE(int, crypto_support);
 #define	V_ip4_ipsec_ecn		VNET(ip4_ipsec_ecn)
 #define	V_ip4_esp_randpad	VNET(ip4_esp_randpad)
 #define	V_crypto_support	VNET(crypto_support)
+#define	V_natt_cksum_policy	VNET(natt_cksum_policy)
 
 #define ipseclog(x)	do { if (V_ipsec_debug) log x; } while (0)
 /* for openbsd compatibility */
 #define	DPRINTF(x)	do { if (V_ipsec_debug) printf x; } while (0)
 
-extern	struct ipsecrequest *ipsec_newisr(void);
-extern	void ipsec_delisr(struct ipsecrequest *);
-
-struct tdb_ident;
-extern struct secpolicy *ipsec_getpolicy __P((struct tdb_ident*, u_int));
 struct inpcb;
-extern struct secpolicy *ipsec4_checkpolicy __P((struct mbuf *, u_int, u_int,
-	int *, struct inpcb *));
-extern struct secpolicy * ipsec_getpolicybyaddr(struct mbuf *, u_int,
-	int, int *);
+struct m_tag;
+struct secasvar;
+struct sockopt;
+struct tcphdr;
+union sockaddr_union;
 
-struct inpcb;
-extern int ipsec_init_policy __P((struct socket *so, struct inpcbpolicy **));
-extern int ipsec_copy_policy
-	__P((struct inpcbpolicy *, struct inpcbpolicy *));
-extern u_int ipsec_get_reqlevel __P((struct ipsecrequest *));
-extern int ipsec_in_reject __P((struct secpolicy *, struct mbuf *));
-
-extern int ipsec_set_policy __P((struct inpcb *inp, int optname,
-	caddr_t request, size_t len, struct ucred *cred));
-extern int ipsec_get_policy __P((struct inpcb *inpcb, caddr_t request,
-	size_t len, struct mbuf **mp));
-extern int ipsec_delete_pcbpolicy __P((struct inpcb *));
-extern int ipsec4_in_reject __P((struct mbuf *, struct inpcb *));
-
-struct secas;
-struct tcpcb;
-extern int ipsec_chkreplay __P((u_int32_t, struct secasvar *));
-extern int ipsec_updatereplay __P((u_int32_t, struct secasvar *));
-
-extern size_t ipsec_hdrsiz __P((struct mbuf *, u_int, struct inpcb *));
-extern size_t ipsec_hdrsiz_tcp __P((struct tcpcb *));
+int ipsec_if_input(struct mbuf *, struct secasvar *, uint32_t);
 
-union sockaddr_union;
-extern char * ipsec_address(union sockaddr_union* sa);
-extern const char *ipsec_logsastr __P((struct secasvar *));
+struct ipsecrequest *ipsec_newisr(void);
+void ipsec_delisr(struct ipsecrequest *);
+struct secpolicy *ipsec4_checkpolicy(const struct mbuf *, struct inpcb *,
+    int *);
 
-extern void ipsec_dumpmbuf __P((struct mbuf *));
+u_int ipsec_get_reqlevel(struct secpolicy *, u_int);
 
-struct m_tag;
-extern void ah4_input(struct mbuf *m, int off);
-extern void ah4_ctlinput(int cmd, struct sockaddr *sa, void *);
-extern void esp4_input(struct mbuf *m, int off);
-extern void esp4_ctlinput(int cmd, struct sockaddr *sa, void *);
-extern void ipcomp4_input(struct mbuf *m, int off);
-extern int ipsec4_common_input(struct mbuf *m, ...);
-extern int ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
-			int skip, int protoff, struct m_tag *mt);
-extern int ipsec4_process_packet __P((struct mbuf *, struct ipsecrequest *,
-			int, int));
-extern int ipsec_process_done __P((struct mbuf *, struct ipsecrequest *));
-
-extern struct mbuf *ipsec_copypkt __P((struct mbuf *));
+void udp_ipsec_adjust_cksum(struct mbuf *, struct secasvar *, int, int);
+int udp_ipsec_output(struct mbuf *, struct secasvar *);
+int udp_ipsec_input(struct mbuf *, int, int);
+int udp_ipsec_pcbctl(struct inpcb *, struct sockopt *);
+
+int ipsec_chkreplay(uint32_t, struct secasvar *);
+int ipsec_updatereplay(uint32_t, struct secasvar *);
+int ipsec_updateid(struct secasvar *, uint64_t *, uint64_t *);
+int ipsec_initialized(void);
+
+void ipsec_setspidx_inpcb(struct inpcb *, struct secpolicyindex *, u_int);
+
+void ipsec4_setsockaddrs(const struct mbuf *, union sockaddr_union *,
+    union sockaddr_union *);
+int ipsec4_in_reject(const struct mbuf *, struct inpcb *);
+int ipsec4_input(struct mbuf *, int, int);
+int ipsec4_forward(struct mbuf *);
+int ipsec4_pcbctl(struct inpcb *, struct sockopt *);
+int ipsec4_output(struct mbuf *, struct inpcb *);
+int ipsec4_capability(struct mbuf *, u_int);
+int ipsec4_common_input_cb(struct mbuf *, struct secasvar *, int, int);
+int ipsec4_process_packet(struct mbuf *, struct secpolicy *, struct inpcb *);
+int ipsec_process_done(struct mbuf *, struct secpolicy *, struct secasvar *,
+    u_int);
 
 extern	void m_checkalignment(const char* where, struct mbuf *m0,
 		int off, int len);
@@ -436,22 +348,13 @@ extern	struct mbuf *m_makespace(struct mbuf *m0, int skip, int hlen, int *off);
 extern	caddr_t m_pad(struct mbuf *m, int n);
 extern	int m_striphdr(struct mbuf *m, int skip, int hlen);
 
-#ifdef DEV_ENC
-#define	ENC_BEFORE	0x0001
-#define	ENC_AFTER	0x0002
-#define	ENC_IN		0x0100
-#define	ENC_OUT		0x0200
-extern	int ipsec_filter(struct mbuf **, int, int);
-extern	void ipsec_bpf(struct mbuf *, struct secasvar *, int, int);
-#endif
 #endif /* _KERNEL */
 
 #ifndef _KERNEL
-extern caddr_t ipsec_set_policy __P((char *, int));
-extern int ipsec_get_policylen __P((caddr_t));
-extern char *ipsec_dump_policy __P((caddr_t, char *));
-
-extern const char *ipsec_strerror __P((void));
+extern caddr_t ipsec_set_policy(char *, int);
+extern int ipsec_get_policylen(caddr_t);
+extern char *ipsec_dump_policy(caddr_t, char *);
+extern const char *ipsec_strerror(void);
 
 #endif /* ! KERNEL */
 
diff --git a/freebsd/sys/netipsec/ipsec6.h b/freebsd/sys/netipsec/ipsec6.h
index 21ec6b36..a5fae4d1 100644
--- a/freebsd/sys/netipsec/ipsec6.h
+++ b/freebsd/sys/netipsec/ipsec6.h
@@ -41,15 +41,17 @@
 #include <netipsec/keydb.h>
 
 #ifdef _KERNEL
-VNET_DECLARE(struct ipsecstat, ipsec6stat);
+#include <sys/counter.h>
+
+VNET_PCPUSTAT_DECLARE(struct ipsecstat, ipsec6stat);
 VNET_DECLARE(int, ip6_esp_trans_deflev);
 VNET_DECLARE(int, ip6_esp_net_deflev);
 VNET_DECLARE(int, ip6_ah_trans_deflev);
 VNET_DECLARE(int, ip6_ah_net_deflev);
 VNET_DECLARE(int, ip6_ipsec_ecn);
 
-#define	IPSEC6STAT_INC(name)	V_ipsec6stat.name += 1
-#define	V_ipsec6stat		VNET(ipsec6stat)
+#define	IPSEC6STAT_INC(name)	\
+    VNET_PCPUSTAT_ADD(struct ipsecstat, ipsec6stat, name, 1)
 #define	V_ip6_esp_trans_deflev	VNET(ip6_esp_trans_deflev)
 #define	V_ip6_esp_net_deflev	VNET(ip6_esp_net_deflev)
 #define	V_ip6_ah_trans_deflev	VNET(ip6_ah_trans_deflev)
@@ -57,23 +59,22 @@ VNET_DECLARE(int, ip6_ipsec_ecn);
 #define	V_ip6_ipsec_ecn		VNET(ip6_ipsec_ecn)
 
 struct inpcb;
+struct secpolicy *ipsec6_checkpolicy(const struct mbuf *,
+    struct inpcb *, int *);
 
-extern int ipsec6_in_reject __P((struct mbuf *, struct inpcb *));
-
-struct ip6_hdr;
-extern const char *ipsec6_logpacketstr __P((struct ip6_hdr *, u_int32_t));
-
-struct m_tag;
-extern int ipsec6_common_input(struct mbuf **mp, int *offp, int proto);
-extern int ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav,
-			int skip, int protoff, struct m_tag *mt);
-extern void esp6_ctlinput(int, struct sockaddr *, void *);
+void ipsec6_setsockaddrs(const struct mbuf *, union sockaddr_union *,
+    union sockaddr_union *);
+int ipsec6_input(struct mbuf *, int, int);
+int ipsec6_in_reject(const struct mbuf *, struct inpcb *);
+int ipsec6_forward(struct mbuf *);
+int ipsec6_pcbctl(struct inpcb *, struct sockopt *);
+int ipsec6_output(struct mbuf *, struct inpcb *);
+int ipsec6_capability(struct mbuf *, u_int);
+int ipsec6_common_input_cb(struct mbuf *, struct secasvar *, int, int);
+int ipsec6_process_packet(struct mbuf *, struct secpolicy *, struct inpcb *);
 
-struct ipsec_output_state;
-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));
+int ip6_ipsec_filtertunnel(struct mbuf *);
+int ip6_ipsec_pcbctl(struct inpcb *, struct sockopt *);
 #endif /*_KERNEL*/
 
 #endif /*_NETIPSEC_IPSEC6_H_*/
diff --git a/freebsd/sys/netipsec/ipsec_input.c b/freebsd/sys/netipsec/ipsec_input.c
index d910de71..50e7d646 100644
--- a/freebsd/sys/netipsec/ipsec_input.c
+++ b/freebsd/sys/netipsec/ipsec_input.c
@@ -1,6 +1,5 @@
 #include <machine/rtems-bsd-kernel-space.h>
 
-/*	$FreeBSD$	*/
 /*	$OpenBSD: ipsec_input.c,v 1.63 2003/02/20 18:35:43 deraadt Exp $	*/
 /*-
  * The authors of this code are John Ioannidis (ji@tla.org),
@@ -21,6 +20,7 @@
  * Copyright (C) 1995, 1996, 1997, 1998, 1999 by John Ioannidis,
  * Angelos D. Keromytis and Niels Provos.
  * Copyright (c) 2001, Angelos D. Keromytis.
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
  *
  * Permission to use, copy, and modify this software with or without fee
  * is hereby granted, provided that this entire notice is included in
@@ -42,10 +42,12 @@
  * IPsec input processing.
  */
 
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include <rtems/bsd/local/opt_inet.h>
 #include <rtems/bsd/local/opt_inet6.h>
 #include <rtems/bsd/local/opt_ipsec.h>
-#include <rtems/bsd/local/opt_enc.h>
 
 #include <rtems/bsd/sys/param.h>
 #include <sys/systm.h>
@@ -55,11 +57,12 @@
 #include <sys/protosw.h>
 #include <sys/socket.h>
 #include <rtems/bsd/sys/errno.h>
+#include <sys/hhook.h>
 #include <sys/syslog.h>
 
 #include <net/if.h>
-#include <net/pfil.h>
-#include <net/route.h>
+#include <net/if_var.h>
+#include <net/if_enc.h>
 #include <net/netisr.h>
 #include <net/vnet.h>
 
@@ -89,6 +92,7 @@
 
 #include <netipsec/key.h>
 #include <netipsec/keydb.h>
+#include <netipsec/key_debug.h>
 
 #include <netipsec/xform.h>
 #include <netinet6/ip6protosw.h>
@@ -96,10 +100,6 @@
 #include <machine/in_cksum.h>
 #include <machine/stdarg.h>
 
-#ifdef DEV_ENC
-#include <net/if_enc.h>
-#endif
-
 
 #define	IPSEC_ISTAT(proto, name)	do {	\
 	if ((proto) == IPPROTO_ESP)		\
@@ -110,10 +110,6 @@
 		IPCOMPSTAT_INC(ipcomps_##name);	\
 } while (0)
 
-#ifdef INET
-static void ipsec4_common_ctlinput(int, struct sockaddr *, void *, int);
-#endif
-
 /*
  * ipsec_common_input gets called when an IPsec-protected packet
  * is received by IPv4 or IPv6.  Its job is to find the right SA
@@ -123,15 +119,11 @@ static void ipsec4_common_ctlinput(int, struct sockaddr *, void *, int);
 static int
 ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 {
+	char buf[IPSEC_ADDRSTRLEN];
 	union sockaddr_union dst_address;
 	struct secasvar *sav;
-	u_int32_t spi;
+	uint32_t spi;
 	int error;
-#ifdef INET
-#ifdef IPSEC_NAT_T
-	struct m_tag *tag;
-#endif
-#endif
 
 	IPSEC_ISTAT(sproto, input);
 
@@ -183,12 +175,6 @@ ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 		m_copydata(m, offsetof(struct ip, ip_dst),
 		    sizeof(struct in_addr),
 		    (caddr_t) &dst_address.sin.sin_addr);
-#ifdef IPSEC_NAT_T
-		/* Find the source port for NAT-T; see udp*_espdecap. */
-		tag = m_tag_find(m, PACKET_TAG_IPSEC_NAT_T_PORTS, NULL);
-		if (tag != NULL)
-			dst_address.sin.sin_port = ((u_int16_t *)(tag + 1))[1];
-#endif /* IPSEC_NAT_T */
 		break;
 #endif /* INET */
 #ifdef INET6
@@ -197,6 +183,13 @@ ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 		m_copydata(m, offsetof(struct ip6_hdr, ip6_dst),
 		    sizeof(struct in6_addr),
 		    (caddr_t) &dst_address.sin6.sin6_addr);
+		/* We keep addresses in SADB without embedded scope id */
+		if (IN6_IS_SCOPE_LINKLOCAL(&dst_address.sin6.sin6_addr)) {
+			/* XXX: sa6_recoverscope() */
+			dst_address.sin6.sin6_scope_id =
+			    ntohs(dst_address.sin6.sin6_addr.s6_addr16[1]);
+			dst_address.sin6.sin6_addr.s6_addr16[1] = 0;
+		}
 		break;
 #endif /* INET6 */
 	default:
@@ -207,11 +200,11 @@ ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 	}
 
 	/* NB: only pass dst since key_allocsa follows RFC2401 */
-	sav = KEY_ALLOCSA(&dst_address, sproto, spi);
+	sav = key_allocsa(&dst_address, sproto, spi);
 	if (sav == NULL) {
 		DPRINTF(("%s: no key association found for SA %s/%08lx/%u\n",
-			  __func__, ipsec_address(&dst_address),
-			  (u_long) ntohl(spi), sproto));
+		    __func__, ipsec_address(&dst_address, buf, sizeof(buf)),
+		    (u_long) ntohl(spi), sproto));
 		IPSEC_ISTAT(sproto, notdb);
 		m_freem(m);
 		return ENOENT;
@@ -219,10 +212,10 @@ ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 
 	if (sav->tdb_xform == NULL) {
 		DPRINTF(("%s: attempted to use uninitialized SA %s/%08lx/%u\n",
-			 __func__, ipsec_address(&dst_address),
-			 (u_long) ntohl(spi), sproto));
+		    __func__, ipsec_address(&dst_address, buf, sizeof(buf)),
+		    (u_long) ntohl(spi), sproto));
 		IPSEC_ISTAT(sproto, noxform);
-		KEY_FREESAV(&sav);
+		key_freesav(&sav);
 		m_freem(m);
 		return ENXIO;
 	}
@@ -232,59 +225,50 @@ ipsec_common_input(struct mbuf *m, int skip, int protoff, int af, int sproto)
 	 * everything else.
 	 */
 	error = (*sav->tdb_xform->xf_input)(m, sav, skip, protoff);
-	KEY_FREESAV(&sav);
-	return error;
+	if (error != 0)
+		key_freesav(&sav);
+	return (error);
 }
 
 #ifdef INET
+extern struct protosw inetsw[];
+
 /*
- * Common input handler for IPv4 AH, ESP, and IPCOMP.
+ * IPSEC_INPUT() method implementation for IPv4.
+ *  0 - Permitted by inbound security policy for further processing.
+ *  EACCES - Forbidden by inbound security policy.
+ *  EINPROGRESS - consumed by IPsec.
  */
 int
-ipsec4_common_input(struct mbuf *m, ...)
-{
-	va_list ap;
-	int off, nxt;
-
-	va_start(ap, m);
-	off = va_arg(ap, int);
-	nxt = va_arg(ap, int);
-	va_end(ap);
-
-	return ipsec_common_input(m, off, offsetof(struct ip, ip_p),
-				  AF_INET, nxt);
-}
-
-void
-ah4_input(struct mbuf *m, int off)
-{
-	ipsec4_common_input(m, off, IPPROTO_AH);
-}
-void
-ah4_ctlinput(int cmd, struct sockaddr *sa, void *v)
-{
-	if (sa->sa_family == AF_INET &&
-	    sa->sa_len == sizeof(struct sockaddr_in))
-		ipsec4_common_ctlinput(cmd, sa, v, IPPROTO_AH);
-}
-
-void
-esp4_input(struct mbuf *m, int off)
-{
-	ipsec4_common_input(m, off, IPPROTO_ESP);
-}
-void
-esp4_ctlinput(int cmd, struct sockaddr *sa, void *v)
+ipsec4_input(struct mbuf *m, int offset, int proto)
 {
-	if (sa->sa_family == AF_INET &&
-	    sa->sa_len == sizeof(struct sockaddr_in))
-		ipsec4_common_ctlinput(cmd, sa, v, IPPROTO_ESP);
-}
 
-void
-ipcomp4_input(struct mbuf *m, int off)
-{
-	ipsec4_common_input(m, off, IPPROTO_IPCOMP);
+	switch (proto) {
+	case IPPROTO_AH:
+	case IPPROTO_ESP:
+	case IPPROTO_IPCOMP:
+		/* Do inbound IPsec processing for AH/ESP/IPCOMP */
+		ipsec_common_input(m, offset,
+		    offsetof(struct ip, ip_p), AF_INET, proto);
+		return (EINPROGRESS); /* mbuf consumed by IPsec */
+	default:
+		/*
+		 * Protocols with further headers get their IPsec treatment
+		 * within the protocol specific processing.
+		 */
+		if ((inetsw[ip_protox[proto]].pr_flags & PR_LASTHDR) == 0)
+			return (0);
+		/* FALLTHROUGH */
+	};
+	/*
+	 * Enforce IPsec policy checking if we are seeing last header.
+	 */
+	if (ipsec4_in_reject(m, NULL) != 0) {
+		/* Forbidden by inbound security policy */
+		m_freem(m);
+		return (EACCES);
+	}
+	return (0);
 }
 
 /*
@@ -294,22 +278,17 @@ ipcomp4_input(struct mbuf *m, int off)
  * the processed packet.
  */
 int
-ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
-			int skip, int protoff, struct m_tag *mt)
+ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip,
+    int protoff)
 {
-	int prot, af, sproto;
-	struct ip *ip;
-	struct m_tag *mtag;
-	struct tdb_ident *tdbi;
+	char buf[IPSEC_ADDRSTRLEN];
+	struct ipsec_ctx_data ctx;
+	struct xform_history *xh;
 	struct secasindex *saidx;
-	int error;
-#ifdef INET6
-#ifdef notyet
-	char ip6buf[INET6_ADDRSTRLEN];
-#endif
-#endif
+	struct m_tag *mtag;
+	struct ip *ip;
+	int error, prot, af, sproto, isr_prot;
 
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	IPSEC_ASSERT(sav->sah != NULL, ("null SAH"));
 	saidx = &sav->sah->saidx;
@@ -320,20 +299,14 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 		sproto == IPPROTO_IPCOMP,
 		("unexpected security protocol %u", sproto));
 
-	/* Sanity check */
-	if (m == NULL) {
-		DPRINTF(("%s: null mbuf", __func__));
-		IPSEC_ISTAT(sproto, badkcr);
-		KEY_FREESAV(&sav);
-		return EINVAL;
-	}
-
 	if (skip != 0) {
-		/* Fix IPv4 header */
+		/*
+		 * Fix IPv4 header
+		 */
 		if (m->m_len < skip && (m = m_pullup(m, skip)) == NULL) {
 			DPRINTF(("%s: processing failed for SA %s/%08lx\n",
-			    __func__, ipsec_address(&sav->sah->saidx.dst),
-			    (u_long) ntohl(sav->spi)));
+			    __func__, ipsec_address(&sav->sah->saidx.dst,
+			    buf, sizeof(buf)), (u_long) ntohl(sav->spi)));
 			IPSEC_ISTAT(sproto, hdrops);
 			error = ENOBUFS;
 			goto bad;
@@ -341,106 +314,67 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 
 		ip = mtod(m, struct ip *);
 		ip->ip_len = htons(m->m_pkthdr.len);
-		ip->ip_off = htons(ip->ip_off);
 		ip->ip_sum = 0;
 		ip->ip_sum = in_cksum(m, ip->ip_hl << 2);
 	} else {
 		ip = mtod(m, struct ip *);
 	}
 	prot = ip->ip_p;
+	/*
+	 * Check that we have NAT-T enabled and apply transport mode
+	 * decapsulation NAT procedure (RFC3948).
+	 * Do this before invoking into the PFIL.
+	 */
+	if (sav->natt != NULL &&
+	    (prot == IPPROTO_UDP || prot == IPPROTO_TCP))
+		udp_ipsec_adjust_cksum(m, sav, prot, skip);
 
-#ifdef notyet
-	/* IP-in-IP encapsulation */
-	if (prot == IPPROTO_IPIP) {
-		struct ip ipn;
+	IPSEC_INIT_CTX(&ctx, &m, sav, AF_INET, IPSEC_ENC_BEFORE);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_IN)) != 0)
+		goto bad;
+	ip = mtod(m, struct ip *);	/* update pointer */
 
+	/* IP-in-IP encapsulation */
+	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);
-
-		/* XXX PROXY address isn't recorded in SAH */
-		/*
-		 * Check that the inner source address is the same as
-		 * the proxy address, if available.
-		 */
-		if ((saidx->proxy.sa.sa_family == AF_INET &&
-		    saidx->proxy.sin.sin_addr.s_addr !=
-		    INADDR_ANY &&
-		    ipn.ip_src.s_addr !=
-		    saidx->proxy.sin.sin_addr.s_addr) ||
-		    (saidx->proxy.sa.sa_family != AF_INET &&
-			saidx->proxy.sa.sa_family != 0)) {
-
-			DPRINTF(("%s: inner source address %s doesn't "
-			    "correspond to expected proxy source %s, "
-			    "SA %s/%08lx\n", __func__,
-			    inet_ntoa4(ipn.ip_src),
-			    ipsp_address(saidx->proxy),
-			    ipsp_address(saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-
-			IPSEC_ISTAT(sproto, pdrops);
-			error = EACCES;
-			goto bad;
-		}
+		/* enc0: strip outer IPv4 header */
+		m_striphdr(m, 0, ip->ip_hl << 2);
 	}
 #ifdef INET6
 	/* IPv6-in-IP encapsulation. */
-	if (prot == IPPROTO_IPV6) {
-		struct ip6_hdr ip6n;
-
+	else 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);
+	}
+#endif /* INET6 */
+	else if (prot != IPPROTO_IPV6 && saidx->mode == IPSEC_MODE_ANY) {
 		/*
-		 * Check that the inner source address is the same as
-		 * the proxy address, if available.
+		 * When mode is wildcard, inner protocol is IPv6 and
+		 * we have no INET6 support - drop this packet a bit later.
+		 * In other cases we assume transport mode. Set prot to
+		 * correctly choose netisr.
 		 */
-		if ((saidx->proxy.sa.sa_family == AF_INET6 &&
-		    !IN6_IS_ADDR_UNSPECIFIED(&saidx->proxy.sin6.sin6_addr) &&
-		    !IN6_ARE_ADDR_EQUAL(&ip6n.ip6_src,
-			&saidx->proxy.sin6.sin6_addr)) ||
-		    (saidx->proxy.sa.sa_family != AF_INET6 &&
-			saidx->proxy.sa.sa_family != 0)) {
-
-			DPRINTF(("%s: inner source address %s doesn't "
-			    "correspond to expected proxy source %s, "
-			    "SA %s/%08lx\n", __func__,
-			    ip6_sprintf(ip6buf, &ip6n.ip6_src),
-			    ipsec_address(&saidx->proxy),
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-
-			IPSEC_ISTAT(sproto, pdrops);
-			error = EACCES;
-			goto bad;
-		}
+		prot = IPPROTO_IPIP;
 	}
-#endif /* INET6 */
-#endif /*XXX*/
 
 	/*
 	 * Record what we've done to the packet (under what SA it was
-	 * processed). If we've been passed an mtag, it means the packet
-	 * was already processed by an ethernet/crypto combo card and
-	 * thus has a tag attached with all the right information, but
-	 * with a PACKET_TAG_IPSEC_IN_CRYPTO_DONE as opposed to
-	 * PACKET_TAG_IPSEC_IN_DONE type; in that case, just change the type.
+	 * processed).
 	 */
-	if (mt == NULL && sproto != IPPROTO_IPCOMP) {
+	if (sproto != IPPROTO_IPCOMP) {
 		mtag = m_tag_get(PACKET_TAG_IPSEC_IN_DONE,
-		    sizeof(struct tdb_ident), M_NOWAIT);
+		    sizeof(struct xform_history), M_NOWAIT);
 		if (mtag == NULL) {
 			DPRINTF(("%s: failed to get tag\n", __func__));
 			IPSEC_ISTAT(sproto, hdrops);
@@ -448,104 +382,106 @@ ipsec4_common_input_cb(struct mbuf *m, struct secasvar *sav,
 			goto bad;
 		}
 
-		tdbi = (struct tdb_ident *)(mtag + 1);
-		bcopy(&saidx->dst, &tdbi->dst, saidx->dst.sa.sa_len);
-		tdbi->proto = sproto;
-		tdbi->spi = sav->spi;
-		/* Cache those two for enc(4) in xform_ipip. */
-		tdbi->alg_auth = sav->alg_auth;
-		tdbi->alg_enc = sav->alg_enc;
-
+		xh = (struct xform_history *)(mtag + 1);
+		bcopy(&saidx->dst, &xh->dst, saidx->dst.sa.sa_len);
+		xh->spi = sav->spi;
+		xh->proto = sproto;
+		xh->mode = saidx->mode;
 		m_tag_prepend(m, mtag);
-	} else if (mt != NULL) {
-		mt->m_tag_id = PACKET_TAG_IPSEC_IN_DONE;
-		/* XXX do we need to mark m_flags??? */
 	}
 
 	key_sa_recordxfer(sav, m);		/* record data transfer */
 
-	m_addr_changed(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.
+	 * In transport mode requeue decrypted mbuf back to IPv4 protocol
+	 * handler. This is necessary to correctly expose rcvif.
 	 */
-	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
-
+	if (saidx->mode == IPSEC_MODE_TRANSPORT)
+		prot = IPPROTO_IPIP;
 	/*
 	 * Re-dispatch via software interrupt.
 	 */
-	if ((error = netisr_queue_src(NETISR_IP, (uintptr_t)sav->spi, m))) {
-		IPSEC_ISTAT(sproto, qfull);
-		DPRINTF(("%s: queue full; proto %u packet dropped\n",
-			__func__, sproto));
-		return error;
+	switch (prot) {
+	case IPPROTO_IPIP:
+		isr_prot = NETISR_IP;
+		af = AF_INET;
+		break;
+#ifdef INET6
+	case IPPROTO_IPV6:
+		isr_prot = NETISR_IPV6;
+		af = AF_INET6;
+		break;
+#endif
+	default:
+		DPRINTF(("%s: cannot handle inner ip proto %d\n",
+			    __func__, prot));
+		IPSEC_ISTAT(sproto, nopf);
+		error = EPFNOSUPPORT;
+		goto bad;
 	}
-	return 0;
-bad:
-	m_freem(m);
-	return error;
-}
 
-void
-ipsec4_common_ctlinput(int cmd, struct sockaddr *sa, void *v, int proto)
-{
-	/* XXX nothing just yet */
+	IPSEC_INIT_CTX(&ctx, &m, sav, af, IPSEC_ENC_AFTER);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_IN)) != 0)
+		goto bad;
+
+	/* Handle virtual tunneling interfaces */
+	if (saidx->mode == IPSEC_MODE_TUNNEL)
+		error = ipsec_if_input(m, sav, af);
+	if (error == 0) {
+		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));
+		}
+	}
+	key_freesav(&sav);
+	return (error);
+bad:
+	key_freesav(&sav);
+	if (m != NULL)
+		m_freem(m);
+	return (error);
 }
 #endif /* INET */
 
 #ifdef INET6
-/* IPv6 AH wrapper. */
+/*
+ * IPSEC_INPUT() method implementation for IPv6.
+ *  0 - Permitted by inbound security policy for further processing.
+ *  EACCES - Forbidden by inbound security policy.
+ *  EINPROGRESS - consumed by IPsec.
+ */
 int
-ipsec6_common_input(struct mbuf **mp, int *offp, int proto)
+ipsec6_input(struct mbuf *m, int offset, int proto)
 {
-	int l = 0;
-	int protoff;
-	struct ip6_ext ip6e;
-
-	if (*offp < sizeof(struct ip6_hdr)) {
-		DPRINTF(("%s: bad offset %u\n", __func__, *offp));
-		return IPPROTO_DONE;
-	} else if (*offp == sizeof(struct ip6_hdr)) {
-		protoff = offsetof(struct ip6_hdr, ip6_nxt);
-	} else {
-		/* Chase down the header chain... */
-		protoff = sizeof(struct ip6_hdr);
-
-		do {
-			protoff += l;
-			m_copydata(*mp, protoff, sizeof(ip6e),
-			    (caddr_t) &ip6e);
-
-			if (ip6e.ip6e_nxt == IPPROTO_AH)
-				l = (ip6e.ip6e_len + 2) << 2;
-			else
-				l = (ip6e.ip6e_len + 1) << 3;
-			IPSEC_ASSERT(l > 0, ("l went zero or negative"));
-		} while (protoff + l < *offp);
-
-		/* Malformed packet check */
-		if (protoff + l != *offp) {
-			DPRINTF(("%s: bad packet header chain, protoff %u, "
-				"l %u, off %u\n", __func__, protoff, l, *offp));
-			IPSEC_ISTAT(proto, hdrops);
-			m_freem(*mp);
-			*mp = NULL;
-			return IPPROTO_DONE;
-		}
-		protoff += offsetof(struct ip6_ext, ip6e_nxt);
+
+	switch (proto) {
+	case IPPROTO_AH:
+	case IPPROTO_ESP:
+	case IPPROTO_IPCOMP:
+		/* Do inbound IPsec processing for AH/ESP/IPCOMP */
+		ipsec_common_input(m, offset,
+		    offsetof(struct ip6_hdr, ip6_nxt), AF_INET6, proto);
+		return (EINPROGRESS); /* mbuf consumed by IPsec */
+	default:
+		/*
+		 * Protocols with further headers get their IPsec treatment
+		 * within the protocol specific processing.
+		 */
+		if ((inet6sw[ip6_protox[proto]].pr_flags & PR_LASTHDR) == 0)
+			return (0);
+		/* FALLTHROUGH */
+	};
+	/*
+	 * Enforce IPsec policy checking if we are seeing last header.
+	 */
+	if (ipsec6_in_reject(m, NULL) != 0) {
+		/* Forbidden by inbound security policy */
+		m_freem(m);
+		return (EACCES);
 	}
-	(void) ipsec_common_input(*mp, *offp, protoff, AF_INET6, proto);
-	return IPPROTO_DONE;
+	return (0);
 }
 
 /*
@@ -553,22 +489,20 @@ ipsec6_common_input(struct mbuf **mp, int *offp, int proto)
  * filtering and other sanity checks on the processed packet.
  */
 int
-ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int protoff,
-    struct m_tag *mt)
+ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip,
+    int protoff)
 {
-	int prot, af, sproto;
+	char buf[IPSEC_ADDRSTRLEN];
+	struct ipsec_ctx_data ctx;
+	struct xform_history *xh;
+	struct secasindex *saidx;
 	struct ip6_hdr *ip6;
 	struct m_tag *mtag;
-	struct tdb_ident *tdbi;
-	struct secasindex *saidx;
-	int nxt;
-	u_int8_t nxt8;
+	int prot, af, sproto;
+	int nxt, isr_prot;
 	int error, nest;
-#ifdef notyet
-	char ip6buf[INET6_ADDRSTRLEN];
-#endif
+	uint8_t nxt8;
 
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	IPSEC_ASSERT(sav->sah != NULL, ("null SAH"));
 	saidx = &sav->sah->saidx;
@@ -579,122 +513,67 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 		sproto == IPPROTO_IPCOMP,
 		("unexpected security protocol %u", sproto));
 
-	/* Sanity check */
-	if (m == NULL) {
-		DPRINTF(("%s: null mbuf", __func__));
-		IPSEC_ISTAT(sproto, badkcr);
-		error = EINVAL;
-		goto bad;
-	}
-
 	/* Fix IPv6 header */
 	if (m->m_len < sizeof(struct ip6_hdr) &&
 	    (m = m_pullup(m, sizeof(struct ip6_hdr))) == NULL) {
 
 		DPRINTF(("%s: processing failed for SA %s/%08lx\n",
-		    __func__, ipsec_address(&sav->sah->saidx.dst),
-		    (u_long) ntohl(sav->spi)));
+		    __func__, ipsec_address(&sav->sah->saidx.dst, buf,
+		    sizeof(buf)), (u_long) ntohl(sav->spi)));
 
 		IPSEC_ISTAT(sproto, hdrops);
 		error = EACCES;
 		goto bad;
 	}
 
+	IPSEC_INIT_CTX(&ctx, &m, sav, af, IPSEC_ENC_BEFORE);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_IN)) != 0)
+		goto bad;
+
 	ip6 = mtod(m, struct ip6_hdr *);
 	ip6->ip6_plen = htons(m->m_pkthdr.len - sizeof(struct ip6_hdr));
 
 	/* Save protocol */
-	m_copydata(m, protoff, 1, (unsigned char *) &prot);
+	m_copydata(m, protoff, 1, &nxt8);
+	prot = nxt8;
 
-#ifdef notyet
+	/* IPv6-in-IP encapsulation */
+	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_striphdr(m, 0, skip);
+		skip = 0;
+	}
 #ifdef INET
 	/* IP-in-IP encapsulation */
-	if (prot == IPPROTO_IPIP) {
-		struct ip ipn;
-
+	else 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, skip, sizeof(struct ip), (caddr_t) &ipn);
-
-		/*
-		 * Check that the inner source address is the same as
-		 * the proxy address, if available.
-		 */
-		if ((saidx->proxy.sa.sa_family == AF_INET &&
-		    saidx->proxy.sin.sin_addr.s_addr != INADDR_ANY &&
-		    ipn.ip_src.s_addr != saidx->proxy.sin.sin_addr.s_addr) ||
-		    (saidx->proxy.sa.sa_family != AF_INET &&
-			saidx->proxy.sa.sa_family != 0)) {
-
-			DPRINTF(("%s: inner source address %s doesn't "
-			    "correspond to expected proxy source %s, "
-			    "SA %s/%08lx\n", __func__,
-			    inet_ntoa4(ipn.ip_src),
-			    ipsec_address(&saidx->proxy),
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-
-			IPSEC_ISTAT(sproto, pdrops);
-			error = EACCES;
-			goto bad;
-		}
+		m_striphdr(m, 0, skip);
+		skip = 0;
 	}
 #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);
-
-		/*
-		 * Check that the inner source address is the same as
-		 * the proxy address, if available.
-		 */
-		if ((saidx->proxy.sa.sa_family == AF_INET6 &&
-		    !IN6_IS_ADDR_UNSPECIFIED(&saidx->proxy.sin6.sin6_addr) &&
-		    !IN6_ARE_ADDR_EQUAL(&ip6n.ip6_src,
-			&saidx->proxy.sin6.sin6_addr)) ||
-		    (saidx->proxy.sa.sa_family != AF_INET6 &&
-			saidx->proxy.sa.sa_family != 0)) {
-
-			DPRINTF(("%s: inner source address %s doesn't "
-			    "correspond to expected proxy source %s, "
-			    "SA %s/%08lx\n", __func__,
-			    ip6_sprintf(ip6buf, &ip6n.ip6_src),
-			    ipsec_address(&saidx->proxy),
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-
-			IPSEC_ISTAT(sproto, pdrops);
-			error = EACCES;
-			goto bad;
-		}
+	else {
+		prot = IPPROTO_IPV6; /* for correct BPF processing */
 	}
-#endif /*XXX*/
 
 	/*
 	 * Record what we've done to the packet (under what SA it was
-	 * processed). If we've been passed an mtag, it means the packet
-	 * was already processed by an ethernet/crypto combo card and
-	 * thus has a tag attached with all the right information, but
-	 * with a PACKET_TAG_IPSEC_IN_CRYPTO_DONE as opposed to
-	 * PACKET_TAG_IPSEC_IN_DONE type; in that case, just change the type.
+	 * processed).
 	 */
-	if (mt == NULL && sproto != IPPROTO_IPCOMP) {
+	if (sproto != IPPROTO_IPCOMP) {
 		mtag = m_tag_get(PACKET_TAG_IPSEC_IN_DONE,
-		    sizeof(struct tdb_ident), M_NOWAIT);
+		    sizeof(struct xform_history), M_NOWAIT);
 		if (mtag == NULL) {
 			DPRINTF(("%s: failed to get tag\n", __func__));
 			IPSEC_ISTAT(sproto, hdrops);
@@ -702,42 +581,61 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 			goto bad;
 		}
 
-		tdbi = (struct tdb_ident *)(mtag + 1);
-		bcopy(&saidx->dst, &tdbi->dst, sizeof(union sockaddr_union));
-		tdbi->proto = sproto;
-		tdbi->spi = sav->spi;
-		/* Cache those two for enc(4) in xform_ipip. */
-		tdbi->alg_auth = sav->alg_auth;
-		tdbi->alg_enc = sav->alg_enc;
-
+		xh = (struct xform_history *)(mtag + 1);
+		bcopy(&saidx->dst, &xh->dst, saidx->dst.sa.sa_len);
+		xh->spi = sav->spi;
+		xh->proto = sproto;
+		xh->mode = saidx->mode;
 		m_tag_prepend(m, mtag);
-	} else {
-		if (mt != NULL)
-			mt->m_tag_id = PACKET_TAG_IPSEC_IN_DONE;
-		/* XXX do we need to mark m_flags??? */
 	}
 
 	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.
-	 */
-	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)
+		af = AF_INET;
+	else
 #endif
-
-	/* Retrieve new protocol */
-	m_copydata(m, protoff, sizeof(u_int8_t), (caddr_t) &nxt8);
-
+		af = AF_INET6;
+	IPSEC_INIT_CTX(&ctx, &m, sav, af, IPSEC_ENC_AFTER);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_IN)) != 0)
+		goto bad;
+	if (skip == 0) {
+		/*
+		 * We stripped outer IPv6 header.
+		 * Now we should requeue decrypted packet via netisr.
+		 */
+		switch (prot) {
+#ifdef INET
+		case IPPROTO_IPIP:
+			isr_prot = NETISR_IP;
+			break;
+#endif
+		case IPPROTO_IPV6:
+			isr_prot = NETISR_IPV6;
+			break;
+		default:
+			DPRINTF(("%s: cannot handle inner ip proto %d\n",
+			    __func__, prot));
+			IPSEC_ISTAT(sproto, nopf);
+			error = EPFNOSUPPORT;
+			goto bad;
+		}
+		/* Handle virtual tunneling interfaces */
+		if (saidx->mode == IPSEC_MODE_TUNNEL)
+			error = ipsec_if_input(m, sav, af);
+		if (error == 0) {
+			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));
+			}
+		}
+		key_freesav(&sav);
+		return (error);
+	}
 	/*
 	 * See the end of ip6_input for this logic.
 	 * IPPROTO_IPV[46] case will be processed just like other ones
@@ -773,99 +671,12 @@ ipsec6_common_input_cb(struct mbuf *m, struct secasvar *sav, int skip, int proto
 		}
 		nxt = (*inet6sw[ip6_protox[nxt]].pr_input)(&m, &skip, nxt);
 	}
-	return 0;
+	key_freesav(&sav);
+	return (0);
 bad:
+	key_freesav(&sav);
 	if (m)
 		m_freem(m);
-	return error;
-}
-
-void
-esp6_ctlinput(int cmd, struct sockaddr *sa, void *d)
-{
-	struct ip6ctlparam *ip6cp = NULL;
-	struct mbuf *m = NULL;
-	struct ip6_hdr *ip6;
-	int off;
-
-	if (sa->sa_family != AF_INET6 ||
-	    sa->sa_len != sizeof(struct sockaddr_in6))
-		return;
-	if ((unsigned)cmd >= PRC_NCMDS)
-		return;
-
-	/* if the parameter is from icmp6, decode it. */
-	if (d != NULL) {
-		ip6cp = (struct ip6ctlparam *)d;
-		m = ip6cp->ip6c_m;
-		ip6 = ip6cp->ip6c_ip6;
-		off = ip6cp->ip6c_off;
-	} else {
-		m = NULL;
-		ip6 = NULL;
-		off = 0;	/* calm gcc */
-	}
-
-	if (ip6 != NULL) {
-
-		struct ip6ctlparam ip6cp1;
-
-		/*
-		 * Notify the error to all possible sockets via pfctlinput2.
-		 * Since the upper layer information (such as protocol type,
-		 * source and destination ports) is embedded in the encrypted
-		 * data and might have been cut, we can't directly call
-		 * an upper layer ctlinput function. However, the pcbnotify
-		 * function will consider source and destination addresses
-		 * as well as the flow info value, and may be able to find
-		 * some PCB that should be notified.
-		 * Although pfctlinput2 will call esp6_ctlinput(), there is
-		 * no possibility of an infinite loop of function calls,
-		 * because we don't pass the inner IPv6 header.
-		 */
-		bzero(&ip6cp1, sizeof(ip6cp1));
-		ip6cp1.ip6c_src = ip6cp->ip6c_src;
-		pfctlinput2(cmd, sa, (void *)&ip6cp1);
-
-		/*
-		 * Then go to special cases that need ESP header information.
-		 * XXX: We assume that when ip6 is non NULL,
-		 * M and OFF are valid.
-		 */
-
-		if (cmd == PRC_MSGSIZE) {
-			struct secasvar *sav;
-			u_int32_t spi;
-			int valid;
-
-			/* check header length before using m_copydata */
-			if (m->m_pkthdr.len < off + sizeof (struct esp))
-				return;
-			m_copydata(m, off + offsetof(struct esp, esp_spi),
-				sizeof(u_int32_t), (caddr_t) &spi);
-			/*
-			 * Check to see if we have a valid SA corresponding to
-			 * the address in the ICMP message payload.
-			 */
-			sav = KEY_ALLOCSA((union sockaddr_union *)sa,
-					IPPROTO_ESP, spi);
-			valid = (sav != NULL);
-			if (sav)
-				KEY_FREESAV(&sav);
-
-			/* XXX Further validation? */
-
-			/*
-			 * Depending on whether the SA is "valid" and
-			 * routing table size (mtudisc_{hi,lo}wat), we will:
-			 * - recalcurate the new MTU and create the
-			 *   corresponding routing entry, or
-			 * - ignore the MTU change notification.
-			 */
-			icmp6_mtudisc_update(ip6cp, valid);
-		}
-	} else {
-		/* we normally notify any pcb here */
-	}
+	return (error);
 }
 #endif /* INET6 */
diff --git a/freebsd/sys/netipsec/ipsec_mbuf.c b/freebsd/sys/netipsec/ipsec_mbuf.c
index 2cafe058..d81c0deb 100644
--- a/freebsd/sys/netipsec/ipsec_mbuf.c
+++ b/freebsd/sys/netipsec/ipsec_mbuf.c
@@ -32,18 +32,14 @@
  * IPsec-specific mbuf routines.
  */
 
-#include <rtems/bsd/local/opt_param.h>
-
 #include <rtems/bsd/sys/param.h>
 #include <sys/systm.h>
+#include <sys/malloc.h>
 #include <sys/mbuf.h>
 #include <sys/socket.h>
 
-#include <net/route.h>
 #include <net/vnet.h>
-
 #include <netinet/in.h>
-
 #include <netipsec/ipsec.h>
 
 /*
@@ -76,7 +72,21 @@ m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
 	 * the contents of m as needed.
 	 */
 	remain = m->m_len - skip;		/* data to move */
-	if (hlen > M_TRAILINGSPACE(m)) {
+	if (remain > skip &&
+	    hlen + max_linkhdr < M_LEADINGSPACE(m)) {
+		/*
+		 * mbuf has enough free space at the beginning.
+		 * XXX: which operation is the most heavy - copying of
+		 *	possible several hundred of bytes or allocation
+		 *	of new mbuf? We can remove max_linkhdr check
+		 *	here, but it is possible that this will lead
+		 *	to allocation of new mbuf in Layer 2 code.
+		 */
+		m->m_data -= hlen;
+		bcopy(mtodo(m, hlen), mtod(m, caddr_t), skip);
+		m->m_len += hlen;
+		*off = skip;
+	} else if (hlen > M_TRAILINGSPACE(m)) {
 		struct mbuf *n0, *n, **np;
 		int todo, len, done, alloc;
 
@@ -87,11 +97,11 @@ m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
 		todo = remain;
 		while (todo > 0) {
 			if (todo > MHLEN) {
-				n = m_getcl(M_DONTWAIT, m->m_type, 0);
+				n = m_getcl(M_NOWAIT, m->m_type, 0);
 				len = MCLBYTES;
 			}
 			else {
-				n = m_get(M_DONTWAIT, m->m_type);
+				n = m_get(M_NOWAIT, m->m_type);
 				len = MHLEN;
 			}
 			if (n == NULL) {
@@ -117,7 +127,7 @@ m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
 			}
 		}
 		else {
-			n = m_get(M_DONTWAIT, m->m_type);
+			n = m_get(M_NOWAIT, m->m_type);
 			if (n == NULL) {
 				m_freem(n0);
 				return NULL;
@@ -144,7 +154,7 @@ m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
 		 * so there's space to write the new header.
 		 */
 		bcopy(mtod(m, caddr_t) + skip,
-		      mtod(m, caddr_t) + skip + hlen, remain);
+		    mtod(m, caddr_t) + skip + hlen, remain);
 		m->m_len += hlen;
 		*off = skip;
 	}
@@ -205,8 +215,8 @@ m_pad(struct mbuf *m, int n)
 
 	if (pad > M_TRAILINGSPACE(m0)) {
 		/* Add an mbuf to the chain. */
-		MGET(m1, M_DONTWAIT, MT_DATA);
-		if (m1 == 0) {
+		MGET(m1, M_NOWAIT, MT_DATA);
+		if (m1 == NULL) {
 			m_freem(m0);
 			DPRINTF(("%s: unable to get extra mbuf\n", __func__));
 			return NULL;
diff --git a/freebsd/sys/netipsec/ipsec_mod.c b/freebsd/sys/netipsec/ipsec_mod.c
new file mode 100644
index 00000000..78b9f1da
--- /dev/null
+++ b/freebsd/sys/netipsec/ipsec_mod.c
@@ -0,0 +1,150 @@
+#include <machine/rtems-bsd-kernel-space.h>
+
+/*-
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <rtems/bsd/local/opt_inet.h>
+#include <rtems/bsd/local/opt_inet6.h>
+#include <rtems/bsd/local/opt_ipsec.h>
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <rtems/bsd/sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/module.h>
+#include <sys/priv.h>
+#include <sys/rmlock.h>
+#include <sys/socket.h>
+#include <sys/sockopt.h>
+#include <sys/syslog.h>
+#include <sys/proc.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+
+#include <netipsec/ipsec.h>
+#include <netipsec/ipsec6.h>
+#include <netipsec/key.h>
+#include <netipsec/key_debug.h>
+
+#include <netipsec/ipsec_support.h>
+
+#ifdef INET
+static const struct ipsec_methods ipv4_methods = {
+	.input = ipsec4_input,
+	.forward = ipsec4_forward,
+	.output = ipsec4_output,
+	.pcbctl = ipsec4_pcbctl,
+	.capability = ipsec4_capability,
+	.check_policy = ipsec4_in_reject,
+	.hdrsize = ipsec_hdrsiz_inpcb,
+	.udp_input = udp_ipsec_input,
+	.udp_pcbctl = udp_ipsec_pcbctl,
+};
+#ifndef KLD_MODULE
+static const struct ipsec_support ipv4_ipsec = {
+	.enabled = IPSEC_MODULE_ENABLED,
+	.methods = &ipv4_methods
+};
+const struct ipsec_support * const ipv4_ipsec_support = &ipv4_ipsec;
+#endif /* !KLD_MODULE */
+#endif /* INET */
+
+#ifdef INET6
+static const struct ipsec_methods ipv6_methods = {
+	.input = ipsec6_input,
+	.forward = ipsec6_forward,
+	.output = ipsec6_output,
+	.pcbctl = ipsec6_pcbctl,
+	.capability = ipsec6_capability,
+	.check_policy = ipsec6_in_reject,
+	.hdrsize = ipsec_hdrsiz_inpcb,
+};
+#ifndef KLD_MODULE
+static const struct ipsec_support ipv6_ipsec = {
+	.enabled = IPSEC_MODULE_ENABLED,
+	.methods = &ipv6_methods
+};
+const struct ipsec_support * const ipv6_ipsec_support = &ipv6_ipsec;
+#endif /* !KLD_MODULE */
+#endif /* INET6 */
+
+/*
+ * Always register ipsec module.
+ * Even when IPsec is build in the kernel, we need to have
+ * module registered. This will prevent to load ipsec.ko.
+ */
+static int
+ipsec_modevent(module_t mod, int type, void *data)
+{
+
+	switch (type) {
+	case MOD_LOAD:
+		/* All xforms are registered via SYSINIT */
+		if (!ipsec_initialized())
+			return (ENOMEM);
+#ifdef KLD_MODULE
+#ifdef INET
+		ipsec_support_enable(ipv4_ipsec_support, &ipv4_methods);
+#endif
+#ifdef INET6
+		ipsec_support_enable(ipv6_ipsec_support, &ipv6_methods);
+#endif
+#endif /* KLD_MODULE */
+		break;
+	case MOD_UNLOAD:
+		/* All xforms are unregistered via SYSUNINIT */
+#ifdef KLD_MODULE
+#ifdef INET
+		ipsec_support_disable(ipv4_ipsec_support);
+#endif
+#ifdef INET6
+		ipsec_support_disable(ipv6_ipsec_support);
+#endif
+#endif /* KLD_MODULE */
+		break;
+	default:
+		return (EOPNOTSUPP);
+	}
+	return (0);
+}
+
+static moduledata_t ipsec_mod = {
+	"ipsec",
+	ipsec_modevent,
+	0
+};
+
+DECLARE_MODULE(ipsec, ipsec_mod, SI_SUB_PROTO_DOMAIN, SI_ORDER_ANY);
+MODULE_VERSION(ipsec, 1);
+#ifdef KLD_MODULE
+MODULE_DEPEND(ipsec, ipsec_support, 1, 1, 1);
+#endif
diff --git a/freebsd/sys/netipsec/ipsec_output.c b/freebsd/sys/netipsec/ipsec_output.c
index a02b6ce2..3403b0bc 100644
--- a/freebsd/sys/netipsec/ipsec_output.c
+++ b/freebsd/sys/netipsec/ipsec_output.c
@@ -2,6 +2,7 @@
 
 /*-
  * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -34,7 +35,7 @@
 #include <rtems/bsd/local/opt_inet.h>
 #include <rtems/bsd/local/opt_inet6.h>
 #include <rtems/bsd/local/opt_ipsec.h>
-#include <rtems/bsd/local/opt_enc.h>
+#include <rtems/bsd/local/opt_sctp.h>
 
 #include <rtems/bsd/sys/param.h>
 #include <sys/systm.h>
@@ -43,11 +44,12 @@
 #include <sys/protosw.h>
 #include <sys/socket.h>
 #include <rtems/bsd/sys/errno.h>
+#include <sys/hhook.h>
 #include <sys/syslog.h>
 
 #include <net/if.h>
-#include <net/pfil.h>
-#include <net/route.h>
+#include <net/if_enc.h>
+#include <net/if_var.h>
 #include <net/vnet.h>
 
 #include <netinet/in.h>
@@ -63,12 +65,19 @@
 #include <netinet/ip6.h>
 #ifdef INET6
 #include <netinet6/ip6_var.h>
+#include <netinet6/scope6_var.h>
 #endif
 #include <netinet/in_pcb.h>
 #ifdef INET6
 #include <netinet/icmp6.h>
 #endif
+#ifdef SCTP
+#include <netinet/sctp_crc32.h>
+#endif
 
+#include <netinet/udp.h>
+#include <netipsec/ah.h>
+#include <netipsec/esp.h>
 #include <netipsec/ipsec.h>
 #ifdef INET6
 #include <netipsec/ipsec6.h>
@@ -85,834 +94,880 @@
 
 #include <machine/in_cksum.h>
 
-#ifdef IPSEC_NAT_T
-#include <netinet/udp.h>
-#endif
-
-#ifdef DEV_ENC
-#include <net/if_enc.h>
-#endif
+#define	IPSEC_OSTAT_INC(proto, name)	do {		\
+	if ((proto) == IPPROTO_ESP)	\
+		ESPSTAT_INC(esps_##name);	\
+	else if ((proto) == IPPROTO_AH)\
+		AHSTAT_INC(ahs_##name);		\
+	else					\
+		IPCOMPSTAT_INC(ipcomps_##name);	\
+} while (0)
 
+static int ipsec_encap(struct mbuf **mp, struct secasindex *saidx);
 
-int
-ipsec_process_done(struct mbuf *m, struct ipsecrequest *isr)
+#ifdef INET
+static struct secasvar *
+ipsec4_allocsa(struct mbuf *m, struct secpolicy *sp, u_int *pidx, int *error)
 {
-	struct tdb_ident *tdbi;
-	struct m_tag *mtag;
+	struct secasindex *saidx, tmpsaidx;
+	struct ipsecrequest *isr;
+	struct sockaddr_in *sin;
 	struct secasvar *sav;
-	struct secasindex *saidx;
-	int error;
-
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-	IPSEC_ASSERT(isr != NULL, ("null ISR"));
-	sav = isr->sav;
-	IPSEC_ASSERT(sav != NULL, ("null SA"));
-	IPSEC_ASSERT(sav->sah != NULL, ("null SAH"));
+	struct ip *ip;
 
-	saidx = &sav->sah->saidx;
-	switch (saidx->dst.sa.sa_family) {
-#ifdef INET
-	case AF_INET:
-		/* Fix the header length, for AH processing. */
-		mtod(m, struct ip *)->ip_len = htons(m->m_pkthdr.len);
-		break;
-#endif /* INET */
-#ifdef INET6
-	case AF_INET6:
-		/* Fix the header length, for AH processing. */
-		if (m->m_pkthdr.len < sizeof (struct ip6_hdr)) {
-			error = ENXIO;
-			goto bad;
+	/*
+	 * Check system global policy controls.
+	 */
+next:
+	isr = sp->req[*pidx];
+	if ((isr->saidx.proto == IPPROTO_ESP && !V_esp_enable) ||
+	    (isr->saidx.proto == IPPROTO_AH && !V_ah_enable) ||
+	    (isr->saidx.proto == IPPROTO_IPCOMP && !V_ipcomp_enable)) {
+		DPRINTF(("%s: IPsec outbound packet dropped due"
+			" to policy (check your sysctls)\n", __func__));
+		IPSEC_OSTAT_INC(isr->saidx.proto, pdrops);
+		*error = EHOSTUNREACH;
+		return (NULL);
+	}
+	/*
+	 * Craft SA index to search for proper SA.  Note that
+	 * we only initialize unspecified SA peers for transport
+	 * mode; for tunnel mode they must already be filled in.
+	 */
+	if (isr->saidx.mode == IPSEC_MODE_TRANSPORT) {
+		saidx = &tmpsaidx;
+		*saidx = isr->saidx;
+		ip = mtod(m, struct ip *);
+		if (saidx->src.sa.sa_len == 0) {
+			sin = &saidx->src.sin;
+			sin->sin_len = sizeof(*sin);
+			sin->sin_family = AF_INET;
+			sin->sin_port = IPSEC_PORT_ANY;
+			sin->sin_addr = ip->ip_src;
 		}
-		if (m->m_pkthdr.len - sizeof (struct ip6_hdr) > IPV6_MAXPACKET) {
-			/* No jumbogram support. */
-			error = ENXIO;	/*?*/
-			goto bad;
+		if (saidx->dst.sa.sa_len == 0) {
+			sin = &saidx->dst.sin;
+			sin->sin_len = sizeof(*sin);
+			sin->sin_family = AF_INET;
+			sin->sin_port = IPSEC_PORT_ANY;
+			sin->sin_addr = ip->ip_dst;
 		}
-		mtod(m, struct ip6_hdr *)->ip6_plen =
-			htons(m->m_pkthdr.len - sizeof(struct ip6_hdr));
-		break;
-#endif /* INET6 */
-	default:
-		DPRINTF(("%s: unknown protocol family %u\n", __func__,
-		    saidx->dst.sa.sa_family));
-		error = ENXIO;
-		goto bad;
+	} else
+		saidx = &sp->req[*pidx]->saidx;
+	/*
+	 * Lookup SA and validate it.
+	 */
+	sav = key_allocsa_policy(sp, saidx, error);
+	if (sav == NULL) {
+		IPSECSTAT_INC(ips_out_nosa);
+		if (*error != 0)
+			return (NULL);
+		if (ipsec_get_reqlevel(sp, *pidx) != IPSEC_LEVEL_REQUIRE) {
+			/*
+			 * We have no SA and policy that doesn't require
+			 * this IPsec transform, thus we can continue w/o
+			 * IPsec processing, i.e. return EJUSTRETURN.
+			 * But first check if there is some bundled transform.
+			 */
+			if (sp->tcount > ++(*pidx))
+				goto next;
+			*error = EJUSTRETURN;
+		}
+		return (NULL);
 	}
+	IPSEC_ASSERT(sav->tdb_xform != NULL, ("SA with NULL tdb_xform"));
+	return (sav);
+}
+
+/*
+ * IPsec output logic for IPv4.
+ */
+static int
+ipsec4_perform_request(struct mbuf *m, struct secpolicy *sp, u_int idx)
+{
+	char sbuf[IPSEC_ADDRSTRLEN], dbuf[IPSEC_ADDRSTRLEN];
+	struct ipsec_ctx_data ctx;
+	union sockaddr_union *dst;
+	struct secasvar *sav;
+	struct ip *ip;
+	int error, i, off;
+
+	IPSEC_ASSERT(idx < sp->tcount, ("Wrong IPsec request index %d", idx));
 
 	/*
-	 * Add a record of what we've done or what needs to be done to the
-	 * packet.
+	 * We hold the reference to SP. Content of SP couldn't be changed.
+	 * Craft secasindex and do lookup for suitable SA.
+	 * Then do encapsulation if needed and call xform's output.
+	 * We need to store SP in the xform callback parameters.
+	 * In xform callback we will extract SP and it can be used to
+	 * determine next transform. At the end of transform we can
+	 * release reference to SP.
 	 */
-	mtag = m_tag_get(PACKET_TAG_IPSEC_OUT_DONE,
-			sizeof(struct tdb_ident), M_NOWAIT);
-	if (mtag == NULL) {
-		DPRINTF(("%s: could not get packet tag\n", __func__));
-		error = ENOMEM;
+	sav = ipsec4_allocsa(m, sp, &idx, &error);
+	if (sav == NULL) {
+		if (error == EJUSTRETURN) { /* No IPsec required */
+			key_freesp(&sp);
+			return (error);
+		}
 		goto bad;
 	}
-
-	tdbi = (struct tdb_ident *)(mtag + 1);
-	tdbi->dst = saidx->dst;
-	tdbi->proto = saidx->proto;
-	tdbi->spi = sav->spi;
-	m_tag_prepend(m, mtag);
-
 	/*
-	 * If there's another (bundled) SA to apply, do so.
-	 * Note that this puts a burden on the kernel stack size.
-	 * If this is a problem we'll need to introduce a queue
-	 * to set the packet on so we can unwind the stack before
-	 * doing further processing.
+	 * XXXAE: most likely ip_sum at this point is wrong.
 	 */
-	if (isr->next) {
-		IPSECSTAT_INC(ips_out_bundlesa);
-		/* XXX-BZ currently only support same AF bundles. */
-		switch (saidx->dst.sa.sa_family) {
-#ifdef INET
-		case AF_INET:
-			return ipsec4_process_packet(m, isr->next, 0, 0);
-			/* NOTREACHED */
-#endif
-#ifdef notyet
-#ifdef INET6
-		case AF_INET6:
-			/* XXX */
-			ipsec6_output_trans()
-			ipsec6_output_tunnel()
-			/* NOTREACHED */
-#endif /* INET6 */
-#endif
-		default:
-			DPRINTF(("%s: unknown protocol family %u\n", __func__,
-			    saidx->dst.sa.sa_family));
-			error = ENXIO;
+	IPSEC_INIT_CTX(&ctx, &m, sav, AF_INET, IPSEC_ENC_BEFORE);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_OUT)) != 0)
+		goto bad;
+
+	ip = mtod(m, struct ip *);
+	dst = &sav->sah->saidx.dst;
+	/* Do the appropriate encapsulation, if necessary */
+	if (sp->req[idx]->saidx.mode == IPSEC_MODE_TUNNEL || /* Tunnel requ'd */
+	    dst->sa.sa_family != AF_INET ||	    /* PF mismatch */
+	    (dst->sa.sa_family == AF_INET &&	    /* Proxy */
+	     dst->sin.sin_addr.s_addr != INADDR_ANY &&
+	     dst->sin.sin_addr.s_addr != ip->ip_dst.s_addr)) {
+		/* Fix IPv4 header checksum and length */
+		ip->ip_len = htons(m->m_pkthdr.len);
+		ip->ip_sum = 0;
+		ip->ip_sum = in_cksum(m, ip->ip_hl << 2);
+		error = ipsec_encap(&m, &sav->sah->saidx);
+		if (error != 0) {
+			DPRINTF(("%s: encapsulation for SA %s->%s "
+			    "SPI 0x%08x failed with error %d\n", __func__,
+			    ipsec_address(&sav->sah->saidx.src, sbuf,
+				sizeof(sbuf)),
+			    ipsec_address(&sav->sah->saidx.dst, dbuf,
+				sizeof(dbuf)), ntohl(sav->spi), error));
+			/* XXXAE: IPSEC_OSTAT_INC(tunnel); */
 			goto bad;
 		}
 	}
-	key_sa_recordxfer(sav, m);		/* record data transfer */
 
-	m_addr_changed(m);
+	IPSEC_INIT_CTX(&ctx, &m, sav, dst->sa.sa_family, IPSEC_ENC_AFTER);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_OUT)) != 0)
+		goto bad;
 
 	/*
-	 * We're done with IPsec processing, transmit the packet using the
-	 * appropriate network protocol (IP or IPv6). SPD lookup will be
-	 * performed again there.
+	 * Dispatch to the appropriate IPsec transform logic.  The
+	 * packet will be returned for transmission after crypto
+	 * processing, etc. are completed.
+	 *
+	 * NB: m & sav are ``passed to caller'' who's responsible for
+	 *     reclaiming their resources.
 	 */
-	switch (saidx->dst.sa.sa_family) {
-#ifdef INET
-	struct ip *ip;
+	switch(dst->sa.sa_family) {
 	case AF_INET:
 		ip = mtod(m, struct ip *);
-		ip->ip_len = ntohs(ip->ip_len);
-		ip->ip_off = ntohs(ip->ip_off);
-
-#ifdef IPSEC_NAT_T
-		/*
-		 * If NAT-T is enabled, now that all IPsec processing is done
-		 * insert UDP encapsulation header after IP header.
-		 */
-		if (sav->natt_type) {
-#ifdef _IP_VHL
-			const int hlen = IP_VHL_HL(ip->ip_vhl);
-#else
-			const int hlen = (ip->ip_hl << 2);
-#endif
-			int size, off;
-			struct mbuf *mi;
-			struct udphdr *udp;
-
-			size = sizeof(struct udphdr);
-			if (sav->natt_type == UDP_ENCAP_ESPINUDP_NON_IKE) {
-				/*
-				 * draft-ietf-ipsec-nat-t-ike-0[01].txt and
-				 * draft-ietf-ipsec-udp-encaps-(00/)01.txt,
-				 * ignoring possible AH mode
-				 * non-IKE marker + non-ESP marker
-				 * from draft-ietf-ipsec-udp-encaps-00.txt.
-				 */
-				size += sizeof(u_int64_t);
-			}
-			mi = m_makespace(m, hlen, size, &off);
-			if (mi == NULL) {
-				DPRINTF(("%s: m_makespace for udphdr failed\n",
-				    __func__));
-				error = ENOBUFS;
-				goto bad;
-			}
-
-			udp = (struct udphdr *)(mtod(mi, caddr_t) + off);
-			if (sav->natt_type == UDP_ENCAP_ESPINUDP_NON_IKE)
-				udp->uh_sport = htons(UDP_ENCAP_ESPINUDP_PORT);
-			else
-				udp->uh_sport =
-					KEY_PORTFROMSADDR(&sav->sah->saidx.src);
-			udp->uh_dport = KEY_PORTFROMSADDR(&sav->sah->saidx.dst);
-			udp->uh_sum = 0;
-			udp->uh_ulen = htons(m->m_pkthdr.len - hlen);
-			ip->ip_len = m->m_pkthdr.len;
-			ip->ip_p = IPPROTO_UDP;
-
-			if (sav->natt_type == UDP_ENCAP_ESPINUDP_NON_IKE)
-				*(u_int64_t *)(udp + 1) = 0;
-		}
-#endif /* IPSEC_NAT_T */
-
-		return ip_output(m, NULL, NULL, IP_RAWOUTPUT, NULL, NULL);
-#endif /* INET */
+		i = ip->ip_hl << 2;
+		off = offsetof(struct ip, ip_p);
+		break;
 #ifdef INET6
 	case AF_INET6:
-		/*
-		 * We don't need massage, IPv6 header fields are always in
-		 * net endian.
-		 */
-		return ip6_output(m, NULL, NULL, 0, NULL, NULL, NULL);
+		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;
+		IPSEC_OSTAT_INC(sav->sah->saidx.proto, nopf);
+		goto bad;
 	}
-	panic("ipsec_process_done");
+	error = (*sav->tdb_xform->xf_output)(m, sp, sav, idx, i, off);
+	if (error != 0) {
+		key_freesav(&sav);
+		key_freesp(&sp);
+	}
+	return (error);
 bad:
-	m_freem(m);
+	IPSECSTAT_INC(ips_out_inval);
+	if (m != NULL)
+		m_freem(m);
+	if (sav != NULL)
+		key_freesav(&sav);
+	key_freesp(&sp);
 	return (error);
 }
 
-static struct ipsecrequest *
-ipsec_nextisr(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	int af,
-	struct secasindex *saidx,
-	int *error
-)
+int
+ipsec4_process_packet(struct mbuf *m, struct secpolicy *sp,
+    struct inpcb *inp)
 {
-#define	IPSEC_OSTAT(name)	do {		\
-	if (isr->saidx.proto == IPPROTO_ESP)	\
-		ESPSTAT_INC(esps_##name);	\
-	else if (isr->saidx.proto == IPPROTO_AH)\
-		AHSTAT_INC(ahs_##name);		\
-	else					\
-		IPCOMPSTAT_INC(ipcomps_##name);	\
-} while (0)
-	struct secasvar *sav;
 
-	IPSECREQUEST_LOCK_ASSERT(isr);
+	return (ipsec4_perform_request(m, sp, 0));
+}
 
-	IPSEC_ASSERT(af == AF_INET || af == AF_INET6,
-		("invalid address family %u", af));
-again:
-	/*
-	 * Craft SA index to search for proper SA.  Note that
-	 * we only fillin unspecified SA peers for transport
-	 * mode; for tunnel mode they must already be filled in.
-	 */
-	*saidx = isr->saidx;
-	if (isr->saidx.mode == IPSEC_MODE_TRANSPORT) {
-		/* Fillin unspecified SA peers only for transport mode */
-		if (af == AF_INET) {
-			struct sockaddr_in *sin;
-			struct ip *ip = mtod(m, struct ip *);
+static int
+ipsec4_common_output(struct mbuf *m, struct inpcb *inp, int forwarding)
+{
+	struct secpolicy *sp;
+	int error;
 
-			if (saidx->src.sa.sa_len == 0) {
-				sin = &saidx->src.sin;
-				sin->sin_len = sizeof(*sin);
-				sin->sin_family = AF_INET;
-				sin->sin_port = IPSEC_PORT_ANY;
-				sin->sin_addr = ip->ip_src;
-			}
-			if (saidx->dst.sa.sa_len == 0) {
-				sin = &saidx->dst.sin;
-				sin->sin_len = sizeof(*sin);
-				sin->sin_family = AF_INET;
-				sin->sin_port = IPSEC_PORT_ANY;
-				sin->sin_addr = ip->ip_dst;
-			}
-		} else {
-			struct sockaddr_in6 *sin6;
-			struct ip6_hdr *ip6 = mtod(m, struct ip6_hdr *);
-
-			if (saidx->src.sin6.sin6_len == 0) {
-				sin6 = (struct sockaddr_in6 *)&saidx->src;
-				sin6->sin6_len = sizeof(*sin6);
-				sin6->sin6_family = AF_INET6;
-				sin6->sin6_port = IPSEC_PORT_ANY;
-				sin6->sin6_addr = ip6->ip6_src;
-				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]);
-				}
-			}
-			if (saidx->dst.sin6.sin6_len == 0) {
-				sin6 = (struct sockaddr_in6 *)&saidx->dst;
-				sin6->sin6_len = sizeof(*sin6);
-				sin6->sin6_family = AF_INET6;
-				sin6->sin6_port = IPSEC_PORT_ANY;
-				sin6->sin6_addr = ip6->ip6_dst;
-				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]);
-				}
-			}
+	/* Lookup for the corresponding outbound security policy */
+	sp = ipsec4_checkpolicy(m, inp, &error);
+	if (sp == NULL) {
+		if (error == -EINVAL) {
+			/* Discarded by policy. */
+			m_freem(m);
+			return (EACCES);
 		}
+		return (0); /* No IPsec required. */
 	}
 
 	/*
-	 * Lookup SA and validate it.
+	 * Usually we have to have tunnel mode IPsec security policy
+	 * when we are forwarding a packet. Otherwise we could not handle
+	 * encrypted replies, because they are not destined for us. But
+	 * some users are doing source address translation for forwarded
+	 * packets, and thus, even if they are forwarded, the replies will
+	 * return back to us.
 	 */
-	*error = key_checkrequest(isr, saidx);
-	if (*error != 0) {
+	if (!forwarding) {
 		/*
-		 * IPsec processing is required, but no SA found.
-		 * I assume that key_acquire() had been called
-		 * to get/establish the SA. Here I discard
-		 * this packet because it is responsibility for
-		 * upper layer to retransmit the packet.
+		 * Do delayed checksums now because we send before
+		 * this is done in the normal processing path.
 		 */
-		IPSECSTAT_INC(ips_out_nosa);
-		goto bad;
+		if (m->m_pkthdr.csum_flags & CSUM_DELAY_DATA) {
+			in_delayed_cksum(m);
+			m->m_pkthdr.csum_flags &= ~CSUM_DELAY_DATA;
+		}
+#ifdef SCTP
+		if (m->m_pkthdr.csum_flags & CSUM_SCTP) {
+			struct ip *ip = mtod(m, struct ip *);
+
+			sctp_delayed_cksum(m, (uint32_t)(ip->ip_hl << 2));
+			m->m_pkthdr.csum_flags &= ~CSUM_SCTP;
+		}
+#endif
 	}
-	sav = isr->sav;
-	if (sav == NULL) {
-		IPSEC_ASSERT(ipsec_get_reqlevel(isr) == IPSEC_LEVEL_USE,
-			("no SA found, but required; level %u",
-			ipsec_get_reqlevel(isr)));
-		IPSECREQUEST_UNLOCK(isr);
-		isr = isr->next;
+	/* NB: callee frees mbuf and releases reference to SP */
+	error = ipsec4_process_packet(m, sp, inp);
+	if (error == EJUSTRETURN) {
 		/*
-		 * If isr is NULL, we found a 'use' policy w/o SA.
-		 * Return w/o error and w/o isr so we can drop out
-		 * and continue w/o IPsec processing.
+		 * We had a SP with a level of 'use' and no SA. We
+		 * will just continue to process the packet without
+		 * IPsec processing and return without error.
 		 */
-		if (isr == NULL)
-			return isr;
-		IPSECREQUEST_LOCK(isr);
-		goto again;
+		return (0);
 	}
+	if (error == 0)
+		return (EINPROGRESS); /* consumed by IPsec */
+	return (error);
+}
+
+/*
+ * IPSEC_OUTPUT() method implementation for IPv4.
+ * 0 - no IPsec handling needed
+ * other values - mbuf consumed by IPsec.
+ */
+int
+ipsec4_output(struct mbuf *m, struct inpcb *inp)
+{
+
+	/*
+	 * If the packet is resubmitted to ip_output (e.g. after
+	 * AH, ESP, etc. processing), there will be a tag to bypass
+	 * the lookup and related policy checking.
+	 */
+	if (m_tag_find(m, PACKET_TAG_IPSEC_OUT_DONE, NULL) != NULL)
+		return (0);
+
+	return (ipsec4_common_output(m, inp, 0));
+}
+
+/*
+ * IPSEC_FORWARD() method implementation for IPv4.
+ * 0 - no IPsec handling needed
+ * other values - mbuf consumed by IPsec.
+ */
+int
+ipsec4_forward(struct mbuf *m)
+{
+
+	/*
+	 * Check if this packet has an active inbound SP and needs to be
+	 * dropped instead of forwarded.
+	 */
+	if (ipsec4_in_reject(m, NULL) != 0) {
+		m_freem(m);
+		return (EACCES);
+	}
+	return (ipsec4_common_output(m, NULL, 1));
+}
+#endif
+
+#ifdef INET6
+static int
+in6_sa_equal_addrwithscope(const struct sockaddr_in6 *sa,
+    const struct in6_addr *ia)
+{
+	struct in6_addr ia2;
+
+	if (IN6_IS_SCOPE_LINKLOCAL(&sa->sin6_addr)) {
+		memcpy(&ia2, &sa->sin6_addr, sizeof(ia2));
+		ia2.s6_addr16[1] = htons(sa->sin6_scope_id);
+		return (IN6_ARE_ADDR_EQUAL(ia, &ia2));
+	}
+	return (IN6_ARE_ADDR_EQUAL(&sa->sin6_addr, ia));
+}
+
+static struct secasvar *
+ipsec6_allocsa(struct mbuf *m, struct secpolicy *sp, u_int *pidx, int *error)
+{
+	struct secasindex *saidx, tmpsaidx;
+	struct ipsecrequest *isr;
+	struct sockaddr_in6 *sin6;
+	struct secasvar *sav;
+	struct ip6_hdr *ip6;
 
 	/*
 	 * Check system global policy controls.
 	 */
+next:
+	isr = sp->req[*pidx];
 	if ((isr->saidx.proto == IPPROTO_ESP && !V_esp_enable) ||
 	    (isr->saidx.proto == IPPROTO_AH && !V_ah_enable) ||
 	    (isr->saidx.proto == IPPROTO_IPCOMP && !V_ipcomp_enable)) {
 		DPRINTF(("%s: IPsec outbound packet dropped due"
 			" to policy (check your sysctls)\n", __func__));
-		IPSEC_OSTAT(pdrops);
+		IPSEC_OSTAT_INC(isr->saidx.proto, pdrops);
 		*error = EHOSTUNREACH;
-		goto bad;
+		return (NULL);
 	}
-
 	/*
-	 * Sanity check the SA contents for the caller
-	 * before they invoke the xform output method.
+	 * Craft SA index to search for proper SA.  Note that
+	 * we only fillin unspecified SA peers for transport
+	 * mode; for tunnel mode they must already be filled in.
 	 */
-	if (sav->tdb_xform == NULL) {
-		DPRINTF(("%s: no transform for SA\n", __func__));
-		IPSEC_OSTAT(noxform);
-		*error = EHOSTUNREACH;
-		goto bad;
+	if (isr->saidx.mode == IPSEC_MODE_TRANSPORT) {
+		saidx = &tmpsaidx;
+		*saidx = isr->saidx;
+		ip6 = mtod(m, struct ip6_hdr *);
+		if (saidx->src.sin6.sin6_len == 0) {
+			sin6 = (struct sockaddr_in6 *)&saidx->src;
+			sin6->sin6_len = sizeof(*sin6);
+			sin6->sin6_family = AF_INET6;
+			sin6->sin6_port = IPSEC_PORT_ANY;
+			sin6->sin6_addr = ip6->ip6_src;
+			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]);
+			}
+		}
+		if (saidx->dst.sin6.sin6_len == 0) {
+			sin6 = (struct sockaddr_in6 *)&saidx->dst;
+			sin6->sin6_len = sizeof(*sin6);
+			sin6->sin6_family = AF_INET6;
+			sin6->sin6_port = IPSEC_PORT_ANY;
+			sin6->sin6_addr = ip6->ip6_dst;
+			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]);
+			}
+		}
+	} else
+		saidx = &sp->req[*pidx]->saidx;
+	/*
+	 * Lookup SA and validate it.
+	 */
+	sav = key_allocsa_policy(sp, saidx, error);
+	if (sav == NULL) {
+		IPSEC6STAT_INC(ips_out_nosa);
+		if (*error != 0)
+			return (NULL);
+		if (ipsec_get_reqlevel(sp, *pidx) != IPSEC_LEVEL_REQUIRE) {
+			/*
+			 * We have no SA and policy that doesn't require
+			 * this IPsec transform, thus we can continue w/o
+			 * IPsec processing, i.e. return EJUSTRETURN.
+			 * But first check if there is some bundled transform.
+			 */
+			if (sp->tcount > ++(*pidx))
+				goto next;
+			*error = EJUSTRETURN;
+		}
+		return (NULL);
 	}
-	return isr;
-bad:
-	IPSEC_ASSERT(*error != 0, ("error return w/ no error code"));
-	IPSECREQUEST_UNLOCK(isr);
-	return NULL;
-#undef IPSEC_OSTAT
+	IPSEC_ASSERT(sav->tdb_xform != NULL, ("SA with NULL tdb_xform"));
+	return (sav);
 }
 
-#ifdef INET
 /*
- * IPsec output logic for IPv4.
+ * IPsec output logic for IPv6.
  */
-int
-ipsec4_process_packet(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	int flags,
-	int tunalready)
+static int
+ipsec6_perform_request(struct mbuf *m, struct secpolicy *sp, u_int idx)
 {
-	struct secasindex saidx;
+	char sbuf[IPSEC_ADDRSTRLEN], dbuf[IPSEC_ADDRSTRLEN];
+	struct ipsec_ctx_data ctx;
+	union sockaddr_union *dst;
 	struct secasvar *sav;
-	struct ip *ip;
+	struct ip6_hdr *ip6;
 	int error, i, off;
 
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
-	IPSEC_ASSERT(isr != NULL, ("null isr"));
-
-	IPSECREQUEST_LOCK(isr);		/* insure SA contents don't change */
+	IPSEC_ASSERT(idx < sp->tcount, ("Wrong IPsec request index %d", idx));
 
-	isr = ipsec_nextisr(m, isr, AF_INET, &saidx, &error);
-	if (isr == NULL) {
-		if (error != 0)
-			goto bad;
-		return EJUSTRETURN;
+	sav = ipsec6_allocsa(m, sp, &idx, &error);
+	if (sav == NULL) {
+		if (error == EJUSTRETURN) { /* No IPsec required */
+			key_freesp(&sp);
+			return (error);
+		}
+		goto bad;
 	}
 
-	sav = isr->sav;
-
-#ifdef DEV_ENC
-	encif->if_opackets++;
-	encif->if_obytes += m->m_pkthdr.len;
+	/* Fix IP length in case if it is not set yet. */
+	ip6 = mtod(m, struct ip6_hdr *);
+	ip6->ip6_plen = htons(m->m_pkthdr.len - sizeof(*ip6));
 
-	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, sav, AF_INET, ENC_OUT|ENC_BEFORE);
-	/* pass the mbuf to enc0 for packet filtering */
-	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_BEFORE)) != 0)
+	IPSEC_INIT_CTX(&ctx, &m, sav, AF_INET6, IPSEC_ENC_BEFORE);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_OUT)) != 0)
 		goto bad;
-#endif
 
-	if (!tunalready) {
-		union sockaddr_union *dst = &sav->sah->saidx.dst;
-		int setdf;
+	ip6 = mtod(m, struct ip6_hdr *); /* pfil can change mbuf */
+	dst = &sav->sah->saidx.dst;
 
-		/*
-		 * Collect IP_DF state from the outer header.
-		 */
-		if (dst->sa.sa_family == AF_INET) {
-			if (m->m_len < sizeof (struct ip) &&
-			    (m = m_pullup(m, sizeof (struct ip))) == NULL) {
-				error = ENOBUFS;
-				goto bad;
-			}
-			ip = mtod(m, struct ip *);
-			/* Honor system-wide control of how to handle IP_DF */
-			switch (V_ip4_ipsec_dfbit) {
-			case 0:			/* clear in outer header */
-			case 1:			/* set in outer header */
-				setdf = V_ip4_ipsec_dfbit;
-				break;
-			default:		/* propagate to outer header */
-				setdf = ntohs(ip->ip_off & IP_DF);
-				break;
-			}
-		} else {
-			ip = NULL;		/* keep compiler happy */
-			setdf = 0;
+	/* Do the appropriate encapsulation, if necessary */
+	if (sp->req[idx]->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)))) {
+		if (m->m_pkthdr.len - sizeof(*ip6) > IPV6_MAXPACKET) {
+			/* No jumbogram support. */
+			error = ENXIO;   /*XXX*/
+			goto bad;
 		}
-		/* Do the appropriate encapsulation, if necessary */
-		if (isr->saidx.mode == IPSEC_MODE_TUNNEL || /* Tunnel requ'd */
-		    dst->sa.sa_family != AF_INET ||	    /* PF mismatch */
-#if 0
-		    (sav->flags & SADB_X_SAFLAGS_TUNNEL) || /* Tunnel requ'd */
-		    sav->tdb_xform->xf_type == XF_IP4 ||    /* ditto */
-#endif
-		    (dst->sa.sa_family == AF_INET &&	    /* Proxy */
-		     dst->sin.sin_addr.s_addr != INADDR_ANY &&
-		     dst->sin.sin_addr.s_addr != ip->ip_dst.s_addr)) {
-			struct mbuf *mp;
-
-			/* Fix IPv4 header checksum and length */
-			if (m->m_len < sizeof (struct ip) &&
-			    (m = m_pullup(m, sizeof (struct ip))) == NULL) {
-				error = ENOBUFS;
-				goto bad;
-			}
-			ip = mtod(m, struct ip *);
-			ip->ip_len = htons(m->m_pkthdr.len);
-			ip->ip_sum = 0;
-#ifdef _IP_VHL
-			if (ip->ip_vhl == IP_VHL_BORING)
-				ip->ip_sum = in_cksum_hdr(ip);
-			else
-				ip->ip_sum = in_cksum(m,
-					_IP_VHL_HL(ip->ip_vhl) << 2);
-#else
-			ip->ip_sum = in_cksum(m, ip->ip_hl << 2);
-#endif
-
-			/* Encapsulate the packet */
-			error = ipip_output(m, isr, &mp, 0, 0);
-			if (mp == NULL && !error) {
-				/* Should never happen. */
-				DPRINTF(("%s: ipip_output returns no mbuf and "
-					"no error!", __func__));
-				error = EFAULT;
-			}
-			if (error) {
-				if (mp) {
-					/* XXX: Should never happen! */
-					m_freem(mp);
-				}
-				m = NULL; /* ipip_output() already freed it */
-				goto bad;
-			}
-			m = mp, mp = NULL;
-			/*
-			 * ipip_output clears IP_DF in the new header.  If
-			 * we need to propagate IP_DF from the outer header,
-			 * then we have to do it here.
-			 *
-			 * XXX shouldn't assume what ipip_output does.
-			 */
-			if (dst->sa.sa_family == AF_INET && setdf) {
-				if (m->m_len < sizeof (struct ip) &&
-				    (m = m_pullup(m, sizeof (struct ip))) == NULL) {
-					error = ENOBUFS;
-					goto bad;
-				}
-				ip = mtod(m, struct ip *);
-				ip->ip_off = ntohs(ip->ip_off);
-				ip->ip_off |= IP_DF;
-				ip->ip_off = htons(ip->ip_off);
-			}
+		error = ipsec_encap(&m, &sav->sah->saidx);
+		if (error != 0) {
+			DPRINTF(("%s: encapsulation for SA %s->%s "
+			    "SPI 0x%08x failed with error %d\n", __func__,
+			    ipsec_address(&sav->sah->saidx.src, sbuf,
+				sizeof(sbuf)),
+			    ipsec_address(&sav->sah->saidx.dst, dbuf,
+				sizeof(dbuf)), ntohl(sav->spi), error));
+			/* XXXAE: IPSEC_OSTAT_INC(tunnel); */
+			goto bad;
 		}
 	}
 
-#ifdef DEV_ENC
-	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, sav, AF_INET, ENC_OUT|ENC_AFTER);
-	/* pass the mbuf to enc0 for packet filtering */
-	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_AFTER)) != 0)
+	IPSEC_INIT_CTX(&ctx, &m, sav, dst->sa.sa_family, IPSEC_ENC_AFTER);
+	if ((error = ipsec_run_hhooks(&ctx, HHOOK_TYPE_IPSEC_OUT)) != 0)
 		goto bad;
-#endif
 
-	/*
-	 * Dispatch to the appropriate IPsec transform logic.  The
-	 * packet will be returned for transmission after crypto
-	 * processing, etc. are completed.  For encapsulation we
-	 * bypass this call because of the explicit call done above
-	 * (necessary to deal with IP_DF handling for IPv4).
-	 *
-	 * NB: m & sav are ``passed to caller'' who's reponsible for
-	 *     for reclaiming their resources.
-	 */
-	if (sav->tdb_xform->xf_type != XF_IP4) {
+	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);
-		error = (*sav->tdb_xform->xf_output)(m, isr, NULL, i, off);
-	} else {
-		error = ipsec_process_done(m, isr);
+		}
+		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;
+		IPSEC_OSTAT_INC(sav->sah->saidx.proto, nopf);
+		goto bad;
 	}
-	IPSECREQUEST_UNLOCK(isr);
-	return error;
+	error = (*sav->tdb_xform->xf_output)(m, sp, sav, idx, i, off);
+	if (error != 0) {
+		key_freesav(&sav);
+		key_freesp(&sp);
+	}
+	return (error);
 bad:
-	if (isr)
-		IPSECREQUEST_UNLOCK(isr);
-	if (m)
+	IPSEC6STAT_INC(ips_out_inval);
+	if (m != NULL)
 		m_freem(m);
-	return error;
+	if (sav != NULL)
+		key_freesav(&sav);
+	key_freesp(&sp);
+	return (error);
 }
-#endif
 
-#ifdef INET6
-/*
- * Chop IP6 header from the payload.
- */
-static struct mbuf *
-ipsec6_splithdr(struct mbuf *m)
+int
+ipsec6_process_packet(struct mbuf *m, struct secpolicy *sp,
+    struct inpcb *inp)
 {
-	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_DONTWAIT, MT_DATA);
-		if (!mh) {
+	return (ipsec6_perform_request(m, sp, 0));
+}
+
+static int
+ipsec6_common_output(struct mbuf *m, struct inpcb *inp, int forwarding)
+{
+	struct secpolicy *sp;
+	int error;
+
+	/* Lookup for the corresponding outbound security policy */
+	sp = ipsec6_checkpolicy(m, inp, &error);
+	if (sp == NULL) {
+		if (error == -EINVAL) {
+			/* Discarded by policy. */
 			m_freem(m);
-			return NULL;
+			return (EACCES);
+		}
+		return (0); /* No IPsec required. */
+	}
+
+	if (!forwarding) {
+		/*
+		 * Do delayed checksums now because we send before
+		 * this is done in the normal processing path.
+		 */
+		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;
 		}
-		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;
+#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 and releases reference to SP */
+	error = ipsec6_process_packet(m, sp, inp);
+	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 and return without error.
+		 */
+		return (0);
 	}
-	return m;
+	if (error == 0)
+		return (EINPROGRESS); /* consumed by IPsec */
+	return (error);
 }
 
 /*
- * IPsec output logic for IPv6, transport mode.
+ * IPSEC_OUTPUT() method implementation for IPv6.
+ * 0 - no IPsec handling needed
+ * other values - mbuf consumed by IPsec.
  */
 int
-ipsec6_output_trans(
-	struct ipsec_output_state *state,
-	u_char *nexthdrp,
-	struct mbuf *mprev,
-	struct secpolicy *sp,
-	int flags,
-	int *tun)
+ipsec6_output(struct mbuf *m, struct inpcb *inp)
 {
-	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;
-	}
+	/*
+	 * If the packet is resubmitted to ip_output (e.g. after
+	 * AH, ESP, etc. processing), there will be a tag to bypass
+	 * the lookup and related policy checking.
+	 */
+	if (m_tag_find(m, PACKET_TAG_IPSEC_OUT_DONE, NULL) != NULL)
+		return (0);
 
-	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;
+	return (ipsec6_common_output(m, inp, 0));
 }
 
-static int
-ipsec6_encapsulate(struct mbuf *m, struct secasvar *sav)
+/*
+ * IPSEC_FORWARD() method implementation for IPv6.
+ * 0 - no IPsec handling needed
+ * other values - mbuf consumed by IPsec.
+ */
+int
+ipsec6_forward(struct mbuf *m)
 {
-	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.
+	 * Check if this packet has an active inbound SP and needs to be
+	 * dropped instead of forwarded.
 	 */
-	plen = m->m_pkthdr.len;
-	if (M_LEADINGSPACE(m->m_next) < sizeof(struct ip6_hdr)) {
-		struct mbuf *n;
-		MGET(n, M_DONTWAIT, 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() */
+	if (ipsec6_in_reject(m, NULL) != 0) {
+		m_freem(m);
+		return (EACCES);
 	}
-	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;
-
-	/* XXX Should ip6_src be updated later ? */
-
-	return 0;
+	return (ipsec6_common_output(m, NULL, 1));
 }
+#endif /* INET6 */
 
-/*
- * IPsec output logic for IPv6, tunnel mode.
- */
 int
-ipsec6_output_tunnel(struct ipsec_output_state *state, struct secpolicy *sp, int flags)
+ipsec_process_done(struct mbuf *m, struct secpolicy *sp, struct secasvar *sav,
+    u_int idx)
 {
-	struct ip6_hdr *ip6;
-	struct ipsecrequest *isr;
-	struct secasindex saidx;
+	struct xform_history *xh;
+	struct secasindex *saidx;
+	struct m_tag *mtag;
 	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));
+	saidx = &sav->sah->saidx;
+	switch (saidx->dst.sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		/* Fix the header length, for AH processing. */
+		mtod(m, struct ip *)->ip_len = htons(m->m_pkthdr.len);
+		break;
+#endif /* INET */
+#ifdef INET6
+	case AF_INET6:
+		/* Fix the header length, for AH processing. */
+		if (m->m_pkthdr.len < sizeof (struct ip6_hdr)) {
+			error = ENXIO;
+			goto bad;
+		}
+		if (m->m_pkthdr.len - sizeof (struct ip6_hdr) > IPV6_MAXPACKET) {
+			/* No jumbogram support. */
+			error = ENXIO;	/*?*/
+			goto bad;
+		}
+		mtod(m, struct ip6_hdr *)->ip6_plen =
+			htons(m->m_pkthdr.len - sizeof(struct ip6_hdr));
+		break;
+#endif /* INET6 */
+	default:
+		DPRINTF(("%s: unknown protocol family %u\n", __func__,
+		    saidx->dst.sa.sa_family));
+		error = ENXIO;
+		goto bad;
+	}
 
-	m = state->m;
 	/*
-	 * transport mode ipsec (before the 1st tunnel mode) is already
-	 * processed by ipsec6_output_trans().
+	 * Add a record of what we've done to the packet.
 	 */
-	for (isr = sp->req; isr; isr = isr->next) {
-		if (isr->saidx.mode == IPSEC_MODE_TUNNEL)
-			break;
-	}
-
-	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; 
+	mtag = m_tag_get(PACKET_TAG_IPSEC_OUT_DONE, sizeof(*xh), M_NOWAIT);
+	if (mtag == NULL) {
+		DPRINTF(("%s: could not get packet tag\n", __func__));
+		error = ENOMEM;
+		goto bad;
 	}
 
-#ifdef DEV_ENC
-	encif->if_opackets++;
-	encif->if_obytes += m->m_pkthdr.len;
+	xh = (struct xform_history *)(mtag + 1);
+	xh->dst = saidx->dst;
+	xh->proto = saidx->proto;
+	xh->mode = saidx->mode;
+	xh->spi = sav->spi;
+	m_tag_prepend(m, mtag);
 
-	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, isr->sav, AF_INET6, ENC_OUT|ENC_BEFORE);
-	/* pass the mbuf to enc0 for packet filtering */
-	if ((error = ipsec_filter(&m, PFIL_OUT, ENC_OUT|ENC_BEFORE)) != 0)
-		goto bad;
-#endif
+	key_sa_recordxfer(sav, m);		/* record data transfer */
 
 	/*
-	 * There may be the case that SA status will be changed when
-	 * we are refering to one. So calling splsoftnet().
+	 * If there's another (bundled) SA to apply, do so.
+	 * Note that this puts a burden on the kernel stack size.
+	 * If this is a problem we'll need to introduce a queue
+	 * to set the packet on so we can unwind the stack before
+	 * doing further processing.
 	 */
-	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;
-			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;
+	if (++idx < sp->tcount) {
+		switch (saidx->dst.sa.sa_family) {
+#ifdef INET
+		case AF_INET:
+			key_freesav(&sav);
+			IPSECSTAT_INC(ips_out_bundlesa);
+			return (ipsec4_perform_request(m, sp, idx));
+			/* NOTREACHED */
+#endif
+#ifdef INET6
+		case AF_INET6:
+			key_freesav(&sav);
+			IPSEC6STAT_INC(ips_out_bundlesa);
+			return (ipsec6_perform_request(m, sp, idx));
+			/* NOTREACHED */
+#endif /* INET6 */
+		default:
+			DPRINTF(("%s: unknown protocol family %u\n", __func__,
+			    saidx->dst.sa.sa_family));
+			error = EPFNOSUPPORT;
 			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;
+	key_freesp(&sp), sp = NULL;	/* Release reference to SP */
+#ifdef INET
+	/*
+	 * Do UDP encapsulation if SA requires it.
+	 */
+	if (sav->natt != NULL) {
+		error = udp_ipsec_output(m, sav);
+		if (error != 0)
 			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;
 	}
+#endif /* INET */
+	/*
+	 * We're done with IPsec processing, transmit the packet using the
+	 * appropriate network protocol (IP or IPv6).
+	 */
+	switch (saidx->dst.sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		key_freesav(&sav);
+		return ip_output(m, NULL, NULL, IP_RAWOUTPUT, NULL, NULL);
+#endif /* INET */
+#ifdef INET6
+	case AF_INET6:
+		key_freesav(&sav);
+		return ip6_output(m, NULL, NULL, 0, NULL, NULL, NULL);
+#endif /* INET6 */
+	}
+	panic("ipsec_process_done");
+bad:
+	m_freem(m);
+	key_freesav(&sav);
+	if (sp != NULL)
+		key_freesp(&sp);
+	return (error);
+}
 
-	m = ipsec6_splithdr(m);
-	if (!m) {
-		IPSEC6STAT_INC(ips_out_nomem);
-		error = ENOMEM;
-		goto bad;
+/*
+ * ipsec_prepend() is optimized version of M_PREPEND().
+ * ipsec_encap() is called by IPsec output routine for tunnel mode SA.
+ * It is expected that after IP encapsulation some IPsec transform will
+ * be performed. Each IPsec transform inserts its variable length header
+ * just after outer IP header using m_makespace(). If given mbuf has not
+ * enough free space at the beginning, we allocate new mbuf and reserve
+ * some space at the beginning and at the end.
+ * This helps avoid allocating of new mbuf and data copying in m_makespace(),
+ * we place outer header in the middle of mbuf's data with reserved leading
+ * and trailing space:
+ *	[ LEADINGSPACE ][ Outer IP header ][ TRAILINGSPACE ]
+ * LEADINGSPACE will be used to add ethernet header, TRAILINGSPACE will
+ * be used to inject AH/ESP/IPCOMP header.
+ */
+#define	IPSEC_TRAILINGSPACE	(sizeof(struct udphdr) +/* NAT-T */	\
+    max(sizeof(struct newesp) + EALG_MAX_BLOCK_LEN,	/* ESP + IV */	\
+	sizeof(struct newah) + HASH_MAX_LEN		/* AH + ICV */))
+static struct mbuf *
+ipsec_prepend(struct mbuf *m, int len, int how)
+{
+	struct mbuf *n;
+
+	M_ASSERTPKTHDR(m);
+	IPSEC_ASSERT(len < MHLEN, ("wrong length"));
+	if (M_LEADINGSPACE(m) >= len) {
+		/* No need to allocate new mbuf. */
+		m->m_data -= len;
+		m->m_len += len;
+		m->m_pkthdr.len += len;
+		return (m);
 	}
-	ip6 = mtod(m, struct ip6_hdr *);
+	n = m_gethdr(how, m->m_type);
+	if (n == NULL) {
+		m_freem(m);
+		return (NULL);
+	}
+	m_move_pkthdr(n, m);
+	n->m_next = m;
+	if (len + IPSEC_TRAILINGSPACE < M_SIZE(n))
+		m_align(n, len + IPSEC_TRAILINGSPACE);
+	n->m_len = len;
+	n->m_pkthdr.len += len;
+	return (n);
+}
 
-#ifdef DEV_ENC
-	/* pass the mbuf to enc0 for bpf processing */
-	ipsec_bpf(m, isr->sav, AF_INET6, 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;
+static int
+ipsec_encap(struct mbuf **mp, struct secasindex *saidx)
+{
+#ifdef INET6
+	struct ip6_hdr *ip6;
 #endif
+	struct ip *ip;
+	int setdf;
+	uint8_t itos, proto;
 
-	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;
+	ip = mtod(*mp, struct ip *);
+	switch (ip->ip_v) {
+#ifdef INET
+	case IPVERSION:
+		proto = IPPROTO_IPIP;
+		/*
+		 * Collect IP_DF state from the inner header
+		 * and honor system-wide control of how to handle it.
+		 */
+		switch (V_ip4_ipsec_dfbit) {
+		case 0:	/* clear in outer header */
+		case 1:	/* set in outer header */
+			setdf = V_ip4_ipsec_dfbit;
+			break;
+		default:/* propagate to outer header */
+			setdf = (ip->ip_off & htons(IP_DF)) != 0;
+		}
+		itos = ip->ip_tos;
+		break;
+#endif
+#ifdef INET6
+	case (IPV6_VERSION >> 4):
+		proto = IPPROTO_IPV6;
+		ip6 = mtod(*mp, struct ip6_hdr *);
+		itos = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
+		setdf = V_ip4_ipsec_dfbit ? 1: 0;
+		/* scoped address handling */
+		in6_clearscope(&ip6->ip6_src);
+		in6_clearscope(&ip6->ip6_dst);
+		break;
+#endif
+	default:
+		return (EAFNOSUPPORT);
+	}
+	switch (saidx->dst.sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		if (saidx->src.sa.sa_family != AF_INET ||
+		    saidx->src.sin.sin_addr.s_addr == INADDR_ANY ||
+		    saidx->dst.sin.sin_addr.s_addr == INADDR_ANY)
+			return (EINVAL);
+		*mp = ipsec_prepend(*mp, sizeof(struct ip), M_NOWAIT);
+		if (*mp == NULL)
+			return (ENOBUFS);
+		ip = mtod(*mp, struct ip *);
+		ip->ip_v = IPVERSION;
+		ip->ip_hl = sizeof(struct ip) >> 2;
+		ip->ip_p = proto;
+		ip->ip_len = htons((*mp)->m_pkthdr.len);
+		ip->ip_ttl = V_ip_defttl;
+		ip->ip_sum = 0;
+		ip->ip_off = setdf ? htons(IP_DF): 0;
+		ip->ip_src = saidx->src.sin.sin_addr;
+		ip->ip_dst = saidx->dst.sin.sin_addr;
+		ip_ecn_ingress(V_ip4_ipsec_ecn, &ip->ip_tos, &itos);
+		ip_fillid(ip);
+		break;
+#endif /* INET */
+#ifdef INET6
+	case AF_INET6:
+		if (saidx->src.sa.sa_family != AF_INET6 ||
+		    IN6_IS_ADDR_UNSPECIFIED(&saidx->src.sin6.sin6_addr) ||
+		    IN6_IS_ADDR_UNSPECIFIED(&saidx->dst.sin6.sin6_addr))
+			return (EINVAL);
+		*mp = ipsec_prepend(*mp, sizeof(struct ip6_hdr), M_NOWAIT);
+		if (*mp == NULL)
+			return (ENOBUFS);
+		ip6 = mtod(*mp, struct ip6_hdr *);
+		ip6->ip6_flow = 0;
+		ip6->ip6_vfc = IPV6_VERSION;
+		ip6->ip6_hlim = V_ip6_defhlim;
+		ip6->ip6_nxt = proto;
+		ip6->ip6_dst = saidx->dst.sin6.sin6_addr;
+		/* For link-local address embed scope zone id */
+		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst))
+			ip6->ip6_dst.s6_addr16[1] =
+			    htons(saidx->dst.sin6.sin6_scope_id & 0xffff);
+		ip6->ip6_src = saidx->src.sin6.sin6_addr;
+		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src))
+			ip6->ip6_src.s6_addr16[1] =
+			    htons(saidx->src.sin6.sin6_scope_id & 0xffff);
+		ip6->ip6_plen = htons((*mp)->m_pkthdr.len - sizeof(*ip6));
+		ip_ecn_ingress(V_ip6_ipsec_ecn, &proto, &itos);
+		ip6->ip6_flow |= htonl((uint32_t)proto << 20);
+		break;
+#endif /* INET6 */
+	default:
+		return (EAFNOSUPPORT);
+	}
+	(*mp)->m_flags &= ~(M_BCAST | M_MCAST);
+	return (0);
 }
-#endif /*INET6*/
+
diff --git a/freebsd/sys/netipsec/ipsec_pcb.c b/freebsd/sys/netipsec/ipsec_pcb.c
new file mode 100644
index 00000000..21a8182d
--- /dev/null
+++ b/freebsd/sys/netipsec/ipsec_pcb.c
@@ -0,0 +1,481 @@
+#include <machine/rtems-bsd-kernel-space.h>
+
+/*-
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <rtems/bsd/local/opt_inet.h>
+#include <rtems/bsd/local/opt_inet6.h>
+#include <rtems/bsd/local/opt_ipsec.h>
+
+#include <rtems/bsd/sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/priv.h>
+#include <sys/socket.h>
+#include <sys/sockopt.h>
+#include <sys/syslog.h>
+#include <sys/proc.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+
+#include <netipsec/ipsec.h>
+#include <netipsec/ipsec6.h>
+#include <netipsec/ipsec_support.h>
+#include <netipsec/key.h>
+#include <netipsec/key_debug.h>
+
+MALLOC_DEFINE(M_IPSEC_INPCB, "inpcbpolicy", "inpcb-resident ipsec policy");
+
+static void
+ipsec_setsockaddrs_inpcb(struct inpcb *inp, union sockaddr_union *src,
+    union sockaddr_union *dst, u_int dir)
+{
+
+#ifdef INET6
+	if (inp->inp_vflag & INP_IPV6) {
+		struct sockaddr_in6 *sin6;
+
+		bzero(&src->sin6, sizeof(src->sin6));
+		bzero(&dst->sin6, sizeof(dst->sin6));
+		src->sin6.sin6_family = AF_INET6;
+		src->sin6.sin6_len = sizeof(struct sockaddr_in6);
+		dst->sin6.sin6_family = AF_INET6;
+		dst->sin6.sin6_len = sizeof(struct sockaddr_in6);
+
+		if (dir == IPSEC_DIR_OUTBOUND)
+			sin6 = &src->sin6;
+		else
+			sin6 = &dst->sin6;
+		sin6->sin6_addr = inp->in6p_laddr;
+		sin6->sin6_port = inp->inp_lport;
+		if (IN6_IS_SCOPE_LINKLOCAL(&inp->in6p_laddr)) {
+			/* XXXAE: use in6p_zoneid */
+			sin6->sin6_addr.s6_addr16[1] = 0;
+			sin6->sin6_scope_id = ntohs(
+			    inp->in6p_laddr.s6_addr16[1]);
+		}
+
+		if (dir == IPSEC_DIR_OUTBOUND)
+			sin6 = &dst->sin6;
+		else
+			sin6 = &src->sin6;
+		sin6->sin6_addr = inp->in6p_faddr;
+		sin6->sin6_port = inp->inp_fport;
+		if (IN6_IS_SCOPE_LINKLOCAL(&inp->in6p_faddr)) {
+			/* XXXAE: use in6p_zoneid */
+			sin6->sin6_addr.s6_addr16[1] = 0;
+			sin6->sin6_scope_id = ntohs(
+			    inp->in6p_faddr.s6_addr16[1]);
+		}
+	}
+#endif
+#ifdef INET
+	if (inp->inp_vflag & INP_IPV4) {
+		struct sockaddr_in *sin;
+
+		bzero(&src->sin, sizeof(src->sin));
+		bzero(&dst->sin, sizeof(dst->sin));
+		src->sin.sin_family = AF_INET;
+		src->sin.sin_len = sizeof(struct sockaddr_in);
+		dst->sin.sin_family = AF_INET;
+		dst->sin.sin_len = sizeof(struct sockaddr_in);
+
+		if (dir == IPSEC_DIR_OUTBOUND)
+			sin = &src->sin;
+		else
+			sin = &dst->sin;
+		sin->sin_addr = inp->inp_laddr;
+		sin->sin_port = inp->inp_lport;
+
+		if (dir == IPSEC_DIR_OUTBOUND)
+			sin = &dst->sin;
+		else
+			sin = &src->sin;
+		sin->sin_addr = inp->inp_faddr;
+		sin->sin_port = inp->inp_fport;
+	}
+#endif
+}
+
+void
+ipsec_setspidx_inpcb(struct inpcb *inp, struct secpolicyindex *spidx,
+    u_int dir)
+{
+
+	ipsec_setsockaddrs_inpcb(inp, &spidx->src, &spidx->dst, dir);
+#ifdef INET6
+	if (inp->inp_vflag & INP_IPV6) {
+		spidx->prefs = sizeof(struct in6_addr) << 3;
+		spidx->prefd = sizeof(struct in6_addr) << 3;
+	}
+#endif
+#ifdef INET
+	if (inp->inp_vflag & INP_IPV4) {
+		spidx->prefs = sizeof(struct in_addr) << 3;
+		spidx->prefd = sizeof(struct in_addr) << 3;
+	}
+#endif
+	spidx->ul_proto = IPPROTO_TCP; /* XXX: currently only TCP uses this */
+	spidx->dir = dir;
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: ", __func__); kdebug_secpolicyindex(spidx, NULL));
+}
+
+/* Initialize PCB policy. */
+int
+ipsec_init_pcbpolicy(struct inpcb *inp)
+{
+
+	IPSEC_ASSERT(inp != NULL, ("null inp"));
+	IPSEC_ASSERT(inp->inp_sp == NULL, ("inp_sp already initialized"));
+
+	inp->inp_sp = malloc(sizeof(struct inpcbpolicy), M_IPSEC_INPCB,
+	    M_NOWAIT | M_ZERO);
+	if (inp->inp_sp == NULL)
+		return (ENOBUFS);
+	return (0);
+}
+
+/* Delete PCB policy. */
+int
+ipsec_delete_pcbpolicy(struct inpcb *inp)
+{
+
+	if (inp->inp_sp == NULL)
+		return (0);
+
+	if (inp->inp_sp->flags & INP_INBOUND_POLICY)
+		key_freesp(&inp->inp_sp->sp_in);
+
+	if (inp->inp_sp->flags & INP_OUTBOUND_POLICY)
+		key_freesp(&inp->inp_sp->sp_out);
+
+	free(inp->inp_sp, M_IPSEC_INPCB);
+	inp->inp_sp = NULL;
+	return (0);
+}
+
+/* Deep-copy a policy in PCB. */
+static struct secpolicy *
+ipsec_deepcopy_pcbpolicy(struct secpolicy *src)
+{
+	struct secpolicy *dst;
+	int i;
+
+	if (src == NULL)
+		return (NULL);
+
+	IPSEC_ASSERT(src->state == IPSEC_SPSTATE_PCB, ("SP isn't PCB"));
+
+	dst = key_newsp();
+	if (dst == NULL)
+		return (NULL);
+
+	/* spidx is not copied here */
+	dst->policy = src->policy;
+	dst->state = src->state;
+	dst->priority = src->priority;
+	/* Do not touch the refcnt field. */
+
+	/* Copy IPsec request chain. */
+	for (i = 0; i < src->tcount; i++) {
+		dst->req[i] = ipsec_newisr();
+		if (dst->req[i] == NULL) {
+			key_freesp(&dst);
+			return (NULL);
+		}
+		bcopy(src->req[i], dst->req[i], sizeof(struct ipsecrequest));
+		dst->tcount++;
+	}
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: copied SP(%p) -> SP(%p)\n", __func__, src, dst);
+	    kdebug_secpolicy(dst));
+	return (dst);
+}
+
+/*
+ * Copy IPsec policy from old INPCB into new.
+ * It is expected that new INPCB has not configured policies.
+ */
+int
+ipsec_copy_pcbpolicy(struct inpcb *old, struct inpcb *new)
+{
+	struct secpolicy *sp;
+
+	/*
+	 * old->inp_sp can be NULL if PCB was created when an IPsec
+	 * support was unavailable. This is not an error, we don't have
+	 * policies in this PCB, so nothing to copy.
+	 */
+	if (old->inp_sp == NULL)
+		return (0);
+
+	IPSEC_ASSERT(new->inp_sp != NULL, ("new inp_sp is NULL"));
+	IPSEC_ASSERT((new->inp_sp->flags & (
+	    INP_INBOUND_POLICY | INP_OUTBOUND_POLICY)) == 0,
+	    ("new PCB already has configured policies"));
+	INP_WLOCK_ASSERT(new);
+	INP_LOCK_ASSERT(old);
+
+	if (old->inp_sp->flags & INP_INBOUND_POLICY) {
+		sp = ipsec_deepcopy_pcbpolicy(old->inp_sp->sp_in);
+		if (sp == NULL)
+			return (ENOBUFS);
+		ipsec_setspidx_inpcb(new, &sp->spidx, IPSEC_DIR_INBOUND);
+		new->inp_sp->sp_in = sp;
+		new->inp_sp->flags |= INP_INBOUND_POLICY;
+	}
+	if (old->inp_sp->flags & INP_OUTBOUND_POLICY) {
+		sp = ipsec_deepcopy_pcbpolicy(old->inp_sp->sp_out);
+		if (sp == NULL)
+			return (ENOBUFS);
+		ipsec_setspidx_inpcb(new, &sp->spidx, IPSEC_DIR_OUTBOUND);
+		new->inp_sp->sp_out = sp;
+		new->inp_sp->flags |= INP_OUTBOUND_POLICY;
+	}
+	return (0);
+}
+
+static int
+ipsec_set_pcbpolicy(struct inpcb *inp, struct ucred *cred,
+    void *request, size_t len)
+{
+	struct sadb_x_policy *xpl;
+	struct secpolicy **spp, *newsp;
+	int error, flags;
+
+	xpl = (struct sadb_x_policy *)request;
+	/* Select direction. */
+	switch (xpl->sadb_x_policy_dir) {
+	case IPSEC_DIR_INBOUND:
+	case IPSEC_DIR_OUTBOUND:
+		break;
+	default:
+		ipseclog((LOG_ERR, "%s: invalid direction=%u\n", __func__,
+			xpl->sadb_x_policy_dir));
+		return (EINVAL);
+	}
+	/*
+	 * Privileged sockets are allowed to set own security policy
+	 * and configure IPsec bypass. Unprivileged sockets only can
+	 * have ENTRUST policy.
+	 */
+	switch (xpl->sadb_x_policy_type) {
+	case IPSEC_POLICY_IPSEC:
+	case IPSEC_POLICY_BYPASS:
+		if (cred != NULL &&
+		    priv_check_cred(cred, PRIV_NETINET_IPSEC, 0) != 0)
+			return (EACCES);
+		/* Allocate new SP entry. */
+		newsp = key_msg2sp(xpl, len, &error);
+		if (newsp == NULL)
+			return (error);
+		newsp->state = IPSEC_SPSTATE_PCB;
+		newsp->spidx.ul_proto = IPSEC_ULPROTO_ANY;
+#ifdef INET
+		if (inp->inp_vflag & INP_IPV4) {
+			newsp->spidx.src.sin.sin_family =
+			    newsp->spidx.dst.sin.sin_family = AF_INET;
+			newsp->spidx.src.sin.sin_len =
+			    newsp->spidx.dst.sin.sin_len =
+			    sizeof(struct sockaddr_in);
+		}
+#endif
+#ifdef INET6
+		if (inp->inp_vflag & INP_IPV6) {
+			newsp->spidx.src.sin6.sin6_family =
+			    newsp->spidx.dst.sin6.sin6_family = AF_INET6;
+			newsp->spidx.src.sin6.sin6_len =
+			    newsp->spidx.dst.sin6.sin6_len =
+			    sizeof(struct sockaddr_in6);
+		}
+#endif
+		break;
+	case IPSEC_POLICY_ENTRUST:
+		/* We just use NULL pointer for ENTRUST policy */
+		newsp = NULL;
+		break;
+	default:
+		/* Other security policy types aren't allowed for PCB */
+		return (EINVAL);
+	}
+
+	INP_WLOCK(inp);
+	if (xpl->sadb_x_policy_dir == IPSEC_DIR_INBOUND) {
+		spp = &inp->inp_sp->sp_in;
+		flags = INP_INBOUND_POLICY;
+	} else {
+		spp = &inp->inp_sp->sp_out;
+		flags = INP_OUTBOUND_POLICY;
+	}
+	/* Clear old SP and set new SP. */
+	if (*spp != NULL)
+		key_freesp(spp);
+	*spp = newsp;
+	KEYDBG(IPSEC_DUMP,
+	    printf("%s: new SP(%p)\n", __func__, newsp));
+	if (newsp == NULL)
+		inp->inp_sp->flags &= ~flags;
+	else {
+		inp->inp_sp->flags |= flags;
+		KEYDBG(IPSEC_DUMP, kdebug_secpolicy(newsp));
+	}
+	INP_WUNLOCK(inp);
+	return (0);
+}
+
+static int
+ipsec_get_pcbpolicy(struct inpcb *inp, void *request, size_t *len)
+{
+	struct sadb_x_policy *xpl;
+	struct secpolicy *sp;
+	int error, flags;
+
+	xpl = (struct sadb_x_policy *)request;
+
+	INP_RLOCK(inp);
+	flags = inp->inp_sp->flags;
+	/* Select direction. */
+	switch (xpl->sadb_x_policy_dir) {
+	case IPSEC_DIR_INBOUND:
+		sp = inp->inp_sp->sp_in;
+		flags &= INP_INBOUND_POLICY;
+		break;
+	case IPSEC_DIR_OUTBOUND:
+		sp = inp->inp_sp->sp_out;
+		flags &= INP_OUTBOUND_POLICY;
+		break;
+	default:
+		INP_RUNLOCK(inp);
+		ipseclog((LOG_ERR, "%s: invalid direction=%u\n", __func__,
+			xpl->sadb_x_policy_dir));
+		return (EINVAL);
+	}
+
+	if (flags == 0) {
+		/* Return ENTRUST policy */
+		INP_RUNLOCK(inp);
+		xpl->sadb_x_policy_exttype = SADB_X_EXT_POLICY;
+		xpl->sadb_x_policy_type = IPSEC_POLICY_ENTRUST;
+		xpl->sadb_x_policy_id = 0;
+		xpl->sadb_x_policy_priority = 0;
+		xpl->sadb_x_policy_len = PFKEY_UNIT64(sizeof(*xpl));
+		*len = sizeof(*xpl);
+		return (0);
+	}
+
+	IPSEC_ASSERT(sp != NULL,
+	    ("sp is NULL, but flags is 0x%04x", inp->inp_sp->flags));
+
+	key_addref(sp);
+	INP_RUNLOCK(inp);
+	error = key_sp2msg(sp, request, len);
+	key_freesp(&sp);
+	if (error == EINVAL)
+		return (error);
+	/*
+	 * We return "success", but user should check *len.
+	 * *len will be set to size of valid data and
+	 * sadb_x_policy_len will contain needed size.
+	 */
+	return (0);
+}
+
+/* Handle socket option control request for PCB */
+static int
+ipsec_control_pcbpolicy(struct inpcb *inp, struct sockopt *sopt)
+{
+	void *optdata;
+	size_t optlen;
+	int error;
+
+	if (inp->inp_sp == NULL)
+		return (ENOPROTOOPT);
+
+	/* Limit maximum request size to PAGE_SIZE */
+	optlen = sopt->sopt_valsize;
+	if (optlen < sizeof(struct sadb_x_policy) || optlen > PAGE_SIZE)
+		return (EINVAL);
+
+	optdata = malloc(optlen, M_TEMP, sopt->sopt_td ? M_WAITOK: M_NOWAIT);
+	if (optdata == NULL)
+		return (ENOBUFS);
+	/*
+	 * We need a hint from the user, what policy is requested - input
+	 * or output? User should specify it in the buffer, even for
+	 * setsockopt().
+	 */
+	error = sooptcopyin(sopt, optdata, optlen, optlen);
+	if (error == 0) {
+		if (sopt->sopt_dir == SOPT_SET)
+			error = ipsec_set_pcbpolicy(inp,
+			    sopt->sopt_td ? sopt->sopt_td->td_ucred: NULL,
+			    optdata, optlen);
+		else {
+			error = ipsec_get_pcbpolicy(inp, optdata, &optlen);
+			if (error == 0)
+				error = sooptcopyout(sopt, optdata, optlen);
+		}
+	}
+	free(optdata, M_TEMP);
+	return (error);
+}
+
+#ifdef INET
+/*
+ * IPSEC_PCBCTL() method implementation for IPv4.
+ */
+int
+ipsec4_pcbctl(struct inpcb *inp, struct sockopt *sopt)
+{
+
+	if (sopt->sopt_name != IP_IPSEC_POLICY)
+		return (ENOPROTOOPT);
+	return (ipsec_control_pcbpolicy(inp, sopt));
+}
+#endif
+
+#ifdef INET6
+/*
+ * IPSEC_PCBCTL() method implementation for IPv6.
+ */
+int
+ipsec6_pcbctl(struct inpcb *inp, struct sockopt *sopt)
+{
+
+	if (sopt->sopt_name != IPV6_IPSEC_POLICY)
+		return (ENOPROTOOPT);
+	return (ipsec_control_pcbpolicy(inp, sopt));
+}
+#endif
+
diff --git a/freebsd/sys/netipsec/ipsec_support.h b/freebsd/sys/netipsec/ipsec_support.h
new file mode 100644
index 00000000..b72aee20
--- /dev/null
+++ b/freebsd/sys/netipsec/ipsec_support.h
@@ -0,0 +1,190 @@
+/*-
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _NETIPSEC_IPSEC_SUPPORT_H_
+#define	_NETIPSEC_IPSEC_SUPPORT_H_
+
+#ifdef _KERNEL
+#if defined(IPSEC) || defined(IPSEC_SUPPORT)
+struct mbuf;
+struct inpcb;
+struct tcphdr;
+struct sockopt;
+struct sockaddr;
+struct ipsec_support;
+struct tcpmd5_support;
+
+size_t ipsec_hdrsiz_inpcb(struct inpcb *);
+int ipsec_init_pcbpolicy(struct inpcb *);
+int ipsec_delete_pcbpolicy(struct inpcb *);
+int ipsec_copy_pcbpolicy(struct inpcb *, struct inpcb *);
+
+struct ipsec_methods {
+	int	(*input)(struct mbuf *, int, int);
+	int	(*check_policy)(const struct mbuf *, struct inpcb *);
+	int	(*forward)(struct mbuf *);
+	int	(*output)(struct mbuf *, struct inpcb *);
+	int	(*pcbctl)(struct inpcb *, struct sockopt *);
+	size_t	(*hdrsize)(struct inpcb *);
+	int	(*capability)(struct mbuf *, u_int);
+	int	(*ctlinput)(int, struct sockaddr *, void *);
+
+	int	(*udp_input)(struct mbuf *, int, int);
+	int	(*udp_pcbctl)(struct inpcb *, struct sockopt *);
+};
+#define	IPSEC_CAP_OPERABLE		1
+#define	IPSEC_CAP_BYPASS_FILTER		2
+
+struct tcpmd5_methods {
+	int	(*input)(struct mbuf *, struct tcphdr *, u_char *);
+	int	(*output)(struct mbuf *, struct tcphdr *, u_char *);
+	int	(*pcbctl)(struct inpcb *, struct sockopt *);
+};
+
+#define	IPSEC_MODULE_ENABLED	0x0001
+#define	IPSEC_ENABLED(proto)	\
+    ((proto ## _ipsec_support)->enabled & IPSEC_MODULE_ENABLED)
+#define	TCPMD5_ENABLED()	IPSEC_ENABLED(tcp)
+
+#ifdef TCP_SIGNATURE
+/* TCP-MD5 build in the kernel */
+struct tcpmd5_support {
+	const u_int enabled;
+	const struct tcpmd5_methods * const methods;
+};
+extern const struct tcpmd5_support * const tcp_ipsec_support;
+
+#define	TCPMD5_INPUT(m, ...)		\
+    (*tcp_ipsec_support->methods->input)(m, __VA_ARGS__)
+#define	TCPMD5_OUTPUT(m, ...)		\
+    (*tcp_ipsec_support->methods->output)(m, __VA_ARGS__)
+#define	TCPMD5_PCBCTL(inp, sopt)	\
+    (*tcp_ipsec_support->methods->pcbctl)(inp, sopt)
+#elif defined(IPSEC_SUPPORT)
+/* TCP-MD5 build as module */
+struct tcpmd5_support {
+	volatile u_int enabled;
+	const struct tcpmd5_methods * volatile methods;
+};
+extern struct tcpmd5_support * const tcp_ipsec_support;
+
+void tcpmd5_support_enable(const struct tcpmd5_methods * const);
+void tcpmd5_support_disable(void);
+
+int tcpmd5_kmod_pcbctl(struct tcpmd5_support * const, struct inpcb *,
+    struct sockopt *);
+int tcpmd5_kmod_input(struct tcpmd5_support * const, struct mbuf *,
+    struct tcphdr *, u_char *);
+int tcpmd5_kmod_output(struct tcpmd5_support * const, struct mbuf *,
+    struct tcphdr *, u_char *);
+#define	TCPMD5_INPUT(m, ...)		\
+    tcpmd5_kmod_input(tcp_ipsec_support, m, __VA_ARGS__)
+#define	TCPMD5_OUTPUT(m, ...)		\
+    tcpmd5_kmod_output(tcp_ipsec_support, m, __VA_ARGS__)
+#define	TCPMD5_PCBCTL(inp, sopt)	\
+    tcpmd5_kmod_pcbctl(tcp_ipsec_support, inp, sopt)
+#endif
+
+#endif /* IPSEC || IPSEC_SUPPORT */
+
+#if defined(IPSEC)
+struct ipsec_support {
+	const u_int enabled;
+	const struct ipsec_methods * const methods;
+};
+extern const struct ipsec_support * const ipv4_ipsec_support;
+extern const struct ipsec_support * const ipv6_ipsec_support;
+
+#define	IPSEC_INPUT(proto, m, ...)		\
+    (*(proto ## _ipsec_support)->methods->input)(m, __VA_ARGS__)
+#define	IPSEC_CHECK_POLICY(proto, m, ...)	\
+    (*(proto ## _ipsec_support)->methods->check_policy)(m, __VA_ARGS__)
+#define	IPSEC_FORWARD(proto, m)		\
+    (*(proto ## _ipsec_support)->methods->forward)(m)
+#define	IPSEC_OUTPUT(proto, m, ...)		\
+    (*(proto ## _ipsec_support)->methods->output)(m, __VA_ARGS__)
+#define	IPSEC_PCBCTL(proto, inp, sopt)		\
+    (*(proto ## _ipsec_support)->methods->pcbctl)(inp, sopt)
+#define	IPSEC_CAPS(proto, m, ...)		\
+    (*(proto ## _ipsec_support)->methods->capability)(m, __VA_ARGS__)
+#define	IPSEC_HDRSIZE(proto, inp)		\
+    (*(proto ## _ipsec_support)->methods->hdrsize)(inp)
+
+#define	UDPENCAP_INPUT(m, ...)			\
+    (*ipv4_ipsec_support->methods->udp_input)(m, __VA_ARGS__)
+#define	UDPENCAP_PCBCTL(inp, sopt)		\
+    (*ipv4_ipsec_support->methods->udp_pcbctl)(inp, sopt)
+
+#elif defined(IPSEC_SUPPORT)
+struct ipsec_support {
+	volatile u_int enabled;
+	const struct ipsec_methods * volatile methods;
+};
+extern struct ipsec_support * const ipv4_ipsec_support;
+extern struct ipsec_support * const ipv6_ipsec_support;
+
+void ipsec_support_enable(struct ipsec_support * const,
+    const struct ipsec_methods * const);
+void ipsec_support_disable(struct ipsec_support * const);
+
+int ipsec_kmod_input(struct ipsec_support * const, struct mbuf *, int, int);
+int ipsec_kmod_check_policy(struct ipsec_support * const, struct mbuf *,
+    struct inpcb *);
+int ipsec_kmod_forward(struct ipsec_support * const, struct mbuf *);
+int ipsec_kmod_output(struct ipsec_support * const, struct mbuf *,
+    struct inpcb *);
+int ipsec_kmod_pcbctl(struct ipsec_support * const, struct inpcb *,
+    struct sockopt *);
+int ipsec_kmod_capability(struct ipsec_support * const, struct mbuf *, u_int);
+size_t ipsec_kmod_hdrsize(struct ipsec_support * const, struct inpcb *);
+int ipsec_kmod_udp_input(struct ipsec_support * const, struct mbuf *, int, int);
+int ipsec_kmod_udp_pcbctl(struct ipsec_support * const, struct inpcb *,
+    struct sockopt *);
+
+#define	UDPENCAP_INPUT(m, ...)		\
+    ipsec_kmod_udp_input(ipv4_ipsec_support, m, __VA_ARGS__)
+#define	UDPENCAP_PCBCTL(inp, sopt)	\
+    ipsec_kmod_udp_pcbctl(ipv4_ipsec_support, inp, sopt)
+
+#define	IPSEC_INPUT(proto, ...)		\
+    ipsec_kmod_input(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_CHECK_POLICY(proto, ...)	\
+    ipsec_kmod_check_policy(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_FORWARD(proto, ...)	\
+    ipsec_kmod_forward(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_OUTPUT(proto, ...)	\
+    ipsec_kmod_output(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_PCBCTL(proto, ...)	\
+    ipsec_kmod_pcbctl(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_CAPS(proto, ...)		\
+    ipsec_kmod_capability(proto ## _ipsec_support, __VA_ARGS__)
+#define	IPSEC_HDRSIZE(proto, ...)	\
+    ipsec_kmod_hdrsize(proto ## _ipsec_support, __VA_ARGS__)
+#endif /* IPSEC_SUPPORT */
+#endif /* _KERNEL */
+#endif /* _NETIPSEC_IPSEC_SUPPORT_H_ */
diff --git a/freebsd/sys/netipsec/key.c b/freebsd/sys/netipsec/key.c
index 6619c6db..5d3a612d 100644
--- a/freebsd/sys/netipsec/key.c
+++ b/freebsd/sys/netipsec/key.c
@@ -44,12 +44,14 @@
 #include <rtems/bsd/sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
+#include <sys/fnv_hash.h>
 #include <rtems/bsd/sys/lock.h>
 #include <sys/mutex.h>
 #include <sys/mbuf.h>
 #include <sys/domain.h>
 #include <sys/protosw.h>
 #include <sys/malloc.h>
+#include <sys/rmlock.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
@@ -59,15 +61,18 @@
 #include <sys/refcount.h>
 #include <sys/syslog.h>
 
+#include <vm/uma.h>
+
 #include <net/if.h>
-#include <net/route.h>
-#include <net/raw_cb.h>
+#include <net/if_var.h>
 #include <net/vnet.h>
+#include <net/raw_cb.h>
 
 #include <netinet/in.h>
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
 #include <netinet/in_var.h>
+#include <netinet/udp.h>
 
 #ifdef INET6
 #include <netinet/ip6.h>
@@ -75,13 +80,6 @@
 #include <netinet6/ip6_var.h>
 #endif /* INET6 */
 
-#if defined(INET) || defined(INET6)
-#include <netinet/in_pcb.h>
-#endif
-#ifdef INET6
-#include <netinet6/in6_pcb.h>
-#endif /* INET6 */
-
 #include <net/pfkeyv2.h>
 #include <netipsec/keydb.h>
 #include <netipsec/key.h>
@@ -94,7 +92,7 @@
 #endif
 
 #include <netipsec/xform.h>
-
+#include <machine/in_cksum.h>
 #include <machine/stdarg.h>
 
 /* randomness */
@@ -142,28 +140,118 @@ static VNET_DEFINE(int, key_preferred_oldsa) = 1;
 static VNET_DEFINE(u_int32_t, acq_seq) = 0;
 #define	V_acq_seq		VNET(acq_seq)
 
-								/* SPD */
-static VNET_DEFINE(LIST_HEAD(_sptree, secpolicy), sptree[IPSEC_DIR_MAX]);
+static VNET_DEFINE(uint32_t, sp_genid) = 0;
+#define	V_sp_genid		VNET(sp_genid)
+
+/* SPD */
+TAILQ_HEAD(secpolicy_queue, secpolicy);
+LIST_HEAD(secpolicy_list, secpolicy);
+static VNET_DEFINE(struct secpolicy_queue, sptree[IPSEC_DIR_MAX]);
+static VNET_DEFINE(struct secpolicy_queue, sptree_ifnet[IPSEC_DIR_MAX]);
+static struct rmlock sptree_lock;
 #define	V_sptree		VNET(sptree)
-static struct mtx sptree_lock;
-#define	SPTREE_LOCK_INIT() \
-	mtx_init(&sptree_lock, "sptree", \
-		"fast ipsec security policy database", MTX_DEF)
-#define	SPTREE_LOCK_DESTROY()	mtx_destroy(&sptree_lock)
-#define	SPTREE_LOCK()		mtx_lock(&sptree_lock)
-#define	SPTREE_UNLOCK()	mtx_unlock(&sptree_lock)
-#define	SPTREE_LOCK_ASSERT()	mtx_assert(&sptree_lock, MA_OWNED)
-
-static VNET_DEFINE(LIST_HEAD(_sahtree, secashead), sahtree);	/* SAD */
+#define	V_sptree_ifnet		VNET(sptree_ifnet)
+#define	SPTREE_LOCK_INIT()      rm_init(&sptree_lock, "sptree")
+#define	SPTREE_LOCK_DESTROY()   rm_destroy(&sptree_lock)
+#define	SPTREE_RLOCK_TRACKER    struct rm_priotracker sptree_tracker
+#define	SPTREE_RLOCK()          rm_rlock(&sptree_lock, &sptree_tracker)
+#define	SPTREE_RUNLOCK()        rm_runlock(&sptree_lock, &sptree_tracker)
+#define	SPTREE_RLOCK_ASSERT()   rm_assert(&sptree_lock, RA_RLOCKED)
+#define	SPTREE_WLOCK()          rm_wlock(&sptree_lock)
+#define	SPTREE_WUNLOCK()        rm_wunlock(&sptree_lock)
+#define	SPTREE_WLOCK_ASSERT()   rm_assert(&sptree_lock, RA_WLOCKED)
+#define	SPTREE_UNLOCK_ASSERT()  rm_assert(&sptree_lock, RA_UNLOCKED)
+
+/* Hash table for lookup SP using unique id */
+static VNET_DEFINE(struct secpolicy_list *, sphashtbl);
+static VNET_DEFINE(u_long, sphash_mask);
+#define	V_sphashtbl		VNET(sphashtbl)
+#define	V_sphash_mask		VNET(sphash_mask)
+
+#define	SPHASH_NHASH_LOG2	7
+#define	SPHASH_NHASH		(1 << SPHASH_NHASH_LOG2)
+#define	SPHASH_HASHVAL(id)	(key_u32hash(id) & V_sphash_mask)
+#define	SPHASH_HASH(id)		&V_sphashtbl[SPHASH_HASHVAL(id)]
+
+/* SAD */
+TAILQ_HEAD(secashead_queue, secashead);
+LIST_HEAD(secashead_list, secashead);
+static VNET_DEFINE(struct secashead_queue, sahtree);
+static struct rmlock sahtree_lock;
 #define	V_sahtree		VNET(sahtree)
-static struct mtx sahtree_lock;
-#define	SAHTREE_LOCK_INIT() \
-	mtx_init(&sahtree_lock, "sahtree", \
-		"fast ipsec security association database", MTX_DEF)
-#define	SAHTREE_LOCK_DESTROY()	mtx_destroy(&sahtree_lock)
-#define	SAHTREE_LOCK()		mtx_lock(&sahtree_lock)
-#define	SAHTREE_UNLOCK()	mtx_unlock(&sahtree_lock)
-#define	SAHTREE_LOCK_ASSERT()	mtx_assert(&sahtree_lock, MA_OWNED)
+#define	SAHTREE_LOCK_INIT()	rm_init(&sahtree_lock, "sahtree")
+#define	SAHTREE_LOCK_DESTROY()	rm_destroy(&sahtree_lock)
+#define	SAHTREE_RLOCK_TRACKER	struct rm_priotracker sahtree_tracker
+#define	SAHTREE_RLOCK()		rm_rlock(&sahtree_lock, &sahtree_tracker)
+#define	SAHTREE_RUNLOCK()	rm_runlock(&sahtree_lock, &sahtree_tracker)
+#define	SAHTREE_RLOCK_ASSERT()	rm_assert(&sahtree_lock, RA_RLOCKED)
+#define	SAHTREE_WLOCK()		rm_wlock(&sahtree_lock)
+#define	SAHTREE_WUNLOCK()	rm_wunlock(&sahtree_lock)
+#define	SAHTREE_WLOCK_ASSERT()	rm_assert(&sahtree_lock, RA_WLOCKED)
+#define	SAHTREE_UNLOCK_ASSERT()	rm_assert(&sahtree_lock, RA_UNLOCKED)
+
+/* Hash table for lookup in SAD using SA addresses */
+static VNET_DEFINE(struct secashead_list *, sahaddrhashtbl);
+static VNET_DEFINE(u_long, sahaddrhash_mask);
+#define	V_sahaddrhashtbl	VNET(sahaddrhashtbl)
+#define	V_sahaddrhash_mask	VNET(sahaddrhash_mask)
+
+#define	SAHHASH_NHASH_LOG2	7
+#define	SAHHASH_NHASH		(1 << SAHHASH_NHASH_LOG2)
+#define	SAHADDRHASH_HASHVAL(saidx)	\
+    (key_saidxhash(saidx) & V_sahaddrhash_mask)
+#define	SAHADDRHASH_HASH(saidx)		\
+    &V_sahaddrhashtbl[SAHADDRHASH_HASHVAL(saidx)]
+
+/* Hash table for lookup in SAD using SPI */
+LIST_HEAD(secasvar_list, secasvar);
+static VNET_DEFINE(struct secasvar_list *, savhashtbl);
+static VNET_DEFINE(u_long, savhash_mask);
+#define	V_savhashtbl		VNET(savhashtbl)
+#define	V_savhash_mask		VNET(savhash_mask)
+#define	SAVHASH_NHASH_LOG2	7
+#define	SAVHASH_NHASH		(1 << SAVHASH_NHASH_LOG2)
+#define	SAVHASH_HASHVAL(spi)	(key_u32hash(spi) & V_savhash_mask)
+#define	SAVHASH_HASH(spi)	&V_savhashtbl[SAVHASH_HASHVAL(spi)]
+
+static uint32_t
+key_saidxhash(const struct secasindex *saidx)
+{
+	uint32_t hval;
+
+	hval = fnv_32_buf(&saidx->proto, sizeof(saidx->proto),
+	    FNV1_32_INIT);
+	switch (saidx->dst.sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		hval = fnv_32_buf(&saidx->src.sin.sin_addr,
+		    sizeof(in_addr_t), hval);
+		hval = fnv_32_buf(&saidx->dst.sin.sin_addr,
+		    sizeof(in_addr_t), hval);
+		break;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		hval = fnv_32_buf(&saidx->src.sin6.sin6_addr,
+		    sizeof(struct in6_addr), hval);
+		hval = fnv_32_buf(&saidx->dst.sin6.sin6_addr,
+		    sizeof(struct in6_addr), hval);
+		break;
+#endif
+	default:
+		hval = 0;
+		ipseclog((LOG_DEBUG, "%s: unknown address family %d",
+		    __func__, saidx->dst.sa.sa_family));
+	}
+	return (hval);
+}
+
+static uint32_t
+key_u32hash(uint32_t val)
+{
+
+	return (fnv_32_buf(&val, sizeof(val), FNV1_32_INIT));
+}
 
 							/* registed list */
 static VNET_DEFINE(LIST_HEAD(_regtree, secreg), regtree[SADB_SATYPE_MAX + 1]);
@@ -176,16 +264,40 @@ static struct mtx regtree_lock;
 #define	REGTREE_UNLOCK()	mtx_unlock(&regtree_lock)
 #define	REGTREE_LOCK_ASSERT()	mtx_assert(&regtree_lock, MA_OWNED)
 
-static VNET_DEFINE(LIST_HEAD(_acqtree, secacq), acqtree); /* acquiring list */
+/* Acquiring list */
+LIST_HEAD(secacq_list, secacq);
+static VNET_DEFINE(struct secacq_list, acqtree);
 #define	V_acqtree		VNET(acqtree)
 static struct mtx acq_lock;
 #define	ACQ_LOCK_INIT() \
-	mtx_init(&acq_lock, "acqtree", "fast ipsec acquire list", MTX_DEF)
+    mtx_init(&acq_lock, "acqtree", "ipsec SA acquiring list", MTX_DEF)
 #define	ACQ_LOCK_DESTROY()	mtx_destroy(&acq_lock)
 #define	ACQ_LOCK()		mtx_lock(&acq_lock)
 #define	ACQ_UNLOCK()		mtx_unlock(&acq_lock)
 #define	ACQ_LOCK_ASSERT()	mtx_assert(&acq_lock, MA_OWNED)
 
+/* Hash table for lookup in ACQ list using SA addresses */
+static VNET_DEFINE(struct secacq_list *, acqaddrhashtbl);
+static VNET_DEFINE(u_long, acqaddrhash_mask);
+#define	V_acqaddrhashtbl	VNET(acqaddrhashtbl)
+#define	V_acqaddrhash_mask	VNET(acqaddrhash_mask)
+
+/* Hash table for lookup in ACQ list using SEQ number */
+static VNET_DEFINE(struct secacq_list *, acqseqhashtbl);
+static VNET_DEFINE(u_long, acqseqhash_mask);
+#define	V_acqseqhashtbl		VNET(acqseqhashtbl)
+#define	V_acqseqhash_mask	VNET(acqseqhash_mask)
+
+#define	ACQHASH_NHASH_LOG2	7
+#define	ACQHASH_NHASH		(1 << ACQHASH_NHASH_LOG2)
+#define	ACQADDRHASH_HASHVAL(saidx)	\
+    (key_saidxhash(saidx) & V_acqaddrhash_mask)
+#define	ACQSEQHASH_HASHVAL(seq)		\
+    (key_u32hash(seq) & V_acqseqhash_mask)
+#define	ACQADDRHASH_HASH(saidx)	\
+    &V_acqaddrhashtbl[ACQADDRHASH_HASHVAL(saidx)]
+#define	ACQSEQHASH_HASH(seq)	\
+    &V_acqseqhashtbl[ACQSEQHASH_HASHVAL(seq)]
 							/* SP acquiring list */
 static VNET_DEFINE(LIST_HEAD(_spacqtree, secspacq), spacqtree);
 #define	V_spacqtree		VNET(spacqtree)
@@ -198,22 +310,6 @@ static struct mtx spacq_lock;
 #define	SPACQ_UNLOCK()		mtx_unlock(&spacq_lock)
 #define	SPACQ_LOCK_ASSERT()	mtx_assert(&spacq_lock, MA_OWNED)
 
-/* search order for SAs */
-static const u_int saorder_state_valid_prefer_old[] = {
-	SADB_SASTATE_DYING, SADB_SASTATE_MATURE,
-};
-static const u_int saorder_state_valid_prefer_new[] = {
-	SADB_SASTATE_MATURE, SADB_SASTATE_DYING,
-};
-static const u_int saorder_state_alive[] = {
-	/* except DEAD */
-	SADB_SASTATE_MATURE, SADB_SASTATE_DYING, SADB_SASTATE_LARVAL
-};
-static const u_int saorder_state_any[] = {
-	SADB_SASTATE_MATURE, SADB_SASTATE_DYING,
-	SADB_SASTATE_LARVAL, SADB_SASTATE_DEAD
-};
-
 static const int minsize[] = {
 	sizeof(struct sadb_msg),	/* SADB_EXT_RESERVED */
 	sizeof(struct sadb_sa),		/* SADB_EXT_SA */
@@ -241,7 +337,12 @@ static const int minsize[] = {
 	sizeof(struct sadb_address),	/* SADB_X_EXT_NAT_T_OAI */
 	sizeof(struct sadb_address),	/* SADB_X_EXT_NAT_T_OAR */
 	sizeof(struct sadb_x_nat_t_frag),/* SADB_X_EXT_NAT_T_FRAG */
+	sizeof(struct sadb_x_sa_replay), /* SADB_X_EXT_SA_REPLAY */
+	sizeof(struct sadb_address),	/* SADB_X_EXT_NEW_ADDRESS_SRC */
+	sizeof(struct sadb_address),	/* SADB_X_EXT_NEW_ADDRESS_DST */
 };
+_Static_assert(sizeof(minsize)/sizeof(int) == SADB_EXT_MAX + 1, "minsize size mismatch");
+
 static const int maxsize[] = {
 	sizeof(struct sadb_msg),	/* SADB_EXT_RESERVED */
 	sizeof(struct sadb_sa),		/* SADB_EXT_SA */
@@ -269,7 +370,23 @@ static const int maxsize[] = {
 	0,				/* SADB_X_EXT_NAT_T_OAI */
 	0,				/* SADB_X_EXT_NAT_T_OAR */
 	sizeof(struct sadb_x_nat_t_frag),/* SADB_X_EXT_NAT_T_FRAG */
+	sizeof(struct sadb_x_sa_replay), /* SADB_X_EXT_SA_REPLAY */
+	0,				/* SADB_X_EXT_NEW_ADDRESS_SRC */
+	0,				/* SADB_X_EXT_NEW_ADDRESS_DST */
 };
+_Static_assert(sizeof(maxsize)/sizeof(int) == SADB_EXT_MAX + 1, "minsize size mismatch");
+
+/*
+ * Internal values for SA flags:
+ * SADB_X_EXT_F_CLONED means that SA was cloned by key_updateaddresses,
+ *	thus we will not free the most of SA content in key_delsav().
+ */
+#define	SADB_X_EXT_F_CLONED	0x80000000
+
+#define	SADB_CHECKLEN(_mhp, _ext)			\
+    ((_mhp)->extlen[(_ext)] < minsize[(_ext)] || (maxsize[(_ext)] != 0 && \
+	((_mhp)->extlen[(_ext)] > maxsize[(_ext)])))
+#define	SADB_CHECKHDR(_mhp, _ext)	((_mhp)->ext[(_ext)] == NULL)
 
 static VNET_DEFINE(int, ipsec_esp_keymin) = 256;
 static VNET_DEFINE(int, ipsec_esp_auth) = 0;
@@ -279,88 +396,75 @@ static VNET_DEFINE(int, ipsec_ah_keymin) = 128;
 #define	V_ipsec_esp_auth	VNET(ipsec_esp_auth)
 #define	V_ipsec_ah_keymin	VNET(ipsec_ah_keymin)
 
-#ifdef SYSCTL_DECL
-SYSCTL_DECL(_net_key);
+#ifdef IPSEC_DEBUG
+VNET_DEFINE(int, ipsec_debug) = 1;
+#else
+VNET_DEFINE(int, ipsec_debug) = 0;
+#endif
+
+#ifdef INET
+SYSCTL_DECL(_net_inet_ipsec);
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEBUG, debug,
+    CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_debug), 0,
+    "Enable IPsec debugging output when set.");
+#endif
+#ifdef INET6
+SYSCTL_DECL(_net_inet6_ipsec6);
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG, debug,
+    CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_debug), 0,
+    "Enable IPsec debugging output when set.");
 #endif
 
-SYSCTL_VNET_INT(_net_key, KEYCTL_DEBUG_LEVEL,	debug,
-	CTLFLAG_RW, &VNET_NAME(key_debug_level),	0,	"");
+SYSCTL_DECL(_net_key);
+SYSCTL_INT(_net_key, KEYCTL_DEBUG_LEVEL,	debug,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_debug_level), 0, "");
 
 /* max count of trial for the decision of spi value */
-SYSCTL_VNET_INT(_net_key, KEYCTL_SPI_TRY, spi_trycnt,
-	CTLFLAG_RW, &VNET_NAME(key_spi_trycnt),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_SPI_TRY, spi_trycnt,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_spi_trycnt), 0, "");
 
 /* minimum spi value to allocate automatically. */
-SYSCTL_VNET_INT(_net_key, KEYCTL_SPI_MIN_VALUE,
-	spi_minval,	CTLFLAG_RW, &VNET_NAME(key_spi_minval),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_SPI_MIN_VALUE, spi_minval,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_spi_minval), 0, "");
 
 /* maximun spi value to allocate automatically. */
-SYSCTL_VNET_INT(_net_key, KEYCTL_SPI_MAX_VALUE,
-	spi_maxval,	CTLFLAG_RW, &VNET_NAME(key_spi_maxval),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_SPI_MAX_VALUE, spi_maxval,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_spi_maxval), 0, "");
 
 /* interval to initialize randseed */
-SYSCTL_VNET_INT(_net_key, KEYCTL_RANDOM_INT,
-	int_random,	CTLFLAG_RW, &VNET_NAME(key_int_random),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_RANDOM_INT, int_random,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_int_random), 0, "");
 
 /* lifetime for larval SA */
-SYSCTL_VNET_INT(_net_key, KEYCTL_LARVAL_LIFETIME,
-	larval_lifetime, CTLFLAG_RW, &VNET_NAME(key_larval_lifetime),	0, "");
+SYSCTL_INT(_net_key, KEYCTL_LARVAL_LIFETIME, larval_lifetime,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_larval_lifetime), 0, "");
 
 /* counter for blocking to send SADB_ACQUIRE to IKEd */
-SYSCTL_VNET_INT(_net_key, KEYCTL_BLOCKACQ_COUNT,
-	blockacq_count,	CTLFLAG_RW, &VNET_NAME(key_blockacq_count),	0, "");
+SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_COUNT, blockacq_count,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_blockacq_count), 0, "");
 
 /* lifetime for blocking to send SADB_ACQUIRE to IKEd */
-SYSCTL_VNET_INT(_net_key, KEYCTL_BLOCKACQ_LIFETIME,
-	blockacq_lifetime, CTLFLAG_RW, &VNET_NAME(key_blockacq_lifetime), 0, "");
+SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_LIFETIME, blockacq_lifetime,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_blockacq_lifetime), 0, "");
 
 /* ESP auth */
-SYSCTL_VNET_INT(_net_key, KEYCTL_ESP_AUTH,	esp_auth,
-	CTLFLAG_RW, &VNET_NAME(ipsec_esp_auth),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_ESP_AUTH, esp_auth,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_esp_auth), 0, "");
 
 /* minimum ESP key length */
-SYSCTL_VNET_INT(_net_key, KEYCTL_ESP_KEYMIN,
-	esp_keymin, CTLFLAG_RW, &VNET_NAME(ipsec_esp_keymin),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_ESP_KEYMIN, esp_keymin,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_esp_keymin), 0, "");
 
 /* minimum AH key length */
-SYSCTL_VNET_INT(_net_key, KEYCTL_AH_KEYMIN,	ah_keymin,
-	CTLFLAG_RW, &VNET_NAME(ipsec_ah_keymin),	0,	"");
+SYSCTL_INT(_net_key, KEYCTL_AH_KEYMIN, ah_keymin,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipsec_ah_keymin), 0, "");
 
 /* perfered old SA rather than new SA */
-SYSCTL_VNET_INT(_net_key, KEYCTL_PREFERED_OLDSA,
-	preferred_oldsa, CTLFLAG_RW, &VNET_NAME(key_preferred_oldsa),	0, "");
+SYSCTL_INT(_net_key, KEYCTL_PREFERED_OLDSA, preferred_oldsa,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(key_preferred_oldsa), 0, "");
 
 #define __LIST_CHAINED(elm) \
 	(!((elm)->chain.le_next == NULL && (elm)->chain.le_prev == NULL))
-#define LIST_INSERT_TAIL(head, elm, type, field) \
-do {\
-	struct type *curelm = LIST_FIRST(head); \
-	if (curelm == NULL) {\
-		LIST_INSERT_HEAD(head, elm, field); \
-	} else { \
-		while (LIST_NEXT(curelm, field)) \
-			curelm = LIST_NEXT(curelm, field);\
-		LIST_INSERT_AFTER(curelm, elm, field);\
-	}\
-} while (0)
-
-#define KEY_CHKSASTATE(head, sav, name) \
-do { \
-	if ((head) != (sav)) {						\
-		ipseclog((LOG_DEBUG, "%s: state mismatched (TREE=%d SA=%d)\n", \
-			(name), (head), (sav)));			\
-		continue;						\
-	}								\
-} while (0)
-
-#define KEY_CHKSPDIR(head, sp, name) \
-do { \
-	if ((head) != (sp)) {						\
-		ipseclog((LOG_DEBUG, "%s: direction mismatched (TREE=%d SP=%d), " \
-			"anyway continue.\n",				\
-			(name), (head), (sp)));				\
-	}								\
-} while (0)
 
 MALLOC_DEFINE(M_IPSEC_SA, "secasvar", "ipsec security association");
 MALLOC_DEFINE(M_IPSEC_SAH, "sahead", "ipsec sa head");
@@ -370,6 +474,17 @@ MALLOC_DEFINE(M_IPSEC_MISC, "ipsec-misc", "ipsec miscellaneous");
 MALLOC_DEFINE(M_IPSEC_SAQ, "ipsec-saq", "ipsec sa acquire");
 MALLOC_DEFINE(M_IPSEC_SAR, "ipsec-reg", "ipsec sa acquire");
 
+static VNET_DEFINE(uma_zone_t, key_lft_zone);
+#define	V_key_lft_zone		VNET(key_lft_zone)
+
+static LIST_HEAD(xforms_list, xformsw) xforms = LIST_HEAD_INITIALIZER();
+static struct mtx xforms_lock;
+#define	XFORMS_LOCK_INIT()	\
+    mtx_init(&xforms_lock, "xforms_list", "IPsec transforms list", MTX_DEF)
+#define	XFORMS_LOCK_DESTROY()	mtx_destroy(&xforms_lock)
+#define	XFORMS_LOCK()		mtx_lock(&xforms_lock)
+#define	XFORMS_UNLOCK()		mtx_unlock(&xforms_lock)
+
 /*
  * set parameters into secpolicyindex buffer.
  * Must allocate secpolicyindex buffer passed to this function.
@@ -397,6 +512,8 @@ do { \
 	(idx)->reqid = (r);                                                  \
 	bcopy((s), &(idx)->src, ((const struct sockaddr *)(s))->sa_len);     \
 	bcopy((d), &(idx)->dst, ((const struct sockaddr *)(d))->sa_len);     \
+	key_porttosaddr(&(idx)->src.sa, 0);				     \
+	key_porttosaddr(&(idx)->dst.sa, 0);				     \
 } while (0)
 
 /* key statistics */
@@ -411,176 +528,206 @@ struct sadb_msghdr {
 	int extlen[SADB_EXT_MAX + 1];
 };
 
-static struct secasvar *key_allocsa_policy __P((const struct secasindex *));
-static void key_freesp_so __P((struct secpolicy **));
-static struct secasvar *key_do_allocsa_policy __P((struct secashead *, u_int));
-static void key_delsp __P((struct secpolicy *));
-static struct secpolicy *key_getsp __P((struct secpolicyindex *));
-static void _key_delsp(struct secpolicy *sp);
-static struct secpolicy *key_getspbyid __P((u_int32_t));
-static u_int32_t key_newreqid __P((void));
-static struct mbuf *key_gather_mbuf __P((struct mbuf *,
-	const struct sadb_msghdr *, int, int, ...));
-static int key_spdadd __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static u_int32_t key_getnewspid __P((void));
-static int key_spddelete __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_spddelete2 __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_spdget __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_spdflush __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_spddump __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static struct mbuf *key_setdumpsp __P((struct secpolicy *,
-	u_int8_t, u_int32_t, u_int32_t));
-static u_int key_getspreqmsglen __P((struct secpolicy *));
-static int key_spdexpire __P((struct secpolicy *));
-static struct secashead *key_newsah __P((struct secasindex *));
-static void key_delsah __P((struct secashead *));
-static struct secasvar *key_newsav __P((struct mbuf *,
-	const struct sadb_msghdr *, struct secashead *, int *,
-	const char*, int));
-#define	KEY_NEWSAV(m, sadb, sah, e)				\
-	key_newsav(m, sadb, sah, e, __FILE__, __LINE__)
-static void key_delsav __P((struct secasvar *));
-static struct secashead *key_getsah __P((struct secasindex *));
-static struct secasvar *key_checkspidup __P((struct secasindex *, u_int32_t));
-static struct secasvar *key_getsavbyspi __P((struct secashead *, u_int32_t));
-static int key_setsaval __P((struct secasvar *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_mature __P((struct secasvar *));
-static struct mbuf *key_setdumpsa __P((struct secasvar *, u_int8_t,
-	u_int8_t, u_int32_t, u_int32_t));
-static struct mbuf *key_setsadbmsg __P((u_int8_t, u_int16_t, u_int8_t,
-	u_int32_t, pid_t, u_int16_t));
-static struct mbuf *key_setsadbsa __P((struct secasvar *));
-static struct mbuf *key_setsadbaddr __P((u_int16_t,
-	const struct sockaddr *, u_int8_t, u_int16_t));
-#ifdef IPSEC_NAT_T
+static struct supported_ealgs {
+	int sadb_alg;
+	const struct enc_xform *xform;
+} supported_ealgs[] = {
+	{ SADB_EALG_DESCBC,		&enc_xform_des },
+	{ SADB_EALG_3DESCBC,		&enc_xform_3des },
+	{ SADB_X_EALG_AES,		&enc_xform_rijndael128 },
+	{ SADB_X_EALG_BLOWFISHCBC,	&enc_xform_blf },
+	{ SADB_X_EALG_CAST128CBC,	&enc_xform_cast5 },
+	{ SADB_EALG_NULL,		&enc_xform_null },
+	{ SADB_X_EALG_CAMELLIACBC,	&enc_xform_camellia },
+	{ SADB_X_EALG_AESCTR,		&enc_xform_aes_icm },
+	{ SADB_X_EALG_AESGCM16,		&enc_xform_aes_nist_gcm },
+	{ SADB_X_EALG_AESGMAC,		&enc_xform_aes_nist_gmac },
+};
+
+static struct supported_aalgs {
+	int sadb_alg;
+	const struct auth_hash *xform;
+} supported_aalgs[] = {
+	{ SADB_X_AALG_NULL,		&auth_hash_null },
+	{ SADB_AALG_MD5HMAC,		&auth_hash_hmac_md5 },
+	{ SADB_AALG_SHA1HMAC,		&auth_hash_hmac_sha1 },
+	{ SADB_X_AALG_RIPEMD160HMAC,	&auth_hash_hmac_ripemd_160 },
+	{ SADB_X_AALG_MD5,		&auth_hash_key_md5 },
+	{ SADB_X_AALG_SHA,		&auth_hash_key_sha1 },
+	{ SADB_X_AALG_SHA2_256,		&auth_hash_hmac_sha2_256 },
+	{ SADB_X_AALG_SHA2_384,		&auth_hash_hmac_sha2_384 },
+	{ SADB_X_AALG_SHA2_512,		&auth_hash_hmac_sha2_512 },
+	{ SADB_X_AALG_AES128GMAC,	&auth_hash_nist_gmac_aes_128 },
+	{ SADB_X_AALG_AES192GMAC,	&auth_hash_nist_gmac_aes_192 },
+	{ SADB_X_AALG_AES256GMAC,	&auth_hash_nist_gmac_aes_256 },
+};
+
+static struct supported_calgs {
+	int sadb_alg;
+	const struct comp_algo *xform;
+} supported_calgs[] = {
+	{ SADB_X_CALG_DEFLATE,		&comp_algo_deflate },
+};
+
+#ifndef IPSEC_DEBUG2
+static struct callout key_timer;
+#endif
+
+static void key_unlink(struct secpolicy *);
+static struct secpolicy *key_getsp(struct secpolicyindex *);
+static struct secpolicy *key_getspbyid(u_int32_t);
+static struct mbuf *key_gather_mbuf(struct mbuf *,
+	const struct sadb_msghdr *, int, int, ...);
+static int key_spdadd(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static uint32_t key_getnewspid(void);
+static int key_spddelete(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_spddelete2(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_spdget(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_spdflush(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_spddump(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static struct mbuf *key_setdumpsp(struct secpolicy *,
+	u_int8_t, u_int32_t, u_int32_t);
+static struct mbuf *key_sp2mbuf(struct secpolicy *);
+static size_t key_getspreqmsglen(struct secpolicy *);
+static int key_spdexpire(struct secpolicy *);
+static struct secashead *key_newsah(struct secasindex *);
+static void key_freesah(struct secashead **);
+static void key_delsah(struct secashead *);
+static struct secasvar *key_newsav(const struct sadb_msghdr *,
+    struct secasindex *, uint32_t, int *);
+static void key_delsav(struct secasvar *);
+static void key_unlinksav(struct secasvar *);
+static struct secashead *key_getsah(struct secasindex *);
+static int key_checkspidup(uint32_t);
+static struct secasvar *key_getsavbyspi(uint32_t);
+static int key_setnatt(struct secasvar *, const struct sadb_msghdr *);
+static int key_setsaval(struct secasvar *, const struct sadb_msghdr *);
+static int key_updatelifetimes(struct secasvar *, const struct sadb_msghdr *);
+static int key_updateaddresses(struct socket *, struct mbuf *,
+    const struct sadb_msghdr *, struct secasvar *, struct secasindex *);
+
+static struct mbuf *key_setdumpsa(struct secasvar *, u_int8_t,
+	u_int8_t, u_int32_t, u_int32_t);
+static struct mbuf *key_setsadbmsg(u_int8_t, u_int16_t, u_int8_t,
+	u_int32_t, pid_t, u_int16_t);
+static struct mbuf *key_setsadbsa(struct secasvar *);
+static struct mbuf *key_setsadbaddr(u_int16_t,
+	const struct sockaddr *, u_int8_t, u_int16_t);
 static struct mbuf *key_setsadbxport(u_int16_t, u_int16_t);
 static struct mbuf *key_setsadbxtype(u_int16_t);
-#endif
-static void key_porttosaddr(struct sockaddr *, u_int16_t);
-#define	KEY_PORTTOSADDR(saddr, port)				\
-	key_porttosaddr((struct sockaddr *)(saddr), (port))
-static struct mbuf *key_setsadbxsa2 __P((u_int8_t, u_int32_t, u_int32_t));
-static struct mbuf *key_setsadbxpolicy __P((u_int16_t, u_int8_t,
-	u_int32_t));
-static struct seckey *key_dup_keymsg(const struct sadb_key *, u_int, 
-				     struct malloc_type *);
+static struct mbuf *key_setsadbxsa2(u_int8_t, u_int32_t, u_int32_t);
+static struct mbuf *key_setsadbxsareplay(u_int32_t);
+static struct mbuf *key_setsadbxpolicy(u_int16_t, u_int8_t,
+	u_int32_t, u_int32_t);
+static struct seckey *key_dup_keymsg(const struct sadb_key *, size_t,
+    struct malloc_type *);
 static struct seclifetime *key_dup_lifemsg(const struct sadb_lifetime *src,
-					    struct malloc_type *type);
-#ifdef INET6
-static int key_ismyaddr6 __P((struct sockaddr_in6 *));
-#endif
+    struct malloc_type *);
 
 /* flags for key_cmpsaidx() */
 #define CMP_HEAD	1	/* protocol, addresses. */
 #define CMP_MODE_REQID	2	/* additionally HEAD, reqid, mode. */
 #define CMP_REQID	3	/* additionally HEAD, reaid. */
 #define CMP_EXACTLY	4	/* all elements. */
-static int key_cmpsaidx
-	__P((const struct secasindex *, const struct secasindex *, int));
-
-static int key_cmpspidx_exactly
-	__P((struct secpolicyindex *, struct secpolicyindex *));
-static int key_cmpspidx_withmask
-	__P((struct secpolicyindex *, struct secpolicyindex *));
-static int key_sockaddrcmp __P((const struct sockaddr *, const struct sockaddr *, int));
-static int key_bbcmp __P((const void *, const void *, u_int));
-static u_int16_t key_satype2proto __P((u_int8_t));
-static u_int8_t key_proto2satype __P((u_int16_t));
-
-static int key_getspi __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static u_int32_t key_do_getnewspi __P((struct sadb_spirange *,
-					struct secasindex *));
-static int key_update __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-#ifdef IPSEC_DOSEQCHECK
-static struct secasvar *key_getsavbyseq __P((struct secashead *, u_int32_t));
-#endif
-static int key_add __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_setident __P((struct secashead *, struct mbuf *,
-	const struct sadb_msghdr *));
-static struct mbuf *key_getmsgbuf_x1 __P((struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_delete __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_get __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-
-static void key_getcomb_setlifetime __P((struct sadb_comb *));
-static struct mbuf *key_getcomb_esp __P((void));
-static struct mbuf *key_getcomb_ah __P((void));
-static struct mbuf *key_getcomb_ipcomp __P((void));
-static struct mbuf *key_getprop __P((const struct secasindex *));
-
-static int key_acquire __P((const struct secasindex *, struct secpolicy *));
-static struct secacq *key_newacq __P((const struct secasindex *));
-static struct secacq *key_getacq __P((const struct secasindex *));
-static struct secacq *key_getacqbyseq __P((u_int32_t));
-static struct secspacq *key_newspacq __P((struct secpolicyindex *));
-static struct secspacq *key_getspacq __P((struct secpolicyindex *));
-static int key_acquire2 __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_register __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_expire __P((struct secasvar *));
-static int key_flush __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_dump __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_promisc __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *));
-static int key_senderror __P((struct socket *, struct mbuf *, int));
-static int key_validate_ext __P((const struct sadb_ext *, int));
-static int key_align __P((struct mbuf *, struct sadb_msghdr *));
-static struct mbuf *key_setlifetime(struct seclifetime *src, 
-				     u_int16_t exttype);
-static struct mbuf *key_setkey(struct seckey *src, u_int16_t exttype);
-
-#if 0
-static const char *key_getfqdn __P((void));
-static const char *key_getuserfqdn __P((void));
-#endif
-static void key_sa_chgstate __P((struct secasvar *, u_int8_t));
-static struct mbuf *key_alloc_mbuf __P((int));
-
-static __inline void
-sa_initref(struct secasvar *sav)
-{
+static int key_cmpsaidx(const struct secasindex *,
+    const struct secasindex *, int);
+static int key_cmpspidx_exactly(struct secpolicyindex *,
+    struct secpolicyindex *);
+static int key_cmpspidx_withmask(struct secpolicyindex *,
+    struct secpolicyindex *);
+static int key_bbcmp(const void *, const void *, u_int);
+static uint8_t key_satype2proto(uint8_t);
+static uint8_t key_proto2satype(uint8_t);
+
+static int key_getspi(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static uint32_t key_do_getnewspi(struct sadb_spirange *, struct secasindex *);
+static int key_update(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_add(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_setident(struct secashead *, const struct sadb_msghdr *);
+static struct mbuf *key_getmsgbuf_x1(struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_delete(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_delete_all(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *, struct secasindex *);
+static void key_delete_xform(const struct xformsw *);
+static int key_get(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+
+static void key_getcomb_setlifetime(struct sadb_comb *);
+static struct mbuf *key_getcomb_ealg(void);
+static struct mbuf *key_getcomb_ah(void);
+static struct mbuf *key_getcomb_ipcomp(void);
+static struct mbuf *key_getprop(const struct secasindex *);
+
+static int key_acquire(const struct secasindex *, struct secpolicy *);
+static uint32_t key_newacq(const struct secasindex *, int *);
+static uint32_t key_getacq(const struct secasindex *, int *);
+static int key_acqdone(const struct secasindex *, uint32_t);
+static int key_acqreset(uint32_t);
+static struct secspacq *key_newspacq(struct secpolicyindex *);
+static struct secspacq *key_getspacq(struct secpolicyindex *);
+static int key_acquire2(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_register(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_expire(struct secasvar *, int);
+static int key_flush(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_dump(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_promisc(struct socket *, struct mbuf *,
+	const struct sadb_msghdr *);
+static int key_senderror(struct socket *, struct mbuf *, int);
+static int key_validate_ext(const struct sadb_ext *, int);
+static int key_align(struct mbuf *, struct sadb_msghdr *);
+static struct mbuf *key_setlifetime(struct seclifetime *, uint16_t);
+static struct mbuf *key_setkey(struct seckey *, uint16_t);
+static int xform_init(struct secasvar *, u_short);
+
+#define	DBG_IPSEC_INITREF(t, p)	do {				\
+	refcount_init(&(p)->refcnt, 1);				\
+	KEYDBG(KEY_STAMP,					\
+	    printf("%s: Initialize refcnt %s(%p) = %u\n",	\
+	    __func__, #t, (p), (p)->refcnt));			\
+} while (0)
+#define	DBG_IPSEC_ADDREF(t, p)	do {				\
+	refcount_acquire(&(p)->refcnt);				\
+	KEYDBG(KEY_STAMP,					\
+	    printf("%s: Acquire refcnt %s(%p) -> %u\n",		\
+	    __func__, #t, (p), (p)->refcnt));			\
+} while (0)
+#define	DBG_IPSEC_DELREF(t, p)	do {				\
+	KEYDBG(KEY_STAMP,					\
+	    printf("%s: Release refcnt %s(%p) -> %u\n",		\
+	    __func__, #t, (p), (p)->refcnt - 1));		\
+	refcount_release(&(p)->refcnt);				\
+} while (0)
 
-	refcount_init(&sav->refcnt, 1);
-}
-static __inline void
-sa_addref(struct secasvar *sav)
-{
+#define	IPSEC_INITREF(t, p)	refcount_init(&(p)->refcnt, 1)
+#define	IPSEC_ADDREF(t, p)	refcount_acquire(&(p)->refcnt)
+#define	IPSEC_DELREF(t, p)	refcount_release(&(p)->refcnt)
 
-	refcount_acquire(&sav->refcnt);
-	IPSEC_ASSERT(sav->refcnt != 0, ("SA refcnt overflow"));
-}
-static __inline int
-sa_delref(struct secasvar *sav)
-{
+#define	SP_INITREF(p)	IPSEC_INITREF(SP, p)
+#define	SP_ADDREF(p)	IPSEC_ADDREF(SP, p)
+#define	SP_DELREF(p)	IPSEC_DELREF(SP, p)
 
-	IPSEC_ASSERT(sav->refcnt > 0, ("SA refcnt underflow"));
-	return (refcount_release(&sav->refcnt));
-}
+#define	SAH_INITREF(p)	IPSEC_INITREF(SAH, p)
+#define	SAH_ADDREF(p)	IPSEC_ADDREF(SAH, p)
+#define	SAH_DELREF(p)	IPSEC_DELREF(SAH, p)
 
-#define	SP_ADDREF(p) do {						\
-	(p)->refcnt++;							\
-	IPSEC_ASSERT((p)->refcnt != 0, ("SP refcnt overflow"));		\
-} while (0)
-#define	SP_DELREF(p) do {						\
-	IPSEC_ASSERT((p)->refcnt > 0, ("SP refcnt underflow"));		\
-	(p)->refcnt--;							\
-} while (0)
- 
+#define	SAV_INITREF(p)	IPSEC_INITREF(SAV, p)
+#define	SAV_ADDREF(p)	IPSEC_ADDREF(SAV, p)
+#define	SAV_DELREF(p)	IPSEC_DELREF(SAV, p)
 
 /*
  * Update the refcnt while holding the SPTREE lock.
@@ -588,9 +735,8 @@ sa_delref(struct secasvar *sav)
 void
 key_addref(struct secpolicy *sp)
 {
-	SPTREE_LOCK();
+
 	SP_ADDREF(sp);
-	SPTREE_UNLOCK();
 }
 
 /*
@@ -603,60 +749,54 @@ key_havesp(u_int dir)
 {
 
 	return (dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND ?
-		LIST_FIRST(&V_sptree[dir]) != NULL : 1);
+		TAILQ_FIRST(&V_sptree[dir]) != NULL : 1);
 }
 
 /* %%% IPsec policy management */
 /*
- * allocating a SP for OUTBOUND or INBOUND packet.
- * Must call key_freesp() later.
- * OUT:	NULL:	not found
- *	others:	found and return the pointer.
+ * Return current SPDB generation.
  */
-struct secpolicy *
-key_allocsp(struct secpolicyindex *spidx, u_int dir, const char* where, int tag)
+uint32_t
+key_getspgen(void)
 {
-	struct secpolicy *sp;
 
-	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
-	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
-		("invalid direction %u", dir));
-
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u\n", __func__, where, tag));
+	return (V_sp_genid);
+}
 
-	/* get a SP entry */
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("*** objects\n");
-		kdebug_secpolicyindex(spidx));
+void
+key_bumpspgen(void)
+{
 
-	SPTREE_LOCK();
-	LIST_FOREACH(sp, &V_sptree[dir], chain) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-			printf("*** in SPD\n");
-			kdebug_secpolicyindex(&sp->spidx));
+	V_sp_genid++;
+}
 
-		if (sp->state == IPSEC_SPSTATE_DEAD)
-			continue;
-		if (key_cmpspidx_withmask(&sp->spidx, spidx))
-			goto found;
-	}
-	sp = NULL;
-found:
-	if (sp) {
-		/* sanity check */
-		KEY_CHKSPDIR(sp->spidx.dir, dir, __func__);
+static int
+key_checksockaddrs(struct sockaddr *src, struct sockaddr *dst)
+{
 
-		/* found a SPD entry */
-		sp->lastused = time_second;
-		SP_ADDREF(sp);
+	/* family match */
+	if (src->sa_family != dst->sa_family)
+		return (EINVAL);
+	/* sa_len match */
+	if (src->sa_len != dst->sa_len)
+		return (EINVAL);
+	switch (src->sa_family) {
+#ifdef INET
+	case AF_INET:
+		if (src->sa_len != sizeof(struct sockaddr_in))
+			return (EINVAL);
+		break;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		if (src->sa_len != sizeof(struct sockaddr_in6))
+			return (EINVAL);
+		break;
+#endif
+	default:
+		return (EAFNOSUPPORT);
 	}
-	SPTREE_UNLOCK();
-
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s return SP:%p (ID=%u) refcnt %u\n", __func__,
-			sp, sp ? sp->id : 0, sp ? sp->refcnt : 0));
-	return sp;
+	return (0);
 }
 
 /*
@@ -666,577 +806,415 @@ found:
  *	others:	found and return the pointer.
  */
 struct secpolicy *
-key_allocsp2(u_int32_t spi,
-	     union sockaddr_union *dst,
-	     u_int8_t proto,
-	     u_int dir,
-	     const char* where, int tag)
+key_allocsp(struct secpolicyindex *spidx, u_int dir)
 {
+	SPTREE_RLOCK_TRACKER;
 	struct secpolicy *sp;
 
-	IPSEC_ASSERT(dst != NULL, ("null dst"));
+	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
 	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
 		("invalid direction %u", dir));
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u\n", __func__, where, tag));
-
-	/* get a SP entry */
-	KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-		printf("*** objects\n");
-		printf("spi %u proto %u dir %u\n", spi, proto, dir);
-		kdebug_sockaddr(&dst->sa));
-
-	SPTREE_LOCK();
-	LIST_FOREACH(sp, &V_sptree[dir], chain) {
-		KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-			printf("*** in SPD\n");
-			kdebug_secpolicyindex(&sp->spidx));
-
-		if (sp->state == IPSEC_SPSTATE_DEAD)
-			continue;
-		/* compare simple values, then dst address */
-		if (sp->spidx.ul_proto != proto)
-			continue;
-		/* NB: spi's must exist and match */
-		if (!sp->req || !sp->req->sav || sp->req->sav->spi != spi)
-			continue;
-		if (key_sockaddrcmp(&sp->spidx.dst.sa, &dst->sa, 1) == 0)
-			goto found;
+	SPTREE_RLOCK();
+	TAILQ_FOREACH(sp, &V_sptree[dir], chain) {
+		if (key_cmpspidx_withmask(&sp->spidx, spidx)) {
+			SP_ADDREF(sp);
+			break;
+		}
 	}
-	sp = NULL;
-found:
-	if (sp) {
-		/* sanity check */
-		KEY_CHKSPDIR(sp->spidx.dir, dir, __func__);
+	SPTREE_RUNLOCK();
 
-		/* found a SPD entry */
+	if (sp != NULL) {	/* found a SPD entry */
 		sp->lastused = time_second;
-		SP_ADDREF(sp);
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: return SP(%p)\n", __func__, sp));
+		KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
+	} else {
+		KEYDBG(IPSEC_DATA,
+		    printf("%s: lookup failed for ", __func__);
+		    kdebug_secpolicyindex(spidx, NULL));
 	}
-	SPTREE_UNLOCK();
-
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s return SP:%p (ID=%u) refcnt %u\n", __func__,
-			sp, sp ? sp->id : 0, sp ? sp->refcnt : 0));
-	return sp;
+	return (sp);
 }
 
-#if 0
 /*
- * return a policy that matches this particular inbound packet.
- * XXX slow
+ * Allocating an SA entry for an *INBOUND* or *OUTBOUND* TCP packet, signed
+ * or should be signed by MD5 signature.
+ * We don't use key_allocsa() for such lookups, because we don't know SPI.
+ * Unlike ESP and AH protocols, SPI isn't transmitted in the TCP header with
+ * signed packet. We use SADB only as storage for password.
+ * OUT:	positive:	corresponding SA for given saidx found.
+ *	NULL:		SA not found
  */
-struct secpolicy *
-key_gettunnel(const struct sockaddr *osrc,
-	      const struct sockaddr *odst,
-	      const struct sockaddr *isrc,
-	      const struct sockaddr *idst,
-	      const char* where, int tag)
+struct secasvar *
+key_allocsa_tcpmd5(struct secasindex *saidx)
 {
-	struct secpolicy *sp;
-	const int dir = IPSEC_DIR_INBOUND;
-	struct ipsecrequest *r1, *r2, *p;
-	struct secpolicyindex spidx;
-
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u\n", __func__, where, tag));
-
-	if (isrc->sa_family != idst->sa_family) {
-		ipseclog((LOG_ERR, "%s: protocol family mismatched %d != %d\n.",
-			__func__, isrc->sa_family, idst->sa_family));
-		sp = NULL;
-		goto done;
-	}
+	SAHTREE_RLOCK_TRACKER;
+	struct secashead *sah;
+	struct secasvar *sav;
 
-	SPTREE_LOCK();
-	LIST_FOREACH(sp, &V_sptree[dir], chain) {
-		if (sp->state == IPSEC_SPSTATE_DEAD)
+	IPSEC_ASSERT(saidx->proto == IPPROTO_TCP,
+	    ("unexpected security protocol %u", saidx->proto));
+	IPSEC_ASSERT(saidx->mode == IPSEC_MODE_TCPMD5,
+	    ("unexpected mode %u", saidx->mode));
+
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(saidx), addrhash) {
+		KEYDBG(IPSEC_DUMP,
+		    printf("%s: checking SAH\n", __func__);
+		    kdebug_secash(sah, "  "));
+		if (sah->saidx.proto != IPPROTO_TCP)
 			continue;
-
-		r1 = r2 = NULL;
-		for (p = sp->req; p; p = p->next) {
-			if (p->saidx.mode != IPSEC_MODE_TUNNEL)
-				continue;
-
-			r1 = r2;
-			r2 = p;
-
-			if (!r1) {
-				/* here we look at address matches only */
-				spidx = sp->spidx;
-				if (isrc->sa_len > sizeof(spidx.src) ||
-				    idst->sa_len > sizeof(spidx.dst))
-					continue;
-				bcopy(isrc, &spidx.src, isrc->sa_len);
-				bcopy(idst, &spidx.dst, idst->sa_len);
-				if (!key_cmpspidx_withmask(&sp->spidx, &spidx))
-					continue;
-			} else {
-				if (key_sockaddrcmp(&r1->saidx.src.sa, isrc, 0) ||
-				    key_sockaddrcmp(&r1->saidx.dst.sa, idst, 0))
-					continue;
-			}
-
-			if (key_sockaddrcmp(&r2->saidx.src.sa, osrc, 0) ||
-			    key_sockaddrcmp(&r2->saidx.dst.sa, odst, 0))
-				continue;
-
-			goto found;
-		}
+		if (!key_sockaddrcmp(&saidx->dst.sa, &sah->saidx.dst.sa, 0))
+			break;
 	}
-	sp = NULL;
-found:
-	if (sp) {
-		sp->lastused = time_second;
-		SP_ADDREF(sp);
+	if (sah != NULL) {
+		if (V_key_preferred_oldsa)
+			sav = TAILQ_LAST(&sah->savtree_alive, secasvar_queue);
+		else
+			sav = TAILQ_FIRST(&sah->savtree_alive);
+		if (sav != NULL)
+			SAV_ADDREF(sav);
+	} else
+		sav = NULL;
+	SAHTREE_RUNLOCK();
+
+	if (sav != NULL) {
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: return SA(%p)\n", __func__, sav));
+		KEYDBG(IPSEC_DATA, kdebug_secasv(sav));
+	} else {
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: SA not found\n", __func__));
+		KEYDBG(IPSEC_DATA, kdebug_secasindex(saidx, NULL));
 	}
-	SPTREE_UNLOCK();
-done:
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s return SP:%p (ID=%u) refcnt %u\n", __func__,
-			sp, sp ? sp->id : 0, sp ? sp->refcnt : 0));
-	return sp;
+	return (sav);
 }
-#endif
 
 /*
- * allocating an SA entry for an *OUTBOUND* packet.
- * checking each request entries in SP, and acquire an SA if need.
- * OUT:	0: there are valid requests.
- *	ENOENT: policy may be valid, but SA with REQUIRE is on acquiring.
+ * Allocating an SA entry for an *OUTBOUND* packet.
+ * OUT:	positive:	corresponding SA for given saidx found.
+ *	NULL:		SA not found, but will be acquired, check *error
+ *			for acquiring status.
  */
-int
-key_checkrequest(struct ipsecrequest *isr, const struct secasindex *saidx)
+struct secasvar *
+key_allocsa_policy(struct secpolicy *sp, const struct secasindex *saidx,
+    int *error)
 {
-	u_int level;
-	int error;
+	SAHTREE_RLOCK_TRACKER;
+	struct secashead *sah;
 	struct secasvar *sav;
 
-	IPSEC_ASSERT(isr != NULL, ("null isr"));
 	IPSEC_ASSERT(saidx != NULL, ("null saidx"));
 	IPSEC_ASSERT(saidx->mode == IPSEC_MODE_TRANSPORT ||
 		saidx->mode == IPSEC_MODE_TUNNEL,
 		("unexpected policy %u", saidx->mode));
 
 	/*
-	 * XXX guard against protocol callbacks from the crypto
-	 * thread as they reference ipsecrequest.sav which we
-	 * temporarily null out below.  Need to rethink how we
-	 * handle bundled SA's in the callback thread.
-	 */
-	IPSECREQUEST_LOCK_ASSERT(isr);
-
-	/* get current level */
-	level = ipsec_get_reqlevel(isr);
-
-	/*
 	 * We check new SA in the IPsec request because a different
 	 * SA may be involved each time this request is checked, either
 	 * because new SAs are being configured, or this request is
 	 * associated with an unconnected datagram socket, or this request
 	 * is associated with a system default policy.
-	 *
-	 * key_allocsa_policy should allocate the oldest SA available.
-	 * See key_do_allocsa_policy(), and draft-jenkins-ipsec-rekeying-03.txt.
 	 */
-	sav = key_allocsa_policy(saidx);
-	if (sav != isr->sav) {
-		/* SA need to be updated. */
-		if (!IPSECREQUEST_UPGRADE(isr)) {
-			/* Kick everyone off. */
-			IPSECREQUEST_UNLOCK(isr);
-			IPSECREQUEST_WLOCK(isr);
-		}
-		if (isr->sav != NULL)
-			KEY_FREESAV(&isr->sav);
-		isr->sav = sav;
-		IPSECREQUEST_DOWNGRADE(isr);
-	} else if (sav != NULL)
-		KEY_FREESAV(&sav);
-
-	/* When there is SA. */
-	if (isr->sav != NULL) {
-		if (isr->sav->state != SADB_SASTATE_MATURE &&
-		    isr->sav->state != SADB_SASTATE_DYING)
-			return EINVAL;
-		return 0;
-	}
-
-	/* there is no SA */
-	error = key_acquire(saidx, isr->sp);
-	if (error != 0) {
-		/* XXX What should I do ? */
-		ipseclog((LOG_DEBUG, "%s: error %d returned from key_acquire\n",
-			__func__, error));
-		return error;
-	}
-
-	if (level != IPSEC_LEVEL_REQUIRE) {
-		/* XXX sigh, the interface to this routine is botched */
-		IPSEC_ASSERT(isr->sav == NULL, ("unexpected SA"));
-		return 0;
-	} else {
-		return ENOENT;
-	}
-}
-
-/*
- * allocating a SA for policy entry from SAD.
- * NOTE: searching SAD of aliving state.
- * OUT:	NULL:	not found.
- *	others:	found and return the pointer.
- */
-static struct secasvar *
-key_allocsa_policy(const struct secasindex *saidx)
-{
-#define	N(a)	_ARRAYLEN(a)
-	struct secashead *sah;
-	struct secasvar *sav;
-	u_int stateidx, arraysize;
-	const u_int *state_valid;
-
-	state_valid = NULL;	/* silence gcc */
-	arraysize = 0;		/* silence gcc */
-
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
-		if (key_cmpsaidx(&sah->saidx, saidx, CMP_MODE_REQID)) {
-			if (V_key_preferred_oldsa) {
-				state_valid = saorder_state_valid_prefer_old;
-				arraysize = N(saorder_state_valid_prefer_old);
-			} else {
-				state_valid = saorder_state_valid_prefer_new;
-				arraysize = N(saorder_state_valid_prefer_new);
-			}
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(saidx), addrhash) {
+		KEYDBG(IPSEC_DUMP,
+		    printf("%s: checking SAH\n", __func__);
+		    kdebug_secash(sah, "  "));
+		if (key_cmpsaidx(&sah->saidx, saidx, CMP_MODE_REQID))
 			break;
-		}
-	}
-	SAHTREE_UNLOCK();
-	if (sah == NULL)
-		return NULL;
 
-	/* search valid state */
-	for (stateidx = 0; stateidx < arraysize; stateidx++) {
-		sav = key_do_allocsa_policy(sah, state_valid[stateidx]);
-		if (sav != NULL)
-			return sav;
 	}
-
-	return NULL;
-#undef N
-}
-
-/*
- * searching SAD with direction, protocol, mode and state.
- * called by key_allocsa_policy().
- * OUT:
- *	NULL	: not found
- *	others	: found, pointer to a SA.
- */
-static struct secasvar *
-key_do_allocsa_policy(struct secashead *sah, u_int state)
-{
-	struct secasvar *sav, *nextsav, *candidate, *d;
-
-	/* initilize */
-	candidate = NULL;
-
-	SAHTREE_LOCK();
-	for (sav = LIST_FIRST(&sah->savtree[state]);
-	     sav != NULL;
-	     sav = nextsav) {
-
-		nextsav = LIST_NEXT(sav, chain);
-
-		/* sanity check */
-		KEY_CHKSASTATE(sav->state, state, __func__);
-
-		/* initialize */
-		if (candidate == NULL) {
-			candidate = sav;
-			continue;
-		}
-
-		/* Which SA is the better ? */
-
-		IPSEC_ASSERT(candidate->lft_c != NULL,
-			("null candidate lifetime"));
-		IPSEC_ASSERT(sav->lft_c != NULL, ("null sav lifetime"));
-
-		/* What the best method is to compare ? */
-		if (V_key_preferred_oldsa) {
-			if (candidate->lft_c->addtime >
-					sav->lft_c->addtime) {
-				candidate = sav;
-			}
-			continue;
-			/*NOTREACHED*/
-		}
-
-		/* preferred new sa rather than old sa */
-		if (candidate->lft_c->addtime <
-				sav->lft_c->addtime) {
-			d = candidate;
-			candidate = sav;
-		} else
-			d = sav;
-
+	if (sah != NULL) {
 		/*
-		 * prepared to delete the SA when there is more
-		 * suitable candidate and the lifetime of the SA is not
-		 * permanent.
+		 * Allocate the oldest SA available according to
+		 * draft-jenkins-ipsec-rekeying-03.
 		 */
-		if (d->lft_h->addtime != 0) {
-			struct mbuf *m, *result;
-			u_int8_t satype;
-
-			key_sa_chgstate(d, SADB_SASTATE_DEAD);
-
-			IPSEC_ASSERT(d->refcnt > 0, ("bogus ref count"));
-
-			satype = key_proto2satype(d->sah->saidx.proto);
-			if (satype == 0)
-				goto msgfail;
-
-			m = key_setsadbmsg(SADB_DELETE, 0,
-			    satype, 0, 0, d->refcnt - 1);
-			if (!m)
-				goto msgfail;
-			result = m;
-
-			/* set sadb_address for saidx's. */
-			m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
-				&d->sah->saidx.src.sa,
-				d->sah->saidx.src.sa.sa_len << 3,
-				IPSEC_ULPROTO_ANY);
-			if (!m)
-				goto msgfail;
-			m_cat(result, m);
-
-			/* set sadb_address for saidx's. */
-			m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
-				&d->sah->saidx.dst.sa,
-				d->sah->saidx.dst.sa.sa_len << 3,
-				IPSEC_ULPROTO_ANY);
-			if (!m)
-				goto msgfail;
-			m_cat(result, m);
-
-			/* create SA extension */
-			m = key_setsadbsa(d);
-			if (!m)
-				goto msgfail;
-			m_cat(result, m);
-
-			if (result->m_len < sizeof(struct sadb_msg)) {
-				result = m_pullup(result,
-						sizeof(struct sadb_msg));
-				if (result == NULL)
-					goto msgfail;
-			}
-
-			result->m_pkthdr.len = 0;
-			for (m = result; m; m = m->m_next)
-				result->m_pkthdr.len += m->m_len;
-			mtod(result, struct sadb_msg *)->sadb_msg_len =
-				PFKEY_UNIT64(result->m_pkthdr.len);
+		if (V_key_preferred_oldsa)
+			sav = TAILQ_LAST(&sah->savtree_alive, secasvar_queue);
+		else
+			sav = TAILQ_FIRST(&sah->savtree_alive);
+		if (sav != NULL)
+			SAV_ADDREF(sav);
+	} else
+		sav = NULL;
+	SAHTREE_RUNLOCK();
 
-			if (key_sendup_mbuf(NULL, result,
-					KEY_SENDUP_REGISTERED))
-				goto msgfail;
-		 msgfail:
-			KEY_FREESAV(&d);
-		}
-	}
-	if (candidate) {
-		sa_addref(candidate);
-		KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-			printf("DP %s cause refcnt++:%d SA:%p\n",
-				__func__, candidate->refcnt, candidate));
+	if (sav != NULL) {
+		*error = 0;
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: chosen SA(%p) for SP(%p)\n", __func__,
+			sav, sp));
+		KEYDBG(IPSEC_DATA, kdebug_secasv(sav));
+		return (sav); /* return referenced SA */
 	}
-	SAHTREE_UNLOCK();
 
-	return candidate;
+	/* there is no SA */
+	*error = key_acquire(saidx, sp);
+	if ((*error) != 0)
+		ipseclog((LOG_DEBUG,
+		    "%s: error %d returned from key_acquire()\n",
+			__func__, *error));
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: acquire SA for SP(%p), error %d\n",
+		__func__, sp, *error));
+	KEYDBG(IPSEC_DATA, kdebug_secasindex(saidx, NULL));
+	return (NULL);
 }
 
 /*
  * allocating a usable SA entry for a *INBOUND* packet.
  * Must call key_freesav() later.
  * OUT: positive:	pointer to a usable sav (i.e. MATURE or DYING state).
- *	NULL:		not found, or error occured.
+ *	NULL:		not found, or error occurred.
+ *
+ * According to RFC 2401 SA is uniquely identified by a triple SPI,
+ * destination address, and security protocol. But according to RFC 4301,
+ * SPI by itself suffices to specify an SA.
  *
- * In the comparison, no source address is used--for RFC2401 conformance.
- * To quote, from section 4.1:
- *	A security association is uniquely identified by a triple consisting
- *	of a Security Parameter Index (SPI), an IP Destination Address, and a
- *	security protocol (AH or ESP) identifier.
  * Note that, however, we do need to keep source address in IPsec SA.
  * IKE specification and PF_KEY specification do assume that we
  * keep source address in IPsec SA.  We see a tricky situation here.
  */
 struct secasvar *
-key_allocsa(
-	union sockaddr_union *dst,
-	u_int proto,
-	u_int32_t spi,
-	const char* where, int tag)
+key_allocsa(union sockaddr_union *dst, uint8_t proto, uint32_t spi)
 {
-	struct secashead *sah;
+	SAHTREE_RLOCK_TRACKER;
 	struct secasvar *sav;
-	u_int stateidx, arraysize, state;
-	const u_int *saorder_state_valid;
-	int chkport;
-
-	IPSEC_ASSERT(dst != NULL, ("null dst address"));
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u\n", __func__, where, tag));
-
-#ifdef IPSEC_NAT_T
-        chkport = (dst->sa.sa_family == AF_INET &&
-	    dst->sa.sa_len == sizeof(struct sockaddr_in) &&
-	    dst->sin.sin_port != 0);
-#else
-	chkport = 0;
-#endif
+	IPSEC_ASSERT(proto == IPPROTO_ESP || proto == IPPROTO_AH ||
+	    proto == IPPROTO_IPCOMP, ("unexpected security protocol %u",
+	    proto));
 
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sav, SAVHASH_HASH(spi), spihash) {
+		if (sav->spi == spi)
+			break;
+	}
 	/*
-	 * searching SAD.
-	 * XXX: to be checked internal IP header somewhere.  Also when
-	 * IPsec tunnel packet is received.  But ESP tunnel mode is
-	 * encrypted so we can't check internal IP header.
+	 * We use single SPI namespace for all protocols, so it is
+	 * impossible to have SPI duplicates in the SAVHASH.
 	 */
-	SAHTREE_LOCK();
-	if (V_key_preferred_oldsa) {
-		saorder_state_valid = saorder_state_valid_prefer_old;
-		arraysize = _ARRAYLEN(saorder_state_valid_prefer_old);
+	if (sav != NULL) {
+		if (sav->state != SADB_SASTATE_LARVAL &&
+		    sav->sah->saidx.proto == proto &&
+		    key_sockaddrcmp(&dst->sa,
+			&sav->sah->saidx.dst.sa, 0) == 0)
+			SAV_ADDREF(sav);
+		else
+			sav = NULL;
+	}
+	SAHTREE_RUNLOCK();
+
+	if (sav == NULL) {
+		KEYDBG(IPSEC_STAMP,
+		    char buf[IPSEC_ADDRSTRLEN];
+		    printf("%s: SA not found for spi %u proto %u dst %s\n",
+			__func__, ntohl(spi), proto, ipsec_address(dst, buf,
+			sizeof(buf))));
 	} else {
-		saorder_state_valid = saorder_state_valid_prefer_new;
-		arraysize = _ARRAYLEN(saorder_state_valid_prefer_new);
-	}
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		/* search valid state */
-		for (stateidx = 0; stateidx < arraysize; stateidx++) {
-			state = saorder_state_valid[stateidx];
-			LIST_FOREACH(sav, &sah->savtree[state], chain) {
-				/* sanity check */
-				KEY_CHKSASTATE(sav->state, state, __func__);
-				/* do not return entries w/ unusable state */
-				if (sav->state != SADB_SASTATE_MATURE &&
-				    sav->state != SADB_SASTATE_DYING)
-					continue;
-				if (proto != sav->sah->saidx.proto)
-					continue;
-				if (spi != sav->spi)
-					continue;
-#if 0	/* don't check src */
-				/* check src address */
-				if (key_sockaddrcmp(&src->sa, &sav->sah->saidx.src.sa, chkport) != 0)
-					continue;
-#endif
-				/* check dst address */
-				if (key_sockaddrcmp(&dst->sa, &sav->sah->saidx.dst.sa, chkport) != 0)
-					continue;
-				sa_addref(sav);
-				goto done;
-			}
-		}
+		KEYDBG(IPSEC_STAMP,
+		    printf("%s: return SA(%p)\n", __func__, sav));
+		KEYDBG(IPSEC_DATA, kdebug_secasv(sav));
 	}
-	sav = NULL;
-done:
-	SAHTREE_UNLOCK();
+	return (sav);
+}
+
+struct secasvar *
+key_allocsa_tunnel(union sockaddr_union *src, union sockaddr_union *dst,
+    uint8_t proto)
+{
+	SAHTREE_RLOCK_TRACKER;
+	struct secasindex saidx;
+	struct secashead *sah;
+	struct secasvar *sav;
+
+	IPSEC_ASSERT(src != NULL, ("null src address"));
+	IPSEC_ASSERT(dst != NULL, ("null dst address"));
+
+	KEY_SETSECASIDX(proto, IPSEC_MODE_TUNNEL, 0, &src->sa,
+	    &dst->sa, &saidx);
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s return SA:%p; refcnt %u\n", __func__,
-			sav, sav ? sav->refcnt : 0));
-	return sav;
+	sav = NULL;
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(&saidx), addrhash) {
+		if (IPSEC_MODE_TUNNEL != sah->saidx.mode)
+			continue;
+		if (proto != sah->saidx.proto)
+			continue;
+		if (key_sockaddrcmp(&src->sa, &sah->saidx.src.sa, 0) != 0)
+			continue;
+		if (key_sockaddrcmp(&dst->sa, &sah->saidx.dst.sa, 0) != 0)
+			continue;
+		/* XXXAE: is key_preferred_oldsa reasonably?*/
+		if (V_key_preferred_oldsa)
+			sav = TAILQ_LAST(&sah->savtree_alive, secasvar_queue);
+		else
+			sav = TAILQ_FIRST(&sah->savtree_alive);
+		if (sav != NULL) {
+			SAV_ADDREF(sav);
+			break;
+		}
+	}
+	SAHTREE_RUNLOCK();
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: return SA(%p)\n", __func__, sav));
+	if (sav != NULL)
+		KEYDBG(IPSEC_DATA, kdebug_secasv(sav));
+	return (sav);
 }
 
 /*
  * Must be called after calling key_allocsp().
- * For both the packet without socket and key_freeso().
  */
 void
-_key_freesp(struct secpolicy **spp, const char* where, int tag)
+key_freesp(struct secpolicy **spp)
 {
 	struct secpolicy *sp = *spp;
 
 	IPSEC_ASSERT(sp != NULL, ("null sp"));
+	if (SP_DELREF(sp) == 0)
+		return;
 
-	SPTREE_LOCK();
-	SP_DELREF(sp);
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: last reference to SP(%p)\n", __func__, sp));
+	KEYDBG(IPSEC_DATA, kdebug_secpolicy(sp));
+
+	*spp = NULL;
+	while (sp->tcount > 0)
+		ipsec_delisr(sp->req[--sp->tcount]);
+	free(sp, M_IPSEC_SP);
+}
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s SP:%p (ID=%u) from %s:%u; refcnt now %u\n",
-			__func__, sp, sp->id, where, tag, sp->refcnt));
+static void
+key_unlink(struct secpolicy *sp)
+{
 
-	if (sp->refcnt == 0) {
-		*spp = NULL;
-		key_delsp(sp);
+	IPSEC_ASSERT(sp->spidx.dir == IPSEC_DIR_INBOUND ||
+	    sp->spidx.dir == IPSEC_DIR_OUTBOUND,
+	    ("invalid direction %u", sp->spidx.dir));
+	SPTREE_UNLOCK_ASSERT();
+
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, sp));
+	SPTREE_WLOCK();
+	if (sp->state != IPSEC_SPSTATE_ALIVE) {
+		/* SP is already unlinked */
+		SPTREE_WUNLOCK();
+		return;
 	}
-	SPTREE_UNLOCK();
+	sp->state = IPSEC_SPSTATE_DEAD;
+	TAILQ_REMOVE(&V_sptree[sp->spidx.dir], sp, chain);
+	LIST_REMOVE(sp, idhash);
+	V_sp_genid++;
+	SPTREE_WUNLOCK();
+	key_freesp(&sp);
 }
 
 /*
- * Must be called after calling key_allocsp().
- * For the packet with socket.
+ * insert a secpolicy into the SP database. Lower priorities first
  */
-void
-key_freeso(struct socket *so)
+static void
+key_insertsp(struct secpolicy *newsp)
 {
-	IPSEC_ASSERT(so != NULL, ("null so"));
-
-	switch (so->so_proto->pr_domain->dom_family) {
-#if defined(INET) || defined(INET6)
-#ifdef INET
-	case PF_INET:
-#endif
-#ifdef INET6
-	case PF_INET6:
-#endif
-	    {
-		struct inpcb *pcb = sotoinpcb(so);
+	struct secpolicy *sp;
 
-		/* Does it have a PCB ? */
-		if (pcb == NULL)
-			return;
-		key_freesp_so(&pcb->inp_sp->sp_in);
-		key_freesp_so(&pcb->inp_sp->sp_out);
-	    }
-		break;
-#endif /* INET || INET6 */
-	default:
-		ipseclog((LOG_DEBUG, "%s: unknown address family=%d.\n",
-		    __func__, so->so_proto->pr_domain->dom_family));
-		return;
+	SPTREE_WLOCK_ASSERT();
+	TAILQ_FOREACH(sp, &V_sptree[newsp->spidx.dir], chain) {
+		if (newsp->priority < sp->priority) {
+			TAILQ_INSERT_BEFORE(sp, newsp, chain);
+			goto done;
+		}
 	}
+	TAILQ_INSERT_TAIL(&V_sptree[newsp->spidx.dir], newsp, chain);
+done:
+	LIST_INSERT_HEAD(SPHASH_HASH(newsp->id), newsp, idhash);
+	newsp->state = IPSEC_SPSTATE_ALIVE;
+	V_sp_genid++;
 }
 
-static void
-key_freesp_so(struct secpolicy **sp)
+/*
+ * Insert a bunch of VTI secpolicies into the SPDB.
+ * We keep VTI policies in the separate list due to following reasons:
+ * 1) they should be immutable to user's or some deamon's attempts to
+ *    delete. The only way delete such policies - destroy or unconfigure
+ *    corresponding virtual inteface.
+ * 2) such policies have traffic selector that matches all traffic per
+ *    address family.
+ * Since all VTI policies have the same priority, we don't care about
+ * policies order.
+ */
+int
+key_register_ifnet(struct secpolicy **spp, u_int count)
 {
-	IPSEC_ASSERT(sp != NULL && *sp != NULL, ("null sp"));
+	struct mbuf *m;
+	u_int i;
 
-	if ((*sp)->policy == IPSEC_POLICY_ENTRUST ||
-	    (*sp)->policy == IPSEC_POLICY_BYPASS)
-		return;
+	SPTREE_WLOCK();
+	/*
+	 * First of try to acquire id for each SP.
+	 */
+	for (i = 0; i < count; i++) {
+		IPSEC_ASSERT(spp[i]->spidx.dir == IPSEC_DIR_INBOUND ||
+		    spp[i]->spidx.dir == IPSEC_DIR_OUTBOUND,
+		    ("invalid direction %u", spp[i]->spidx.dir));
 
-	IPSEC_ASSERT((*sp)->policy == IPSEC_POLICY_IPSEC,
-		("invalid policy %u", (*sp)->policy));
-	KEY_FREESP(sp);
+		if ((spp[i]->id = key_getnewspid()) == 0) {
+			SPTREE_WUNLOCK();
+			return (EAGAIN);
+		}
+	}
+	for (i = 0; i < count; i++) {
+		TAILQ_INSERT_TAIL(&V_sptree_ifnet[spp[i]->spidx.dir],
+		    spp[i], chain);
+		/*
+		 * NOTE: despite the fact that we keep VTI SP in the
+		 * separate list, SPHASH contains policies from both
+		 * sources. Thus SADB_X_SPDGET will correctly return
+		 * SP by id, because it uses SPHASH for lookups.
+		 */
+		LIST_INSERT_HEAD(SPHASH_HASH(spp[i]->id), spp[i], idhash);
+		spp[i]->state = IPSEC_SPSTATE_IFNET;
+	}
+	SPTREE_WUNLOCK();
+	/*
+	 * Notify user processes about new SP.
+	 */
+	for (i = 0; i < count; i++) {
+		m = key_setdumpsp(spp[i], SADB_X_SPDADD, 0, 0);
+		if (m != NULL)
+			key_sendup_mbuf(NULL, m, KEY_SENDUP_ALL);
+	}
+	return (0);
 }
 
 void
-key_addrefsa(struct secasvar *sav, const char* where, int tag)
+key_unregister_ifnet(struct secpolicy **spp, u_int count)
 {
+	struct mbuf *m;
+	u_int i;
 
-	IPSEC_ASSERT(sav != NULL, ("null sav"));
-	IPSEC_ASSERT(sav->refcnt > 0, ("refcount must exist"));
+	SPTREE_WLOCK();
+	for (i = 0; i < count; i++) {
+		IPSEC_ASSERT(spp[i]->spidx.dir == IPSEC_DIR_INBOUND ||
+		    spp[i]->spidx.dir == IPSEC_DIR_OUTBOUND,
+		    ("invalid direction %u", spp[i]->spidx.dir));
+
+		if (spp[i]->state != IPSEC_SPSTATE_IFNET)
+			continue;
+		spp[i]->state = IPSEC_SPSTATE_DEAD;
+		TAILQ_REMOVE(&V_sptree_ifnet[spp[i]->spidx.dir],
+		    spp[i], chain);
+		LIST_REMOVE(spp[i], idhash);
+	}
+	SPTREE_WUNLOCK();
 
-	sa_addref(sav);
+	for (i = 0; i < count; i++) {
+		m = key_setdumpsp(spp[i], SADB_X_SPDDELETE, 0, 0);
+		if (m != NULL)
+			key_sendup_mbuf(NULL, m, KEY_SENDUP_ALL);
+	}
 }
 
 /*
@@ -1245,58 +1223,57 @@ key_addrefsa(struct secasvar *sav, const char* where, int tag)
  * for a policy.
  */
 void
-key_freesav(struct secasvar **psav, const char* where, int tag)
+key_freesav(struct secasvar **psav)
 {
 	struct secasvar *sav = *psav;
 
 	IPSEC_ASSERT(sav != NULL, ("null sav"));
+	if (SAV_DELREF(sav) == 0)
+		return;
 
-	if (sa_delref(sav)) {
-		KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-			printf("DP %s SA:%p (SPI %u) from %s:%u; refcnt now %u\n",
-				__func__, sav, ntohl(sav->spi), where, tag, sav->refcnt));
-		*psav = NULL;
-		key_delsav(sav);
-	} else {
-		KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-			printf("DP %s SA:%p (SPI %u) from %s:%u; refcnt now %u\n",
-				__func__, sav, ntohl(sav->spi), where, tag, sav->refcnt));
-	}
+	KEYDBG(IPSEC_STAMP,
+	    printf("%s: last reference to SA(%p)\n", __func__, sav));
+
+	*psav = NULL;
+	key_delsav(sav);
 }
 
-/* %%% SPD management */
 /*
- * free security policy entry.
+ * Unlink SA from SAH and SPI hash under SAHTREE_WLOCK.
+ * Expect that SA has extra reference due to lookup.
+ * Release this references, also release SAH reference after unlink.
  */
 static void
-key_delsp(struct secpolicy *sp)
+key_unlinksav(struct secasvar *sav)
 {
-	struct ipsecrequest *isr, *nextisr;
-
-	IPSEC_ASSERT(sp != NULL, ("null sp"));
-	SPTREE_LOCK_ASSERT();
-
-	sp->state = IPSEC_SPSTATE_DEAD;
-
-	IPSEC_ASSERT(sp->refcnt == 0,
-		("SP with references deleted (refcnt %u)", sp->refcnt));
-
-	/* remove from SP index */
-	if (__LIST_CHAINED(sp))
-		LIST_REMOVE(sp, chain);
+	struct secashead *sah;
 
-	for (isr = sp->req; isr != NULL; isr = nextisr) {
-		if (isr->sav != NULL) {
-			KEY_FREESAV(&isr->sav);
-			isr->sav = NULL;
-		}
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p)\n", __func__, sav));
 
-		nextisr = isr->next;
-		ipsec_delisr(isr);
+	SAHTREE_UNLOCK_ASSERT();
+	SAHTREE_WLOCK();
+	if (sav->state == SADB_SASTATE_DEAD) {
+		/* SA is already unlinked */
+		SAHTREE_WUNLOCK();
+		return;
 	}
-	_key_delsp(sp);
+	/* Unlink from SAH */
+	if (sav->state == SADB_SASTATE_LARVAL)
+		TAILQ_REMOVE(&sav->sah->savtree_larval, sav, chain);
+	else
+		TAILQ_REMOVE(&sav->sah->savtree_alive, sav, chain);
+	/* Unlink from SPI hash */
+	LIST_REMOVE(sav, spihash);
+	sav->state = SADB_SASTATE_DEAD;
+	sah = sav->sah;
+	SAHTREE_WUNLOCK();
+	key_freesav(&sav);
+	/* Since we are unlinked, release reference to SAH */
+	key_freesah(&sah);
 }
 
+/* %%% SPD management */
 /*
  * search SPD
  * OUT:	NULL	: not found
@@ -1305,20 +1282,19 @@ key_delsp(struct secpolicy *sp)
 static struct secpolicy *
 key_getsp(struct secpolicyindex *spidx)
 {
+	SPTREE_RLOCK_TRACKER;
 	struct secpolicy *sp;
 
 	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
 
-	SPTREE_LOCK();
-	LIST_FOREACH(sp, &V_sptree[spidx->dir], chain) {
-		if (sp->state == IPSEC_SPSTATE_DEAD)
-			continue;
+	SPTREE_RLOCK();
+	TAILQ_FOREACH(sp, &V_sptree[spidx->dir], chain) {
 		if (key_cmpspidx_exactly(spidx, &sp->spidx)) {
 			SP_ADDREF(sp);
 			break;
 		}
 	}
-	SPTREE_UNLOCK();
+	SPTREE_RUNLOCK();
 
 	return sp;
 }
@@ -1326,73 +1302,58 @@ key_getsp(struct secpolicyindex *spidx)
 /*
  * get SP by index.
  * OUT:	NULL	: not found
- *	others	: found, pointer to a SP.
+ *	others	: found, pointer to referenced SP.
  */
 static struct secpolicy *
-key_getspbyid(u_int32_t id)
+key_getspbyid(uint32_t id)
 {
+	SPTREE_RLOCK_TRACKER;
 	struct secpolicy *sp;
 
-	SPTREE_LOCK();
-	LIST_FOREACH(sp, &V_sptree[IPSEC_DIR_INBOUND], chain) {
-		if (sp->state == IPSEC_SPSTATE_DEAD)
-			continue;
-		if (sp->id == id) {
-			SP_ADDREF(sp);
-			goto done;
-		}
-	}
-
-	LIST_FOREACH(sp, &V_sptree[IPSEC_DIR_OUTBOUND], chain) {
-		if (sp->state == IPSEC_SPSTATE_DEAD)
-			continue;
+	SPTREE_RLOCK();
+	LIST_FOREACH(sp, SPHASH_HASH(id), idhash) {
 		if (sp->id == id) {
 			SP_ADDREF(sp);
-			goto done;
+			break;
 		}
 	}
-done:
-	SPTREE_UNLOCK();
-
-	return sp;
+	SPTREE_RUNLOCK();
+	return (sp);
 }
 
 struct secpolicy *
-key_newsp(const char* where, int tag)
+key_newsp(void)
 {
-	struct secpolicy *newsp = NULL;
+	struct secpolicy *sp;
 
-	newsp = (struct secpolicy *)
-		malloc(sizeof(struct secpolicy), M_IPSEC_SP, M_NOWAIT|M_ZERO);
-	if (newsp) {
-		SECPOLICY_LOCK_INIT(newsp);
-		newsp->refcnt = 1;
-		newsp->req = NULL;
-	}
+	sp = malloc(sizeof(*sp), M_IPSEC_SP, M_NOWAIT | M_ZERO);
+	if (sp != NULL)
+		SP_INITREF(sp);
+	return (sp);
+}
 
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u return SP:%p\n", __func__,
-			where, tag, newsp));
-	return newsp;
+struct ipsecrequest *
+ipsec_newisr(void)
+{
+
+	return (malloc(sizeof(struct ipsecrequest), M_IPSEC_SR,
+	    M_NOWAIT | M_ZERO));
 }
 
-static void
-_key_delsp(struct secpolicy *sp)
+void
+ipsec_delisr(struct ipsecrequest *p)
 {
-	SECPOLICY_LOCK_DESTROY(sp);
-	free(sp, M_IPSEC_SP);
+
+	free(p, M_IPSEC_SR);
 }
 
 /*
  * create secpolicy structure from sadb_x_policy structure.
- * NOTE: `state', `secpolicyindex' in secpolicy structure are not set,
- * so must be set properly later.
+ * NOTE: `state', `secpolicyindex' and 'id' in secpolicy structure
+ * are not set, so must be set properly later.
  */
 struct secpolicy *
-key_msg2sp(xpl0, len, error)
-	struct sadb_x_policy *xpl0;
-	size_t len;
-	int *error;
+key_msg2sp(struct sadb_x_policy *xpl0, size_t len, int *error)
 {
 	struct secpolicy *newsp;
 
@@ -1405,13 +1366,15 @@ key_msg2sp(xpl0, len, error)
 		return NULL;
 	}
 
-	if ((newsp = KEY_NEWSP()) == NULL) {
+	if ((newsp = key_newsp()) == NULL) {
 		*error = ENOBUFS;
 		return NULL;
 	}
 
 	newsp->spidx.dir = xpl0->sadb_x_policy_dir;
 	newsp->policy = xpl0->sadb_x_policy_type;
+	newsp->priority = xpl0->sadb_x_policy_priority;
+	newsp->tcount = 0;
 
 	/* check policy */
 	switch (xpl0->sadb_x_policy_type) {
@@ -1419,20 +1382,19 @@ key_msg2sp(xpl0, len, error)
 	case IPSEC_POLICY_NONE:
 	case IPSEC_POLICY_ENTRUST:
 	case IPSEC_POLICY_BYPASS:
-		newsp->req = NULL;
 		break;
 
 	case IPSEC_POLICY_IPSEC:
 	    {
-		int tlen;
 		struct sadb_x_ipsecrequest *xisr;
-		struct ipsecrequest **p_isr = &newsp->req;
+		struct ipsecrequest *isr;
+		int tlen;
 
 		/* validity check */
 		if (PFKEY_EXTLEN(xpl0) < sizeof(*xpl0)) {
 			ipseclog((LOG_DEBUG, "%s: Invalid msg length.\n",
 				__func__));
-			KEY_FREESP(&newsp);
+			key_freesp(&newsp);
 			*error = EINVAL;
 			return NULL;
 		}
@@ -1445,22 +1407,33 @@ key_msg2sp(xpl0, len, error)
 			if (xisr->sadb_x_ipsecrequest_len < sizeof(*xisr)) {
 				ipseclog((LOG_DEBUG, "%s: invalid ipsecrequest "
 					"length.\n", __func__));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = EINVAL;
 				return NULL;
 			}
 
+			if (newsp->tcount >= IPSEC_MAXREQ) {
+				ipseclog((LOG_DEBUG,
+				    "%s: too many ipsecrequests.\n",
+				    __func__));
+				key_freesp(&newsp);
+				*error = EINVAL;
+				return (NULL);
+			}
+
 			/* allocate request buffer */
 			/* NB: data structure is zero'd */
-			*p_isr = ipsec_newisr();
-			if ((*p_isr) == NULL) {
+			isr = ipsec_newisr();
+			if (isr == NULL) {
 				ipseclog((LOG_DEBUG,
 				    "%s: No more memory.\n", __func__));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = ENOBUFS;
 				return NULL;
 			}
 
+			newsp->req[newsp->tcount++] = isr;
+
 			/* set values */
 			switch (xisr->sadb_x_ipsecrequest_proto) {
 			case IPPROTO_ESP:
@@ -1471,11 +1444,12 @@ key_msg2sp(xpl0, len, error)
 				ipseclog((LOG_DEBUG,
 				    "%s: invalid proto type=%u\n", __func__,
 				    xisr->sadb_x_ipsecrequest_proto));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = EPROTONOSUPPORT;
 				return NULL;
 			}
-			(*p_isr)->saidx.proto = xisr->sadb_x_ipsecrequest_proto;
+			isr->saidx.proto =
+			    (uint8_t)xisr->sadb_x_ipsecrequest_proto;
 
 			switch (xisr->sadb_x_ipsecrequest_mode) {
 			case IPSEC_MODE_TRANSPORT:
@@ -1486,11 +1460,11 @@ key_msg2sp(xpl0, len, error)
 				ipseclog((LOG_DEBUG,
 				    "%s: invalid mode=%u\n", __func__,
 				    xisr->sadb_x_ipsecrequest_mode));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = EINVAL;
 				return NULL;
 			}
-			(*p_isr)->saidx.mode = xisr->sadb_x_ipsecrequest_mode;
+			isr->saidx.mode = xisr->sadb_x_ipsecrequest_mode;
 
 			switch (xisr->sadb_x_ipsecrequest_level) {
 			case IPSEC_LEVEL_DEFAULT:
@@ -1517,16 +1491,16 @@ key_msg2sp(xpl0, len, error)
 				if (xisr->sadb_x_ipsecrequest_reqid == 0) {
 					u_int32_t reqid;
 					if ((reqid = key_newreqid()) == 0) {
-						KEY_FREESP(&newsp);
+						key_freesp(&newsp);
 						*error = ENOBUFS;
 						return NULL;
 					}
-					(*p_isr)->saidx.reqid = reqid;
+					isr->saidx.reqid = reqid;
 					xisr->sadb_x_ipsecrequest_reqid = reqid;
 				} else {
 				/* set it for manual keying. */
-					(*p_isr)->saidx.reqid =
-						xisr->sadb_x_ipsecrequest_reqid;
+					isr->saidx.reqid =
+					    xisr->sadb_x_ipsecrequest_reqid;
 				}
 				break;
 
@@ -1534,59 +1508,72 @@ key_msg2sp(xpl0, len, error)
 				ipseclog((LOG_DEBUG, "%s: invalid level=%u\n",
 					__func__,
 					xisr->sadb_x_ipsecrequest_level));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = EINVAL;
 				return NULL;
 			}
-			(*p_isr)->level = xisr->sadb_x_ipsecrequest_level;
+			isr->level = xisr->sadb_x_ipsecrequest_level;
 
 			/* set IP addresses if there */
 			if (xisr->sadb_x_ipsecrequest_len > sizeof(*xisr)) {
 				struct sockaddr *paddr;
 
 				paddr = (struct sockaddr *)(xisr + 1);
-
 				/* validity check */
 				if (paddr->sa_len
-				    > sizeof((*p_isr)->saidx.src)) {
+				    > sizeof(isr->saidx.src)) {
 					ipseclog((LOG_DEBUG, "%s: invalid "
 						"request address length.\n",
 						__func__));
-					KEY_FREESP(&newsp);
+					key_freesp(&newsp);
 					*error = EINVAL;
 					return NULL;
 				}
-				bcopy(paddr, &(*p_isr)->saidx.src,
-					paddr->sa_len);
-
-				paddr = (struct sockaddr *)((caddr_t)paddr
-							+ paddr->sa_len);
+				bcopy(paddr, &isr->saidx.src, paddr->sa_len);
+				paddr = (struct sockaddr *)((caddr_t)paddr +
+				    paddr->sa_len);
 
 				/* validity check */
 				if (paddr->sa_len
-				    > sizeof((*p_isr)->saidx.dst)) {
+				    > sizeof(isr->saidx.dst)) {
 					ipseclog((LOG_DEBUG, "%s: invalid "
 						"request address length.\n",
 						__func__));
-					KEY_FREESP(&newsp);
+					key_freesp(&newsp);
 					*error = EINVAL;
 					return NULL;
 				}
-				bcopy(paddr, &(*p_isr)->saidx.dst,
-					paddr->sa_len);
+				/* AF family should match */
+				if (paddr->sa_family !=
+				    isr->saidx.src.sa.sa_family) {
+					ipseclog((LOG_DEBUG, "%s: address "
+					    "family doesn't match.\n",
+						__func__));
+					key_freesp(&newsp);
+					*error = EINVAL;
+					return (NULL);
+				}
+				bcopy(paddr, &isr->saidx.dst, paddr->sa_len);
+			} else {
+				/*
+				 * Addresses for TUNNEL mode requests are
+				 * mandatory.
+				 */
+				if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
+					ipseclog((LOG_DEBUG, "%s: missing "
+					    "request addresses.\n", __func__));
+					key_freesp(&newsp);
+					*error = EINVAL;
+					return (NULL);
+				}
 			}
-
-			(*p_isr)->sp = newsp;
-
-			/* initialization for the next. */
-			p_isr = &(*p_isr)->next;
 			tlen -= xisr->sadb_x_ipsecrequest_len;
 
 			/* validity check */
 			if (tlen < 0) {
 				ipseclog((LOG_DEBUG, "%s: becoming tlen < 0.\n",
 					__func__));
-				KEY_FREESP(&newsp);
+				key_freesp(&newsp);
 				*error = EINVAL;
 				return NULL;
 			}
@@ -1594,76 +1581,114 @@ key_msg2sp(xpl0, len, error)
 			xisr = (struct sadb_x_ipsecrequest *)((caddr_t)xisr
 			                 + xisr->sadb_x_ipsecrequest_len);
 		}
+		/* XXXAE: LARVAL SP */
+		if (newsp->tcount < 1) {
+			ipseclog((LOG_DEBUG, "%s: valid IPSEC transforms "
+			    "not found.\n", __func__));
+			key_freesp(&newsp);
+			*error = EINVAL;
+			return (NULL);
+		}
 	    }
 		break;
 	default:
 		ipseclog((LOG_DEBUG, "%s: invalid policy type.\n", __func__));
-		KEY_FREESP(&newsp);
+		key_freesp(&newsp);
 		*error = EINVAL;
 		return NULL;
 	}
 
 	*error = 0;
-	return newsp;
+	return (newsp);
 }
 
-static u_int32_t
-key_newreqid()
+uint32_t
+key_newreqid(void)
 {
-	static u_int32_t auto_reqid = IPSEC_MANUAL_REQID_MAX + 1;
+	static uint32_t auto_reqid = IPSEC_MANUAL_REQID_MAX + 1;
 
-	auto_reqid = (auto_reqid == ~0
-			? IPSEC_MANUAL_REQID_MAX + 1 : auto_reqid + 1);
+	if (auto_reqid == ~0)
+		auto_reqid = IPSEC_MANUAL_REQID_MAX + 1;
+	else
+		auto_reqid++;
 
 	/* XXX should be unique check */
-
-	return auto_reqid;
+	return (auto_reqid);
 }
 
 /*
  * copy secpolicy struct to sadb_x_policy structure indicated.
  */
-struct mbuf *
-key_sp2msg(sp)
-	struct secpolicy *sp;
+static struct mbuf *
+key_sp2mbuf(struct secpolicy *sp)
 {
-	struct sadb_x_policy *xpl;
-	int tlen;
-	caddr_t p;
 	struct mbuf *m;
-
-	IPSEC_ASSERT(sp != NULL, ("null policy"));
+	size_t tlen;
 
 	tlen = key_getspreqmsglen(sp);
-
-	m = key_alloc_mbuf(tlen);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
-		return NULL;
+	m = m_get2(tlen, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, tlen);
+	m->m_len = tlen;
+	if (key_sp2msg(sp, m->m_data, &tlen) != 0) {
+		m_freem(m);
+		return (NULL);
 	}
+	return (m);
+}
 
-	m->m_len = tlen;
-	m->m_next = NULL;
-	xpl = mtod(m, struct sadb_x_policy *);
-	bzero(xpl, tlen);
+int
+key_sp2msg(struct secpolicy *sp, void *request, size_t *len)
+{
+	struct sadb_x_ipsecrequest *xisr;
+	struct sadb_x_policy *xpl;
+	struct ipsecrequest *isr;
+	size_t xlen, ilen;
+	caddr_t p;
+	int error, i;
+
+	IPSEC_ASSERT(sp != NULL, ("null policy"));
+
+	xlen = sizeof(*xpl);
+	if (*len < xlen)
+		return (EINVAL);
 
-	xpl->sadb_x_policy_len = PFKEY_UNIT64(tlen);
+	error = 0;
+	bzero(request, *len);
+	xpl = (struct sadb_x_policy *)request;
 	xpl->sadb_x_policy_exttype = SADB_X_EXT_POLICY;
 	xpl->sadb_x_policy_type = sp->policy;
 	xpl->sadb_x_policy_dir = sp->spidx.dir;
 	xpl->sadb_x_policy_id = sp->id;
-	p = (caddr_t)xpl + sizeof(*xpl);
+	xpl->sadb_x_policy_priority = sp->priority;
+	switch (sp->state) {
+	case IPSEC_SPSTATE_IFNET:
+		xpl->sadb_x_policy_scope = IPSEC_POLICYSCOPE_IFNET;
+		break;
+	case IPSEC_SPSTATE_PCB:
+		xpl->sadb_x_policy_scope = IPSEC_POLICYSCOPE_PCB;
+		break;
+	default:
+		xpl->sadb_x_policy_scope = IPSEC_POLICYSCOPE_GLOBAL;
+	}
 
 	/* if is the policy for ipsec ? */
 	if (sp->policy == IPSEC_POLICY_IPSEC) {
-		struct sadb_x_ipsecrequest *xisr;
-		struct ipsecrequest *isr;
-
-		for (isr = sp->req; isr != NULL; isr = isr->next) {
-
+		p = (caddr_t)xpl + sizeof(*xpl);
+		for (i = 0; i < sp->tcount; i++) {
+			isr = sp->req[i];
+			ilen = PFKEY_ALIGN8(sizeof(*xisr) +
+			    isr->saidx.src.sa.sa_len +
+			    isr->saidx.dst.sa.sa_len);
+			xlen += ilen;
+			if (xlen > *len) {
+				error = ENOBUFS;
+				/* Calculate needed size */
+				continue;
+			}
 			xisr = (struct sadb_x_ipsecrequest *)p;
-
+			xisr->sadb_x_ipsecrequest_len = ilen;
 			xisr->sadb_x_ipsecrequest_proto = isr->saidx.proto;
 			xisr->sadb_x_ipsecrequest_mode = isr->saidx.mode;
 			xisr->sadb_x_ipsecrequest_level = isr->level;
@@ -1673,31 +1698,21 @@ key_sp2msg(sp)
 			bcopy(&isr->saidx.src, p, isr->saidx.src.sa.sa_len);
 			p += isr->saidx.src.sa.sa_len;
 			bcopy(&isr->saidx.dst, p, isr->saidx.dst.sa.sa_len);
-			p += isr->saidx.src.sa.sa_len;
-
-			xisr->sadb_x_ipsecrequest_len =
-				PFKEY_ALIGN8(sizeof(*xisr)
-					+ isr->saidx.src.sa.sa_len
-					+ isr->saidx.dst.sa.sa_len);
+			p += isr->saidx.dst.sa.sa_len;
 		}
 	}
-
-	return m;
+	xpl->sadb_x_policy_len = PFKEY_UNIT64(xlen);
+	if (error == 0)
+		*len = xlen;
+	else
+		*len = sizeof(*xpl);
+	return (error);
 }
 
 /* m will not be freed nor modified */
 static struct mbuf *
-#ifdef __STDC__
 key_gather_mbuf(struct mbuf *m, const struct sadb_msghdr *mhp,
-	int ndeep, int nitem, ...)
-#else
-key_gather_mbuf(m, mhp, ndeep, nitem, va_alist)
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
-	int ndeep;
-	int nitem;
-	va_dcl
-#endif
+    int ndeep, int nitem, ...)
 {
 	va_list ap;
 	int idx;
@@ -1725,7 +1740,7 @@ key_gather_mbuf(m, mhp, ndeep, nitem, va_alist)
 
 			IPSEC_ASSERT(len <= MHLEN, ("header too big %u", len));
 
-			MGETHDR(n, M_DONTWAIT, MT_DATA);
+			MGETHDR(n, M_NOWAIT, MT_DATA);
 			if (!n)
 				goto fail;
 			n->m_len = len;
@@ -1734,17 +1749,16 @@ key_gather_mbuf(m, mhp, ndeep, nitem, va_alist)
 			    mtod(n, caddr_t));
 		} else if (i < ndeep) {
 			len = mhp->extlen[idx];
-			n = key_alloc_mbuf(len);
-			if (!n || n->m_next) {	/*XXX*/
-				if (n)
-					m_freem(n);
+			n = m_get2(len, M_NOWAIT, MT_DATA, 0);
+			if (n == NULL)
 				goto fail;
-			}
+			m_align(n, len);
+			n->m_len = len;
 			m_copydata(m, mhp->extoff[idx], mhp->extlen[idx],
 			    mtod(n, caddr_t));
 		} else {
 			n = m_copym(m, mhp->extoff[idx], mhp->extlen[idx],
-			    M_DONTWAIT);
+			    M_NOWAIT);
 		}
 		if (n == NULL)
 			goto fail;
@@ -1784,18 +1798,16 @@ fail:
  * SPDSETIDX like SPDADD without a part of policy requests.
  * SPDUPDATE replace a unique policy entry.
  *
+ * XXXAE: serialize this in PF_KEY to avoid races.
  * m will always be freed.
  */
 static int
-key_spdadd(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spdadd(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
+	struct secpolicyindex spidx;
 	struct sadb_address *src0, *dst0;
 	struct sadb_x_policy *xpl0, *xpl;
 	struct sadb_lifetime *lft = NULL;
-	struct secpolicyindex spidx;
 	struct secpolicy *newsp;
 	int error;
 
@@ -1804,24 +1816,26 @@ key_spdadd(so, m, mhp)
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
-	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
-	    mhp->ext[SADB_X_EXT_POLICY] == NULL) {
-		ipseclog((LOG_DEBUG, "key_spdadd: invalid message is passed.\n"));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKHDR(mhp, SADB_X_EXT_POLICY)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKLEN(mhp, SADB_X_EXT_POLICY)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->ext[SADB_EXT_LIFETIME_HARD] != NULL) {
-		if (mhp->extlen[SADB_EXT_LIFETIME_HARD]
-			< sizeof(struct sadb_lifetime)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-				__func__));
+	if (!SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD)) {
+		if (SADB_CHECKLEN(mhp, SADB_EXT_LIFETIME_HARD)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
 			return key_senderror(so, m, EINVAL);
 		}
 		lft = (struct sadb_lifetime *)mhp->ext[SADB_EXT_LIFETIME_HARD];
@@ -1831,141 +1845,93 @@ key_spdadd(so, m, mhp)
 	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
 	xpl0 = (struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY];
 
-	/* 
-	 * Note: do not parse SADB_X_EXT_NAT_T_* here:
-	 * we are processing traffic endpoints.
-	 */
-
-	/* make secindex */
-	/* XXX boundary check against sa_len */
-	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
-	                src0 + 1,
-	                dst0 + 1,
-	                src0->sadb_address_prefixlen,
-	                dst0->sadb_address_prefixlen,
-	                src0->sadb_address_proto,
-	                &spidx);
-
-	/* checking the direciton. */
+	/* check the direciton */
 	switch (xpl0->sadb_x_policy_dir) {
 	case IPSEC_DIR_INBOUND:
 	case IPSEC_DIR_OUTBOUND:
 		break;
 	default:
-		ipseclog((LOG_DEBUG, "%s: Invalid SP direction.\n", __func__));
-		mhp->msg->sadb_msg_errno = EINVAL;
-		return 0;
+		ipseclog((LOG_DEBUG, "%s: invalid SP direction.\n", __func__));
+		return key_senderror(so, m, EINVAL);
 	}
-
-	/* check policy */
 	/* key_spdadd() accepts DISCARD, NONE and IPSEC. */
-	if (xpl0->sadb_x_policy_type == IPSEC_POLICY_ENTRUST
-	 || xpl0->sadb_x_policy_type == IPSEC_POLICY_BYPASS) {
-		ipseclog((LOG_DEBUG, "%s: Invalid policy type.\n", __func__));
+	if (xpl0->sadb_x_policy_type != IPSEC_POLICY_DISCARD &&
+	    xpl0->sadb_x_policy_type != IPSEC_POLICY_NONE &&
+	    xpl0->sadb_x_policy_type != IPSEC_POLICY_IPSEC) {
+		ipseclog((LOG_DEBUG, "%s: invalid policy type.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
 	/* policy requests are mandatory when action is ipsec. */
-        if (mhp->msg->sadb_msg_type != SADB_X_SPDSETIDX
-	 && xpl0->sadb_x_policy_type == IPSEC_POLICY_IPSEC
-	 && mhp->extlen[SADB_X_EXT_POLICY] <= sizeof(*xpl0)) {
-		ipseclog((LOG_DEBUG, "%s: some policy requests part required\n",
-			__func__));
+	if (xpl0->sadb_x_policy_type == IPSEC_POLICY_IPSEC &&
+	    mhp->extlen[SADB_X_EXT_POLICY] <= sizeof(*xpl0)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: policy requests required.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	/*
-	 * checking there is SP already or not.
-	 * SPDUPDATE doesn't depend on whether there is a SP or not.
-	 * If the type is either SPDADD or SPDSETIDX AND a SP is found,
-	 * then error.
-	 */
-	newsp = key_getsp(&spidx);
-	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
-		if (newsp) {
-			SPTREE_LOCK();
-			newsp->state = IPSEC_SPSTATE_DEAD;
-			SPTREE_UNLOCK();
-			KEY_FREESP(&newsp);
-		}
-	} else {
-		if (newsp != NULL) {
-			KEY_FREESP(&newsp);
-			ipseclog((LOG_DEBUG, "%s: a SP entry exists already.\n",
-				__func__));
-			return key_senderror(so, m, EEXIST);
-		}
-	}
-
-	/* allocation new SP entry */
-	if ((newsp = key_msg2sp(xpl0, PFKEY_EXTLEN(xpl0), &error)) == NULL) {
+	error = key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1));
+	if (error != 0 ||
+	    src0->sadb_address_proto != dst0->sadb_address_proto) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
 		return key_senderror(so, m, error);
 	}
-
-	if ((newsp->id = key_getnewspid()) == 0) {
-		_key_delsp(newsp);
-		return key_senderror(so, m, ENOBUFS);
-	}
-
-	/* XXX boundary check against sa_len */
+	/* make secindex */
 	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
 	                src0 + 1,
 	                dst0 + 1,
 	                src0->sadb_address_prefixlen,
 	                dst0->sadb_address_prefixlen,
 	                src0->sadb_address_proto,
-	                &newsp->spidx);
-
-	/* sanity check on addr pair */
-	if (((struct sockaddr *)(src0 + 1))->sa_family !=
-			((struct sockaddr *)(dst0+ 1))->sa_family) {
-		_key_delsp(newsp);
-		return key_senderror(so, m, EINVAL);
-	}
-	if (((struct sockaddr *)(src0 + 1))->sa_len !=
-			((struct sockaddr *)(dst0+ 1))->sa_len) {
-		_key_delsp(newsp);
-		return key_senderror(so, m, EINVAL);
-	}
-#if 1
-	if (newsp->req && newsp->req->saidx.src.sa.sa_family && newsp->req->saidx.dst.sa.sa_family) {
-		if (newsp->req->saidx.src.sa.sa_family != newsp->req->saidx.dst.sa.sa_family) {
-			_key_delsp(newsp);
-			return key_senderror(so, m, EINVAL);
+	                &spidx);
+	/* Checking there is SP already or not. */
+	newsp = key_getsp(&spidx);
+	if (newsp != NULL) {
+		if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
+			KEYDBG(KEY_STAMP,
+			    printf("%s: unlink SP(%p) for SPDUPDATE\n",
+				__func__, newsp));
+			KEYDBG(KEY_DATA, kdebug_secpolicy(newsp));
+			key_unlink(newsp);
+			key_freesp(&newsp);
+		} else {
+			key_freesp(&newsp);
+			ipseclog((LOG_DEBUG, "%s: a SP entry exists already.",
+			    __func__));
+			return (key_senderror(so, m, EEXIST));
 		}
 	}
-#endif
 
-	newsp->created = time_second;
-	newsp->lastused = newsp->created;
+	/* allocate new SP entry */
+	if ((newsp = key_msg2sp(xpl0, PFKEY_EXTLEN(xpl0), &error)) == NULL) {
+		return key_senderror(so, m, error);
+	}
+
+	newsp->lastused = newsp->created = time_second;
 	newsp->lifetime = lft ? lft->sadb_lifetime_addtime : 0;
 	newsp->validtime = lft ? lft->sadb_lifetime_usetime : 0;
+	bcopy(&spidx, &newsp->spidx, sizeof(spidx));
 
-	newsp->refcnt = 1;	/* do not reclaim until I say I do */
-	newsp->state = IPSEC_SPSTATE_ALIVE;
-	LIST_INSERT_TAIL(&V_sptree[newsp->spidx.dir], newsp, secpolicy, chain);
-
-	/* delete the entry in spacqtree */
-	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
-		struct secspacq *spacq = key_getspacq(&spidx);
-		if (spacq != NULL) {
-			/* reset counter in order to deletion by timehandler. */
-			spacq->created = time_second;
-			spacq->count = 0;
-			SPACQ_UNLOCK();
-		}
-    	}
+	/* XXXAE: there is race between key_getsp() and key_insertsp() */
+	SPTREE_WLOCK();
+	if ((newsp->id = key_getnewspid()) == 0) {
+		SPTREE_WUNLOCK();
+		key_freesp(&newsp);
+		return key_senderror(so, m, ENOBUFS);
+	}
+	key_insertsp(newsp);
+	SPTREE_WUNLOCK();
+
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, newsp));
+	KEYDBG(KEY_DATA, kdebug_secpolicy(newsp));
 
     {
 	struct mbuf *n, *mpolicy;
 	struct sadb_msg *newmsg;
 	int off;
 
-	/*
-	 * Note: do not send SADB_X_EXT_NAT_T_* here:
-	 * we are sending traffic endpoints.
-	 */
-
 	/* create new sadb_msg to reply. */
 	if (lft) {
 		n = key_gather_mbuf(m, mhp, 2, 5, SADB_EXT_RESERVED,
@@ -2013,30 +1979,32 @@ key_spdadd(so, m, mhp)
  *	0:	failure.
  *	others: success.
  */
-static u_int32_t
-key_getnewspid()
+static uint32_t
+key_getnewspid(void)
 {
-	u_int32_t newid = 0;
-	int count = V_key_spi_trycnt;	/* XXX */
 	struct secpolicy *sp;
+	uint32_t newid = 0;
+	int count = V_key_spi_trycnt;	/* XXX */
 
-	/* when requesting to allocate spi ranged */
+	SPTREE_WLOCK_ASSERT();
 	while (count--) {
-		newid = (V_policy_id = (V_policy_id == ~0 ? 1 : V_policy_id + 1));
-
-		if ((sp = key_getspbyid(newid)) == NULL)
+		if (V_policy_id == ~0) /* overflowed */
+			newid = V_policy_id = 1;
+		else
+			newid = ++V_policy_id;
+		LIST_FOREACH(sp, SPHASH_HASH(newid), idhash) {
+			if (sp->id == newid)
+				break;
+		}
+		if (sp == NULL)
 			break;
-
-		KEY_FREESP(&sp);
 	}
-
 	if (count == 0 || newid == 0) {
-		ipseclog((LOG_DEBUG, "%s: to allocate policy id is failed.\n",
-			__func__));
-		return 0;
+		ipseclog((LOG_DEBUG, "%s: failed to allocate policy id.\n",
+		    __func__));
+		return (0);
 	}
-
-	return newid;
+	return (newid);
 }
 
 /*
@@ -2052,14 +2020,12 @@ key_getnewspid()
  * m will always be freed.
  */
 static int
-key_spddelete(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spddelete(struct socket *so, struct mbuf *m,
+    const struct sadb_msghdr *mhp)
 {
+	struct secpolicyindex spidx;
 	struct sadb_address *src0, *dst0;
 	struct sadb_x_policy *xpl0;
-	struct secpolicyindex spidx;
 	struct secpolicy *sp;
 
 	IPSEC_ASSERT(so != NULL, ("null so"));
@@ -2067,18 +2033,19 @@ key_spddelete(so, m, mhp)
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
-	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
-	    mhp->ext[SADB_X_EXT_POLICY] == NULL) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKHDR(mhp, SADB_X_EXT_POLICY)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKLEN(mhp, SADB_X_EXT_POLICY)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
@@ -2086,13 +2053,29 @@ key_spddelete(so, m, mhp)
 	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
 	xpl0 = (struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY];
 
-	/*
-	 * Note: do not parse SADB_X_EXT_NAT_T_* here:
-	 * we are processing traffic endpoints.
-	 */
-
+	/* check the direciton */
+	switch (xpl0->sadb_x_policy_dir) {
+	case IPSEC_DIR_INBOUND:
+	case IPSEC_DIR_OUTBOUND:
+		break;
+	default:
+		ipseclog((LOG_DEBUG, "%s: invalid SP direction.\n", __func__));
+		return key_senderror(so, m, EINVAL);
+	}
+	/* Only DISCARD, NONE and IPSEC are allowed */
+	if (xpl0->sadb_x_policy_type != IPSEC_POLICY_DISCARD &&
+	    xpl0->sadb_x_policy_type != IPSEC_POLICY_NONE &&
+	    xpl0->sadb_x_policy_type != IPSEC_POLICY_IPSEC) {
+		ipseclog((LOG_DEBUG, "%s: invalid policy type.\n", __func__));
+		return key_senderror(so, m, EINVAL);
+	}
+	if (key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1)) != 0 ||
+	    src0->sadb_address_proto != dst0->sadb_address_proto) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		return key_senderror(so, m, EINVAL);
+	}
 	/* make secindex */
-	/* XXX boundary check against sa_len */
 	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
 	                src0 + 1,
 	                dst0 + 1,
@@ -2101,16 +2084,6 @@ key_spddelete(so, m, mhp)
 	                src0->sadb_address_proto,
 	                &spidx);
 
-	/* checking the direciton. */
-	switch (xpl0->sadb_x_policy_dir) {
-	case IPSEC_DIR_INBOUND:
-	case IPSEC_DIR_OUTBOUND:
-		break;
-	default:
-		ipseclog((LOG_DEBUG, "%s: Invalid SP direction.\n", __func__));
-		return key_senderror(so, m, EINVAL);
-	}
-
 	/* Is there SP in SPD ? */
 	if ((sp = key_getsp(&spidx)) == NULL) {
 		ipseclog((LOG_DEBUG, "%s: no SP found.\n", __func__));
@@ -2120,20 +2093,16 @@ key_spddelete(so, m, mhp)
 	/* save policy id to buffer to be returned. */
 	xpl0->sadb_x_policy_id = sp->id;
 
-	SPTREE_LOCK();
-	sp->state = IPSEC_SPSTATE_DEAD;
-	SPTREE_UNLOCK();
-	KEY_FREESP(&sp);
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, sp));
+	KEYDBG(KEY_DATA, kdebug_secpolicy(sp));
+	key_unlink(sp);
+	key_freesp(&sp);
 
     {
 	struct mbuf *n;
 	struct sadb_msg *newmsg;
 
-	/*
-	 * Note: do not send SADB_X_EXT_NAT_T_* here:
-	 * we are sending traffic endpoints.
-	 */
-
 	/* create new sadb_msg to reply. */
 	n = key_gather_mbuf(m, mhp, 1, 4, SADB_EXT_RESERVED,
 	    SADB_X_EXT_POLICY, SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
@@ -2162,37 +2131,45 @@ key_spddelete(so, m, mhp)
  * m will always be freed.
  */
 static int
-key_spddelete2(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spddelete2(struct socket *so, struct mbuf *m,
+    const struct sadb_msghdr *mhp)
 {
-	u_int32_t id;
 	struct secpolicy *sp;
+	uint32_t id;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
-	if (mhp->ext[SADB_X_EXT_POLICY] == NULL ||
-	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n", __func__));
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_POLICY) ||
+	    SADB_CHECKLEN(mhp, SADB_X_EXT_POLICY)) {
+		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	id = ((struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY])->sadb_x_policy_id;
+	id = ((struct sadb_x_policy *)
+	    mhp->ext[SADB_X_EXT_POLICY])->sadb_x_policy_id;
 
 	/* Is there SP in SPD ? */
 	if ((sp = key_getspbyid(id)) == NULL) {
-		ipseclog((LOG_DEBUG, "%s: no SP found id:%u.\n", __func__, id));
+		ipseclog((LOG_DEBUG, "%s: no SP found for id %u.\n",
+		    __func__, id));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	SPTREE_LOCK();
-	sp->state = IPSEC_SPSTATE_DEAD;
-	SPTREE_UNLOCK();
-	KEY_FREESP(&sp);
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, sp));
+	KEYDBG(KEY_DATA, kdebug_secpolicy(sp));
+	key_unlink(sp);
+	if (sp->state != IPSEC_SPSTATE_DEAD) {
+		ipseclog((LOG_DEBUG, "%s: failed to delete SP with id %u.\n",
+		    __func__, id));
+		key_freesp(&sp);
+		return (key_senderror(so, m, EACCES));
+	}
+	key_freesp(&sp);
 
     {
 	struct mbuf *n, *nn;
@@ -2202,10 +2179,9 @@ key_spddelete2(so, m, mhp)
 	/* create new sadb_msg to reply. */
 	len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
 
-	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	MGETHDR(n, M_NOWAIT, MT_DATA);
 	if (n && len > MHLEN) {
-		MCLGET(n, M_DONTWAIT);
-		if ((n->m_flags & M_EXT) == 0) {
+		if (!(MCLGET(n, M_NOWAIT))) {
 			m_freem(n);
 			n = NULL;
 		}
@@ -2224,7 +2200,7 @@ key_spddelete2(so, m, mhp)
 		off, len));
 
 	n->m_next = m_copym(m, mhp->extoff[SADB_X_EXT_POLICY],
-	    mhp->extlen[SADB_X_EXT_POLICY], M_DONTWAIT);
+	    mhp->extlen[SADB_X_EXT_POLICY], M_NOWAIT);
 	if (!n->m_next) {
 		m_freem(n);
 		return key_senderror(so, m, ENOBUFS);
@@ -2244,7 +2220,7 @@ key_spddelete2(so, m, mhp)
 }
 
 /*
- * SADB_X_GET processing
+ * SADB_X_SPDGET processing
  * receive
  *   <base, policy(*)>
  * from the user(?),
@@ -2256,37 +2232,37 @@ key_spddelete2(so, m, mhp)
  * m will always be freed.
  */
 static int
-key_spdget(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spdget(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	u_int32_t id;
 	struct secpolicy *sp;
 	struct mbuf *n;
+	uint32_t id;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
-	if (mhp->ext[SADB_X_EXT_POLICY] == NULL ||
-	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_POLICY) ||
+	    SADB_CHECKLEN(mhp, SADB_X_EXT_POLICY)) {
 		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	id = ((struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY])->sadb_x_policy_id;
+	id = ((struct sadb_x_policy *)
+	    mhp->ext[SADB_X_EXT_POLICY])->sadb_x_policy_id;
 
 	/* Is there SP in SPD ? */
 	if ((sp = key_getspbyid(id)) == NULL) {
-		ipseclog((LOG_DEBUG, "%s: no SP found id:%u.\n", __func__, id));
+		ipseclog((LOG_DEBUG, "%s: no SP found for id %u.\n",
+		    __func__, id));
 		return key_senderror(so, m, ENOENT);
 	}
 
-	n = key_setdumpsp(sp, SADB_X_SPDGET, 0, mhp->msg->sadb_msg_pid);
-	KEY_FREESP(&sp);
+	n = key_setdumpsp(sp, SADB_X_SPDGET, mhp->msg->sadb_msg_seq,
+	    mhp->msg->sadb_msg_pid);
+	key_freesp(&sp);
 	if (n != NULL) {
 		m_freem(m);
 		return key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
@@ -2300,7 +2276,7 @@ key_spdget(so, m, mhp)
  * send
  *   <base, policy(*)>
  * to KMD, and expect to receive
- *   <base> with SADB_X_SPDACQUIRE if error occured,
+ *   <base> with SADB_X_SPDACQUIRE if error occurred,
  * or
  *   <base, policy>
  * with SADB_X_SPDUPDATE from KMD by PF_KEY.
@@ -2310,8 +2286,7 @@ key_spdget(so, m, mhp)
  *    others: error number
  */
 int
-key_spdacquire(sp)
-	struct secpolicy *sp;
+key_spdacquire(struct secpolicy *sp)
 {
 	struct mbuf *result = NULL, *m;
 	struct secspacq *newspacq;
@@ -2330,7 +2305,8 @@ key_spdacquire(sp)
 		} else {
 			/* increment counter and do nothing. */
 			newspacq->count++;
-			return 0;
+			SPACQ_UNLOCK();
+			return (0);
 		}
 		SPACQ_UNLOCK();
 	} else {
@@ -2370,13 +2346,11 @@ key_spdacquire(sp)
  * m will always be freed.
  */
 static int
-key_spdflush(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spdflush(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
+	struct secpolicy_queue drainq;
 	struct sadb_msg *newmsg;
-	struct secpolicy *sp;
+	struct secpolicy *sp, *nextsp;
 	u_int dir;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
@@ -2387,11 +2361,27 @@ key_spdflush(so, m, mhp)
 	if (m->m_len != PFKEY_ALIGN8(sizeof(struct sadb_msg)))
 		return key_senderror(so, m, EINVAL);
 
+	TAILQ_INIT(&drainq);
+	SPTREE_WLOCK();
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		SPTREE_LOCK();
-		LIST_FOREACH(sp, &V_sptree[dir], chain)
-			sp->state = IPSEC_SPSTATE_DEAD;
-		SPTREE_UNLOCK();
+		TAILQ_CONCAT(&drainq, &V_sptree[dir], chain);
+	}
+	/*
+	 * We need to set state to DEAD for each policy to be sure,
+	 * that another thread won't try to unlink it.
+	 * Also remove SP from sphash.
+	 */
+	TAILQ_FOREACH(sp, &drainq, chain) {
+		sp->state = IPSEC_SPSTATE_DEAD;
+		LIST_REMOVE(sp, idhash);
+	}
+	V_sp_genid++;
+	SPTREE_WUNLOCK();
+	sp = TAILQ_FIRST(&drainq);
+	while (sp != NULL) {
+		nextsp = TAILQ_NEXT(sp, chain);
+		key_freesp(&sp);
+		sp = nextsp;
 	}
 
 	if (sizeof(struct sadb_msg) > m->m_len + M_TRAILINGSPACE(m)) {
@@ -2410,27 +2400,34 @@ key_spdflush(so, m, mhp)
 	return key_sendup_mbuf(so, m, KEY_SENDUP_ALL);
 }
 
+static uint8_t
+key_satype2scopemask(uint8_t satype)
+{
+
+	if (satype == IPSEC_POLICYSCOPE_ANY)
+		return (0xff);
+	return (satype);
+}
 /*
  * SADB_SPDDUMP processing
  * receive
  *   <base>
- * from the user, and dump all SP leaves
- * and send,
+ * from the user, and dump all SP leaves and send,
  *   <base> .....
  * to the ikmpd.
  *
- * m will always be freed.
+ * NOTE:
+ *   sadb_msg_satype is considered as mask of policy scopes.
+ *   m will always be freed.
  */
 static int
-key_spddump(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_spddump(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
+	SPTREE_RLOCK_TRACKER;
 	struct secpolicy *sp;
-	int cnt;
-	u_int dir;
 	struct mbuf *n;
+	int cnt;
+	u_int dir, scope;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -2439,36 +2436,55 @@ key_spddump(so, m, mhp)
 
 	/* search SPD entry and get buffer size. */
 	cnt = 0;
-	SPTREE_LOCK();
+	scope = key_satype2scopemask(mhp->msg->sadb_msg_satype);
+	SPTREE_RLOCK();
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		LIST_FOREACH(sp, &V_sptree[dir], chain) {
-			cnt++;
+		if (scope & IPSEC_POLICYSCOPE_GLOBAL) {
+			TAILQ_FOREACH(sp, &V_sptree[dir], chain)
+				cnt++;
+		}
+		if (scope & IPSEC_POLICYSCOPE_IFNET) {
+			TAILQ_FOREACH(sp, &V_sptree_ifnet[dir], chain)
+				cnt++;
 		}
 	}
 
 	if (cnt == 0) {
-		SPTREE_UNLOCK();
+		SPTREE_RUNLOCK();
 		return key_senderror(so, m, ENOENT);
 	}
 
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		LIST_FOREACH(sp, &V_sptree[dir], chain) {
-			--cnt;
-			n = key_setdumpsp(sp, SADB_X_SPDDUMP, cnt,
-			    mhp->msg->sadb_msg_pid);
+		if (scope & IPSEC_POLICYSCOPE_GLOBAL) {
+			TAILQ_FOREACH(sp, &V_sptree[dir], chain) {
+				--cnt;
+				n = key_setdumpsp(sp, SADB_X_SPDDUMP, cnt,
+				    mhp->msg->sadb_msg_pid);
+
+				if (n != NULL)
+					key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+			}
+		}
+		if (scope & IPSEC_POLICYSCOPE_IFNET) {
+			TAILQ_FOREACH(sp, &V_sptree_ifnet[dir], chain) {
+				--cnt;
+				n = key_setdumpsp(sp, SADB_X_SPDDUMP, cnt,
+				    mhp->msg->sadb_msg_pid);
 
-			if (n)
-				key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+				if (n != NULL)
+					key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+			}
 		}
 	}
 
-	SPTREE_UNLOCK();
+	SPTREE_RUNLOCK();
 	m_freem(m);
-	return 0;
+	return (0);
 }
 
 static struct mbuf *
-key_setdumpsp(struct secpolicy *sp, u_int8_t type, u_int32_t seq, u_int32_t pid)
+key_setdumpsp(struct secpolicy *sp, u_int8_t type, u_int32_t seq,
+    u_int32_t pid)
 {
 	struct mbuf *result = NULL, *m;
 	struct seclifetime lt;
@@ -2478,10 +2494,6 @@ key_setdumpsp(struct secpolicy *sp, u_int8_t type, u_int32_t seq, u_int32_t pid)
 		goto fail;
 	result = m;
 
-	/*
-	 * Note: do not send SADB_X_EXT_NAT_T_* here:
-	 * we are sending traffic endpoints.
-	 */
 	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
 	    &sp->spidx.src.sa, sp->spidx.prefs,
 	    sp->spidx.ul_proto);
@@ -2496,7 +2508,7 @@ key_setdumpsp(struct secpolicy *sp, u_int8_t type, u_int32_t seq, u_int32_t pid)
 		goto fail;
 	m_cat(result, m);
 
-	m = key_sp2msg(sp);
+	m = key_sp2mbuf(sp);
 	if (!m)
 		goto fail;
 	m_cat(result, m);
@@ -2539,37 +2551,29 @@ fail:
 	m_freem(result);
 	return NULL;
 }
-
 /*
  * get PFKEY message length for security policy and request.
  */
-static u_int
-key_getspreqmsglen(sp)
-	struct secpolicy *sp;
+static size_t
+key_getspreqmsglen(struct secpolicy *sp)
 {
-	u_int tlen;
+	size_t tlen, len;
+	int i;
 
 	tlen = sizeof(struct sadb_x_policy);
-
 	/* if is the policy for ipsec ? */
 	if (sp->policy != IPSEC_POLICY_IPSEC)
-		return tlen;
+		return (tlen);
 
 	/* get length of ipsec requests */
-    {
-	struct ipsecrequest *isr;
-	int len;
-
-	for (isr = sp->req; isr != NULL; isr = isr->next) {
+	for (i = 0; i < sp->tcount; i++) {
 		len = sizeof(struct sadb_x_ipsecrequest)
-			+ isr->saidx.src.sa.sa_len
-			+ isr->saidx.dst.sa.sa_len;
+			+ sp->req[i]->saidx.src.sa.sa_len
+			+ sp->req[i]->saidx.dst.sa.sa_len;
 
 		tlen += PFKEY_ALIGN8(len);
 	}
-    }
-
-	return tlen;
+	return (tlen);
 }
 
 /*
@@ -2582,18 +2586,18 @@ key_getspreqmsglen(sp)
  *	others	: error number
  */
 static int
-key_spdexpire(sp)
-	struct secpolicy *sp;
+key_spdexpire(struct secpolicy *sp)
 {
-	struct mbuf *result = NULL, *m;
-	int len;
-	int error = -1;
 	struct sadb_lifetime *lt;
-
-	/* XXX: Why do we lock ? */
+	struct mbuf *result = NULL, *m;
+	int len, error = -1;
 
 	IPSEC_ASSERT(sp != NULL, ("null secpolicy"));
 
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, sp));
+	KEYDBG(KEY_DATA, kdebug_secpolicy(sp));
+
 	/* set msg header */
 	m = key_setsadbmsg(SADB_X_SPDEXPIRE, 0, 0, 0, 0, 0);
 	if (!m) {
@@ -2604,13 +2608,13 @@ key_spdexpire(sp)
 
 	/* create lifetime extension (current and hard) */
 	len = PFKEY_ALIGN8(sizeof(*lt)) * 2;
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL) {
 		error = ENOBUFS;
 		goto fail;
 	}
+	m_align(m, len);
+	m->m_len = len;
 	bzero(mtod(m, caddr_t), len);
 	lt = mtod(m, struct sadb_lifetime *);
 	lt->sadb_lifetime_len = PFKEY_UNIT64(sizeof(struct sadb_lifetime));
@@ -2628,11 +2632,6 @@ key_spdexpire(sp)
 	lt->sadb_lifetime_usetime = sp->validtime;
 	m_cat(result, m);
 
-	/*
-	 * Note: do not send SADB_X_EXT_NAT_T_* here:
-	 * we are sending traffic endpoints.
-	 */
-
 	/* set sadb_address for source */
 	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
 	    &sp->spidx.src.sa,
@@ -2654,7 +2653,7 @@ key_spdexpire(sp)
 	m_cat(result, m);
 
 	/* set secpolicy */
-	m = key_sp2msg(sp);
+	m = key_sp2mbuf(sp);
 	if (!m) {
 		error = ENOBUFS;
 		goto fail;
@@ -2691,185 +2690,220 @@ key_spdexpire(sp)
 
 /* %%% SAD management */
 /*
- * allocating a memory for new SA head, and copy from the values of mhp.
+ * allocating and initialize new SA head.
  * OUT:	NULL	: failure due to the lack of memory.
  *	others	: pointer to new SA head.
  */
 static struct secashead *
-key_newsah(saidx)
-	struct secasindex *saidx;
+key_newsah(struct secasindex *saidx)
 {
-	struct secashead *newsah;
+	struct secashead *sah;
 
-	IPSEC_ASSERT(saidx != NULL, ("null saidx"));
+	sah = malloc(sizeof(struct secashead), M_IPSEC_SAH,
+	    M_NOWAIT | M_ZERO);
+	if (sah == NULL) {
+		PFKEYSTAT_INC(in_nomem);
+		return (NULL);
+	}
+	TAILQ_INIT(&sah->savtree_larval);
+	TAILQ_INIT(&sah->savtree_alive);
+	sah->saidx = *saidx;
+	sah->state = SADB_SASTATE_DEAD;
+	SAH_INITREF(sah);
 
-	newsah = malloc(sizeof(struct secashead), M_IPSEC_SAH, M_NOWAIT|M_ZERO);
-	if (newsah != NULL) {
-		int i;
-		for (i = 0; i < sizeof(newsah->savtree)/sizeof(newsah->savtree[0]); i++)
-			LIST_INIT(&newsah->savtree[i]);
-		newsah->saidx = *saidx;
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SAH(%p)\n", __func__, sah));
+	KEYDBG(KEY_DATA, kdebug_secash(sah, NULL));
+	return (sah);
+}
 
-		/* add to saidxtree */
-		newsah->state = SADB_SASTATE_MATURE;
+static void
+key_freesah(struct secashead **psah)
+{
+	struct secashead *sah = *psah;
 
-		SAHTREE_LOCK();
-		LIST_INSERT_HEAD(&V_sahtree, newsah, chain);
-		SAHTREE_UNLOCK();
-	}
-	return(newsah);
+	if (SAH_DELREF(sah) == 0)
+		return;
+
+	KEYDBG(KEY_STAMP,
+	    printf("%s: last reference to SAH(%p)\n", __func__, sah));
+	KEYDBG(KEY_DATA, kdebug_secash(sah, NULL));
+
+	*psah = NULL;
+	key_delsah(sah);
 }
 
-/*
- * delete SA index and all SA registerd.
- */
 static void
-key_delsah(sah)
-	struct secashead *sah;
+key_delsah(struct secashead *sah)
 {
-	struct secasvar *sav, *nextsav;
-	u_int stateidx;
-	int zombie = 0;
-
 	IPSEC_ASSERT(sah != NULL, ("NULL sah"));
-	SAHTREE_LOCK_ASSERT();
-
-	/* searching all SA registerd in the secindex. */
-	for (stateidx = 0;
-	     stateidx < _ARRAYLEN(saorder_state_any);
-	     stateidx++) {
-		u_int state = saorder_state_any[stateidx];
-		LIST_FOREACH_SAFE(sav, &sah->savtree[state], chain, nextsav) {
-			if (sav->refcnt == 0) {
-				/* sanity check */
-				KEY_CHKSASTATE(state, sav->state, __func__);
-				/* 
-				 * do NOT call KEY_FREESAV here:
-				 * it will only delete the sav if refcnt == 1,
-				 * where we already know that refcnt == 0
-				 */
-				key_delsav(sav);
-			} else {
-				/* give up to delete this sa */
-				zombie++;
-			}
-		}
-	}
-	if (!zombie) {		/* delete only if there are savs */
-		/* remove from tree of SA index */
-		if (__LIST_CHAINED(sah))
-			LIST_REMOVE(sah, chain);
-		if (sah->route_cache.sa_route.ro_rt) {
-			RTFREE(sah->route_cache.sa_route.ro_rt);
-			sah->route_cache.sa_route.ro_rt = (struct rtentry *)NULL;
-		}
-		free(sah, M_IPSEC_SAH);
-	}
+	IPSEC_ASSERT(sah->state == SADB_SASTATE_DEAD,
+	    ("Attempt to free non DEAD SAH %p", sah));
+	IPSEC_ASSERT(TAILQ_EMPTY(&sah->savtree_larval),
+	    ("Attempt to free SAH %p with LARVAL SA", sah));
+	IPSEC_ASSERT(TAILQ_EMPTY(&sah->savtree_alive),
+	    ("Attempt to free SAH %p with ALIVE SA", sah));
+
+	free(sah, M_IPSEC_SAH);
 }
 
 /*
- * allocating a new SA with LARVAL state.  key_add() and key_getspi() call,
+ * allocating a new SA for key_add() and key_getspi() call,
  * and copy the values of mhp into new buffer.
- * When SAD message type is GETSPI:
- *	to set sequence number from acq_seq++,
- *	to set zero to SPI.
- *	not to call key_setsava().
+ * When SAD message type is SADB_GETSPI set SA state to LARVAL.
+ * For SADB_ADD create and initialize SA with MATURE state.
  * OUT:	NULL	: fail
  *	others	: pointer to new secasvar.
- *
- * does not modify mbuf.  does not free mbuf on error.
  */
 static struct secasvar *
-key_newsav(m, mhp, sah, errp, where, tag)
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
-	struct secashead *sah;
-	int *errp;
-	const char* where;
-	int tag;
+key_newsav(const struct sadb_msghdr *mhp, struct secasindex *saidx,
+    uint32_t spi, int *errp)
 {
-	struct secasvar *newsav;
-	const struct sadb_sa *xsa;
+	struct secashead *sah;
+	struct secasvar *sav;
+	int isnew;
 
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
-	IPSEC_ASSERT(sah != NULL, ("null secashead"));
-
-	newsav = malloc(sizeof(struct secasvar), M_IPSEC_SA, M_NOWAIT|M_ZERO);
-	if (newsav == NULL) {
-		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-		*errp = ENOBUFS;
-		goto done;
-	}
-
-	switch (mhp->msg->sadb_msg_type) {
-	case SADB_GETSPI:
-		newsav->spi = 0;
-
-#ifdef IPSEC_DOSEQCHECK
-		/* sync sequence number */
-		if (mhp->msg->sadb_msg_seq == 0)
-			newsav->seq =
-				(V_acq_seq = (V_acq_seq == ~0 ? 1 : ++V_acq_seq));
-		else
-#endif
-			newsav->seq = mhp->msg->sadb_msg_seq;
-		break;
+	IPSEC_ASSERT(mhp->msg->sadb_msg_type == SADB_GETSPI ||
+	    mhp->msg->sadb_msg_type == SADB_ADD, ("wrong message type"));
 
-	case SADB_ADD:
-		/* sanity check */
-		if (mhp->ext[SADB_EXT_SA] == NULL) {
-			free(newsav, M_IPSEC_SA);
-			newsav = NULL;
-			ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-				__func__));
+	sav = NULL;
+	sah = NULL;
+	/* check SPI value */
+	switch (saidx->proto) {
+	case IPPROTO_ESP:
+	case IPPROTO_AH:
+		/*
+		 * RFC 4302, 2.4. Security Parameters Index (SPI), SPI values
+		 * 1-255 reserved by IANA for future use,
+		 * 0 for implementation specific, local use.
+		 */
+		if (ntohl(spi) <= 255) {
+			ipseclog((LOG_DEBUG, "%s: illegal range of SPI %u.\n",
+			    __func__, ntohl(spi)));
 			*errp = EINVAL;
 			goto done;
 		}
-		xsa = (const struct sadb_sa *)mhp->ext[SADB_EXT_SA];
-		newsav->spi = xsa->sadb_sa_spi;
-		newsav->seq = mhp->msg->sadb_msg_seq;
 		break;
-	default:
-		free(newsav, M_IPSEC_SA);
-		newsav = NULL;
-		*errp = EINVAL;
-		goto done;
 	}
 
+	sav = malloc(sizeof(struct secasvar), M_IPSEC_SA, M_NOWAIT | M_ZERO);
+	if (sav == NULL) {
+		*errp = ENOBUFS;
+		goto done;
+	}
+	sav->lock = malloc(sizeof(struct mtx), M_IPSEC_MISC,
+	    M_NOWAIT | M_ZERO);
+	if (sav->lock == NULL) {
+		*errp = ENOBUFS;
+		goto done;
+	}
+	mtx_init(sav->lock, "ipsec association", NULL, MTX_DEF);
+	sav->lft_c = uma_zalloc(V_key_lft_zone, M_NOWAIT);
+	if (sav->lft_c == NULL) {
+		*errp = ENOBUFS;
+		goto done;
+	}
+	counter_u64_zero(sav->lft_c_allocations);
+	counter_u64_zero(sav->lft_c_bytes);
 
-	/* copy sav values */
-	if (mhp->msg->sadb_msg_type != SADB_GETSPI) {
-		*errp = key_setsaval(newsav, m, mhp);
-		if (*errp) {
-			free(newsav, M_IPSEC_SA);
-			newsav = NULL;
+	sav->spi = spi;
+	sav->seq = mhp->msg->sadb_msg_seq;
+	sav->state = SADB_SASTATE_LARVAL;
+	sav->pid = (pid_t)mhp->msg->sadb_msg_pid;
+	SAV_INITREF(sav);
+again:
+	sah = key_getsah(saidx);
+	if (sah == NULL) {
+		/* create a new SA index */
+		sah = key_newsah(saidx);
+		if (sah == NULL) {
+			ipseclog((LOG_DEBUG,
+			    "%s: No more memory.\n", __func__));
+			*errp = ENOBUFS;
 			goto done;
 		}
-	}
-
-	SECASVAR_LOCK_INIT(newsav);
-
-	/* reset created */
-	newsav->created = time_second;
-	newsav->pid = mhp->msg->sadb_msg_pid;
+		isnew = 1;
+	} else
+		isnew = 0;
 
-	/* add to satree */
-	newsav->sah = sah;
-	sa_initref(newsav);
-	newsav->state = SADB_SASTATE_LARVAL;
+	sav->sah = sah;
+	if (mhp->msg->sadb_msg_type == SADB_GETSPI) {
+		sav->created = time_second;
+	} else if (sav->state == SADB_SASTATE_LARVAL) {
+		/*
+		 * Do not call key_setsaval() second time in case
+		 * of `goto again`. We will have MATURE state.
+		 */
+		*errp = key_setsaval(sav, mhp);
+		if (*errp != 0)
+			goto done;
+		sav->state = SADB_SASTATE_MATURE;
+	}
 
-	SAHTREE_LOCK();
-	LIST_INSERT_TAIL(&sah->savtree[SADB_SASTATE_LARVAL], newsav,
-			secasvar, chain);
-	SAHTREE_UNLOCK();
+	SAHTREE_WLOCK();
+	/*
+	 * Check that existing SAH wasn't unlinked.
+	 * Since we didn't hold the SAHTREE lock, it is possible,
+	 * that callout handler or key_flush() or key_delete() could
+	 * unlink this SAH.
+	 */
+	if (isnew == 0 && sah->state == SADB_SASTATE_DEAD) {
+		SAHTREE_WUNLOCK();
+		key_freesah(&sah);	/* reference from key_getsah() */
+		goto again;
+	}
+	if (isnew != 0) {
+		/*
+		 * Add new SAH into SADB.
+		 *
+		 * XXXAE: we can serialize key_add and key_getspi calls, so
+		 * several threads will not fight in the race.
+		 * Otherwise we should check under SAHTREE lock, that this
+		 * SAH would not added twice.
+		 */
+		TAILQ_INSERT_HEAD(&V_sahtree, sah, chain);
+		/* Add new SAH into hash by addresses */
+		LIST_INSERT_HEAD(SAHADDRHASH_HASH(saidx), sah, addrhash);
+		/* Now we are linked in the chain */
+		sah->state = SADB_SASTATE_MATURE;
+		/*
+		 * SAV references this new SAH.
+		 * In case of existing SAH we reuse reference
+		 * from key_getsah().
+		 */
+		SAH_ADDREF(sah);
+	}
+	/* Link SAV with SAH */
+	if (sav->state == SADB_SASTATE_MATURE)
+		TAILQ_INSERT_HEAD(&sah->savtree_alive, sav, chain);
+	else
+		TAILQ_INSERT_HEAD(&sah->savtree_larval, sav, chain);
+	/* Add SAV into SPI hash */
+	LIST_INSERT_HEAD(SAVHASH_HASH(sav->spi), sav, spihash);
+	SAHTREE_WUNLOCK();
+	*errp = 0;	/* success */
 done:
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s from %s:%u return SP:%p\n", __func__,
-			where, tag, newsav));
-
-	return newsav;
+	if (*errp != 0) {
+		if (sav != NULL) {
+			if (sav->lock != NULL) {
+				mtx_destroy(sav->lock);
+				free(sav->lock, M_IPSEC_MISC);
+			}
+			if (sav->lft_c != NULL)
+				uma_zfree(V_key_lft_zone, sav->lft_c);
+			free(sav, M_IPSEC_SA), sav = NULL;
+		}
+		if (sah != NULL)
+			key_freesah(&sah);
+		if (*errp == ENOBUFS) {
+			ipseclog((LOG_DEBUG, "%s: No more memory.\n",
+			    __func__));
+			PFKEYSTAT_INC(in_nomem);
+		}
+	}
+	return (sav);
 }
 
 /*
@@ -2878,6 +2912,13 @@ done:
 static void
 key_cleansav(struct secasvar *sav)
 {
+
+	if (sav->natt != NULL) {
+		free(sav->natt, M_IPSEC_MISC);
+		sav->natt = NULL;
+	}
+	if (sav->flags & SADB_X_EXT_F_CLONED)
+		return;
 	/*
 	 * Cleanup xform state.  Note that zeroize'ing causes the
 	 * keys to be cleared; otherwise we must do it ourself.
@@ -2886,7 +2927,6 @@ key_cleansav(struct secasvar *sav)
 		sav->tdb_xform->xf_zeroize(sav);
 		sav->tdb_xform = NULL;
 	} else {
-		KASSERT(sav->iv == NULL, ("iv but no xform"));
 		if (sav->key_auth != NULL)
 			bzero(sav->key_auth->key_data, _KEYLEN(sav->key_auth));
 		if (sav->key_enc != NULL)
@@ -2904,19 +2944,12 @@ key_cleansav(struct secasvar *sav)
 		free(sav->key_enc, M_IPSEC_MISC);
 		sav->key_enc = NULL;
 	}
-	if (sav->sched) {
-		bzero(sav->sched, sav->schedlen);
-		free(sav->sched, M_IPSEC_MISC);
-		sav->sched = NULL;
-	}
 	if (sav->replay != NULL) {
+		if (sav->replay->bitmap != NULL)
+			free(sav->replay->bitmap, M_IPSEC_MISC);
 		free(sav->replay, M_IPSEC_MISC);
 		sav->replay = NULL;
 	}
-	if (sav->lft_c != NULL) {
-		free(sav->lft_c, M_IPSEC_MISC);
-		sav->lft_c = NULL;
-	}
 	if (sav->lft_h != NULL) {
 		free(sav->lft_h, M_IPSEC_MISC);
 		sav->lft_h = NULL;
@@ -2931,202 +2964,288 @@ key_cleansav(struct secasvar *sav)
  * free() SA variable entry.
  */
 static void
-key_delsav(sav)
-	struct secasvar *sav;
+key_delsav(struct secasvar *sav)
 {
 	IPSEC_ASSERT(sav != NULL, ("null sav"));
-	IPSEC_ASSERT(sav->refcnt == 0, ("reference count %u > 0", sav->refcnt));
+	IPSEC_ASSERT(sav->state == SADB_SASTATE_DEAD,
+	    ("attempt to free non DEAD SA %p", sav));
+	IPSEC_ASSERT(sav->refcnt == 0, ("reference count %u > 0",
+	    sav->refcnt));
 
-	/* remove from SA header */
-	if (__LIST_CHAINED(sav))
-		LIST_REMOVE(sav, chain);
+	/*
+	 * SA must be unlinked from the chain and hashtbl.
+	 * If SA was cloned, we leave all fields untouched,
+	 * except NAT-T config.
+	 */
 	key_cleansav(sav);
-	SECASVAR_LOCK_DESTROY(sav);
+	if ((sav->flags & SADB_X_EXT_F_CLONED) == 0) {
+		mtx_destroy(sav->lock);
+		free(sav->lock, M_IPSEC_MISC);
+		uma_zfree(V_key_lft_zone, sav->lft_c);
+	}
 	free(sav, M_IPSEC_SA);
 }
 
 /*
- * search SAD.
+ * search SAH.
  * OUT:
  *	NULL	: not found
- *	others	: found, pointer to a SA.
+ *	others	: found, referenced pointer to a SAH.
  */
 static struct secashead *
-key_getsah(saidx)
-	struct secasindex *saidx;
+key_getsah(struct secasindex *saidx)
 {
+	SAHTREE_RLOCK_TRACKER;
 	struct secashead *sah;
 
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
-		if (key_cmpsaidx(&sah->saidx, saidx, CMP_REQID))
-			break;
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(saidx), addrhash) {
+	    if (key_cmpsaidx(&sah->saidx, saidx, CMP_MODE_REQID) != 0) {
+		    SAH_ADDREF(sah);
+		    break;
+	    }
 	}
-	SAHTREE_UNLOCK();
-
-	return sah;
+	SAHTREE_RUNLOCK();
+	return (sah);
 }
 
 /*
- * check not to be duplicated SPI.
- * NOTE: this function is too slow due to searching all SAD.
+ * Check not to be duplicated SPI.
  * OUT:
- *	NULL	: not found
- *	others	: found, pointer to a SA.
+ *	0	: not found
+ *	1	: found SA with given SPI.
  */
-static struct secasvar *
-key_checkspidup(saidx, spi)
-	struct secasindex *saidx;
-	u_int32_t spi;
+static int
+key_checkspidup(uint32_t spi)
 {
-	struct secashead *sah;
+	SAHTREE_RLOCK_TRACKER;
 	struct secasvar *sav;
 
-	/* check address family */
-	if (saidx->src.sa.sa_family != saidx->dst.sa.sa_family) {
-		ipseclog((LOG_DEBUG, "%s: address family mismatched.\n",
-			__func__));
-		return NULL;
-	}
-
-	sav = NULL;
-	/* check all SAD */
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (!key_ismyaddr((struct sockaddr *)&sah->saidx.dst))
-			continue;
-		sav = key_getsavbyspi(sah, spi);
-		if (sav != NULL)
+	/* Assume SPI is in network byte order */
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sav, SAVHASH_HASH(spi), spihash) {
+		if (sav->spi == spi)
 			break;
 	}
-	SAHTREE_UNLOCK();
-
-	return sav;
+	SAHTREE_RUNLOCK();
+	return (sav != NULL);
 }
 
 /*
- * search SAD litmited alive SA, protocol, SPI.
+ * Search SA by SPI.
  * OUT:
  *	NULL	: not found
- *	others	: found, pointer to a SA.
+ *	others	: found, referenced pointer to a SA.
  */
 static struct secasvar *
-key_getsavbyspi(sah, spi)
-	struct secashead *sah;
-	u_int32_t spi;
+key_getsavbyspi(uint32_t spi)
 {
+	SAHTREE_RLOCK_TRACKER;
 	struct secasvar *sav;
-	u_int stateidx, state;
-
-	sav = NULL;
-	SAHTREE_LOCK_ASSERT();
-	/* search all status */
-	for (stateidx = 0;
-	     stateidx < _ARRAYLEN(saorder_state_alive);
-	     stateidx++) {
-
-		state = saorder_state_alive[stateidx];
-		LIST_FOREACH(sav, &sah->savtree[state], chain) {
-
-			/* sanity check */
-			if (sav->state != state) {
-				ipseclog((LOG_DEBUG, "%s: "
-				    "invalid sav->state (queue: %d SA: %d)\n",
-				    __func__, state, sav->state));
-				continue;
-			}
 
-			if (sav->spi == spi)
-				return sav;
-		}
+	/* Assume SPI is in network byte order */
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sav, SAVHASH_HASH(spi), spihash) {
+		if (sav->spi != spi)
+			continue;
+		SAV_ADDREF(sav);
+		break;
 	}
+	SAHTREE_RUNLOCK();
+	return (sav);
+}
 
-	return NULL;
+static int
+key_updatelifetimes(struct secasvar *sav, const struct sadb_msghdr *mhp)
+{
+	struct seclifetime *lft_h, *lft_s, *tmp;
+
+	/* Lifetime extension is optional, check that it is present. */
+	if (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD) &&
+	    SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT)) {
+		/*
+		 * In case of SADB_UPDATE we may need to change
+		 * existing lifetimes.
+		 */
+		if (sav->state == SADB_SASTATE_MATURE) {
+			lft_h = lft_s = NULL;
+			goto reset;
+		}
+		return (0);
+	}
+	/* Both HARD and SOFT extensions must present */
+	if ((SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD) &&
+	    !SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT)) ||
+	    (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT) &&
+	    !SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD))) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
+		return (EINVAL);
+	}
+	if (SADB_CHECKLEN(mhp, SADB_EXT_LIFETIME_HARD) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_LIFETIME_SOFT)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
+		return (EINVAL);
+	}
+	lft_h = key_dup_lifemsg((const struct sadb_lifetime *)
+	    mhp->ext[SADB_EXT_LIFETIME_HARD], M_IPSEC_MISC);
+	if (lft_h == NULL) {
+		PFKEYSTAT_INC(in_nomem);
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
+		return (ENOBUFS);
+	}
+	lft_s = key_dup_lifemsg((const struct sadb_lifetime *)
+	    mhp->ext[SADB_EXT_LIFETIME_SOFT], M_IPSEC_MISC);
+	if (lft_s == NULL) {
+		PFKEYSTAT_INC(in_nomem);
+		free(lft_h, M_IPSEC_MISC);
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
+		return (ENOBUFS);
+	}
+reset:
+	if (sav->state != SADB_SASTATE_LARVAL) {
+		/*
+		 * key_update() holds reference to this SA,
+		 * so it won't be deleted in meanwhile.
+		 */
+		SECASVAR_LOCK(sav);
+		tmp = sav->lft_h;
+		sav->lft_h = lft_h;
+		lft_h = tmp;
+
+		tmp = sav->lft_s;
+		sav->lft_s = lft_s;
+		lft_s = tmp;
+		SECASVAR_UNLOCK(sav);
+		if (lft_h != NULL)
+			free(lft_h, M_IPSEC_MISC);
+		if (lft_s != NULL)
+			free(lft_s, M_IPSEC_MISC);
+		return (0);
+	}
+	/* We can update lifetime without holding a lock */
+	IPSEC_ASSERT(sav->lft_h == NULL, ("lft_h is already initialized\n"));
+	IPSEC_ASSERT(sav->lft_s == NULL, ("lft_s is already initialized\n"));
+	sav->lft_h = lft_h;
+	sav->lft_s = lft_s;
+	return (0);
 }
 
 /*
- * copy SA values from PF_KEY message except *SPI, SEQ, PID, STATE and TYPE*.
- * You must update these if need.
+ * copy SA values from PF_KEY message except *SPI, SEQ, PID and TYPE*.
+ * You must update these if need. Expects only LARVAL SAs.
  * OUT:	0:	success.
  *	!0:	failure.
- *
- * does not modify mbuf.  does not free mbuf on error.
  */
 static int
-key_setsaval(sav, m, mhp)
-	struct secasvar *sav;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_setsaval(struct secasvar *sav, const struct sadb_msghdr *mhp)
 {
-	int error = 0;
+	const struct sadb_sa *sa0;
+	const struct sadb_key *key0;
+	uint32_t replay;
+	size_t len;
+	int error;
 
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
+	IPSEC_ASSERT(sav->state == SADB_SASTATE_LARVAL,
+	    ("Attempt to update non LARVAL SA"));
 
-	/* initialization */
-	sav->replay = NULL;
-	sav->key_auth = NULL;
-	sav->key_enc = NULL;
-	sav->sched = NULL;
-	sav->schedlen = 0;
-	sav->iv = NULL;
-	sav->lft_c = NULL;
-	sav->lft_h = NULL;
-	sav->lft_s = NULL;
-	sav->tdb_xform = NULL;		/* transform */
-	sav->tdb_encalgxform = NULL;	/* encoding algorithm */
-	sav->tdb_authalgxform = NULL;	/* authentication algorithm */
-	sav->tdb_compalgxform = NULL;	/* compression algorithm */
-	/*  Initialize even if NAT-T not compiled in: */
-	sav->natt_type = 0;
-	sav->natt_esp_frag_len = 0;
+	/* XXX rewrite */
+	error = key_setident(sav->sah, mhp);
+	if (error != 0)
+		goto fail;
 
 	/* SA */
-	if (mhp->ext[SADB_EXT_SA] != NULL) {
-		const struct sadb_sa *sa0;
-
-		sa0 = (const struct sadb_sa *)mhp->ext[SADB_EXT_SA];
-		if (mhp->extlen[SADB_EXT_SA] < sizeof(*sa0)) {
+	if (!SADB_CHECKHDR(mhp, SADB_EXT_SA)) {
+		if (SADB_CHECKLEN(mhp, SADB_EXT_SA)) {
 			error = EINVAL;
 			goto fail;
 		}
-
+		sa0 = (const struct sadb_sa *)mhp->ext[SADB_EXT_SA];
 		sav->alg_auth = sa0->sadb_sa_auth;
 		sav->alg_enc = sa0->sadb_sa_encrypt;
 		sav->flags = sa0->sadb_sa_flags;
+		if ((sav->flags & SADB_KEY_FLAGS_MAX) != sav->flags) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid sa_flags 0x%08x.\n", __func__,
+			    sav->flags));
+			error = EINVAL;
+			goto fail;
+		}
+
+		/* Optional replay window */
+		replay = 0;
+		if ((sa0->sadb_sa_flags & SADB_X_EXT_OLD) == 0)
+			replay = sa0->sadb_sa_replay;
+		if (!SADB_CHECKHDR(mhp, SADB_X_EXT_SA_REPLAY)) {
+			if (SADB_CHECKLEN(mhp, SADB_X_EXT_SA_REPLAY)) {
+				error = EINVAL;
+				goto fail;
+			}
+			replay = ((const struct sadb_x_sa_replay *)
+			    mhp->ext[SADB_X_EXT_SA_REPLAY])->sadb_x_sa_replay_replay;
+
+			if (replay > UINT32_MAX - 32) {
+				ipseclog((LOG_DEBUG,
+				    "%s: replay window too big.\n", __func__));
+				error = EINVAL;
+				goto fail;
+			}
+
+			replay = (replay + 7) >> 3;
+		}
+
+		sav->replay = malloc(sizeof(struct secreplay), M_IPSEC_MISC,
+		    M_NOWAIT | M_ZERO);
+		if (sav->replay == NULL) {
+			PFKEYSTAT_INC(in_nomem);
+			ipseclog((LOG_DEBUG, "%s: No more memory.\n",
+			    __func__));
+			error = ENOBUFS;
+			goto fail;
+		}
+
+		if (replay != 0) {
+			/* number of 32b blocks to be allocated */
+			uint32_t bitmap_size;
 
-		/* replay window */
-		if ((sa0->sadb_sa_flags & SADB_X_EXT_OLD) == 0) {
-			sav->replay = (struct secreplay *)
-				malloc(sizeof(struct secreplay)+sa0->sadb_sa_replay, M_IPSEC_MISC, M_NOWAIT|M_ZERO);
-			if (sav->replay == NULL) {
+			/* RFC 6479:
+			 * - the allocated replay window size must be
+			 *   a power of two.
+			 * - use an extra 32b block as a redundant window.
+			 */
+			bitmap_size = 1;
+			while (replay + 4 > bitmap_size)
+				bitmap_size <<= 1;
+			bitmap_size = bitmap_size / 4;
+
+			sav->replay->bitmap = malloc(
+			    bitmap_size * sizeof(uint32_t), M_IPSEC_MISC,
+			    M_NOWAIT | M_ZERO);
+			if (sav->replay->bitmap == NULL) {
+				PFKEYSTAT_INC(in_nomem);
 				ipseclog((LOG_DEBUG, "%s: No more memory.\n",
 					__func__));
 				error = ENOBUFS;
 				goto fail;
 			}
-			if (sa0->sadb_sa_replay != 0)
-				sav->replay->bitmap = (caddr_t)(sav->replay+1);
-			sav->replay->wsize = sa0->sadb_sa_replay;
+			sav->replay->bitmap_size = bitmap_size;
+			sav->replay->wsize = replay;
 		}
 	}
 
 	/* Authentication keys */
-	if (mhp->ext[SADB_EXT_KEY_AUTH] != NULL) {
-		const struct sadb_key *key0;
-		int len;
-
-		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_AUTH];
-		len = mhp->extlen[SADB_EXT_KEY_AUTH];
-
-		error = 0;
-		if (len < sizeof(*key0)) {
+	if (!SADB_CHECKHDR(mhp, SADB_EXT_KEY_AUTH)) {
+		if (SADB_CHECKLEN(mhp, SADB_EXT_KEY_AUTH)) {
 			error = EINVAL;
 			goto fail;
 		}
+		error = 0;
+		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_AUTH];
+		len = mhp->extlen[SADB_EXT_KEY_AUTH];
 		switch (mhp->msg->sadb_msg_satype) {
 		case SADB_SATYPE_AH:
 		case SADB_SATYPE_ESP:
@@ -3146,29 +3265,25 @@ key_setsaval(sav, m, mhp)
 			goto fail;
 		}
 
-		sav->key_auth = (struct seckey *)key_dup_keymsg(key0, len,
-								M_IPSEC_MISC);
+		sav->key_auth = key_dup_keymsg(key0, len, M_IPSEC_MISC);
 		if (sav->key_auth == NULL ) {
 			ipseclog((LOG_DEBUG, "%s: No more memory.\n",
 				  __func__));
+			PFKEYSTAT_INC(in_nomem);
 			error = ENOBUFS;
 			goto fail;
 		}
 	}
 
 	/* Encryption key */
-	if (mhp->ext[SADB_EXT_KEY_ENCRYPT] != NULL) {
-		const struct sadb_key *key0;
-		int len;
-
-		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_ENCRYPT];
-		len = mhp->extlen[SADB_EXT_KEY_ENCRYPT];
-
-		error = 0;
-		if (len < sizeof(*key0)) {
+	if (!SADB_CHECKHDR(mhp, SADB_EXT_KEY_ENCRYPT)) {
+		if (SADB_CHECKLEN(mhp, SADB_EXT_KEY_ENCRYPT)) {
 			error = EINVAL;
 			goto fail;
 		}
+		error = 0;
+		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_ENCRYPT];
+		len = mhp->extlen[SADB_EXT_KEY_ENCRYPT];
 		switch (mhp->msg->sadb_msg_satype) {
 		case SADB_SATYPE_ESP:
 			if (len == PFKEY_ALIGN8(sizeof(struct sadb_key)) &&
@@ -3176,12 +3291,11 @@ key_setsaval(sav, m, mhp)
 				error = EINVAL;
 				break;
 			}
-			sav->key_enc = (struct seckey *)key_dup_keymsg(key0,
-								       len,
-								       M_IPSEC_MISC);
+			sav->key_enc = key_dup_keymsg(key0, len, M_IPSEC_MISC);
 			if (sav->key_enc == NULL) {
 				ipseclog((LOG_DEBUG, "%s: No more memory.\n",
 					__func__));
+				PFKEYSTAT_INC(in_nomem);
 				error = ENOBUFS;
 				goto fail;
 			}
@@ -3206,172 +3320,83 @@ key_setsaval(sav, m, mhp)
 
 	/* set iv */
 	sav->ivlen = 0;
-
 	switch (mhp->msg->sadb_msg_satype) {
 	case SADB_SATYPE_AH:
-		error = xform_init(sav, XF_AH);
-		break;
-	case SADB_SATYPE_ESP:
-		error = xform_init(sav, XF_ESP);
-		break;
-	case SADB_X_SATYPE_IPCOMP:
-		error = xform_init(sav, XF_IPCOMP);
-		break;
-	case SADB_X_SATYPE_TCPSIGNATURE:
-		error = xform_init(sav, XF_TCPSIGNATURE);
-		break;
-	}
-	if (error) {
-		ipseclog((LOG_DEBUG, "%s: unable to initialize SA type %u.\n",
-		        __func__, mhp->msg->sadb_msg_satype));
-		goto fail;
-	}
-
-	/* reset created */
-	sav->created = time_second;
-
-	/* make lifetime for CURRENT */
-	sav->lft_c = malloc(sizeof(struct seclifetime), M_IPSEC_MISC, M_NOWAIT);
-	if (sav->lft_c == NULL) {
-		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-		error = ENOBUFS;
-		goto fail;
-	}
-
-	sav->lft_c->allocations = 0;
-	sav->lft_c->bytes = 0;
-	sav->lft_c->addtime = time_second;
-	sav->lft_c->usetime = 0;
-
-	/* lifetimes for HARD and SOFT */
-    {
-	const struct sadb_lifetime *lft0;
-
-	lft0 = (struct sadb_lifetime *)mhp->ext[SADB_EXT_LIFETIME_HARD];
-	if (lft0 != NULL) {
-		if (mhp->extlen[SADB_EXT_LIFETIME_HARD] < sizeof(*lft0)) {
+		if (sav->flags & SADB_X_EXT_DERIV) {
+			ipseclog((LOG_DEBUG, "%s: invalid flag (derived) "
+			    "given to AH SA.\n", __func__));
 			error = EINVAL;
 			goto fail;
 		}
-		sav->lft_h = key_dup_lifemsg(lft0, M_IPSEC_MISC);
-		if (sav->lft_h == NULL) {
-			ipseclog((LOG_DEBUG, "%s: No more memory.\n",__func__));
-			error = ENOBUFS;
-			goto fail;
-		}
-		/* to be initialize ? */
-	}
-
-	lft0 = (struct sadb_lifetime *)mhp->ext[SADB_EXT_LIFETIME_SOFT];
-	if (lft0 != NULL) {
-		if (mhp->extlen[SADB_EXT_LIFETIME_SOFT] < sizeof(*lft0)) {
+		if (sav->alg_enc != SADB_EALG_NONE) {
+			ipseclog((LOG_DEBUG, "%s: protocol and algorithm "
+			    "mismated.\n", __func__));
 			error = EINVAL;
 			goto fail;
 		}
-		sav->lft_s = key_dup_lifemsg(lft0, M_IPSEC_MISC);
-		if (sav->lft_s == NULL) {
-			ipseclog((LOG_DEBUG, "%s: No more memory.\n",__func__));
-			error = ENOBUFS;
-			goto fail;
-		}
-		/* to be initialize ? */
-	}
-    }
-
-	return 0;
-
- fail:
-	/* initialization */
-	key_cleansav(sav);
-
-	return error;
-}
-
-/*
- * validation with a secasvar entry, and set SADB_SATYPE_MATURE.
- * OUT:	0:	valid
- *	other:	errno
- */
-static int
-key_mature(struct secasvar *sav)
-{
-	int error;
-
-	/* check SPI value */
-	switch (sav->sah->saidx.proto) {
-	case IPPROTO_ESP:
-	case IPPROTO_AH:
-		/*
-		 * RFC 4302, 2.4. Security Parameters Index (SPI), SPI values
-		 * 1-255 reserved by IANA for future use,
-		 * 0 for implementation specific, local use.
-		 */
-		if (ntohl(sav->spi) <= 255) {
-			ipseclog((LOG_DEBUG, "%s: illegal range of SPI %u.\n",
-			    __func__, (u_int32_t)ntohl(sav->spi)));
-			return EINVAL;
-		}
+		error = xform_init(sav, XF_AH);
 		break;
-	}
-
-	/* check satype */
-	switch (sav->sah->saidx.proto) {
-	case IPPROTO_ESP:
-		/* check flags */
-		if ((sav->flags & (SADB_X_EXT_OLD|SADB_X_EXT_DERIV)) ==
-		    (SADB_X_EXT_OLD|SADB_X_EXT_DERIV)) {
+	case SADB_SATYPE_ESP:
+		if ((sav->flags & (SADB_X_EXT_OLD | SADB_X_EXT_DERIV)) ==
+		    (SADB_X_EXT_OLD | SADB_X_EXT_DERIV)) {
 			ipseclog((LOG_DEBUG, "%s: invalid flag (derived) "
-				"given to old-esp.\n", __func__));
-			return EINVAL;
+			    "given to old-esp.\n", __func__));
+			error = EINVAL;
+			goto fail;
 		}
 		error = xform_init(sav, XF_ESP);
 		break;
-	case IPPROTO_AH:
-		/* check flags */
-		if (sav->flags & SADB_X_EXT_DERIV) {
-			ipseclog((LOG_DEBUG, "%s: invalid flag (derived) "
-				"given to AH SA.\n", __func__));
-			return EINVAL;
-		}
-		if (sav->alg_enc != SADB_EALG_NONE) {
-			ipseclog((LOG_DEBUG, "%s: protocol and algorithm "
-				"mismated.\n", __func__));
-			return(EINVAL);
-		}
-		error = xform_init(sav, XF_AH);
-		break;
-	case IPPROTO_IPCOMP:
+	case SADB_X_SATYPE_IPCOMP:
 		if (sav->alg_auth != SADB_AALG_NONE) {
 			ipseclog((LOG_DEBUG, "%s: protocol and algorithm "
-				"mismated.\n", __func__));
-			return(EINVAL);
+			    "mismated.\n", __func__));
+			error = EINVAL;
+			goto fail;
 		}
-		if ((sav->flags & SADB_X_EXT_RAWCPI) == 0
-		 && ntohl(sav->spi) >= 0x10000) {
+		if ((sav->flags & SADB_X_EXT_RAWCPI) == 0 &&
+		    ntohl(sav->spi) >= 0x10000) {
 			ipseclog((LOG_DEBUG, "%s: invalid cpi for IPComp.\n",
-				__func__));
-			return(EINVAL);
+			    __func__));
+			error = EINVAL;
+			goto fail;
 		}
 		error = xform_init(sav, XF_IPCOMP);
 		break;
-	case IPPROTO_TCP:
+	case SADB_X_SATYPE_TCPSIGNATURE:
 		if (sav->alg_enc != SADB_EALG_NONE) {
 			ipseclog((LOG_DEBUG, "%s: protocol and algorithm "
-				"mismated.\n", __func__));
-			return(EINVAL);
+			    "mismated.\n", __func__));
+			error = EINVAL;
+			goto fail;
 		}
 		error = xform_init(sav, XF_TCPSIGNATURE);
 		break;
 	default:
 		ipseclog((LOG_DEBUG, "%s: Invalid satype.\n", __func__));
 		error = EPROTONOSUPPORT;
-		break;
+		goto fail;
 	}
-	if (error == 0) {
-		SAHTREE_LOCK();
-		key_sa_chgstate(sav, SADB_SASTATE_MATURE);
-		SAHTREE_UNLOCK();
+	if (error) {
+		ipseclog((LOG_DEBUG, "%s: unable to initialize SA type %u.\n",
+		    __func__, mhp->msg->sadb_msg_satype));
+		goto fail;
 	}
+
+	/* Handle NAT-T headers */
+	error = key_setnatt(sav, mhp);
+	if (error != 0)
+		goto fail;
+
+	/* Initialize lifetime for CURRENT */
+	sav->firstused = 0;
+	sav->created = time_second;
+
+	/* lifetimes for HARD and SOFT */
+	error = key_updatelifetimes(sav, mhp);
+	if (error == 0)
+		return (0);
+fail:
+	key_cleansav(sav);
 	return (error);
 }
 
@@ -3379,32 +3404,32 @@ key_mature(struct secasvar *sav)
  * subroutine for SADB_GET and SADB_DUMP.
  */
 static struct mbuf *
-key_setdumpsa(struct secasvar *sav, u_int8_t type, u_int8_t satype,
-    u_int32_t seq, u_int32_t pid)
+key_setdumpsa(struct secasvar *sav, uint8_t type, uint8_t satype,
+    uint32_t seq, uint32_t pid)
 {
+	struct seclifetime lft_c;
 	struct mbuf *result = NULL, *tres = NULL, *m;
-	int i;
-	int dumporder[] = {
-		SADB_EXT_SA, SADB_X_EXT_SA2,
+	int i, dumporder[] = {
+		SADB_EXT_SA, SADB_X_EXT_SA2, SADB_X_EXT_SA_REPLAY,
 		SADB_EXT_LIFETIME_HARD, SADB_EXT_LIFETIME_SOFT,
 		SADB_EXT_LIFETIME_CURRENT, SADB_EXT_ADDRESS_SRC,
-		SADB_EXT_ADDRESS_DST, SADB_EXT_ADDRESS_PROXY, SADB_EXT_KEY_AUTH,
-		SADB_EXT_KEY_ENCRYPT, SADB_EXT_IDENTITY_SRC,
-		SADB_EXT_IDENTITY_DST, SADB_EXT_SENSITIVITY,
-#ifdef IPSEC_NAT_T
+		SADB_EXT_ADDRESS_DST, SADB_EXT_ADDRESS_PROXY,
+		SADB_EXT_KEY_AUTH, SADB_EXT_KEY_ENCRYPT,
+		SADB_EXT_IDENTITY_SRC, SADB_EXT_IDENTITY_DST,
+		SADB_EXT_SENSITIVITY,
 		SADB_X_EXT_NAT_T_TYPE,
 		SADB_X_EXT_NAT_T_SPORT, SADB_X_EXT_NAT_T_DPORT,
 		SADB_X_EXT_NAT_T_OAI, SADB_X_EXT_NAT_T_OAR,
 		SADB_X_EXT_NAT_T_FRAG,
-#endif
 	};
+	uint32_t replay_count;
 
 	m = key_setsadbmsg(type, 0, satype, seq, pid, sav->refcnt);
 	if (m == NULL)
 		goto fail;
 	result = m;
 
-	for (i = sizeof(dumporder)/sizeof(dumporder[0]) - 1; i >= 0; i--) {
+	for (i = nitems(dumporder) - 1; i >= 0; i--) {
 		m = NULL;
 		switch (dumporder[i]) {
 		case SADB_EXT_SA:
@@ -3414,13 +3439,25 @@ key_setdumpsa(struct secasvar *sav, u_int8_t type, u_int8_t satype,
 			break;
 
 		case SADB_X_EXT_SA2:
-			m = key_setsadbxsa2(sav->sah->saidx.mode,
-					sav->replay ? sav->replay->count : 0,
+			SECASVAR_LOCK(sav);
+			replay_count = sav->replay ? sav->replay->count : 0;
+			SECASVAR_UNLOCK(sav);
+			m = key_setsadbxsa2(sav->sah->saidx.mode, replay_count,
 					sav->sah->saidx.reqid);
 			if (!m)
 				goto fail;
 			break;
 
+		case SADB_X_EXT_SA_REPLAY:
+			if (sav->replay == NULL ||
+			    sav->replay->wsize <= UINT8_MAX)
+				continue;
+
+			m = key_setsadbxsareplay(sav->replay->wsize);
+			if (!m)
+				goto fail;
+			break;
+
 		case SADB_EXT_ADDRESS_SRC:
 			m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
 			    &sav->sah->saidx.src.sa,
@@ -3454,10 +3491,12 @@ key_setdumpsa(struct secasvar *sav, u_int8_t type, u_int8_t satype,
 			break;
 
 		case SADB_EXT_LIFETIME_CURRENT:
-			if (!sav->lft_c)
-				continue;
-			m = key_setlifetime(sav->lft_c, 
-					    SADB_EXT_LIFETIME_CURRENT);
+			lft_c.addtime = sav->created;
+			lft_c.allocations = (uint32_t)counter_u64_fetch(
+			    sav->lft_c_allocations);
+			lft_c.bytes = counter_u64_fetch(sav->lft_c_bytes);
+			lft_c.usetime = sav->firstused;
+			m = key_setlifetime(&lft_c, SADB_EXT_LIFETIME_CURRENT);
 			if (!m)
 				goto fail;
 			break;
@@ -3481,35 +3520,53 @@ key_setdumpsa(struct secasvar *sav, u_int8_t type, u_int8_t satype,
 				goto fail;
 			break;
 
-#ifdef IPSEC_NAT_T
 		case SADB_X_EXT_NAT_T_TYPE:
-			m = key_setsadbxtype(sav->natt_type);
+			if (sav->natt == NULL)
+				continue;
+			m = key_setsadbxtype(UDP_ENCAP_ESPINUDP);
 			if (!m)
 				goto fail;
 			break;
-		
+
 		case SADB_X_EXT_NAT_T_DPORT:
-			m = key_setsadbxport(
-			    KEY_PORTFROMSADDR(&sav->sah->saidx.dst),
+			if (sav->natt == NULL)
+				continue;
+			m = key_setsadbxport(sav->natt->dport,
 			    SADB_X_EXT_NAT_T_DPORT);
 			if (!m)
 				goto fail;
 			break;
 
 		case SADB_X_EXT_NAT_T_SPORT:
-			m = key_setsadbxport(
-			    KEY_PORTFROMSADDR(&sav->sah->saidx.src),
+			if (sav->natt == NULL)
+				continue;
+			m = key_setsadbxport(sav->natt->sport,
 			    SADB_X_EXT_NAT_T_SPORT);
 			if (!m)
 				goto fail;
 			break;
 
 		case SADB_X_EXT_NAT_T_OAI:
+			if (sav->natt == NULL ||
+			    (sav->natt->flags & IPSEC_NATT_F_OAI) == 0)
+				continue;
+			m = key_setsadbaddr(SADB_X_EXT_NAT_T_OAI,
+			    &sav->natt->oai.sa, FULLMASK, IPSEC_ULPROTO_ANY);
+			if (!m)
+				goto fail;
+			break;
 		case SADB_X_EXT_NAT_T_OAR:
+			if (sav->natt == NULL ||
+			    (sav->natt->flags & IPSEC_NATT_F_OAR) == 0)
+				continue;
+			m = key_setsadbaddr(SADB_X_EXT_NAT_T_OAR,
+			    &sav->natt->oar.sa, FULLMASK, IPSEC_ULPROTO_ANY);
+			if (!m)
+				goto fail;
+			break;
 		case SADB_X_EXT_NAT_T_FRAG:
 			/* We do not (yet) support those. */
 			continue;
-#endif
 
 		case SADB_EXT_ADDRESS_PROXY:
 		case SADB_EXT_IDENTITY_SRC:
@@ -3525,10 +3582,10 @@ key_setdumpsa(struct secasvar *sav, u_int8_t type, u_int8_t satype,
 		if (tres)
 			m_cat(m, tres);
 		tres = m;
-		  
 	}
 
 	m_cat(result, tres);
+	tres = NULL;
 	if (result->m_len < sizeof(struct sadb_msg)) {
 		result = m_pullup(result, sizeof(struct sadb_msg));
 		if (result == NULL)
@@ -3564,10 +3621,9 @@ key_setsadbmsg(u_int8_t type, u_int16_t tlen, u_int8_t satype, u_int32_t seq,
 	len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
 	if (len > MCLBYTES)
 		return NULL;
-	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	MGETHDR(m, M_NOWAIT, MT_DATA);
 	if (m && len > MHLEN) {
-		MCLGET(m, M_DONTWAIT);
-		if ((m->m_flags & M_EXT) == 0) {
+		if (!(MCLGET(m, M_NOWAIT))) {
 			m_freem(m);
 			m = NULL;
 		}
@@ -3596,41 +3652,39 @@ key_setsadbmsg(u_int8_t type, u_int16_t tlen, u_int8_t satype, u_int32_t seq,
  * copy secasvar data into sadb_address.
  */
 static struct mbuf *
-key_setsadbsa(sav)
-	struct secasvar *sav;
+key_setsadbsa(struct secasvar *sav)
 {
 	struct mbuf *m;
 	struct sadb_sa *p;
 	int len;
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_sa));
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
-		return NULL;
-	}
-
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_sa *);
-
 	bzero(p, len);
 	p->sadb_sa_len = PFKEY_UNIT64(len);
 	p->sadb_sa_exttype = SADB_EXT_SA;
 	p->sadb_sa_spi = sav->spi;
-	p->sadb_sa_replay = (sav->replay != NULL ? sav->replay->wsize : 0);
+	p->sadb_sa_replay = sav->replay ?
+	    (sav->replay->wsize > UINT8_MAX ? UINT8_MAX :
+		sav->replay->wsize): 0;
 	p->sadb_sa_state = sav->state;
 	p->sadb_sa_auth = sav->alg_auth;
 	p->sadb_sa_encrypt = sav->alg_enc;
-	p->sadb_sa_flags = sav->flags;
-
-	return m;
+	p->sadb_sa_flags = sav->flags & SADB_KEY_FLAGS_MAX;
+	return (m);
 }
 
 /*
  * set data into sadb_address.
  */
 static struct mbuf *
-key_setsadbaddr(u_int16_t exttype, const struct sockaddr *saddr, u_int8_t prefixlen, u_int16_t ul_proto)
+key_setsadbaddr(u_int16_t exttype, const struct sockaddr *saddr,
+    u_int8_t prefixlen, u_int16_t ul_proto)
 {
 	struct mbuf *m;
 	struct sadb_address *p;
@@ -3638,13 +3692,11 @@ key_setsadbaddr(u_int16_t exttype, const struct sockaddr *saddr, u_int8_t prefix
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_address)) +
 	    PFKEY_ALIGN8(saddr->sa_len);
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
-		return NULL;
-	}
-
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_address *);
 
 	bzero(p, len);
@@ -3684,13 +3736,11 @@ key_setsadbxsa2(u_int8_t mode, u_int32_t seq, u_int32_t reqid)
 	size_t len;
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_x_sa2));
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
-		return NULL;
-	}
-
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_x_sa2 *);
 
 	bzero(p, len);
@@ -3705,7 +3755,32 @@ key_setsadbxsa2(u_int8_t mode, u_int32_t seq, u_int32_t reqid)
 	return m;
 }
 
-#ifdef IPSEC_NAT_T
+/*
+ * Set data into sadb_x_sa_replay.
+ */
+static struct mbuf *
+key_setsadbxsareplay(u_int32_t replay)
+{
+	struct mbuf *m;
+	struct sadb_x_sa_replay *p;
+	size_t len;
+
+	len = PFKEY_ALIGN8(sizeof(struct sadb_x_sa_replay));
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, len);
+	m->m_len = len;
+	p = mtod(m, struct sadb_x_sa_replay *);
+
+	bzero(p, len);
+	p->sadb_x_sa_replay_len = PFKEY_UNIT64(len);
+	p->sadb_x_sa_replay_exttype = SADB_X_EXT_SA_REPLAY;
+	p->sadb_x_sa_replay_replay = (replay << 3);
+
+	return m;
+}
+
 /*
  * Set a type in sadb_x_nat_t_type.
  */
@@ -3718,13 +3793,11 @@ key_setsadbxtype(u_int16_t type)
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_x_nat_t_type));
 
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
 		return (NULL);
-	}
-
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_x_nat_t_type *);
 
 	bzero(p, len);
@@ -3747,13 +3820,11 @@ key_setsadbxport(u_int16_t port, u_int16_t type)
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_x_nat_t_port));
 
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
 		return (NULL);
-	}
-
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_x_nat_t_port *);
 
 	bzero(p, len);
@@ -3764,10 +3835,10 @@ key_setsadbxport(u_int16_t port, u_int16_t type)
 	return (m);
 }
 
-/* 
+/*
  * Get port from sockaddr. Port is in network byte order.
  */
-u_int16_t 
+uint16_t
 key_portfromsaddr(struct sockaddr *sa)
 {
 
@@ -3781,18 +3852,14 @@ key_portfromsaddr(struct sockaddr *sa)
 		return ((struct sockaddr_in6 *)sa)->sin6_port;
 #endif
 	}
-	KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-		printf("DP %s unexpected address family %d\n",
-			__func__, sa->sa_family));
 	return (0);
 }
-#endif /* IPSEC_NAT_T */
 
 /*
  * Set port in struct sockaddr. Port is in network byte order.
  */
-static void
-key_porttosaddr(struct sockaddr *sa, u_int16_t port)
+void
+key_porttosaddr(struct sockaddr *sa, uint16_t port)
 {
 
 	switch (sa->sa_family) {
@@ -3817,20 +3884,18 @@ key_porttosaddr(struct sockaddr *sa, u_int16_t port)
  * set data into sadb_x_policy
  */
 static struct mbuf *
-key_setsadbxpolicy(u_int16_t type, u_int8_t dir, u_int32_t id)
+key_setsadbxpolicy(u_int16_t type, u_int8_t dir, u_int32_t id, u_int32_t priority)
 {
 	struct mbuf *m;
 	struct sadb_x_policy *p;
 	size_t len;
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_x_policy));
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
-		return NULL;
-	}
-
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL)
+		return (NULL);
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_x_policy *);
 
 	bzero(p, len);
@@ -3839,6 +3904,7 @@ key_setsadbxpolicy(u_int16_t type, u_int8_t dir, u_int32_t id)
 	p->sadb_x_policy_type = type;
 	p->sadb_x_policy_dir = dir;
 	p->sadb_x_policy_id = id;
+	p->sadb_x_policy_priority = priority;
 
 	return m;
 }
@@ -3851,29 +3917,29 @@ key_setsadbxpolicy(u_int16_t type, u_int8_t dir, u_int32_t id)
  * OUT: NULL no more memory
  */
 struct seckey *
-key_dup_keymsg(const struct sadb_key *src, u_int len,
-	       struct malloc_type *type)
+key_dup_keymsg(const struct sadb_key *src, size_t len,
+    struct malloc_type *type)
 {
 	struct seckey *dst;
-	dst = (struct seckey *)malloc(sizeof(struct seckey), type, M_NOWAIT);
+
+	dst = malloc(sizeof(*dst), type, M_NOWAIT);
 	if (dst != NULL) {
 		dst->bits = src->sadb_key_bits;
-		dst->key_data = (char *)malloc(len, type, M_NOWAIT);
+		dst->key_data = malloc(len, type, M_NOWAIT);
 		if (dst->key_data != NULL) {
-			bcopy((const char *)src + sizeof(struct sadb_key), 
-			      dst->key_data, len);
+			bcopy((const char *)(src + 1), dst->key_data, len);
 		} else {
-			ipseclog((LOG_DEBUG, "%s: No more memory.\n", 
-				  __func__));
+			ipseclog((LOG_DEBUG, "%s: No more memory.\n",
+			    __func__));
 			free(dst, type);
 			dst = NULL;
 		}
 	} else {
-		ipseclog((LOG_DEBUG, "%s: No more memory.\n", 
-			  __func__));
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n",
+		    __func__));
 
 	}
-	return dst;
+	return (dst);
 }
 
 /* Take a lifetime message (sadb_lifetime) passed in on a socket and
@@ -3884,118 +3950,21 @@ key_dup_keymsg(const struct sadb_key *src, u_int len,
  */
 
 static struct seclifetime *
-key_dup_lifemsg(const struct sadb_lifetime *src,
-		 struct malloc_type *type)
+key_dup_lifemsg(const struct sadb_lifetime *src, struct malloc_type *type)
 {
-	struct seclifetime *dst = NULL;
+	struct seclifetime *dst;
 
-	dst = (struct seclifetime *)malloc(sizeof(struct seclifetime), 
-					   type, M_NOWAIT);
+	dst = malloc(sizeof(*dst), type, M_NOWAIT);
 	if (dst == NULL) {
-		/* XXX counter */
 		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-	} else {
-		dst->allocations = src->sadb_lifetime_allocations;
-		dst->bytes = src->sadb_lifetime_bytes;
-		dst->addtime = src->sadb_lifetime_addtime;
-		dst->usetime = src->sadb_lifetime_usetime;
-	}
-	return dst;
-}
-
-/* compare my own address
- * OUT:	1: true, i.e. my address.
- *	0: false
- */
-int
-key_ismyaddr(sa)
-	struct sockaddr *sa;
-{
-#ifdef INET
-	struct sockaddr_in *sin;
-	struct in_ifaddr *ia;
-#endif
-
-	IPSEC_ASSERT(sa != NULL, ("null sockaddr"));
-
-	switch (sa->sa_family) {
-#ifdef INET
-	case AF_INET:
-		sin = (struct sockaddr_in *)sa;
-		IN_IFADDR_RLOCK();
-		for (ia = V_in_ifaddrhead.tqh_first; ia;
-		     ia = ia->ia_link.tqe_next)
-		{
-			if (sin->sin_family == ia->ia_addr.sin_family &&
-			    sin->sin_len == ia->ia_addr.sin_len &&
-			    sin->sin_addr.s_addr == ia->ia_addr.sin_addr.s_addr)
-			{
-				IN_IFADDR_RUNLOCK();
-				return 1;
-			}
-		}
-		IN_IFADDR_RUNLOCK();
-		break;
-#endif
-#ifdef INET6
-	case AF_INET6:
-		return key_ismyaddr6((struct sockaddr_in6 *)sa);
-#endif
-	}
-
-	return 0;
-}
-
-#ifdef INET6
-/*
- * compare my own address for IPv6.
- * 1: ours
- * 0: other
- * NOTE: derived ip6_input() in KAME. This is necessary to modify more.
- */
-#include <netinet6/in6_var.h>
-
-static int
-key_ismyaddr6(sin6)
-	struct sockaddr_in6 *sin6;
-{
-	struct in6_ifaddr *ia;
-#if 0
-	struct in6_multi *in6m;
-#endif
-
-	IN6_IFADDR_RLOCK();
-	TAILQ_FOREACH(ia, &V_in6_ifaddrhead, ia_link) {
-		if (key_sockaddrcmp((struct sockaddr *)&sin6,
-		    (struct sockaddr *)&ia->ia_addr, 0) == 0) {
-			IN6_IFADDR_RUNLOCK();
-			return 1;
-		}
-
-#if 0
-		/*
-		 * XXX Multicast
-		 * XXX why do we care about multlicast here while we don't care
-		 * about IPv4 multicast??
-		 * XXX scope
-		 */
-		in6m = NULL;
-		IN6_LOOKUP_MULTI(sin6->sin6_addr, ia->ia_ifp, in6m);
-		if (in6m) {
-			IN6_IFADDR_RUNLOCK();
-			return 1;
-		}
-#endif
+		return (NULL);
 	}
-	IN6_IFADDR_RUNLOCK();
-
-	/* loopback, just for safety */
-	if (IN6_IS_ADDR_LOOPBACK(&sin6->sin6_addr))
-		return 1;
-
-	return 0;
+	dst->allocations = src->sadb_lifetime_allocations;
+	dst->bytes = src->sadb_lifetime_bytes;
+	dst->addtime = src->sadb_lifetime_addtime;
+	dst->usetime = src->sadb_lifetime_usetime;
+	return (dst);
 }
-#endif /*INET6*/
 
 /*
  * compare two secasindex structure.
@@ -4010,12 +3979,9 @@ key_ismyaddr6(sin6)
  *      0 : not equal
  */
 static int
-key_cmpsaidx(
-	const struct secasindex *saidx0,
-	const struct secasindex *saidx1,
-	int flag)
+key_cmpsaidx(const struct secasindex *saidx0, const struct secasindex *saidx1,
+    int flag)
 {
-	int chkport = 0;
 
 	/* sanity */
 	if (saidx0 == NULL && saidx1 == NULL)
@@ -4032,19 +3998,21 @@ key_cmpsaidx(
 			return 0;
 		if (saidx0->reqid != saidx1->reqid)
 			return 0;
-		if (bcmp(&saidx0->src, &saidx1->src, saidx0->src.sa.sa_len) != 0 ||
-		    bcmp(&saidx0->dst, &saidx1->dst, saidx0->dst.sa.sa_len) != 0)
+		if (bcmp(&saidx0->src, &saidx1->src,
+		    saidx0->src.sa.sa_len) != 0 ||
+		    bcmp(&saidx0->dst, &saidx1->dst,
+		    saidx0->dst.sa.sa_len) != 0)
 			return 0;
 	} else {
 
 		/* CMP_MODE_REQID, CMP_REQID, CMP_HEAD */
-		if (flag == CMP_MODE_REQID
-		  ||flag == CMP_REQID) {
+		if (flag == CMP_MODE_REQID || flag == CMP_REQID) {
 			/*
 			 * If reqid of SPD is non-zero, unique SA is required.
 			 * The result must be of same reqid in this case.
 			 */
-			if (saidx1->reqid != 0 && saidx0->reqid != saidx1->reqid)
+			if (saidx1->reqid != 0 &&
+			    saidx0->reqid != saidx1->reqid)
 				return 0;
 		}
 
@@ -4054,27 +4022,10 @@ key_cmpsaidx(
 				return 0;
 		}
 
-#ifdef IPSEC_NAT_T
-		/*
-		 * If NAT-T is enabled, check ports for tunnel mode.
-		 * Do not check ports if they are set to zero in the SPD.
-		 * Also do not do it for transport mode, as there is no
-		 * port information available in the SP.
-		 */
-		if (saidx1->mode == IPSEC_MODE_TUNNEL &&
-		    saidx1->src.sa.sa_family == AF_INET &&
-		    saidx1->dst.sa.sa_family == AF_INET &&
-		    ((const struct sockaddr_in *)(&saidx1->src))->sin_port &&
-		    ((const struct sockaddr_in *)(&saidx1->dst))->sin_port)
-			chkport = 1;
-#endif /* IPSEC_NAT_T */
-
-		if (key_sockaddrcmp(&saidx0->src.sa, &saidx1->src.sa, chkport) != 0) {
+		if (key_sockaddrcmp(&saidx0->src.sa, &saidx1->src.sa, 0) != 0)
 			return 0;
-		}
-		if (key_sockaddrcmp(&saidx0->dst.sa, &saidx1->dst.sa, chkport) != 0) {
+		if (key_sockaddrcmp(&saidx0->dst.sa, &saidx1->dst.sa, 0) != 0)
 			return 0;
-		}
 	}
 
 	return 1;
@@ -4090,9 +4041,8 @@ key_cmpsaidx(
  *	0 : not equal
  */
 static int
-key_cmpspidx_exactly(
-	struct secpolicyindex *spidx0,
-	struct secpolicyindex *spidx1)
+key_cmpspidx_exactly(struct secpolicyindex *spidx0,
+    struct secpolicyindex *spidx1)
 {
 	/* sanity */
 	if (spidx0 == NULL && spidx1 == NULL)
@@ -4120,9 +4070,8 @@ key_cmpspidx_exactly(
  *	0 : not equal
  */
 static int
-key_cmpspidx_withmask(
-	struct secpolicyindex *spidx0,
-	struct secpolicyindex *spidx1)
+key_cmpspidx_withmask(struct secpolicyindex *spidx0,
+    struct secpolicyindex *spidx1)
 {
 	/* sanity */
 	if (spidx0 == NULL && spidx1 == NULL)
@@ -4211,13 +4160,6 @@ key_cmpspidx_withmask(
 	return 1;
 }
 
-/* returns 0 on match */
-static int
-key_sockaddrcmp(
-	const struct sockaddr *sa1,
-	const struct sockaddr *sa2,
-	int port)
-{
 #ifdef satosin
 #undef satosin
 #endif
@@ -4226,10 +4168,16 @@ key_sockaddrcmp(
 #undef satosin6
 #endif
 #define satosin6(s) ((const struct sockaddr_in6 *)s)
+/* returns 0 on match */
+int
+key_sockaddrcmp(const struct sockaddr *sa1, const struct sockaddr *sa2,
+    int port)
+{
 	if (sa1->sa_family != sa2->sa_family || sa1->sa_len != sa2->sa_len)
 		return 1;
 
 	switch (sa1->sa_family) {
+#ifdef INET
 	case AF_INET:
 		if (sa1->sa_len != sizeof(struct sockaddr_in))
 			return 1;
@@ -4240,6 +4188,8 @@ key_sockaddrcmp(
 		if (port && satosin(sa1)->sin_port != satosin(sa2)->sin_port)
 			return 1;
 		break;
+#endif
+#ifdef INET6
 	case AF_INET6:
 		if (sa1->sa_len != sizeof(struct sockaddr_in6))
 			return 1;	/*EINVAL*/
@@ -4256,6 +4206,7 @@ key_sockaddrcmp(
 			return 1;
 		}
 		break;
+#endif
 	default:
 		if (bcmp(sa1, sa2, sa1->sa_len) != 0)
 			return 1;
@@ -4263,9 +4214,35 @@ key_sockaddrcmp(
 	}
 
 	return 0;
+}
+
+/* returns 0 on match */
+int
+key_sockaddrcmp_withmask(const struct sockaddr *sa1,
+    const struct sockaddr *sa2, size_t mask)
+{
+	if (sa1->sa_family != sa2->sa_family || sa1->sa_len != sa2->sa_len)
+		return (1);
+
+	switch (sa1->sa_family) {
+#ifdef INET
+	case AF_INET:
+		return (!key_bbcmp(&satosin(sa1)->sin_addr,
+		    &satosin(sa2)->sin_addr, mask));
+#endif
+#ifdef INET6
+	case AF_INET6:
+		if (satosin6(sa1)->sin6_scope_id !=
+		    satosin6(sa2)->sin6_scope_id)
+			return (1);
+		return (!key_bbcmp(&satosin6(sa1)->sin6_addr,
+		    &satosin6(sa2)->sin6_addr, mask));
+#endif
+	}
+	return (1);
+}
 #undef satosin
 #undef satosin6
-}
 
 /*
  * compare two buffers with mask.
@@ -4307,185 +4284,256 @@ key_bbcmp(const void *a1, const void *a2, u_int bits)
 static void
 key_flush_spd(time_t now)
 {
-	static u_int16_t sptree_scangen = 0;
-	u_int16_t gen = sptree_scangen++;
-	struct secpolicy *sp;
+	SPTREE_RLOCK_TRACKER;
+	struct secpolicy_list drainq;
+	struct secpolicy *sp, *nextsp;
 	u_int dir;
 
-	/* SPD */
+	LIST_INIT(&drainq);
+	SPTREE_RLOCK();
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-restart:
-		SPTREE_LOCK();
-		LIST_FOREACH(sp, &V_sptree[dir], chain) {
-			if (sp->scangen == gen)		/* previously handled */
-				continue;
-			sp->scangen = gen;
-			if (sp->state == IPSEC_SPSTATE_DEAD &&
-			    sp->refcnt == 1) {
-				/*
-				 * Ensure that we only decrease refcnt once,
-				 * when we're the last consumer.
-				 * Directly call SP_DELREF/key_delsp instead
-				 * of KEY_FREESP to avoid unlocking/relocking
-				 * SPTREE_LOCK before key_delsp: may refcnt
-				 * be increased again during that time ?
-				 * NB: also clean entries created by
-				 * key_spdflush
-				 */
-				SP_DELREF(sp);
-				key_delsp(sp);
-				SPTREE_UNLOCK();
-				goto restart;
-			}
+		TAILQ_FOREACH(sp, &V_sptree[dir], chain) {
 			if (sp->lifetime == 0 && sp->validtime == 0)
 				continue;
-			if ((sp->lifetime && now - sp->created > sp->lifetime)
-			 || (sp->validtime && now - sp->lastused > sp->validtime)) {
-				sp->state = IPSEC_SPSTATE_DEAD;
-				SPTREE_UNLOCK();
-				key_spdexpire(sp);
-				goto restart;
+			if ((sp->lifetime &&
+			    now - sp->created > sp->lifetime) ||
+			    (sp->validtime &&
+			    now - sp->lastused > sp->validtime)) {
+				/* Hold extra reference to send SPDEXPIRE */
+				SP_ADDREF(sp);
+				LIST_INSERT_HEAD(&drainq, sp, drainq);
 			}
 		}
-		SPTREE_UNLOCK();
+	}
+	SPTREE_RUNLOCK();
+	if (LIST_EMPTY(&drainq))
+		return;
+
+	SPTREE_WLOCK();
+	sp = LIST_FIRST(&drainq);
+	while (sp != NULL) {
+		nextsp = LIST_NEXT(sp, drainq);
+		/* Check that SP is still linked */
+		if (sp->state != IPSEC_SPSTATE_ALIVE) {
+			LIST_REMOVE(sp, drainq);
+			key_freesp(&sp); /* release extra reference */
+			sp = nextsp;
+			continue;
+		}
+		TAILQ_REMOVE(&V_sptree[sp->spidx.dir], sp, chain);
+		LIST_REMOVE(sp, idhash);
+		sp->state = IPSEC_SPSTATE_DEAD;
+		sp = nextsp;
+	}
+	V_sp_genid++;
+	SPTREE_WUNLOCK();
+
+	sp = LIST_FIRST(&drainq);
+	while (sp != NULL) {
+		nextsp = LIST_NEXT(sp, drainq);
+		key_spdexpire(sp);
+		key_freesp(&sp); /* release extra reference */
+		key_freesp(&sp); /* release last reference */
+		sp = nextsp;
 	}
 }
 
 static void
 key_flush_sad(time_t now)
 {
+	SAHTREE_RLOCK_TRACKER;
+	struct secashead_list emptyq;
+	struct secasvar_list drainq, hexpireq, sexpireq, freeq;
 	struct secashead *sah, *nextsah;
 	struct secasvar *sav, *nextsav;
 
-	/* SAD */
-	SAHTREE_LOCK();
-	LIST_FOREACH_SAFE(sah, &V_sahtree, chain, nextsah) {
-		/* if sah has been dead, then delete it and process next sah. */
-		if (sah->state == SADB_SASTATE_DEAD) {
-			key_delsah(sah);
+	LIST_INIT(&drainq);
+	LIST_INIT(&hexpireq);
+	LIST_INIT(&sexpireq);
+	LIST_INIT(&emptyq);
+
+	SAHTREE_RLOCK();
+	TAILQ_FOREACH(sah, &V_sahtree, chain) {
+		/* Check for empty SAH */
+		if (TAILQ_EMPTY(&sah->savtree_larval) &&
+		    TAILQ_EMPTY(&sah->savtree_alive)) {
+			SAH_ADDREF(sah);
+			LIST_INSERT_HEAD(&emptyq, sah, drainq);
 			continue;
 		}
-
-		/* if LARVAL entry doesn't become MATURE, delete it. */
-		LIST_FOREACH_SAFE(sav, &sah->savtree[SADB_SASTATE_LARVAL], chain, nextsav) {
-			/* Need to also check refcnt for a larval SA ??? */
-			if (now - sav->created > V_key_larval_lifetime)
-				KEY_FREESAV(&sav);
+		/* Add all stale LARVAL SAs into drainq */
+		TAILQ_FOREACH(sav, &sah->savtree_larval, chain) {
+			if (now - sav->created < V_key_larval_lifetime)
+				continue;
+			SAV_ADDREF(sav);
+			LIST_INSERT_HEAD(&drainq, sav, drainq);
 		}
-
-		/*
-		 * check MATURE entry to start to send expire message
-		 * whether or not.
-		 */
-		LIST_FOREACH_SAFE(sav, &sah->savtree[SADB_SASTATE_MATURE], chain, nextsav) {
-			/* we don't need to check. */
-			if (sav->lft_s == NULL)
+		TAILQ_FOREACH(sav, &sah->savtree_alive, chain) {
+			/* lifetimes aren't specified */
+			if (sav->lft_h == NULL)
 				continue;
-
-			/* sanity check */
-			if (sav->lft_c == NULL) {
-				ipseclog((LOG_DEBUG,"%s: there is no CURRENT "
-					"time, why?\n", __func__));
+			SECASVAR_LOCK(sav);
+			/*
+			 * Check again with lock held, because it may
+			 * be updated by SADB_UPDATE.
+			 */
+			if (sav->lft_h == NULL) {
+				SECASVAR_UNLOCK(sav);
 				continue;
 			}
-
-			/* check SOFT lifetime */
-			if (sav->lft_s->addtime != 0 &&
-			    now - sav->created > sav->lft_s->addtime) {
-				key_sa_chgstate(sav, SADB_SASTATE_DYING);
-				/* 
-				 * Actually, only send expire message if
-				 * SA has been used, as it was done before,
-				 * but should we always send such message,
-				 * and let IKE daemon decide if it should be
-				 * renegotiated or not ?
-				 * XXX expire message will actually NOT be
-				 * sent if SA is only used after soft
-				 * lifetime has been reached, see below
-				 * (DYING state)
-				 */
-				if (sav->lft_c->usetime != 0)
-					key_expire(sav);
-			}
-			/* check SOFT lifetime by bytes */
 			/*
-			 * XXX I don't know the way to delete this SA
-			 * when new SA is installed.  Caution when it's
-			 * installed too big lifetime by time.
+			 * RFC 2367:
+			 * HARD lifetimes MUST take precedence over SOFT
+			 * lifetimes, meaning if the HARD and SOFT lifetimes
+			 * are the same, the HARD lifetime will appear on the
+			 * EXPIRE message.
 			 */
-			else if (sav->lft_s->bytes != 0 &&
-			    sav->lft_s->bytes < sav->lft_c->bytes) {
-
-				key_sa_chgstate(sav, SADB_SASTATE_DYING);
-				/*
-				 * XXX If we keep to send expire
-				 * message in the status of
-				 * DYING. Do remove below code.
-				 */
-				key_expire(sav);
-			}
-		}
-
-		/* check DYING entry to change status to DEAD. */
-		LIST_FOREACH_SAFE(sav, &sah->savtree[SADB_SASTATE_DYING], chain, nextsav) {
-			/* we don't need to check. */
-			if (sav->lft_h == NULL)
+			/* check HARD lifetime */
+			if ((sav->lft_h->addtime != 0 &&
+			    now - sav->created > sav->lft_h->addtime) ||
+			    (sav->lft_h->usetime != 0 && sav->firstused &&
+			    now - sav->firstused > sav->lft_h->usetime) ||
+			    (sav->lft_h->bytes != 0 && counter_u64_fetch(
+			        sav->lft_c_bytes) > sav->lft_h->bytes)) {
+				SECASVAR_UNLOCK(sav);
+				SAV_ADDREF(sav);
+				LIST_INSERT_HEAD(&hexpireq, sav, drainq);
 				continue;
-
-			/* sanity check */
-			if (sav->lft_c == NULL) {
-				ipseclog((LOG_DEBUG, "%s: there is no CURRENT "
-					"time, why?\n", __func__));
+			}
+			/* check SOFT lifetime (only for MATURE SAs) */
+			if (sav->state == SADB_SASTATE_MATURE && (
+			    (sav->lft_s->addtime != 0 &&
+			    now - sav->created > sav->lft_s->addtime) ||
+			    (sav->lft_s->usetime != 0 && sav->firstused &&
+			    now - sav->firstused > sav->lft_s->usetime) ||
+			    (sav->lft_s->bytes != 0 && counter_u64_fetch(
+				sav->lft_c_bytes) > sav->lft_s->bytes))) {
+				SECASVAR_UNLOCK(sav);
+				SAV_ADDREF(sav);
+				LIST_INSERT_HEAD(&sexpireq, sav, drainq);
 				continue;
 			}
+			SECASVAR_UNLOCK(sav);
+		}
+	}
+	SAHTREE_RUNLOCK();
 
-			if (sav->lft_h->addtime != 0 &&
-			    now - sav->created > sav->lft_h->addtime) {
-				key_sa_chgstate(sav, SADB_SASTATE_DEAD);
-				KEY_FREESAV(&sav);
-			}
-#if 0	/* XXX Should we keep to send expire message until HARD lifetime ? */
-			else if (sav->lft_s != NULL
-			      && sav->lft_s->addtime != 0
-			      && now - sav->created > sav->lft_s->addtime) {
-				/*
-				 * XXX: should be checked to be
-				 * installed the valid SA.
-				 */
+	if (LIST_EMPTY(&emptyq) && LIST_EMPTY(&drainq) &&
+	    LIST_EMPTY(&hexpireq) && LIST_EMPTY(&sexpireq))
+		return;
 
-				/*
-				 * If there is no SA then sending
-				 * expire message.
-				 */
-				key_expire(sav);
-			}
-#endif
-			/* check HARD lifetime by bytes */
-			else if (sav->lft_h->bytes != 0 &&
-			    sav->lft_h->bytes < sav->lft_c->bytes) {
-				key_sa_chgstate(sav, SADB_SASTATE_DEAD);
-				KEY_FREESAV(&sav);
-			}
+	LIST_INIT(&freeq);
+	SAHTREE_WLOCK();
+	/* Unlink stale LARVAL SAs */
+	sav = LIST_FIRST(&drainq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		/* Check that SA is still LARVAL */
+		if (sav->state != SADB_SASTATE_LARVAL) {
+			LIST_REMOVE(sav, drainq);
+			LIST_INSERT_HEAD(&freeq, sav, drainq);
+			sav = nextsav;
+			continue;
 		}
-
-		/* delete entry in DEAD */
-		LIST_FOREACH_SAFE(sav, &sah->savtree[SADB_SASTATE_DEAD], chain, nextsav) {
-			/* sanity check */
-			if (sav->state != SADB_SASTATE_DEAD) {
-				ipseclog((LOG_DEBUG, "%s: invalid sav->state "
-					"(queue: %d SA: %d): kill it anyway\n",
-					__func__,
-					SADB_SASTATE_DEAD, sav->state));
-			}
-			/*
-			 * do not call key_freesav() here.
-			 * sav should already be freed, and sav->refcnt
-			 * shows other references to sav
-			 * (such as from SPD).
-			 */
+		TAILQ_REMOVE(&sav->sah->savtree_larval, sav, chain);
+		LIST_REMOVE(sav, spihash);
+		sav->state = SADB_SASTATE_DEAD;
+		sav = nextsav;
+	}
+	/* Unlink all SAs with expired HARD lifetime */
+	sav = LIST_FIRST(&hexpireq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		/* Check that SA is not unlinked */
+		if (sav->state == SADB_SASTATE_DEAD) {
+			LIST_REMOVE(sav, drainq);
+			LIST_INSERT_HEAD(&freeq, sav, drainq);
+			sav = nextsav;
+			continue;
+		}
+		TAILQ_REMOVE(&sav->sah->savtree_alive, sav, chain);
+		LIST_REMOVE(sav, spihash);
+		sav->state = SADB_SASTATE_DEAD;
+		sav = nextsav;
+	}
+	/* Mark all SAs with expired SOFT lifetime as DYING */
+	sav = LIST_FIRST(&sexpireq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		/* Check that SA is not unlinked */
+		if (sav->state == SADB_SASTATE_DEAD) {
+			LIST_REMOVE(sav, drainq);
+			LIST_INSERT_HEAD(&freeq, sav, drainq);
+			sav = nextsav;
+			continue;
+		}
+		/*
+		 * NOTE: this doesn't change SA order in the chain.
+		 */
+		sav->state = SADB_SASTATE_DYING;
+		sav = nextsav;
+	}
+	/* Unlink empty SAHs */
+	sah = LIST_FIRST(&emptyq);
+	while (sah != NULL) {
+		nextsah = LIST_NEXT(sah, drainq);
+		/* Check that SAH is still empty and not unlinked */
+		if (sah->state == SADB_SASTATE_DEAD ||
+		    !TAILQ_EMPTY(&sah->savtree_larval) ||
+		    !TAILQ_EMPTY(&sah->savtree_alive)) {
+			LIST_REMOVE(sah, drainq);
+			key_freesah(&sah); /* release extra reference */
+			sah = nextsah;
+			continue;
 		}
+		TAILQ_REMOVE(&V_sahtree, sah, chain);
+		LIST_REMOVE(sah, addrhash);
+		sah->state = SADB_SASTATE_DEAD;
+		sah = nextsah;
+	}
+	SAHTREE_WUNLOCK();
+
+	/* Send SPDEXPIRE messages */
+	sav = LIST_FIRST(&hexpireq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		key_expire(sav, 1);
+		key_freesah(&sav->sah); /* release reference from SAV */
+		key_freesav(&sav); /* release extra reference */
+		key_freesav(&sav); /* release last reference */
+		sav = nextsav;
+	}
+	sav = LIST_FIRST(&sexpireq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		key_expire(sav, 0);
+		key_freesav(&sav); /* release extra reference */
+		sav = nextsav;
+	}
+	/* Free stale LARVAL SAs */
+	sav = LIST_FIRST(&drainq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		key_freesah(&sav->sah); /* release reference from SAV */
+		key_freesav(&sav); /* release extra reference */
+		key_freesav(&sav); /* release last reference */
+		sav = nextsav;
+	}
+	/* Free SAs that were unlinked/changed by someone else */
+	sav = LIST_FIRST(&freeq);
+	while (sav != NULL) {
+		nextsav = LIST_NEXT(sav, drainq);
+		key_freesav(&sav); /* release extra reference */
+		sav = nextsav;
+	}
+	/* Free empty SAH */
+	sah = LIST_FIRST(&emptyq);
+	while (sah != NULL) {
+		nextsah = LIST_NEXT(sah, drainq);
+		key_freesah(&sah); /* release extra reference */
+		key_freesah(&sah); /* release last reference */
+		sah = nextsah;
 	}
-	SAHTREE_UNLOCK();
 }
 
 static void
@@ -4495,13 +4543,16 @@ key_flush_acq(time_t now)
 
 	/* ACQ tree */
 	ACQ_LOCK();
-	for (acq = LIST_FIRST(&V_acqtree); acq != NULL; acq = nextacq) {
+	acq = LIST_FIRST(&V_acqtree);
+	while (acq != NULL) {
 		nextacq = LIST_NEXT(acq, chain);
-		if (now - acq->created > V_key_blockacq_lifetime
-		 && __LIST_CHAINED(acq)) {
+		if (now - acq->created > V_key_blockacq_lifetime) {
 			LIST_REMOVE(acq, chain);
+			LIST_REMOVE(acq, addrhash);
+			LIST_REMOVE(acq, seqhash);
 			free(acq, M_IPSEC_SAQ);
 		}
+		acq = nextacq;
 	}
 	ACQ_UNLOCK();
 }
@@ -4530,8 +4581,8 @@ key_flush_spacq(time_t now)
  * and do to remove or to expire.
  * XXX: year 2038 problem may remain.
  */
-void
-key_timehandler(void)
+static void
+key_timehandler(void *arg)
 {
 	VNET_ITERATOR_DECL(vnet_iter);
 	time_t now = time_second;
@@ -4549,7 +4600,7 @@ key_timehandler(void)
 
 #ifndef IPSEC_DEBUG2
 	/* do exchange to tick time !! */
-	(void)timeout((void *)key_timehandler, (void *)0, hz);
+	callout_schedule(&key_timer, hz);
 #endif /* IPSEC_DEBUG2 */
 }
 
@@ -4563,9 +4614,7 @@ key_random()
 }
 
 void
-key_randomfill(p, l)
-	void *p;
-	size_t l;
+key_randomfill(void *p, size_t l)
 {
 	size_t n;
 	u_long v;
@@ -4594,8 +4643,8 @@ key_randomfill(p, l)
  * OUT:
  *	0: invalid satype.
  */
-static u_int16_t
-key_satype2proto(u_int8_t satype)
+static uint8_t
+key_satype2proto(uint8_t satype)
 {
 	switch (satype) {
 	case SADB_SATYPE_UNSPEC:
@@ -4619,8 +4668,8 @@ key_satype2proto(u_int8_t satype)
  * OUT:
  *	0: invalid protocol type.
  */
-static u_int8_t
-key_proto2satype(u_int16_t proto)
+static uint8_t
+key_proto2satype(uint8_t proto)
 {
 	switch (proto) {
 	case IPPROTO_AH:
@@ -4651,44 +4700,58 @@ key_proto2satype(u_int16_t proto)
  *	other if success, return pointer to the message to send.
  */
 static int
-key_getspi(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_getspi(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
-	struct secashead *newsah;
-	struct secasvar *newsav;
-	u_int8_t proto;
-	u_int32_t spi;
-	u_int8_t mode;
-	u_int32_t reqid;
+	struct sadb_address *src0, *dst0;
+	struct secasvar *sav;
+	uint32_t reqid, spi;
 	int error;
+	uint8_t mode, proto;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
-	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
-		return key_senderror(so, m, EINVAL);
+	if (SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST)
+#ifdef PFKEY_STRICT_CHECKS
+	    || SADB_CHECKHDR(mhp, SADB_EXT_SPIRANGE)
+#endif
+	    ) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
+		error = EINVAL;
+		goto fail;
 	}
-	if (mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
-		return key_senderror(so, m, EINVAL);
+	if (SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST)
+#ifdef PFKEY_STRICT_CHECKS
+	    || SADB_CHECKLEN(mhp, SADB_EXT_SPIRANGE)
+#endif
+	    ) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
+		error = EINVAL;
+		goto fail;
 	}
-	if (mhp->ext[SADB_X_EXT_SA2] != NULL) {
-		mode = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
-		reqid = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
-	} else {
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_SA2)) {
 		mode = IPSEC_MODE_ANY;
 		reqid = 0;
+	} else {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_SA2)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			error = EINVAL;
+			goto fail;
+		}
+		mode = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
+		reqid = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
 	}
 
 	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
@@ -4698,121 +4761,55 @@ key_getspi(so, m, mhp)
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
 			__func__));
-		return key_senderror(so, m, EINVAL);
-	}
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	switch (((struct sockaddr *)(src0 + 1))->sa_family) {
-	case AF_INET:
-		if (((struct sockaddr *)(src0 + 1))->sa_len !=
-		    sizeof(struct sockaddr_in))
-			return key_senderror(so, m, EINVAL);
-		((struct sockaddr_in *)(src0 + 1))->sin_port = 0;
-		break;
-	case AF_INET6:
-		if (((struct sockaddr *)(src0 + 1))->sa_len !=
-		    sizeof(struct sockaddr_in6))
-			return key_senderror(so, m, EINVAL);
-		((struct sockaddr_in6 *)(src0 + 1))->sin6_port = 0;
-		break;
-	default:
-		; /*???*/
+		error = EINVAL;
+		goto fail;
 	}
-	switch (((struct sockaddr *)(dst0 + 1))->sa_family) {
-	case AF_INET:
-		if (((struct sockaddr *)(dst0 + 1))->sa_len !=
-		    sizeof(struct sockaddr_in))
-			return key_senderror(so, m, EINVAL);
-		((struct sockaddr_in *)(dst0 + 1))->sin_port = 0;
-		break;
-	case AF_INET6:
-		if (((struct sockaddr *)(dst0 + 1))->sa_len !=
-		    sizeof(struct sockaddr_in6))
-			return key_senderror(so, m, EINVAL);
-		((struct sockaddr_in6 *)(dst0 + 1))->sin6_port = 0;
-		break;
-	default:
-		; /*???*/
+	error = key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1));
+	if (error != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		error = EINVAL;
+		goto fail;
 	}
-
-	/* XXX boundary check against sa_len */
 	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
 
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle NAT-T info if present.
-	 * We made sure the port numbers are zero above, so we do
-	 * not have to worry in case we do not update them.
-	 */
-	if (mhp->ext[SADB_X_EXT_NAT_T_OAI] != NULL)
-		ipseclog((LOG_DEBUG, "%s: NAT-T OAi present\n", __func__));
-	if (mhp->ext[SADB_X_EXT_NAT_T_OAR] != NULL)
-		ipseclog((LOG_DEBUG, "%s: NAT-T OAr present\n", __func__));
-
-	if (mhp->ext[SADB_X_EXT_NAT_T_TYPE] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_type *type;
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_TYPE] < sizeof(*type) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid nat-t message "
-			    "passed.\n", __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src, sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst, dport->sadb_x_nat_t_port_port);
-	}
-#endif
-
 	/* SPI allocation */
-	spi = key_do_getnewspi((struct sadb_spirange *)mhp->ext[SADB_EXT_SPIRANGE],
-	                       &saidx);
-	if (spi == 0)
-		return key_senderror(so, m, EINVAL);
-
-	/* get a SA index */
-	if ((newsah = key_getsah(&saidx)) == NULL) {
-		/* create a new SA index */
-		if ((newsah = key_newsah(&saidx)) == NULL) {
-			ipseclog((LOG_DEBUG, "%s: No more memory.\n",__func__));
-			return key_senderror(so, m, ENOBUFS);
-		}
+	spi = key_do_getnewspi(
+	    (struct sadb_spirange *)mhp->ext[SADB_EXT_SPIRANGE], &saidx);
+	if (spi == 0) {
+		/*
+		 * Requested SPI or SPI range is not available or
+		 * already used.
+		 */
+		error = EEXIST;
+		goto fail;
 	}
+	sav = key_newsav(mhp, &saidx, spi, &error);
+	if (sav == NULL)
+		goto fail;
 
-	/* get a new SA */
-	/* XXX rewrite */
-	newsav = KEY_NEWSAV(m, mhp, newsah, &error);
-	if (newsav == NULL) {
-		/* XXX don't free new SA index allocated in above. */
-		return key_senderror(so, m, error);
+	if (sav->seq != 0) {
+		/*
+		 * RFC2367:
+		 * If the SADB_GETSPI message is in response to a
+		 * kernel-generated SADB_ACQUIRE, the sadb_msg_seq
+		 * MUST be the same as the SADB_ACQUIRE message.
+		 *
+		 * XXXAE: However it doesn't definethe behaviour how to
+		 * check this and what to do if it doesn't match.
+		 * Also what we should do if it matches?
+		 *
+		 * We can compare saidx used in SADB_ACQUIRE with saidx
+		 * used in SADB_GETSPI, but this probably can break
+		 * existing software. For now just warn if it doesn't match.
+		 *
+		 * XXXAE: anyway it looks useless.
+		 */
+		key_acqdone(&saidx, sav->seq);
 	}
-
-	/* set spi */
-	newsav->spi = htonl(spi);
-
-	/* delete the entry in acqtree */
-	if (mhp->msg->sadb_msg_seq != 0) {
-		struct secacq *acq;
-		if ((acq = key_getacqbyseq(mhp->msg->sadb_msg_seq)) != NULL) {
-			/* reset counter in order to deletion by timehandler. */
-			acq->created = time_second;
-			acq->count = 0;
-		}
-    	}
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p)\n", __func__, sav));
+	KEYDBG(KEY_DATA, kdebug_secasv(sav));
 
     {
 	struct mbuf *n, *nn;
@@ -4824,16 +4821,17 @@ key_getspi(so, m, mhp)
 	len = PFKEY_ALIGN8(sizeof(struct sadb_msg)) +
 	    PFKEY_ALIGN8(sizeof(struct sadb_sa));
 
-	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	MGETHDR(n, M_NOWAIT, MT_DATA);
 	if (len > MHLEN) {
-		MCLGET(n, M_DONTWAIT);
-		if ((n->m_flags & M_EXT) == 0) {
+		if (!(MCLGET(n, M_NOWAIT))) {
 			m_freem(n);
 			n = NULL;
 		}
 	}
-	if (!n)
-		return key_senderror(so, m, ENOBUFS);
+	if (!n) {
+		error = ENOBUFS;
+		goto fail;
+	}
 
 	n->m_len = len;
 	n->m_next = NULL;
@@ -4845,7 +4843,7 @@ key_getspi(so, m, mhp)
 	m_sa = (struct sadb_sa *)(mtod(n, caddr_t) + off);
 	m_sa->sadb_sa_len = PFKEY_UNIT64(sizeof(struct sadb_sa));
 	m_sa->sadb_sa_exttype = SADB_EXT_SA;
-	m_sa->sadb_sa_spi = htonl(spi);
+	m_sa->sadb_sa_spi = spi; /* SPI is already in network byte order */
 	off += PFKEY_ALIGN8(sizeof(struct sadb_sa));
 
 	IPSEC_ASSERT(off == len,
@@ -4855,7 +4853,8 @@ key_getspi(so, m, mhp)
 	    SADB_EXT_ADDRESS_DST);
 	if (!n->m_next) {
 		m_freem(n);
-		return key_senderror(so, m, ENOBUFS);
+		error = ENOBUFS;
+		goto fail;
 	}
 
 	if (n->m_len < sizeof(struct sadb_msg)) {
@@ -4869,13 +4868,16 @@ key_getspi(so, m, mhp)
 		n->m_pkthdr.len += nn->m_len;
 
 	newmsg = mtod(n, struct sadb_msg *);
-	newmsg->sadb_msg_seq = newsav->seq;
+	newmsg->sadb_msg_seq = sav->seq;
 	newmsg->sadb_msg_errno = 0;
 	newmsg->sadb_msg_len = PFKEY_UNIT64(n->m_pkthdr.len);
 
 	m_freem(m);
 	return key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
     }
+
+fail:
+	return (key_senderror(so, m, error));
 }
 
 /*
@@ -4883,15 +4885,12 @@ key_getspi(so, m, mhp)
  * called by key_getspi().
  * OUT:
  *	0:	failure.
- *	others: success.
+ *	others: success, SPI in network byte order.
  */
-static u_int32_t
-key_do_getnewspi(spirange, saidx)
-	struct sadb_spirange *spirange;
-	struct secasindex *saidx;
+static uint32_t
+key_do_getnewspi(struct sadb_spirange *spirange, struct secasindex *saidx)
 {
-	u_int32_t newspi;
-	u_int32_t min, max;
+	uint32_t min, max, newspi, t;
 	int count = V_key_spi_trycnt;
 
 	/* set spi range to allocate */
@@ -4904,7 +4903,6 @@ key_do_getnewspi(spirange, saidx)
 	}
 	/* IPCOMP needs 2-byte SPI */
 	if (saidx->proto == IPPROTO_IPCOMP) {
-		u_int32_t t;
 		if (min >= 0x10000)
 			min = 0xffff;
 		if (max >= 0x10000)
@@ -4915,15 +4913,14 @@ key_do_getnewspi(spirange, saidx)
 	}
 
 	if (min == max) {
-		if (key_checkspidup(saidx, min) != NULL) {
+		if (!key_checkspidup(htonl(min))) {
 			ipseclog((LOG_DEBUG, "%s: SPI %u exists already.\n",
-				__func__, min));
+			    __func__, min));
 			return 0;
 		}
 
 		count--; /* taking one cost. */
 		newspi = min;
-
 	} else {
 
 		/* init SPI */
@@ -4933,23 +4930,244 @@ key_do_getnewspi(spirange, saidx)
 		while (count--) {
 			/* generate pseudo-random SPI value ranged. */
 			newspi = min + (key_random() % (max - min + 1));
-
-			if (key_checkspidup(saidx, newspi) == NULL)
+			if (!key_checkspidup(htonl(newspi)))
 				break;
 		}
 
 		if (count == 0 || newspi == 0) {
-			ipseclog((LOG_DEBUG, "%s: to allocate spi is failed.\n",
-				__func__));
+			ipseclog((LOG_DEBUG,
+			    "%s: failed to allocate SPI.\n", __func__));
 			return 0;
 		}
 	}
 
 	/* statistics */
 	keystat.getspi_count =
-		(keystat.getspi_count + V_key_spi_trycnt - count) / 2;
+	    (keystat.getspi_count + V_key_spi_trycnt - count) / 2;
+
+	return (htonl(newspi));
+}
+
+/*
+ * Find TCP-MD5 SA with corresponding secasindex.
+ * If not found, return NULL and fill SPI with usable value if needed.
+ */
+static struct secasvar *
+key_getsav_tcpmd5(struct secasindex *saidx, uint32_t *spi)
+{
+	SAHTREE_RLOCK_TRACKER;
+	struct secashead *sah;
+	struct secasvar *sav;
+
+	IPSEC_ASSERT(saidx->proto == IPPROTO_TCP, ("wrong proto"));
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(saidx), addrhash) {
+		if (sah->saidx.proto != IPPROTO_TCP)
+			continue;
+		if (!key_sockaddrcmp(&saidx->dst.sa, &sah->saidx.dst.sa, 0))
+			break;
+	}
+	if (sah != NULL) {
+		if (V_key_preferred_oldsa)
+			sav = TAILQ_LAST(&sah->savtree_alive, secasvar_queue);
+		else
+			sav = TAILQ_FIRST(&sah->savtree_alive);
+		if (sav != NULL) {
+			SAV_ADDREF(sav);
+			SAHTREE_RUNLOCK();
+			return (sav);
+		}
+	}
+	if (spi == NULL) {
+		/* No SPI required */
+		SAHTREE_RUNLOCK();
+		return (NULL);
+	}
+	/* Check that SPI is unique */
+	LIST_FOREACH(sav, SAVHASH_HASH(*spi), spihash) {
+		if (sav->spi == *spi)
+			break;
+	}
+	if (sav == NULL) {
+		SAHTREE_RUNLOCK();
+		/* SPI is already unique */
+		return (NULL);
+	}
+	SAHTREE_RUNLOCK();
+	/* XXX: not optimal */
+	*spi = key_do_getnewspi(NULL, saidx);
+	return (NULL);
+}
+
+static int
+key_updateaddresses(struct socket *so, struct mbuf *m,
+    const struct sadb_msghdr *mhp, struct secasvar *sav,
+    struct secasindex *saidx)
+{
+	struct sockaddr *newaddr;
+	struct secashead *sah;
+	struct secasvar *newsav, *tmp;
+	struct mbuf *n;
+	int error, isnew;
+
+	/* Check that we need to change SAH */
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_SRC)) {
+		newaddr = (struct sockaddr *)(
+		    ((struct sadb_address *)
+		    mhp->ext[SADB_X_EXT_NEW_ADDRESS_SRC]) + 1);
+		bcopy(newaddr, &saidx->src, newaddr->sa_len);
+		key_porttosaddr(&saidx->src.sa, 0);
+	}
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_DST)) {
+		newaddr = (struct sockaddr *)(
+		    ((struct sadb_address *)
+		    mhp->ext[SADB_X_EXT_NEW_ADDRESS_DST]) + 1);
+		bcopy(newaddr, &saidx->dst, newaddr->sa_len);
+		key_porttosaddr(&saidx->dst.sa, 0);
+	}
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_SRC) ||
+	    !SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_DST)) {
+		error = key_checksockaddrs(&saidx->src.sa, &saidx->dst.sa);
+		if (error != 0) {
+			ipseclog((LOG_DEBUG, "%s: invalid new sockaddr.\n",
+			    __func__));
+			return (error);
+		}
+
+		sah = key_getsah(saidx);
+		if (sah == NULL) {
+			/* create a new SA index */
+			sah = key_newsah(saidx);
+			if (sah == NULL) {
+				ipseclog((LOG_DEBUG,
+				    "%s: No more memory.\n", __func__));
+				return (ENOBUFS);
+			}
+			isnew = 2; /* SAH is new */
+		} else
+			isnew = 1; /* existing SAH is referenced */
+	} else {
+		/*
+		 * src and dst addresses are still the same.
+		 * Do we want to change NAT-T config?
+		 */
+		if (sav->sah->saidx.proto != IPPROTO_ESP ||
+		    SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_TYPE) ||
+		    SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_SPORT) ||
+		    SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_DPORT)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: missing required header.\n",
+			    __func__));
+			return (EINVAL);
+		}
+		/* We hold reference to SA, thus SAH will be referenced too. */
+		sah = sav->sah;
+		isnew = 0;
+	}
+
+	newsav = malloc(sizeof(struct secasvar), M_IPSEC_SA,
+	    M_NOWAIT | M_ZERO);
+	if (newsav == NULL) {
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
+		error = ENOBUFS;
+		goto fail;
+	}
+
+	/* Clone SA's content into newsav */
+	SAV_INITREF(newsav);
+	bcopy(sav, newsav, offsetof(struct secasvar, chain));
+	/*
+	 * We create new NAT-T config if it is needed.
+	 * Old NAT-T config will be freed by key_cleansav() when
+	 * last reference to SA will be released.
+	 */
+	newsav->natt = NULL;
+	newsav->sah = sah;
+	newsav->state = SADB_SASTATE_MATURE;
+	error = key_setnatt(sav, mhp);
+	if (error != 0)
+		goto fail;
+
+	SAHTREE_WLOCK();
+	/* Check that SA is still alive */
+	if (sav->state == SADB_SASTATE_DEAD) {
+		/* SA was unlinked */
+		SAHTREE_WUNLOCK();
+		error = ESRCH;
+		goto fail;
+	}
+
+	/* Unlink SA from SAH and SPI hash */
+	IPSEC_ASSERT((sav->flags & SADB_X_EXT_F_CLONED) == 0,
+	    ("SA is already cloned"));
+	IPSEC_ASSERT(sav->state == SADB_SASTATE_MATURE ||
+	    sav->state == SADB_SASTATE_DYING,
+	    ("Wrong SA state %u\n", sav->state));
+	TAILQ_REMOVE(&sav->sah->savtree_alive, sav, chain);
+	LIST_REMOVE(sav, spihash);
+	sav->state = SADB_SASTATE_DEAD;
+
+	/*
+	 * Link new SA with SAH. Keep SAs ordered by
+	 * create time (newer are first).
+	 */
+	TAILQ_FOREACH(tmp, &sah->savtree_alive, chain) {
+		if (newsav->created > tmp->created) {
+			TAILQ_INSERT_BEFORE(tmp, newsav, chain);
+			break;
+		}
+	}
+	if (tmp == NULL)
+		TAILQ_INSERT_TAIL(&sah->savtree_alive, newsav, chain);
+
+	/* Add new SA into SPI hash. */
+	LIST_INSERT_HEAD(SAVHASH_HASH(newsav->spi), newsav, spihash);
+
+	/* Add new SAH into SADB. */
+	if (isnew == 2) {
+		TAILQ_INSERT_HEAD(&V_sahtree, sah, chain);
+		LIST_INSERT_HEAD(SAHADDRHASH_HASH(saidx), sah, addrhash);
+		sah->state = SADB_SASTATE_MATURE;
+		SAH_ADDREF(sah); /* newsav references new SAH */
+	}
+	/*
+	 * isnew == 1 -> @sah was referenced by key_getsah().
+	 * isnew == 0 -> we use the same @sah, that was used by @sav,
+	 *	and we use its reference for @newsav.
+	 */
+	SECASVAR_LOCK(sav);
+	/* XXX: replace cntr with pointer? */
+	newsav->cntr = sav->cntr;
+	sav->flags |= SADB_X_EXT_F_CLONED;
+	SECASVAR_UNLOCK(sav);
+
+	SAHTREE_WUNLOCK();
 
-	return newspi;
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p) cloned into SA(%p)\n",
+	    __func__, sav, newsav));
+	KEYDBG(KEY_DATA, kdebug_secasv(newsav));
+
+	key_freesav(&sav); /* release last reference */
+
+	/* set msg buf from mhp */
+	n = key_getmsgbuf_x1(m, mhp);
+	if (n == NULL) {
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
+		return (ENOBUFS);
+	}
+	m_freem(m);
+	key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
+	return (0);
+fail:
+	if (isnew != 0)
+		key_freesah(&sah);
+	if (newsav != NULL) {
+		if (newsav->natt != NULL)
+			free(newsav->natt, M_IPSEC_MISC);
+		free(newsav, M_IPSEC_SA);
+	}
+	return (error);
 }
 
 /*
@@ -4966,26 +5184,15 @@ key_do_getnewspi(spirange, saidx)
  * m will always be freed.
  */
 static int
-key_update(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_update(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct sadb_sa *sa0;
-	struct sadb_address *src0, *dst0;
-#ifdef IPSEC_NAT_T
-	struct sadb_x_nat_t_type *type;
-	struct sadb_x_nat_t_port *sport, *dport;
-	struct sadb_address *iaddr, *raddr;
-	struct sadb_x_nat_t_frag *frag;
-#endif
 	struct secasindex saidx;
-	struct secashead *sah;
+	struct sadb_address *src0, *dst0;
+	struct sadb_sa *sa0;
 	struct secasvar *sav;
-	u_int16_t proto;
-	u_int8_t mode;
-	u_int32_t reqid;
+	uint32_t reqid;
 	int error;
+	uint8_t mode, proto;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -4995,199 +5202,182 @@ key_update(so, m, mhp)
 	/* map satype to proto */
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->ext[SADB_EXT_SA] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
-	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_ESP &&
-	     mhp->ext[SADB_EXT_KEY_ENCRYPT] == NULL) ||
-	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_AH &&
-	     mhp->ext[SADB_EXT_KEY_AUTH] == NULL) ||
-	    (mhp->ext[SADB_EXT_LIFETIME_HARD] != NULL &&
-	     mhp->ext[SADB_EXT_LIFETIME_SOFT] == NULL) ||
-	    (mhp->ext[SADB_EXT_LIFETIME_HARD] == NULL &&
-	     mhp->ext[SADB_EXT_LIFETIME_SOFT] != NULL)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD) &&
+		!SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT)) ||
+	    (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT) &&
+		!SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD))) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_SA] < sizeof(struct sadb_sa) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->ext[SADB_X_EXT_SA2] != NULL) {
-		mode = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
-		reqid = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
-	} else {
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_SA2)) {
 		mode = IPSEC_MODE_ANY;
 		reqid = 0;
+	} else {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_SA2)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			return key_senderror(so, m, EINVAL);
+		}
+		mode = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
+		reqid = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
 	}
-	/* XXX boundary checking for other extensions */
 
 	sa0 = (struct sadb_sa *)mhp->ext[SADB_EXT_SA];
 	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
 	dst0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_DST]);
 
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifdef IPSEC_NAT_T
 	/*
-	 * Handle NAT-T info if present.
+	 * Only SADB_SASTATE_MATURE SAs may be submitted in an
+	 * SADB_UPDATE message.
 	 */
-	if (mhp->ext[SADB_X_EXT_NAT_T_TYPE] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_TYPE] < sizeof(*type) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		type = (struct sadb_x_nat_t_type *)
-		    mhp->ext[SADB_X_EXT_NAT_T_TYPE];
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-	} else {
-		type = 0;
-		sport = dport = 0;
-	}
-	if (mhp->ext[SADB_X_EXT_NAT_T_OAI] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_OAR] != NULL) {
-		if (mhp->extlen[SADB_X_EXT_NAT_T_OAI] < sizeof(*iaddr) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_OAR] < sizeof(*raddr)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-		iaddr = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAI];
-		raddr = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAR];
-		ipseclog((LOG_DEBUG, "%s: NAT-T OAi/r present\n", __func__));
-	} else {
-		iaddr = raddr = NULL;
-	}
-	if (mhp->ext[SADB_X_EXT_NAT_T_FRAG] != NULL) {
-		if (mhp->extlen[SADB_X_EXT_NAT_T_FRAG] < sizeof(*frag)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-		frag = (struct sadb_x_nat_t_frag *)
-		    mhp->ext[SADB_X_EXT_NAT_T_FRAG];
-	} else {
-		frag = 0;
-	}
+	if (sa0->sadb_sa_state != SADB_SASTATE_MATURE) {
+		ipseclog((LOG_DEBUG, "%s: invalid state.\n", __func__));
+#ifdef PFKEY_STRICT_CHECKS
+		return key_senderror(so, m, EINVAL);
 #endif
-
-	/* get a SA header */
-	if ((sah = key_getsah(&saidx)) == NULL) {
-		ipseclog((LOG_DEBUG, "%s: no SA index found.\n", __func__));
-		return key_senderror(so, m, ENOENT);
 	}
-
-	/* set spidx if there */
-	/* XXX rewrite */
-	error = key_setident(sah, m, mhp);
-	if (error)
+	error = key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1));
+	if (error != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
 		return key_senderror(so, m, error);
-
-	/* find a SA with sequence number. */
-#ifdef IPSEC_DOSEQCHECK
-	if (mhp->msg->sadb_msg_seq != 0
-	 && (sav = key_getsavbyseq(sah, mhp->msg->sadb_msg_seq)) == NULL) {
-		ipseclog((LOG_DEBUG, "%s: no larval SA with sequence %u "
-			"exists.\n", __func__, mhp->msg->sadb_msg_seq));
-		return key_senderror(so, m, ENOENT);
 	}
-#else
-	SAHTREE_LOCK();
-	sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
-	SAHTREE_UNLOCK();
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
+	sav = key_getsavbyspi(sa0->sadb_sa_spi);
 	if (sav == NULL) {
-		ipseclog((LOG_DEBUG, "%s: no such a SA found (spi:%u)\n",
-			__func__, (u_int32_t)ntohl(sa0->sadb_sa_spi)));
-		return key_senderror(so, m, EINVAL);
-	}
-#endif
-
-	/* validity check */
-	if (sav->sah->saidx.proto != proto) {
-		ipseclog((LOG_DEBUG, "%s: protocol mismatched "
-			"(DB=%u param=%u)\n", __func__,
-			sav->sah->saidx.proto, proto));
+		ipseclog((LOG_DEBUG, "%s: no SA found for SPI %u\n",
+		    __func__, ntohl(sa0->sadb_sa_spi)));
 		return key_senderror(so, m, EINVAL);
 	}
-#ifdef IPSEC_DOSEQCHECK
-	if (sav->spi != sa0->sadb_sa_spi) {
-		ipseclog((LOG_DEBUG, "%s: SPI mismatched (DB:%u param:%u)\n",
-		    __func__,
-		    (u_int32_t)ntohl(sav->spi),
-		    (u_int32_t)ntohl(sa0->sadb_sa_spi)));
-		return key_senderror(so, m, EINVAL);
-	}
-#endif
+	/*
+	 * Check that SADB_UPDATE issued by the same process that did
+	 * SADB_GETSPI or SADB_ADD.
+	 */
 	if (sav->pid != mhp->msg->sadb_msg_pid) {
-		ipseclog((LOG_DEBUG, "%s: pid mismatched (DB:%u param:%u)\n",
-		    __func__, sav->pid, mhp->msg->sadb_msg_pid));
+		ipseclog((LOG_DEBUG,
+		    "%s: pid mismatched (SPI %u, pid %u vs. %u)\n", __func__,
+		    ntohl(sav->spi), sav->pid, mhp->msg->sadb_msg_pid));
+		key_freesav(&sav);
 		return key_senderror(so, m, EINVAL);
 	}
-
-	/* copy sav values */
-	error = key_setsaval(sav, m, mhp);
-	if (error) {
-		KEY_FREESAV(&sav);
-		return key_senderror(so, m, error);
-	}
-
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle more NAT-T info if present,
-	 * now that we have a sav to fill.
-	 */
-	if (type)
-		sav->natt_type = type->sadb_x_nat_t_type_type;
-
-	if (sport)
-		KEY_PORTTOSADDR(&sav->sah->saidx.src,
-		    sport->sadb_x_nat_t_port_port);
-	if (dport)
-		KEY_PORTTOSADDR(&sav->sah->saidx.dst,
-		    dport->sadb_x_nat_t_port_port);
-
-#if 0
-	/*
-	 * In case SADB_X_EXT_NAT_T_FRAG was not given, leave it at 0.
-	 * We should actually check for a minimum MTU here, if we
-	 * want to support it in ip_output.
-	 */
-	if (frag)
-		sav->natt_esp_frag_len = frag->sadb_x_nat_t_frag_fraglen;
-#endif
-#endif
-
-	/* check SA values to be mature. */
-	if ((mhp->msg->sadb_msg_errno = key_mature(sav)) != 0) {
-		KEY_FREESAV(&sav);
-		return key_senderror(so, m, 0);
+	/* saidx should match with SA. */
+	if (key_cmpsaidx(&sav->sah->saidx, &saidx, CMP_MODE_REQID) == 0) {
+		ipseclog((LOG_DEBUG, "%s: saidx mismatched for SPI %u",
+		    __func__, ntohl(sav->spi)));
+		key_freesav(&sav);
+		return key_senderror(so, m, ESRCH);
+	}
+
+	if (sav->state == SADB_SASTATE_LARVAL) {
+		if ((mhp->msg->sadb_msg_satype == SADB_SATYPE_ESP &&
+		    SADB_CHECKHDR(mhp, SADB_EXT_KEY_ENCRYPT)) ||
+		    (mhp->msg->sadb_msg_satype == SADB_SATYPE_AH &&
+		    SADB_CHECKHDR(mhp, SADB_EXT_KEY_AUTH))) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: missing required header.\n",
+			    __func__));
+			key_freesav(&sav);
+			return key_senderror(so, m, EINVAL);
+		}
+		/*
+		 * We can set any values except src, dst and SPI.
+		 */
+		error = key_setsaval(sav, mhp);
+		if (error != 0) {
+			key_freesav(&sav);
+			return (key_senderror(so, m, error));
+		}
+		/* Change SA state to MATURE */
+		SAHTREE_WLOCK();
+		if (sav->state != SADB_SASTATE_LARVAL) {
+			/* SA was deleted or another thread made it MATURE. */
+			SAHTREE_WUNLOCK();
+			key_freesav(&sav);
+			return (key_senderror(so, m, ESRCH));
+		}
+		/*
+		 * NOTE: we keep SAs in savtree_alive ordered by created
+		 * time. When SA's state changed from LARVAL to MATURE,
+		 * we update its created time in key_setsaval() and move
+		 * it into head of savtree_alive.
+		 */
+		TAILQ_REMOVE(&sav->sah->savtree_larval, sav, chain);
+		TAILQ_INSERT_HEAD(&sav->sah->savtree_alive, sav, chain);
+		sav->state = SADB_SASTATE_MATURE;
+		SAHTREE_WUNLOCK();
+	} else {
+		/*
+		 * For DYING and MATURE SA we can change only state
+		 * and lifetimes. Report EINVAL if something else attempted
+		 * to change.
+		 */
+		if (!SADB_CHECKHDR(mhp, SADB_EXT_KEY_ENCRYPT) ||
+		    !SADB_CHECKHDR(mhp, SADB_EXT_KEY_AUTH)) {
+			key_freesav(&sav);
+			return (key_senderror(so, m, EINVAL));
+		}
+		error = key_updatelifetimes(sav, mhp);
+		if (error != 0) {
+			key_freesav(&sav);
+			return (key_senderror(so, m, error));
+		}
+		/*
+		 * This is FreeBSD extension to RFC2367.
+		 * IKEd can specify SADB_X_EXT_NEW_ADDRESS_SRC and/or
+		 * SADB_X_EXT_NEW_ADDRESS_DST when it wants to change
+		 * SA addresses (for example to implement MOBIKE protocol
+		 * as described in RFC4555). Also we allow to change
+		 * NAT-T config.
+		 */
+		if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_SRC) ||
+		    !SADB_CHECKHDR(mhp, SADB_X_EXT_NEW_ADDRESS_DST) ||
+		    !SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_TYPE) ||
+		    sav->natt != NULL) {
+			error = key_updateaddresses(so, m, mhp, sav, &saidx);
+			key_freesav(&sav);
+			if (error != 0)
+				return (key_senderror(so, m, error));
+			return (0);
+		}
+		/* Check that SA is still alive */
+		SAHTREE_WLOCK();
+		if (sav->state == SADB_SASTATE_DEAD) {
+			/* SA was unlinked */
+			SAHTREE_WUNLOCK();
+			key_freesav(&sav);
+			return (key_senderror(so, m, ESRCH));
+		}
+		/*
+		 * NOTE: there is possible state moving from DYING to MATURE,
+		 * but this doesn't change created time, so we won't reorder
+		 * this SA.
+		 */
+		sav->state = SADB_SASTATE_MATURE;
+		SAHTREE_WUNLOCK();
 	}
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p)\n", __func__, sav));
+	KEYDBG(KEY_DATA, kdebug_secasv(sav));
+	key_freesav(&sav);
 
     {
 	struct mbuf *n;
@@ -5205,42 +5395,6 @@ key_update(so, m, mhp)
 }
 
 /*
- * search SAD with sequence for a SA which state is SADB_SASTATE_LARVAL.
- * only called by key_update().
- * OUT:
- *	NULL	: not found
- *	others	: found, pointer to a SA.
- */
-#ifdef IPSEC_DOSEQCHECK
-static struct secasvar *
-key_getsavbyseq(sah, seq)
-	struct secashead *sah;
-	u_int32_t seq;
-{
-	struct secasvar *sav;
-	u_int state;
-
-	state = SADB_SASTATE_LARVAL;
-
-	/* search SAD with sequence number ? */
-	LIST_FOREACH(sav, &sah->savtree[state], chain) {
-
-		KEY_CHKSASTATE(state, sav->state, __func__);
-
-		if (sav->seq == seq) {
-			sa_addref(sav);
-			KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
-				printf("DP %s cause refcnt++:%d SA:%p\n",
-					__func__, sav->refcnt, sav));
-			return sav;
-		}
-	}
-
-	return NULL;
-}
-#endif
-
-/*
  * SADB_ADD processing
  * add an entry to SA database, when received
  *   <base, SA, (SA2), (lifetime(HSC),) address(SD), (address(P),)
@@ -5256,24 +5410,14 @@ key_getsavbyseq(sah, seq)
  * m will always be freed.
  */
 static int
-key_add(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_add(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct sadb_sa *sa0;
-	struct sadb_address *src0, *dst0;
-#ifdef IPSEC_NAT_T
-	struct sadb_x_nat_t_type *type;
-	struct sadb_address *iaddr, *raddr;
-	struct sadb_x_nat_t_frag *frag;
-#endif
 	struct secasindex saidx;
-	struct secashead *newsah;
-	struct secasvar *newsav;
-	u_int16_t proto;
-	u_int8_t mode;
-	u_int32_t reqid;
+	struct sadb_address *src0, *dst0;
+	struct sadb_sa *sa0;
+	struct secasvar *sav;
+	uint32_t reqid, spi;
+	uint8_t mode, proto;
 	int error;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
@@ -5284,176 +5428,115 @@ key_add(so, m, mhp)
 	/* map satype to proto */
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->ext[SADB_EXT_SA] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
-	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_ESP &&
-	     mhp->ext[SADB_EXT_KEY_ENCRYPT] == NULL) ||
-	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_AH &&
-	     mhp->ext[SADB_EXT_KEY_AUTH] == NULL) ||
-	    (mhp->ext[SADB_EXT_LIFETIME_HARD] != NULL &&
-	     mhp->ext[SADB_EXT_LIFETIME_SOFT] == NULL) ||
-	    (mhp->ext[SADB_EXT_LIFETIME_HARD] == NULL &&
-	     mhp->ext[SADB_EXT_LIFETIME_SOFT] != NULL)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_ESP && (
+		SADB_CHECKHDR(mhp, SADB_EXT_KEY_ENCRYPT) ||
+		SADB_CHECKLEN(mhp, SADB_EXT_KEY_ENCRYPT))) ||
+	    (mhp->msg->sadb_msg_satype == SADB_SATYPE_AH && (
+		SADB_CHECKHDR(mhp, SADB_EXT_KEY_AUTH) ||
+		SADB_CHECKLEN(mhp, SADB_EXT_KEY_AUTH))) ||
+	    (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD) &&
+		!SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT)) ||
+	    (SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_SOFT) &&
+		!SADB_CHECKHDR(mhp, SADB_EXT_LIFETIME_HARD))) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_SA] < sizeof(struct sadb_sa) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address)) {
-		/* XXX need more */
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->ext[SADB_X_EXT_SA2] != NULL) {
-		mode = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
-		reqid = ((struct sadb_x_sa2 *)mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
-	} else {
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_SA2)) {
 		mode = IPSEC_MODE_ANY;
 		reqid = 0;
+	} else {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_SA2)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			return key_senderror(so, m, EINVAL);
+		}
+		mode = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
+		reqid = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
 	}
 
 	sa0 = (struct sadb_sa *)mhp->ext[SADB_EXT_SA];
 	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
 	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
 
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifdef IPSEC_NAT_T
 	/*
-	 * Handle NAT-T info if present.
+	 * Only SADB_SASTATE_MATURE SAs may be submitted in an
+	 * SADB_ADD message.
 	 */
-	if (mhp->ext[SADB_X_EXT_NAT_T_TYPE] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_TYPE] < sizeof(*type) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		type = (struct sadb_x_nat_t_type *)
-		    mhp->ext[SADB_X_EXT_NAT_T_TYPE];
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src,
-			    sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst,
-			    dport->sadb_x_nat_t_port_port);
-	} else {
-		type = 0;
+	if (sa0->sadb_sa_state != SADB_SASTATE_MATURE) {
+		ipseclog((LOG_DEBUG, "%s: invalid state.\n", __func__));
+#ifdef PFKEY_STRICT_CHECKS
+		return key_senderror(so, m, EINVAL);
+#endif
 	}
-	if (mhp->ext[SADB_X_EXT_NAT_T_OAI] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_OAR] != NULL) {
-		if (mhp->extlen[SADB_X_EXT_NAT_T_OAI] < sizeof(*iaddr) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_OAR] < sizeof(*raddr)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-		iaddr = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAI];
-		raddr = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAR];
-		ipseclog((LOG_DEBUG, "%s: NAT-T OAi/r present\n", __func__));
-	} else {
-		iaddr = raddr = NULL;
+	error = key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1));
+	if (error != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		return key_senderror(so, m, error);
 	}
-	if (mhp->ext[SADB_X_EXT_NAT_T_FRAG] != NULL) {
-		if (mhp->extlen[SADB_X_EXT_NAT_T_FRAG] < sizeof(*frag)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message\n",
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
+	spi = sa0->sadb_sa_spi;
+	/*
+	 * For TCP-MD5 SAs we don't use SPI. Check the uniqueness using
+	 * secasindex.
+	 * XXXAE: IPComp seems also doesn't use SPI.
+	 */
+	if (proto == IPPROTO_TCP) {
+		sav = key_getsav_tcpmd5(&saidx, &spi);
+		if (sav == NULL && spi == 0) {
+			/* Failed to allocate SPI */
+			ipseclog((LOG_DEBUG, "%s: SA already exists.\n",
 			    __func__));
-			return key_senderror(so, m, EINVAL);
+			return key_senderror(so, m, EEXIST);
 		}
-		frag = (struct sadb_x_nat_t_frag *)
-		    mhp->ext[SADB_X_EXT_NAT_T_FRAG];
+		/* XXX: SPI that we report back can have another value */
 	} else {
-		frag = 0;
+		/* We can create new SA only if SPI is different. */
+		sav = key_getsavbyspi(spi);
 	}
-#endif
-
-	/* get a SA header */
-	if ((newsah = key_getsah(&saidx)) == NULL) {
-		/* create a new SA header */
-		if ((newsah = key_newsah(&saidx)) == NULL) {
-			ipseclog((LOG_DEBUG, "%s: No more memory.\n",__func__));
-			return key_senderror(so, m, ENOBUFS);
-		}
-	}
-
-	/* set spidx if there */
-	/* XXX rewrite */
-	error = key_setident(newsah, m, mhp);
-	if (error) {
-		return key_senderror(so, m, error);
-	}
-
-	/* create new SA entry. */
-	/* We can create new SA only if SPI is differenct. */
-	SAHTREE_LOCK();
-	newsav = key_getsavbyspi(newsah, sa0->sadb_sa_spi);
-	SAHTREE_UNLOCK();
-	if (newsav != NULL) {
+	if (sav != NULL) {
+		key_freesav(&sav);
 		ipseclog((LOG_DEBUG, "%s: SA already exists.\n", __func__));
 		return key_senderror(so, m, EEXIST);
 	}
-	newsav = KEY_NEWSAV(m, mhp, newsah, &error);
-	if (newsav == NULL) {
-		return key_senderror(so, m, error);
-	}
 
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle more NAT-T info if present,
-	 * now that we have a sav to fill.
-	 */
-	if (type)
-		newsav->natt_type = type->sadb_x_nat_t_type_type;
-
-#if 0
-	/*
-	 * In case SADB_X_EXT_NAT_T_FRAG was not given, leave it at 0.
-	 * We should actually check for a minimum MTU here, if we
-	 * want to support it in ip_output.
-	 */
-	if (frag)
-		newsav->natt_esp_frag_len = frag->sadb_x_nat_t_frag_fraglen;
-#endif
-#endif
-
-	/* check SA values to be mature. */
-	if ((error = key_mature(newsav)) != 0) {
-		KEY_FREESAV(&newsav);
+	sav = key_newsav(mhp, &saidx, spi, &error);
+	if (sav == NULL)
 		return key_senderror(so, m, error);
-	}
-
+	KEYDBG(KEY_STAMP,
+	    printf("%s: return SA(%p)\n", __func__, sav));
+	KEYDBG(KEY_DATA, kdebug_secasv(sav));
 	/*
-	 * don't call key_freesav() here, as we would like to keep the SA
-	 * in the database on success.
+	 * If SADB_ADD was in response to SADB_ACQUIRE, we need to schedule
+	 * ACQ for deletion.
 	 */
+	if (sav->seq != 0)
+		key_acqdone(&saidx, sav->seq);
 
     {
+	/*
+	 * Don't call key_freesav() on error here, as we would like to
+	 * keep the SA in the database.
+	 */
 	struct mbuf *n;
 
 	/* set msg buf from mhp */
@@ -5468,33 +5551,199 @@ key_add(so, m, mhp)
     }
 }
 
-/* m is retained */
+/*
+ * NAT-T support.
+ * IKEd may request the use ESP in UDP encapsulation when it detects the
+ * presence of NAT. It uses NAT-T extension headers for such SAs to specify
+ * parameters needed for encapsulation and decapsulation. These PF_KEY
+ * extension headers are not standardized, so this comment addresses our
+ * implementation.
+ * SADB_X_EXT_NAT_T_TYPE specifies type of encapsulation, we support only
+ * UDP_ENCAP_ESPINUDP as described in RFC3948.
+ * SADB_X_EXT_NAT_T_SPORT/DPORT specifies source and destination ports for
+ * UDP header. We use these ports in UDP encapsulation procedure, also we
+ * can check them in UDP decapsulation procedure.
+ * SADB_X_EXT_NAT_T_OA[IR] specifies original address of initiator or
+ * responder. These addresses can be used for transport mode to adjust
+ * checksum after decapsulation and decryption. Since original IP addresses
+ * used by peer usually different (we detected presence of NAT), TCP/UDP
+ * pseudo header checksum and IP header checksum was calculated using original
+ * addresses. After decapsulation and decryption we need to adjust checksum
+ * to have correct datagram.
+ *
+ * We expect presence of NAT-T extension headers only in SADB_ADD and
+ * SADB_UPDATE messages. We report NAT-T extension headers in replies
+ * to SADB_ADD, SADB_UPDATE, SADB_GET, and SADB_DUMP messages.
+ */
 static int
-key_setident(sah, m, mhp)
-	struct secashead *sah;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_setnatt(struct secasvar *sav, const struct sadb_msghdr *mhp)
+{
+	struct sadb_x_nat_t_port *port;
+	struct sadb_x_nat_t_type *type;
+	struct sadb_address *oai, *oar;
+	struct sockaddr *sa;
+	uint32_t addr;
+	uint16_t cksum;
+
+	IPSEC_ASSERT(sav->natt == NULL, ("natt is already initialized"));
+	/*
+	 * Ignore NAT-T headers if sproto isn't ESP.
+	 */
+	if (sav->sah->saidx.proto != IPPROTO_ESP)
+		return (0);
+
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_TYPE) &&
+	    !SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_SPORT) &&
+	    !SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_DPORT)) {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_NAT_T_TYPE) ||
+		    SADB_CHECKLEN(mhp, SADB_X_EXT_NAT_T_SPORT) ||
+		    SADB_CHECKLEN(mhp, SADB_X_EXT_NAT_T_DPORT)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			return (EINVAL);
+		}
+	} else
+		return (0);
+
+	type = (struct sadb_x_nat_t_type *)mhp->ext[SADB_X_EXT_NAT_T_TYPE];
+	if (type->sadb_x_nat_t_type_type != UDP_ENCAP_ESPINUDP) {
+		ipseclog((LOG_DEBUG, "%s: unsupported NAT-T type %u.\n",
+		    __func__, type->sadb_x_nat_t_type_type));
+		return (EINVAL);
+	}
+	/*
+	 * Allocate storage for NAT-T config.
+	 * On error it will be released by key_cleansav().
+	 */
+	sav->natt = malloc(sizeof(struct secnatt), M_IPSEC_MISC,
+	    M_NOWAIT | M_ZERO);
+	if (sav->natt == NULL) {
+		PFKEYSTAT_INC(in_nomem);
+		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
+		return (ENOBUFS);
+	}
+	port = (struct sadb_x_nat_t_port *)mhp->ext[SADB_X_EXT_NAT_T_SPORT];
+	if (port->sadb_x_nat_t_port_port == 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid NAT-T sport specified.\n",
+		    __func__));
+		return (EINVAL);
+	}
+	sav->natt->sport = port->sadb_x_nat_t_port_port;
+	port = (struct sadb_x_nat_t_port *)mhp->ext[SADB_X_EXT_NAT_T_DPORT];
+	if (port->sadb_x_nat_t_port_port == 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid NAT-T dport specified.\n",
+		    __func__));
+		return (EINVAL);
+	}
+	sav->natt->dport = port->sadb_x_nat_t_port_port;
+
+	/*
+	 * SADB_X_EXT_NAT_T_OAI and SADB_X_EXT_NAT_T_OAR are optional
+	 * and needed only for transport mode IPsec.
+	 * Usually NAT translates only one address, but it is possible,
+	 * that both addresses could be translated.
+	 * NOTE: Value of SADB_X_EXT_NAT_T_OAI is equal to SADB_X_EXT_NAT_T_OA.
+	 */
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_OAI)) {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_NAT_T_OAI)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			return (EINVAL);
+		}
+		oai = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAI];
+	} else
+		oai = NULL;
+	if (!SADB_CHECKHDR(mhp, SADB_X_EXT_NAT_T_OAR)) {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_NAT_T_OAR)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
+			    __func__));
+			return (EINVAL);
+		}
+		oar = (struct sadb_address *)mhp->ext[SADB_X_EXT_NAT_T_OAR];
+	} else
+		oar = NULL;
+
+	/* Initialize addresses only for transport mode */
+	if (sav->sah->saidx.mode != IPSEC_MODE_TUNNEL) {
+		cksum = 0;
+		if (oai != NULL) {
+			/* Currently we support only AF_INET */
+			sa = (struct sockaddr *)(oai + 1);
+			if (sa->sa_family != AF_INET ||
+			    sa->sa_len != sizeof(struct sockaddr_in)) {
+				ipseclog((LOG_DEBUG,
+				    "%s: wrong NAT-OAi header.\n",
+				    __func__));
+				return (EINVAL);
+			}
+			/* Ignore address if it the same */
+			if (((struct sockaddr_in *)sa)->sin_addr.s_addr !=
+			    sav->sah->saidx.src.sin.sin_addr.s_addr) {
+				bcopy(sa, &sav->natt->oai.sa, sa->sa_len);
+				sav->natt->flags |= IPSEC_NATT_F_OAI;
+				/* Calculate checksum delta */
+				addr = sav->sah->saidx.src.sin.sin_addr.s_addr;
+				cksum = in_addword(cksum, ~addr >> 16);
+				cksum = in_addword(cksum, ~addr & 0xffff);
+				addr = sav->natt->oai.sin.sin_addr.s_addr;
+				cksum = in_addword(cksum, addr >> 16);
+				cksum = in_addword(cksum, addr & 0xffff);
+			}
+		}
+		if (oar != NULL) {
+			/* Currently we support only AF_INET */
+			sa = (struct sockaddr *)(oar + 1);
+			if (sa->sa_family != AF_INET ||
+			    sa->sa_len != sizeof(struct sockaddr_in)) {
+				ipseclog((LOG_DEBUG,
+				    "%s: wrong NAT-OAr header.\n",
+				    __func__));
+				return (EINVAL);
+			}
+			/* Ignore address if it the same */
+			if (((struct sockaddr_in *)sa)->sin_addr.s_addr !=
+			    sav->sah->saidx.dst.sin.sin_addr.s_addr) {
+				bcopy(sa, &sav->natt->oar.sa, sa->sa_len);
+				sav->natt->flags |= IPSEC_NATT_F_OAR;
+				/* Calculate checksum delta */
+				addr = sav->sah->saidx.dst.sin.sin_addr.s_addr;
+				cksum = in_addword(cksum, ~addr >> 16);
+				cksum = in_addword(cksum, ~addr & 0xffff);
+				addr = sav->natt->oar.sin.sin_addr.s_addr;
+				cksum = in_addword(cksum, addr >> 16);
+				cksum = in_addword(cksum, addr & 0xffff);
+			}
+		}
+		sav->natt->cksum = cksum;
+	}
+	return (0);
+}
+
+static int
+key_setident(struct secashead *sah, const struct sadb_msghdr *mhp)
 {
 	const struct sadb_ident *idsrc, *iddst;
 	int idsrclen, iddstlen;
 
 	IPSEC_ASSERT(sah != NULL, ("null secashead"));
-	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
 	/* don't make buffer if not there */
-	if (mhp->ext[SADB_EXT_IDENTITY_SRC] == NULL &&
-	    mhp->ext[SADB_EXT_IDENTITY_DST] == NULL) {
+	if (SADB_CHECKHDR(mhp, SADB_EXT_IDENTITY_SRC) &&
+	    SADB_CHECKHDR(mhp, SADB_EXT_IDENTITY_DST)) {
 		sah->idents = NULL;
 		sah->identd = NULL;
-		return 0;
+		return (0);
 	}
-	
-	if (mhp->ext[SADB_EXT_IDENTITY_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_IDENTITY_DST] == NULL) {
+
+	if (SADB_CHECKHDR(mhp, SADB_EXT_IDENTITY_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_IDENTITY_DST)) {
 		ipseclog((LOG_DEBUG, "%s: invalid identity.\n", __func__));
-		return EINVAL;
+		return (EINVAL);
 	}
 
 	idsrc = (const struct sadb_ident *)mhp->ext[SADB_EXT_IDENTITY_SRC];
@@ -5543,12 +5792,13 @@ key_setident(sah, m, mhp)
 
 /*
  * m will not be freed on return.
- * it is caller's responsibility to free the result. 
+ * it is caller's responsibility to free the result.
+ *
+ * Called from SADB_ADD and SADB_UPDATE. Reply will contain headers
+ * from the request in defined order.
  */
 static struct mbuf *
-key_getmsgbuf_x1(m, mhp)
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_getmsgbuf_x1(struct mbuf *m, const struct sadb_msghdr *mhp)
 {
 	struct mbuf *n;
 
@@ -5557,11 +5807,15 @@ key_getmsgbuf_x1(m, mhp)
 	IPSEC_ASSERT(mhp->msg != NULL, ("null msg"));
 
 	/* create new sadb_msg to reply. */
-	n = key_gather_mbuf(m, mhp, 1, 9, SADB_EXT_RESERVED,
+	n = key_gather_mbuf(m, mhp, 1, 16, SADB_EXT_RESERVED,
 	    SADB_EXT_SA, SADB_X_EXT_SA2,
 	    SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST,
 	    SADB_EXT_LIFETIME_HARD, SADB_EXT_LIFETIME_SOFT,
-	    SADB_EXT_IDENTITY_SRC, SADB_EXT_IDENTITY_DST);
+	    SADB_EXT_IDENTITY_SRC, SADB_EXT_IDENTITY_DST,
+	    SADB_X_EXT_NAT_T_TYPE, SADB_X_EXT_NAT_T_SPORT,
+	    SADB_X_EXT_NAT_T_DPORT, SADB_X_EXT_NAT_T_OAI,
+	    SADB_X_EXT_NAT_T_OAR, SADB_X_EXT_NEW_ADDRESS_SRC,
+	    SADB_X_EXT_NEW_ADDRESS_DST);
 	if (!n)
 		return NULL;
 
@@ -5577,9 +5831,6 @@ key_getmsgbuf_x1(m, mhp)
 	return n;
 }
 
-static int key_delete_all __P((struct socket *, struct mbuf *,
-	const struct sadb_msghdr *, u_int16_t));
-
 /*
  * SADB_DELETE processing
  * receive
@@ -5592,17 +5843,13 @@ static int key_delete_all __P((struct socket *, struct mbuf *,
  * m will always be freed.
  */
 static int
-key_delete(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_delete(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct sadb_sa *sa0;
-	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
-	struct secashead *sah;
-	struct secasvar *sav = NULL;
-	u_int16_t proto;
+	struct sadb_address *src0, *dst0;
+	struct secasvar *sav;
+	struct sadb_sa *sa0;
+	uint8_t proto;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -5612,110 +5859,70 @@ key_delete(so, m, mhp)
 	/* map satype to proto */
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
+	if (SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST)) {
 		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
-		return key_senderror(so, m, EINVAL);
-	}
+	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
+	dst0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_DST]);
 
-	if (mhp->ext[SADB_EXT_SA] == NULL) {
+	if (key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1)) != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		return (key_senderror(so, m, EINVAL));
+	}
+	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
+	if (SADB_CHECKHDR(mhp, SADB_EXT_SA)) {
 		/*
 		 * Caller wants us to delete all non-LARVAL SAs
 		 * that match the src/dst.  This is used during
 		 * IKE INITIAL-CONTACT.
+		 * XXXAE: this looks like some extension to RFC2367.
 		 */
 		ipseclog((LOG_DEBUG, "%s: doing delete all.\n", __func__));
-		return key_delete_all(so, m, mhp, proto);
-	} else if (mhp->extlen[SADB_EXT_SA] < sizeof(struct sadb_sa)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
-		return key_senderror(so, m, EINVAL);
+		return (key_delete_all(so, m, mhp, &saidx));
 	}
-
-	sa0 = (struct sadb_sa *)mhp->ext[SADB_EXT_SA];
-	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_DST]);
-
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle NAT-T info if present.
-	 */
-	if (mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src,
-			    sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst,
-			    dport->sadb_x_nat_t_port_port);
-	}
-#endif
-
-	/* get a SA header */
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
-		if (key_cmpsaidx(&sah->saidx, &saidx, CMP_HEAD) == 0)
-			continue;
-
-		/* get a SA with SPI. */
-		sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
-		if (sav)
-			break;
-	}
-	if (sah == NULL) {
-		SAHTREE_UNLOCK();
-		ipseclog((LOG_DEBUG, "%s: no SA found.\n", __func__));
-		return key_senderror(so, m, ENOENT);
+	if (SADB_CHECKLEN(mhp, SADB_EXT_SA)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
+		return (key_senderror(so, m, EINVAL));
 	}
-
-	key_sa_chgstate(sav, SADB_SASTATE_DEAD);
-	KEY_FREESAV(&sav);
-	SAHTREE_UNLOCK();
+	sa0 = (struct sadb_sa *)mhp->ext[SADB_EXT_SA];
+	if (proto == IPPROTO_TCP)
+		sav = key_getsav_tcpmd5(&saidx, NULL);
+	else
+		sav = key_getsavbyspi(sa0->sadb_sa_spi);
+	if (sav == NULL) {
+		ipseclog((LOG_DEBUG, "%s: no SA found for SPI %u.\n",
+		    __func__, ntohl(sa0->sadb_sa_spi)));
+		return (key_senderror(so, m, ESRCH));
+	}
+	if (key_cmpsaidx(&sav->sah->saidx, &saidx, CMP_HEAD) == 0) {
+		ipseclog((LOG_DEBUG, "%s: saidx mismatched for SPI %u.\n",
+		    __func__, ntohl(sav->spi)));
+		key_freesav(&sav);
+		return (key_senderror(so, m, ESRCH));
+	}
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p)\n", __func__, sav));
+	KEYDBG(KEY_DATA, kdebug_secasv(sav));
+	key_unlinksav(sav);
+	key_freesav(&sav);
 
     {
 	struct mbuf *n;
 	struct sadb_msg *newmsg;
 
 	/* create new sadb_msg to reply. */
-	/* XXX-BZ NAT-T extensions? */
 	n = key_gather_mbuf(m, mhp, 1, 4, SADB_EXT_RESERVED,
 	    SADB_EXT_SA, SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
 	if (!n)
@@ -5739,95 +5946,44 @@ key_delete(so, m, mhp)
  * delete all SAs for src/dst.  Called from key_delete().
  */
 static int
-key_delete_all(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp,
-    u_int16_t proto)
+key_delete_all(struct socket *so, struct mbuf *m,
+    const struct sadb_msghdr *mhp, struct secasindex *saidx)
 {
-	struct sadb_address *src0, *dst0;
-	struct secasindex saidx;
+	struct secasvar_queue drainq;
 	struct secashead *sah;
 	struct secasvar *sav, *nextsav;
-	u_int stateidx, state;
-
-	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_DST]);
-
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
 
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle NAT-T info if present.
-	 */
-
-	if (mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src,
-			    sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst,
-			    dport->sadb_x_nat_t_port_port);
-	}
-#endif
-
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
-		if (key_cmpsaidx(&sah->saidx, &saidx, CMP_HEAD) == 0)
+	TAILQ_INIT(&drainq);
+	SAHTREE_WLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(saidx), addrhash) {
+		if (key_cmpsaidx(&sah->saidx, saidx, CMP_HEAD) == 0)
 			continue;
-
-		/* Delete all non-LARVAL SAs. */
-		for (stateidx = 0;
-		     stateidx < _ARRAYLEN(saorder_state_alive);
-		     stateidx++) {
-			state = saorder_state_alive[stateidx];
-			if (state == SADB_SASTATE_LARVAL)
-				continue;
-			for (sav = LIST_FIRST(&sah->savtree[state]);
-			     sav != NULL; sav = nextsav) {
-				nextsav = LIST_NEXT(sav, chain);
-				/* sanity check */
-				if (sav->state != state) {
-					ipseclog((LOG_DEBUG, "%s: invalid "
-						"sav->state (queue %d SA %d)\n",
-						__func__, state, sav->state));
-					continue;
-				}
-				
-				key_sa_chgstate(sav, SADB_SASTATE_DEAD);
-				KEY_FREESAV(&sav);
-			}
-		}
+		/* Move all ALIVE SAs into drainq */
+		TAILQ_CONCAT(&drainq, &sah->savtree_alive, chain);
+	}
+	/* Unlink all queued SAs from SPI hash */
+	TAILQ_FOREACH(sav, &drainq, chain) {
+		sav->state = SADB_SASTATE_DEAD;
+		LIST_REMOVE(sav, spihash);
+	}
+	SAHTREE_WUNLOCK();
+	/* Now we can release reference for all SAs in drainq */
+	sav = TAILQ_FIRST(&drainq);
+	while (sav != NULL) {
+		KEYDBG(KEY_STAMP,
+		    printf("%s: SA(%p)\n", __func__, sav));
+		KEYDBG(KEY_DATA, kdebug_secasv(sav));
+		nextsav = TAILQ_NEXT(sav, chain);
+		key_freesah(&sav->sah); /* release reference from SAV */
+		key_freesav(&sav); /* release last reference */
+		sav = nextsav;
 	}
-	SAHTREE_UNLOCK();
+
     {
 	struct mbuf *n;
 	struct sadb_msg *newmsg;
 
 	/* create new sadb_msg to reply. */
-	/* XXX-BZ NAT-T extensions? */
 	n = key_gather_mbuf(m, mhp, 1, 3, SADB_EXT_RESERVED,
 	    SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
 	if (!n)
@@ -5848,6 +6004,52 @@ key_delete_all(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp,
 }
 
 /*
+ * Delete all alive SAs for corresponding xform.
+ * Larval SAs have not initialized tdb_xform, so it is safe to leave them
+ * here when xform disappears.
+ */
+static void
+key_delete_xform(const struct xformsw *xsp)
+{
+	struct secasvar_queue drainq;
+	struct secashead *sah;
+	struct secasvar *sav, *nextsav;
+
+	TAILQ_INIT(&drainq);
+	SAHTREE_WLOCK();
+	TAILQ_FOREACH(sah, &V_sahtree, chain) {
+		sav = TAILQ_FIRST(&sah->savtree_alive);
+		if (sav == NULL)
+			continue;
+		if (sav->tdb_xform != xsp)
+			continue;
+		/*
+		 * It is supposed that all SAs in the chain are related to
+		 * one xform.
+		 */
+		TAILQ_CONCAT(&drainq, &sah->savtree_alive, chain);
+	}
+	/* Unlink all queued SAs from SPI hash */
+	TAILQ_FOREACH(sav, &drainq, chain) {
+		sav->state = SADB_SASTATE_DEAD;
+		LIST_REMOVE(sav, spihash);
+	}
+	SAHTREE_WUNLOCK();
+
+	/* Now we can release reference for all SAs in drainq */
+	sav = TAILQ_FIRST(&drainq);
+	while (sav != NULL) {
+		KEYDBG(KEY_STAMP,
+		    printf("%s: SA(%p)\n", __func__, sav));
+		KEYDBG(KEY_DATA, kdebug_secasv(sav));
+		nextsav = TAILQ_NEXT(sav, chain);
+		key_freesah(&sav->sah); /* release reference from SAV */
+		key_freesav(&sav); /* release last reference */
+		sav = nextsav;
+	}
+}
+
+/*
  * SADB_GET processing
  * receive
  *   <base, SA(*), address(SD)>
@@ -5860,17 +6062,13 @@ key_delete_all(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp,
  * m will always be freed.
  */
 static int
-key_get(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_get(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct sadb_sa *sa0;
-	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
-	struct secashead *sah;
-	struct secasvar *sav = NULL;
-	u_int16_t proto;
+	struct sadb_address *src0, *dst0;
+	struct sadb_sa *sa0;
+	struct secasvar *sav;
+	uint8_t proto;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -5884,18 +6082,19 @@ key_get(so, m, mhp)
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->ext[SADB_EXT_SA] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_SA] < sizeof(struct sadb_sa) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address)) {
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_SA) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
@@ -5903,79 +6102,45 @@ key_get(so, m, mhp)
 	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
 	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
 
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifdef IPSEC_NAT_T
-	/*
-	 * Handle NAT-T info if present.
-	 */
-
-	if (mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
-			    __func__));
-			return key_senderror(so, m, EINVAL);
-		}
-
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
-
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src,
-			    sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst,
-			    dport->sadb_x_nat_t_port_port);
+	if (key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1)) != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		return key_senderror(so, m, EINVAL);
 	}
-#endif
-
-	/* get a SA header */
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
-		if (key_cmpsaidx(&sah->saidx, &saidx, CMP_HEAD) == 0)
-			continue;
+	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
 
-		/* get a SA with SPI. */
-		sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
-		if (sav)
-			break;
-	}
-	SAHTREE_UNLOCK();
-	if (sah == NULL) {
+	if (proto == IPPROTO_TCP)
+		sav = key_getsav_tcpmd5(&saidx, NULL);
+	else
+		sav = key_getsavbyspi(sa0->sadb_sa_spi);
+	if (sav == NULL) {
 		ipseclog((LOG_DEBUG, "%s: no SA found.\n", __func__));
-		return key_senderror(so, m, ENOENT);
+		return key_senderror(so, m, ESRCH);
+	}
+	if (key_cmpsaidx(&sav->sah->saidx, &saidx, CMP_HEAD) == 0) {
+		ipseclog((LOG_DEBUG, "%s: saidx mismatched for SPI %u.\n",
+		    __func__, ntohl(sa0->sadb_sa_spi)));
+		key_freesav(&sav);
+		return (key_senderror(so, m, ESRCH));
 	}
 
     {
 	struct mbuf *n;
-	u_int8_t satype;
+	uint8_t satype;
 
 	/* map proto to satype */
-	if ((satype = key_proto2satype(sah->saidx.proto)) == 0) {
+	if ((satype = key_proto2satype(sav->sah->saidx.proto)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: there was invalid proto in SAD.\n",
-			__func__));
+		    __func__));
+		key_freesav(&sav);
 		return key_senderror(so, m, EINVAL);
 	}
 
 	/* create new sadb_msg to reply. */
 	n = key_setdumpsa(sav, SADB_GET, satype, mhp->msg->sadb_msg_seq,
 	    mhp->msg->sadb_msg_pid);
+
+	key_freesav(&sav);
 	if (!n)
 		return key_senderror(so, m, ENOBUFS);
 
@@ -5986,8 +6151,7 @@ key_get(so, m, mhp)
 
 /* XXX make it sysctl-configurable? */
 static void
-key_getcomb_setlifetime(comb)
-	struct sadb_comb *comb;
+key_getcomb_setlifetime(struct sadb_comb *comb)
 {
 
 	comb->sadb_comb_soft_allocations = 1;
@@ -6005,10 +6169,10 @@ key_getcomb_setlifetime(comb)
  * XXX no idea if the user wants ESP authentication or not
  */
 static struct mbuf *
-key_getcomb_esp()
+key_getcomb_ealg(void)
 {
 	struct sadb_comb *comb;
-	struct enc_xform *algo;
+	const struct enc_xform *algo;
 	struct mbuf *result = NULL, *m, *n;
 	int encmin;
 	int i, off, o;
@@ -6017,7 +6181,7 @@ key_getcomb_esp()
 
 	m = NULL;
 	for (i = 1; i <= SADB_EALG_MAX; i++) {
-		algo = esp_algorithm_lookup(i);
+		algo = enc_algorithm_lookup(i);
 		if (algo == NULL)
 			continue;
 
@@ -6034,7 +6198,7 @@ key_getcomb_esp()
 		else {
 			IPSEC_ASSERT(l <= MLEN,
 				("l=%u > MLEN=%lu", l, (u_long) MLEN));
-			MGET(m, M_DONTWAIT, MT_DATA);
+			MGET(m, M_NOWAIT, MT_DATA);
 			if (m) {
 				M_ALIGN(m, l);
 				m->m_len = l;
@@ -6079,11 +6243,8 @@ key_getcomb_esp()
 }
 
 static void
-key_getsizes_ah(
-	const struct auth_hash *ah,
-	int alg,
-	u_int16_t* min,
-	u_int16_t* max)
+key_getsizes_ah(const struct auth_hash *ah, int alg, u_int16_t* min,
+    u_int16_t* max)
 {
 
 	*min = *max = ah->keysize;
@@ -6113,8 +6274,8 @@ key_getsizes_ah(
 static struct mbuf *
 key_getcomb_ah()
 {
+	const struct auth_hash *algo;
 	struct sadb_comb *comb;
-	struct auth_hash *algo;
 	struct mbuf *m;
 	u_int16_t minkeysize, maxkeysize;
 	int i;
@@ -6131,7 +6292,7 @@ key_getcomb_ah()
 		    i != SADB_X_AALG_SHA2_512)
 			continue;
 #endif
-		algo = ah_algorithm_lookup(i);
+		algo = auth_algorithm_lookup(i);
 		if (!algo)
 			continue;
 		key_getsizes_ah(algo, i, &minkeysize, &maxkeysize);
@@ -6142,14 +6303,14 @@ key_getcomb_ah()
 		if (!m) {
 			IPSEC_ASSERT(l <= MLEN,
 				("l=%u > MLEN=%lu", l, (u_long) MLEN));
-			MGET(m, M_DONTWAIT, MT_DATA);
+			MGET(m, M_NOWAIT, MT_DATA);
 			if (m) {
 				M_ALIGN(m, l);
 				m->m_len = l;
 				m->m_next = NULL;
 			}
 		} else
-			M_PREPEND(m, l, M_DONTWAIT);
+			M_PREPEND(m, l, M_NOWAIT);
 		if (!m)
 			return NULL;
 
@@ -6171,29 +6332,29 @@ key_getcomb_ah()
 static struct mbuf *
 key_getcomb_ipcomp()
 {
+	const struct comp_algo *algo;
 	struct sadb_comb *comb;
-	struct comp_algo *algo;
 	struct mbuf *m;
 	int i;
 	const int l = PFKEY_ALIGN8(sizeof(struct sadb_comb));
 
 	m = NULL;
 	for (i = 1; i <= SADB_X_CALG_MAX; i++) {
-		algo = ipcomp_algorithm_lookup(i);
+		algo = comp_algorithm_lookup(i);
 		if (!algo)
 			continue;
 
 		if (!m) {
 			IPSEC_ASSERT(l <= MLEN,
 				("l=%u > MLEN=%lu", l, (u_long) MLEN));
-			MGET(m, M_DONTWAIT, MT_DATA);
+			MGET(m, M_NOWAIT, MT_DATA);
 			if (m) {
 				M_ALIGN(m, l);
 				m->m_len = l;
 				m->m_next = NULL;
 			}
 		} else
-			M_PREPEND(m, l, M_DONTWAIT);
+			M_PREPEND(m, l, M_NOWAIT);
 		if (!m)
 			return NULL;
 
@@ -6213,8 +6374,7 @@ key_getcomb_ipcomp()
  * XXX sysctl interface to ipsec_{ah,esp}_keymin
  */
 static struct mbuf *
-key_getprop(saidx)
-	const struct secasindex *saidx;
+key_getprop(const struct secasindex *saidx)
 {
 	struct sadb_prop *prop;
 	struct mbuf *m, *n;
@@ -6223,7 +6383,7 @@ key_getprop(saidx)
 
 	switch (saidx->proto)  {
 	case IPPROTO_ESP:
-		m = key_getcomb_esp();
+		m = key_getcomb_ealg();
 		break;
 	case IPPROTO_AH:
 		m = key_getcomb_ah();
@@ -6237,7 +6397,7 @@ key_getprop(saidx)
 
 	if (!m)
 		return NULL;
-	M_PREPEND(m, l, M_DONTWAIT);
+	M_PREPEND(m, l, M_NOWAIT);
 	if (!m)
 		return NULL;
 
@@ -6260,7 +6420,7 @@ key_getprop(saidx)
  *   <base, SA, address(SD), (address(P)), x_policy,
  *       (identity(SD),) (sensitivity,) proposal>
  * to KMD, and expect to receive
- *   <base> with SADB_ACQUIRE if error occured,
+ *   <base> with SADB_ACQUIRE if error occurred,
  * or
  *   <base, src address, dst address, (SPI range)> with SADB_GETSPI
  * from KMD by PF_KEY.
@@ -6277,40 +6437,26 @@ key_getprop(saidx)
 static int
 key_acquire(const struct secasindex *saidx, struct secpolicy *sp)
 {
-	struct mbuf *result = NULL, *m;
-	struct secacq *newacq;
-	u_int8_t satype;
-	int error = -1;
-	u_int32_t seq;
+	union sockaddr_union addr;
+	struct mbuf *result, *m;
+	uint32_t seq;
+	int error;
+	uint16_t ul_proto;
+	uint8_t mask, satype;
 
 	IPSEC_ASSERT(saidx != NULL, ("null saidx"));
 	satype = key_proto2satype(saidx->proto);
 	IPSEC_ASSERT(satype != 0, ("null satype, protocol %u", saidx->proto));
 
-	/*
-	 * We never do anything about acquirng SA.  There is anather
-	 * solution that kernel blocks to send SADB_ACQUIRE message until
-	 * getting something message from IKEd.  In later case, to be
-	 * managed with ACQUIRING list.
-	 */
-	/* Get an entry to check whether sending message or not. */
-	if ((newacq = key_getacq(saidx)) != NULL) {
-		if (V_key_blockacq_count < newacq->count) {
-			/* reset counter and do send message. */
-			newacq->count = 0;
-		} else {
-			/* increment counter and do nothing. */
-			newacq->count++;
-			return 0;
-		}
-	} else {
-		/* make new entry for blocking to send SADB_ACQUIRE. */
-		if ((newacq = key_newacq(saidx)) == NULL)
-			return ENOBUFS;
-	}
+	error = -1;
+	result = NULL;
+	ul_proto = IPSEC_ULPROTO_ANY;
 
+	/* Get seq number to check whether sending message or not. */
+	seq = key_getacq(saidx, &error);
+	if (seq == 0)
+		return (error);
 
-	seq = newacq->seq;
 	m = key_setsadbmsg(SADB_ACQUIRE, 0, satype, seq, 0, 0);
 	if (!m) {
 		error = ENOBUFS;
@@ -6319,21 +6465,69 @@ key_acquire(const struct secasindex *saidx, struct secpolicy *sp)
 	result = m;
 
 	/*
-	 * No SADB_X_EXT_NAT_T_* here: we do not know
-	 * anything related to NAT-T at this time.
+	 * set sadb_address for saidx's.
+	 *
+	 * Note that if sp is supplied, then we're being called from
+	 * key_allocsa_policy() and should supply port and protocol
+	 * information.
+	 * XXXAE: why only TCP and UDP? ICMP and SCTP looks applicable too.
+	 * XXXAE: probably we can handle this in the ipsec[46]_allocsa().
+	 * XXXAE: it looks like we should save this info in the ACQ entry.
 	 */
-
-	/* set sadb_address for saidx's. */
-	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
-	    &saidx->src.sa, FULLMASK, IPSEC_ULPROTO_ANY);
+	if (sp != NULL && (sp->spidx.ul_proto == IPPROTO_TCP ||
+	    sp->spidx.ul_proto == IPPROTO_UDP))
+		ul_proto = sp->spidx.ul_proto;
+
+	addr = saidx->src;
+	mask = FULLMASK;
+	if (ul_proto != IPSEC_ULPROTO_ANY) {
+		switch (sp->spidx.src.sa.sa_family) {
+		case AF_INET:
+			if (sp->spidx.src.sin.sin_port != IPSEC_PORT_ANY) {
+				addr.sin.sin_port = sp->spidx.src.sin.sin_port;
+				mask = sp->spidx.prefs;
+			}
+			break;
+		case AF_INET6:
+			if (sp->spidx.src.sin6.sin6_port != IPSEC_PORT_ANY) {
+				addr.sin6.sin6_port =
+				    sp->spidx.src.sin6.sin6_port;
+				mask = sp->spidx.prefs;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC, &addr.sa, mask, ul_proto);
 	if (!m) {
 		error = ENOBUFS;
 		goto fail;
 	}
 	m_cat(result, m);
 
-	m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
-	    &saidx->dst.sa, FULLMASK, IPSEC_ULPROTO_ANY);
+	addr = saidx->dst;
+	mask = FULLMASK;
+	if (ul_proto != IPSEC_ULPROTO_ANY) {
+		switch (sp->spidx.dst.sa.sa_family) {
+		case AF_INET:
+			if (sp->spidx.dst.sin.sin_port != IPSEC_PORT_ANY) {
+				addr.sin.sin_port = sp->spidx.dst.sin.sin_port;
+				mask = sp->spidx.prefd;
+			}
+			break;
+		case AF_INET6:
+			if (sp->spidx.dst.sin6.sin6_port != IPSEC_PORT_ANY) {
+				addr.sin6.sin6_port =
+				    sp->spidx.dst.sin6.sin6_port;
+				mask = sp->spidx.prefd;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_DST, &addr.sa, mask, ul_proto);
 	if (!m) {
 		error = ENOBUFS;
 		goto fail;
@@ -6343,8 +6537,9 @@ key_acquire(const struct secasindex *saidx, struct secpolicy *sp)
 	/* XXX proxy address (optional) */
 
 	/* set sadb_x_policy */
-	if (sp) {
-		m = key_setsadbxpolicy(sp->policy, sp->spidx.dir, sp->id);
+	if (sp != NULL) {
+		m = key_setsadbxpolicy(sp->policy, sp->spidx.dir, sp->id,
+		    sp->priority);
 		if (!m) {
 			error = ENOBUFS;
 			goto fail;
@@ -6436,6 +6631,10 @@ key_acquire(const struct secasindex *saidx, struct secpolicy *sp)
 	mtod(result, struct sadb_msg *)->sadb_msg_len =
 	    PFKEY_UNIT64(result->m_pkthdr.len);
 
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SP(%p)\n", __func__, sp));
+	KEYDBG(KEY_DATA, kdebug_secasindex(saidx, NULL));
+
 	return key_sendup_mbuf(NULL, result, KEY_SENDUP_REGISTERED);
 
  fail:
@@ -6444,66 +6643,126 @@ key_acquire(const struct secasindex *saidx, struct secpolicy *sp)
 	return error;
 }
 
-static struct secacq *
-key_newacq(const struct secasindex *saidx)
+static uint32_t
+key_newacq(const struct secasindex *saidx, int *perror)
 {
-	struct secacq *newacq;
+	struct secacq *acq;
+	uint32_t seq;
 
-	/* get new entry */
-	newacq = malloc(sizeof(struct secacq), M_IPSEC_SAQ, M_NOWAIT|M_ZERO);
-	if (newacq == NULL) {
+	acq = malloc(sizeof(*acq), M_IPSEC_SAQ, M_NOWAIT | M_ZERO);
+	if (acq == NULL) {
 		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
-		return NULL;
+		*perror = ENOBUFS;
+		return (0);
 	}
 
 	/* copy secindex */
-	bcopy(saidx, &newacq->saidx, sizeof(newacq->saidx));
-	newacq->seq = (V_acq_seq == ~0 ? 1 : ++V_acq_seq);
-	newacq->created = time_second;
-	newacq->count = 0;
+	bcopy(saidx, &acq->saidx, sizeof(acq->saidx));
+	acq->created = time_second;
+	acq->count = 0;
 
 	/* add to acqtree */
 	ACQ_LOCK();
-	LIST_INSERT_HEAD(&V_acqtree, newacq, chain);
+	seq = acq->seq = (V_acq_seq == ~0 ? 1 : ++V_acq_seq);
+	LIST_INSERT_HEAD(&V_acqtree, acq, chain);
+	LIST_INSERT_HEAD(ACQADDRHASH_HASH(saidx), acq, addrhash);
+	LIST_INSERT_HEAD(ACQSEQHASH_HASH(seq), acq, seqhash);
 	ACQ_UNLOCK();
-
-	return newacq;
+	*perror = 0;
+	return (seq);
 }
 
-static struct secacq *
-key_getacq(const struct secasindex *saidx)
+static uint32_t
+key_getacq(const struct secasindex *saidx, int *perror)
 {
 	struct secacq *acq;
+	uint32_t seq;
 
 	ACQ_LOCK();
-	LIST_FOREACH(acq, &V_acqtree, chain) {
-		if (key_cmpsaidx(saidx, &acq->saidx, CMP_EXACTLY))
+	LIST_FOREACH(acq, ACQADDRHASH_HASH(saidx), addrhash) {
+		if (key_cmpsaidx(&acq->saidx, saidx, CMP_EXACTLY)) {
+			if (acq->count > V_key_blockacq_count) {
+				/*
+				 * Reset counter and send message.
+				 * Also reset created time to keep ACQ for
+				 * this saidx.
+				 */
+				acq->created = time_second;
+				acq->count = 0;
+				seq = acq->seq;
+			} else {
+				/*
+				 * Increment counter and do nothing.
+				 * We send SADB_ACQUIRE message only
+				 * for each V_key_blockacq_count packet.
+				 */
+				acq->count++;
+				seq = 0;
+			}
 			break;
+		}
 	}
 	ACQ_UNLOCK();
-
-	return acq;
+	if (acq != NULL) {
+		*perror = 0;
+		return (seq);
+	}
+	/* allocate new  entry */
+	return (key_newacq(saidx, perror));
 }
 
-static struct secacq *
-key_getacqbyseq(seq)
-	u_int32_t seq;
+static int
+key_acqreset(uint32_t seq)
 {
 	struct secacq *acq;
 
 	ACQ_LOCK();
-	LIST_FOREACH(acq, &V_acqtree, chain) {
-		if (acq->seq == seq)
+	LIST_FOREACH(acq, ACQSEQHASH_HASH(seq), seqhash) {
+		if (acq->seq == seq) {
+			acq->count = 0;
+			acq->created = time_second;
 			break;
+		}
 	}
 	ACQ_UNLOCK();
+	if (acq == NULL)
+		return (ESRCH);
+	return (0);
+}
+/*
+ * Mark ACQ entry as stale to remove it in key_flush_acq().
+ * Called after successful SADB_GETSPI message.
+ */
+static int
+key_acqdone(const struct secasindex *saidx, uint32_t seq)
+{
+	struct secacq *acq;
 
-	return acq;
+	ACQ_LOCK();
+	LIST_FOREACH(acq, ACQSEQHASH_HASH(seq), seqhash) {
+		if (acq->seq == seq)
+			break;
+	}
+	if (acq != NULL) {
+		if (key_cmpsaidx(&acq->saidx, saidx, CMP_EXACTLY) == 0) {
+			ipseclog((LOG_DEBUG,
+			    "%s: Mismatched saidx for ACQ %u", __func__, seq));
+			acq = NULL;
+		} else {
+			acq->created = 0;
+		}
+	} else {
+		ipseclog((LOG_DEBUG,
+		    "%s: ACQ %u is not found.", __func__, seq));
+	}
+	ACQ_UNLOCK();
+	if (acq == NULL)
+		return (ESRCH);
+	return (0);
 }
 
 static struct secspacq *
-key_newspacq(spidx)
-	struct secpolicyindex *spidx;
+key_newspacq(struct secpolicyindex *spidx)
 {
 	struct secspacq *acq;
 
@@ -6528,8 +6787,7 @@ key_newspacq(spidx)
 }
 
 static struct secspacq *
-key_getspacq(spidx)
-	struct secpolicyindex *spidx;
+key_getspacq(struct secpolicyindex *spidx)
 {
 	struct secspacq *acq;
 
@@ -6560,16 +6818,15 @@ key_getspacq(spidx)
  * m will always be freed.
  */
 static int
-key_acquire2(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_acquire2(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	const struct sadb_address *src0, *dst0;
+	SAHTREE_RLOCK_TRACKER;
+	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *sah;
-	u_int16_t proto;
+	uint32_t reqid;
 	int error;
+	uint8_t mode, proto;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -6578,35 +6835,28 @@ key_acquire2(so, m, mhp)
 
 	/*
 	 * Error message from KMd.
-	 * We assume that if error was occured in IKEd, the length of PFKEY
+	 * We assume that if error was occurred in IKEd, the length of PFKEY
 	 * message is equal to the size of sadb_msg structure.
-	 * We do not raise error even if error occured in this function.
+	 * We do not raise error even if error occurred in this function.
 	 */
 	if (mhp->msg->sadb_msg_len == PFKEY_UNIT64(sizeof(struct sadb_msg))) {
-		struct secacq *acq;
-
 		/* check sequence number */
-		if (mhp->msg->sadb_msg_seq == 0) {
+		if (mhp->msg->sadb_msg_seq == 0 ||
+		    mhp->msg->sadb_msg_errno == 0) {
 			ipseclog((LOG_DEBUG, "%s: must specify sequence "
-				"number.\n", __func__));
-			m_freem(m);
-			return 0;
-		}
-
-		if ((acq = key_getacqbyseq(mhp->msg->sadb_msg_seq)) == NULL) {
+				"number and errno.\n", __func__));
+		} else {
 			/*
-			 * the specified larval SA is already gone, or we got
-			 * a bogus sequence number.  we can silently ignore it.
+			 * IKEd reported that error occurred.
+			 * XXXAE: what it expects from the kernel?
+			 * Probably we should send SADB_ACQUIRE again?
+			 * If so, reset ACQ's state.
+			 * XXXAE: it looks useless.
 			 */
-			m_freem(m);
-			return 0;
+			key_acqreset(mhp->msg->sadb_msg_seq);
 		}
-
-		/* reset acq counter in order to deletion by timehander. */
-		acq->created = time_second;
-		acq->count = 0;
 		m_freem(m);
-		return 0;
+		return (0);
 	}
 
 	/*
@@ -6616,79 +6866,60 @@ key_acquire2(so, m, mhp)
 	/* map satype to proto */
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
-	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
-	    mhp->ext[SADB_EXT_PROPOSAL] == NULL) {
-		/* error */
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",
-			__func__));
+	if (SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKHDR(mhp, SADB_EXT_PROPOSAL)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: missing required header.\n",
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
-	if (mhp->extlen[SADB_EXT_ADDRESS_SRC] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_ADDRESS_DST] < sizeof(struct sadb_address) ||
-	    mhp->extlen[SADB_EXT_PROPOSAL] < sizeof(struct sadb_prop)) {
-		/* error */
-		ipseclog((LOG_DEBUG, "%s: invalid message is passed.\n",	
-			__func__));
+	if (SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_SRC) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_ADDRESS_DST) ||
+	    SADB_CHECKLEN(mhp, SADB_EXT_PROPOSAL)) {
+		ipseclog((LOG_DEBUG,
+		    "%s: invalid message: wrong header size.\n", __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
-	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
-	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
-
-	/* XXX boundary check against sa_len */
-	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
-
-	/*
-	 * Make sure the port numbers are zero.
-	 * In case of NAT-T we will update them later if needed.
-	 */
-	KEY_PORTTOSADDR(&saidx.src, 0);
-	KEY_PORTTOSADDR(&saidx.dst, 0);
-
-#ifndef IPSEC_NAT_T
-	/*
-	 * Handle NAT-T info if present.
-	 */
-
-	if (mhp->ext[SADB_X_EXT_NAT_T_SPORT] != NULL &&
-	    mhp->ext[SADB_X_EXT_NAT_T_DPORT] != NULL) {
-		struct sadb_x_nat_t_port *sport, *dport;
-
-		if (mhp->extlen[SADB_X_EXT_NAT_T_SPORT] < sizeof(*sport) ||
-		    mhp->extlen[SADB_X_EXT_NAT_T_DPORT] < sizeof(*dport)) {
-			ipseclog((LOG_DEBUG, "%s: invalid message.\n",
+	if (SADB_CHECKHDR(mhp, SADB_X_EXT_SA2)) {
+		mode = IPSEC_MODE_ANY;
+		reqid = 0;
+	} else {
+		if (SADB_CHECKLEN(mhp, SADB_X_EXT_SA2)) {
+			ipseclog((LOG_DEBUG,
+			    "%s: invalid message: wrong header size.\n",
 			    __func__));
 			return key_senderror(so, m, EINVAL);
 		}
+		mode = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_mode;
+		reqid = ((struct sadb_x_sa2 *)
+		    mhp->ext[SADB_X_EXT_SA2])->sadb_x_sa2_reqid;
+	}
 
-		sport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_SPORT];
-		dport = (struct sadb_x_nat_t_port *)
-		    mhp->ext[SADB_X_EXT_NAT_T_DPORT];
+	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
+	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
 
-		if (sport)
-			KEY_PORTTOSADDR(&saidx.src,
-			    sport->sadb_x_nat_t_port_port);
-		if (dport)
-			KEY_PORTTOSADDR(&saidx.dst,
-			    dport->sadb_x_nat_t_port_port);
+	error = key_checksockaddrs((struct sockaddr *)(src0 + 1),
+	    (struct sockaddr *)(dst0 + 1));
+	if (error != 0) {
+		ipseclog((LOG_DEBUG, "%s: invalid sockaddr.\n", __func__));
+		return key_senderror(so, m, EINVAL);
 	}
-#endif
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
 
 	/* get a SA index */
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (sah->state == SADB_SASTATE_DEAD)
-			continue;
+	SAHTREE_RLOCK();
+	LIST_FOREACH(sah, SAHADDRHASH_HASH(&saidx), addrhash) {
 		if (key_cmpsaidx(&sah->saidx, &saidx, CMP_MODE_REQID))
 			break;
 	}
-	SAHTREE_UNLOCK();
+	SAHTREE_RUNLOCK();
 	if (sah != NULL) {
 		ipseclog((LOG_DEBUG, "%s: a SA exists already.\n", __func__));
 		return key_senderror(so, m, EEXIST);
@@ -6696,12 +6927,13 @@ key_acquire2(so, m, mhp)
 
 	error = key_acquire(&saidx, NULL);
 	if (error != 0) {
-		ipseclog((LOG_DEBUG, "%s: error %d returned from key_acquire\n",
-			__func__, mhp->msg->sadb_msg_errno));
+		ipseclog((LOG_DEBUG,
+		    "%s: error %d returned from key_acquire()\n",
+			__func__, error));
 		return key_senderror(so, m, error);
 	}
-
-	return key_sendup_mbuf(so, m, KEY_SENDUP_REGISTERED);
+	m_freem(m);
+	return (0);
 }
 
 /*
@@ -6718,12 +6950,9 @@ key_acquire2(so, m, mhp)
  * m will always be freed.
  */
 static int
-key_register(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_register(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
-	struct secreg *reg, *newreg = 0;
+	struct secreg *reg, *newreg = NULL;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -6777,14 +7006,14 @@ key_register(so, m, mhp)
 	/* create new sadb_msg to reply. */
 	alen = 0;
 	for (i = 1; i <= SADB_AALG_MAX; i++) {
-		if (ah_algorithm_lookup(i))
+		if (auth_algorithm_lookup(i))
 			alen += sizeof(struct sadb_alg);
 	}
 	if (alen)
 		alen += sizeof(struct sadb_supported);
 	elen = 0;
 	for (i = 1; i <= SADB_EALG_MAX; i++) {
-		if (esp_algorithm_lookup(i))
+		if (enc_algorithm_lookup(i))
 			elen += sizeof(struct sadb_alg);
 	}
 	if (elen)
@@ -6795,10 +7024,9 @@ key_register(so, m, mhp)
 	if (len > MCLBYTES)
 		return key_senderror(so, m, ENOBUFS);
 
-	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	MGETHDR(n, M_NOWAIT, MT_DATA);
 	if (len > MHLEN) {
-		MCLGET(n, M_DONTWAIT);
-		if ((n->m_flags & M_EXT) == 0) {
+		if (!(MCLGET(n, M_NOWAIT))) {
 			m_freem(n);
 			n = NULL;
 		}
@@ -6824,10 +7052,10 @@ key_register(so, m, mhp)
 		off += PFKEY_ALIGN8(sizeof(*sup));
 
 		for (i = 1; i <= SADB_AALG_MAX; i++) {
-			struct auth_hash *aalgo;
+			const struct auth_hash *aalgo;
 			u_int16_t minkeysize, maxkeysize;
 
-			aalgo = ah_algorithm_lookup(i);
+			aalgo = auth_algorithm_lookup(i);
 			if (!aalgo)
 				continue;
 			alg = (struct sadb_alg *)(mtod(n, caddr_t) + off);
@@ -6848,14 +7076,14 @@ key_register(so, m, mhp)
 		off += PFKEY_ALIGN8(sizeof(*sup));
 
 		for (i = 1; i <= SADB_EALG_MAX; i++) {
-			struct enc_xform *ealgo;
+			const struct enc_xform *ealgo;
 
-			ealgo = esp_algorithm_lookup(i);
+			ealgo = enc_algorithm_lookup(i);
 			if (!ealgo)
 				continue;
 			alg = (struct sadb_alg *)(mtod(n, caddr_t) + off);
 			alg->sadb_alg_id = i;
-			alg->sadb_alg_ivlen = ealgo->blocksize;
+			alg->sadb_alg_ivlen = ealgo->ivsize;
 			alg->sadb_alg_minbits = _BITS(ealgo->minkey);
 			alg->sadb_alg_maxbits = _BITS(ealgo->maxkey);
 			off += PFKEY_ALIGN8(sizeof(struct sadb_alg));
@@ -6911,21 +7139,21 @@ key_freereg(struct socket *so)
  *	others	: error number
  */
 static int
-key_expire(struct secasvar *sav)
+key_expire(struct secasvar *sav, int hard)
 {
-	int s;
-	int satype;
 	struct mbuf *result = NULL, *m;
-	int len;
-	int error = -1;
 	struct sadb_lifetime *lt;
-
-	/* XXX: Why do we lock ? */
-	s = splnet();	/*called from softclock()*/
+	uint32_t replay_count;
+	int error, len;
+	uint8_t satype;
 
 	IPSEC_ASSERT (sav != NULL, ("null sav"));
 	IPSEC_ASSERT (sav->sah != NULL, ("null sa header"));
 
+	KEYDBG(KEY_STAMP,
+	    printf("%s: SA(%p) expired %s lifetime\n", __func__,
+		sav, hard ? "hard": "soft"));
+	KEYDBG(KEY_DATA, kdebug_secasv(sav));
 	/* set msg header */
 	satype = key_proto2satype(sav->sah->saidx.proto);
 	IPSEC_ASSERT(satype != 0, ("invalid proto, satype %u", satype));
@@ -6945,8 +7173,11 @@ key_expire(struct secasvar *sav)
 	m_cat(result, m);
 
 	/* create SA extension */
-	m = key_setsadbxsa2(sav->sah->saidx.mode,
-			sav->replay ? sav->replay->count : 0,
+	SECASVAR_LOCK(sav);
+	replay_count = sav->replay ? sav->replay->count : 0;
+	SECASVAR_UNLOCK(sav);
+
+	m = key_setsadbxsa2(sav->sah->saidx.mode, replay_count,
 			sav->sah->saidx.reqid);
 	if (!m) {
 		error = ENOBUFS;
@@ -6954,30 +7185,49 @@ key_expire(struct secasvar *sav)
 	}
 	m_cat(result, m);
 
+	if (sav->replay && sav->replay->wsize > UINT8_MAX) {
+		m = key_setsadbxsareplay(sav->replay->wsize);
+		if (!m) {
+			error = ENOBUFS;
+			goto fail;
+		}
+		m_cat(result, m);
+	}
+
 	/* create lifetime extension (current and soft) */
 	len = PFKEY_ALIGN8(sizeof(*lt)) * 2;
-	m = key_alloc_mbuf(len);
-	if (!m || m->m_next) {	/*XXX*/
-		if (m)
-			m_freem(m);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
+	if (m == NULL) {
 		error = ENOBUFS;
 		goto fail;
 	}
+	m_align(m, len);
+	m->m_len = len;
 	bzero(mtod(m, caddr_t), len);
 	lt = mtod(m, struct sadb_lifetime *);
 	lt->sadb_lifetime_len = PFKEY_UNIT64(sizeof(struct sadb_lifetime));
 	lt->sadb_lifetime_exttype = SADB_EXT_LIFETIME_CURRENT;
-	lt->sadb_lifetime_allocations = sav->lft_c->allocations;
-	lt->sadb_lifetime_bytes = sav->lft_c->bytes;
-	lt->sadb_lifetime_addtime = sav->lft_c->addtime;
-	lt->sadb_lifetime_usetime = sav->lft_c->usetime;
+	lt->sadb_lifetime_allocations =
+	    (uint32_t)counter_u64_fetch(sav->lft_c_allocations);
+	lt->sadb_lifetime_bytes =
+	    counter_u64_fetch(sav->lft_c_bytes);
+	lt->sadb_lifetime_addtime = sav->created;
+	lt->sadb_lifetime_usetime = sav->firstused;
 	lt = (struct sadb_lifetime *)(mtod(m, caddr_t) + len / 2);
 	lt->sadb_lifetime_len = PFKEY_UNIT64(sizeof(struct sadb_lifetime));
-	lt->sadb_lifetime_exttype = SADB_EXT_LIFETIME_SOFT;
-	lt->sadb_lifetime_allocations = sav->lft_s->allocations;
-	lt->sadb_lifetime_bytes = sav->lft_s->bytes;
-	lt->sadb_lifetime_addtime = sav->lft_s->addtime;
-	lt->sadb_lifetime_usetime = sav->lft_s->usetime;
+	if (hard) {
+		lt->sadb_lifetime_exttype = SADB_EXT_LIFETIME_HARD;
+		lt->sadb_lifetime_allocations = sav->lft_h->allocations;
+		lt->sadb_lifetime_bytes = sav->lft_h->bytes;
+		lt->sadb_lifetime_addtime = sav->lft_h->addtime;
+		lt->sadb_lifetime_usetime = sav->lft_h->usetime;
+	} else {
+		lt->sadb_lifetime_exttype = SADB_EXT_LIFETIME_SOFT;
+		lt->sadb_lifetime_allocations = sav->lft_s->allocations;
+		lt->sadb_lifetime_bytes = sav->lft_s->bytes;
+		lt->sadb_lifetime_addtime = sav->lft_s->addtime;
+		lt->sadb_lifetime_usetime = sav->lft_s->usetime;
+	}
 	m_cat(result, m);
 
 	/* set sadb_address for source */
@@ -7002,6 +7252,8 @@ key_expire(struct secasvar *sav)
 
 	/*
 	 * XXX-BZ Handle NAT-T extensions here.
+	 * XXXAE: it doesn't seem quite useful. IKEs should not depend on
+	 * this information, we report only significant SA fields.
 	 */
 
 	if ((result->m_flags & M_PKTHDR) == 0) {
@@ -7024,16 +7276,44 @@ key_expire(struct secasvar *sav)
 	mtod(result, struct sadb_msg *)->sadb_msg_len =
 	    PFKEY_UNIT64(result->m_pkthdr.len);
 
-	splx(s);
 	return key_sendup_mbuf(NULL, result, KEY_SENDUP_REGISTERED);
 
  fail:
 	if (result)
 		m_freem(result);
-	splx(s);
 	return error;
 }
 
+static void
+key_freesah_flushed(struct secashead_queue *flushq)
+{
+	struct secashead *sah, *nextsah;
+	struct secasvar *sav, *nextsav;
+
+	sah = TAILQ_FIRST(flushq);
+	while (sah != NULL) {
+		sav = TAILQ_FIRST(&sah->savtree_larval);
+		while (sav != NULL) {
+			nextsav = TAILQ_NEXT(sav, chain);
+			TAILQ_REMOVE(&sah->savtree_larval, sav, chain);
+			key_freesav(&sav); /* release last reference */
+			key_freesah(&sah); /* release reference from SAV */
+			sav = nextsav;
+		}
+		sav = TAILQ_FIRST(&sah->savtree_alive);
+		while (sav != NULL) {
+			nextsav = TAILQ_NEXT(sav, chain);
+			TAILQ_REMOVE(&sah->savtree_alive, sav, chain);
+			key_freesav(&sav); /* release last reference */
+			key_freesah(&sah); /* release reference from SAV */
+			sav = nextsav;
+		}
+		nextsah = TAILQ_NEXT(sah, chain);
+		key_freesah(&sah);	/* release last reference */
+		sah = nextsah;
+	}
+}
+
 /*
  * SADB_FLUSH processing
  * receive
@@ -7047,17 +7327,14 @@ key_expire(struct secasvar *sav)
  * m will always be freed.
  */
 static int
-key_flush(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_flush(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
+	struct secashead_queue flushq;
 	struct sadb_msg *newmsg;
 	struct secashead *sah, *nextsah;
-	struct secasvar *sav, *nextsav;
-	u_int16_t proto;
-	u_int8_t state;
-	u_int stateidx;
+	struct secasvar *sav;
+	uint8_t proto;
+	int i;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(mhp != NULL, ("null msghdr"));
@@ -7069,37 +7346,71 @@ key_flush(so, m, mhp)
 			__func__));
 		return key_senderror(so, m, EINVAL);
 	}
-
-	/* no SATYPE specified, i.e. flushing all SA. */
-	SAHTREE_LOCK();
-	for (sah = LIST_FIRST(&V_sahtree);
-	     sah != NULL;
-	     sah = nextsah) {
-		nextsah = LIST_NEXT(sah, chain);
-
-		if (mhp->msg->sadb_msg_satype != SADB_SATYPE_UNSPEC
-		 && proto != sah->saidx.proto)
-			continue;
-
-		for (stateidx = 0;
-		     stateidx < _ARRAYLEN(saorder_state_alive);
-		     stateidx++) {
-			state = saorder_state_any[stateidx];
-			for (sav = LIST_FIRST(&sah->savtree[state]);
-			     sav != NULL;
-			     sav = nextsav) {
-
-				nextsav = LIST_NEXT(sav, chain);
-
-				key_sa_chgstate(sav, SADB_SASTATE_DEAD);
-				KEY_FREESAV(&sav);
+	KEYDBG(KEY_STAMP,
+	    printf("%s: proto %u\n", __func__, proto));
+
+	TAILQ_INIT(&flushq);
+	if (proto == IPSEC_PROTO_ANY) {
+		/* no SATYPE specified, i.e. flushing all SA. */
+		SAHTREE_WLOCK();
+		/* Move all SAHs into flushq */
+		TAILQ_CONCAT(&flushq, &V_sahtree, chain);
+		/* Flush all buckets in SPI hash */
+		for (i = 0; i < V_savhash_mask + 1; i++)
+			LIST_INIT(&V_savhashtbl[i]);
+		/* Flush all buckets in SAHADDRHASH */
+		for (i = 0; i < V_sahaddrhash_mask + 1; i++)
+			LIST_INIT(&V_sahaddrhashtbl[i]);
+		/* Mark all SAHs as unlinked */
+		TAILQ_FOREACH(sah, &flushq, chain) {
+			sah->state = SADB_SASTATE_DEAD;
+			/*
+			 * Callout handler makes its job using
+			 * RLOCK and drain queues. In case, when this
+			 * function will be called just before it
+			 * acquires WLOCK, we need to mark SAs as
+			 * unlinked to prevent second unlink.
+			 */
+			TAILQ_FOREACH(sav, &sah->savtree_larval, chain) {
+				sav->state = SADB_SASTATE_DEAD;
+			}
+			TAILQ_FOREACH(sav, &sah->savtree_alive, chain) {
+				sav->state = SADB_SASTATE_DEAD;
 			}
 		}
-
-		sah->state = SADB_SASTATE_DEAD;
+		SAHTREE_WUNLOCK();
+	} else {
+		SAHTREE_WLOCK();
+		sah = TAILQ_FIRST(&V_sahtree);
+		while (sah != NULL) {
+			IPSEC_ASSERT(sah->state != SADB_SASTATE_DEAD,
+			    ("DEAD SAH %p in SADB_FLUSH", sah));
+			nextsah = TAILQ_NEXT(sah, chain);
+			if (sah->saidx.proto != proto) {
+				sah = nextsah;
+				continue;
+			}
+			sah->state = SADB_SASTATE_DEAD;
+			TAILQ_REMOVE(&V_sahtree, sah, chain);
+			LIST_REMOVE(sah, addrhash);
+			/* Unlink all SAs from SPI hash */
+			TAILQ_FOREACH(sav, &sah->savtree_larval, chain) {
+				LIST_REMOVE(sav, spihash);
+				sav->state = SADB_SASTATE_DEAD;
+			}
+			TAILQ_FOREACH(sav, &sah->savtree_alive, chain) {
+				LIST_REMOVE(sav, spihash);
+				sav->state = SADB_SASTATE_DEAD;
+			}
+			/* Add SAH into flushq */
+			TAILQ_INSERT_HEAD(&flushq, sah, chain);
+			sah = nextsah;
+		}
+		SAHTREE_WUNLOCK();
 	}
-	SAHTREE_UNLOCK();
 
+	key_freesah_flushed(&flushq);
+	/* Free all queued SAs and SAHs */
 	if (m->m_len < sizeof(struct sadb_msg) ||
 	    sizeof(struct sadb_msg) > m->m_len + M_TRAILINGSPACE(m)) {
 		ipseclog((LOG_DEBUG, "%s: No more memory.\n", __func__));
@@ -7130,20 +7441,15 @@ key_flush(so, m, mhp)
  * m will always be freed.
  */
 static int
-key_dump(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_dump(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
+	SAHTREE_RLOCK_TRACKER;
 	struct secashead *sah;
 	struct secasvar *sav;
-	u_int16_t proto;
-	u_int stateidx;
-	u_int8_t satype;
-	u_int8_t state;
-	int cnt;
 	struct sadb_msg *newmsg;
 	struct mbuf *n;
+	uint32_t cnt;
+	uint8_t proto, satype;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
@@ -7153,79 +7459,73 @@ key_dump(so, m, mhp)
 	/* map satype to proto */
 	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
 		ipseclog((LOG_DEBUG, "%s: invalid satype is passed.\n",
-			__func__));
+		    __func__));
 		return key_senderror(so, m, EINVAL);
 	}
 
 	/* count sav entries to be sent to the userland. */
 	cnt = 0;
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (mhp->msg->sadb_msg_satype != SADB_SATYPE_UNSPEC
-		 && proto != sah->saidx.proto)
+	SAHTREE_RLOCK();
+	TAILQ_FOREACH(sah, &V_sahtree, chain) {
+		if (mhp->msg->sadb_msg_satype != SADB_SATYPE_UNSPEC &&
+		    proto != sah->saidx.proto)
 			continue;
 
-		for (stateidx = 0;
-		     stateidx < _ARRAYLEN(saorder_state_any);
-		     stateidx++) {
-			state = saorder_state_any[stateidx];
-			LIST_FOREACH(sav, &sah->savtree[state], chain) {
-				cnt++;
-			}
-		}
+		TAILQ_FOREACH(sav, &sah->savtree_larval, chain)
+			cnt++;
+		TAILQ_FOREACH(sav, &sah->savtree_alive, chain)
+			cnt++;
 	}
 
 	if (cnt == 0) {
-		SAHTREE_UNLOCK();
+		SAHTREE_RUNLOCK();
 		return key_senderror(so, m, ENOENT);
 	}
 
 	/* send this to the userland, one at a time. */
 	newmsg = NULL;
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		if (mhp->msg->sadb_msg_satype != SADB_SATYPE_UNSPEC
-		 && proto != sah->saidx.proto)
+	TAILQ_FOREACH(sah, &V_sahtree, chain) {
+		if (mhp->msg->sadb_msg_satype != SADB_SATYPE_UNSPEC &&
+		    proto != sah->saidx.proto)
 			continue;
 
 		/* map proto to satype */
 		if ((satype = key_proto2satype(sah->saidx.proto)) == 0) {
-			SAHTREE_UNLOCK();
+			SAHTREE_RUNLOCK();
 			ipseclog((LOG_DEBUG, "%s: there was invalid proto in "
-				"SAD.\n", __func__));
+			    "SAD.\n", __func__));
 			return key_senderror(so, m, EINVAL);
 		}
-
-		for (stateidx = 0;
-		     stateidx < _ARRAYLEN(saorder_state_any);
-		     stateidx++) {
-			state = saorder_state_any[stateidx];
-			LIST_FOREACH(sav, &sah->savtree[state], chain) {
-				n = key_setdumpsa(sav, SADB_DUMP, satype,
-				    --cnt, mhp->msg->sadb_msg_pid);
-				if (!n) {
-					SAHTREE_UNLOCK();
-					return key_senderror(so, m, ENOBUFS);
-				}
-				key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+		TAILQ_FOREACH(sav, &sah->savtree_larval, chain) {
+			n = key_setdumpsa(sav, SADB_DUMP, satype,
+			    --cnt, mhp->msg->sadb_msg_pid);
+			if (n == NULL) {
+				SAHTREE_RUNLOCK();
+				return key_senderror(so, m, ENOBUFS);
+			}
+			key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+		}
+		TAILQ_FOREACH(sav, &sah->savtree_alive, chain) {
+			n = key_setdumpsa(sav, SADB_DUMP, satype,
+			    --cnt, mhp->msg->sadb_msg_pid);
+			if (n == NULL) {
+				SAHTREE_RUNLOCK();
+				return key_senderror(so, m, ENOBUFS);
 			}
+			key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
 		}
 	}
-	SAHTREE_UNLOCK();
-
+	SAHTREE_RUNLOCK();
 	m_freem(m);
-	return 0;
+	return (0);
 }
-
 /*
  * SADB_X_PROMISC processing
  *
  * m will always be freed.
  */
 static int
-key_promisc(so, m, mhp)
-	struct socket *so;
-	struct mbuf *m;
-	const struct sadb_msghdr *mhp;
+key_promisc(struct socket *so, struct mbuf *m, const struct sadb_msghdr *mhp)
 {
 	int olen;
 
@@ -7272,8 +7572,8 @@ key_promisc(so, m, mhp)
 	}
 }
 
-static int (*key_typesw[]) __P((struct socket *, struct mbuf *,
-		const struct sadb_msghdr *)) = {
+static int (*key_typesw[])(struct socket *, struct mbuf *,
+		const struct sadb_msghdr *) = {
 	NULL,		/* SADB_RESERVED */
 	key_getspi,	/* SADB_GETSPI */
 	key_update,	/* SADB_UPDATE */
@@ -7311,9 +7611,7 @@ static int (*key_typesw[]) __P((struct socket *, struct mbuf *,
  *    length for buffer to send to user process.
  */
 int
-key_parse(m, so)
-	struct mbuf *m;
-	struct socket *so;
+key_parse(struct mbuf *m, struct socket *so)
 {
 	struct sadb_msg *msg;
 	struct sadb_msghdr mh;
@@ -7324,12 +7622,6 @@ key_parse(m, so)
 	IPSEC_ASSERT(so != NULL, ("null socket"));
 	IPSEC_ASSERT(m != NULL, ("null mbuf"));
 
-#if 0	/*kdebug_sadb assumes msg in linear buffer*/
-	KEYDEBUG(KEYDEBUG_KEY_DUMP,
-		ipseclog((LOG_DEBUG, "%s: passed sadb_msg\n", __func__));
-		kdebug_sadb(msg));
-#endif
-
 	if (m->m_len < sizeof(struct sadb_msg)) {
 		m = m_pullup(m, sizeof(struct sadb_msg));
 		if (!m)
@@ -7339,8 +7631,7 @@ key_parse(m, so)
 	orglen = PFKEY_UNUNIT64(msg->sadb_msg_len);
 	target = KEY_SENDUP_ONE;
 
-	if ((m->m_flags & M_PKTHDR) == 0 ||
-	    m->m_pkthdr.len != m->m_pkthdr.len) {
+	if ((m->m_flags & M_PKTHDR) == 0 || m->m_pkthdr.len != orglen) {
 		ipseclog((LOG_DEBUG, "%s: invalid message length.\n",__func__));
 		PFKEYSTAT_INC(out_invlen);
 		error = EINVAL;
@@ -7371,10 +7662,9 @@ key_parse(m, so)
 	if (m->m_next) {
 		struct mbuf *n;
 
-		MGETHDR(n, M_DONTWAIT, MT_DATA);
+		MGETHDR(n, M_NOWAIT, MT_DATA);
 		if (n && m->m_pkthdr.len > MHLEN) {
-			MCLGET(n, M_DONTWAIT);
-			if ((n->m_flags & M_EXT) == 0) {
+			if (!(MCLGET(n, M_NOWAIT))) {
 				m_free(n);
 				n = NULL;
 			}
@@ -7397,64 +7687,79 @@ key_parse(m, so)
 
 	msg = mh.msg;
 
-	/* check SA type */
-	switch (msg->sadb_msg_satype) {
-	case SADB_SATYPE_UNSPEC:
-		switch (msg->sadb_msg_type) {
-		case SADB_GETSPI:
-		case SADB_UPDATE:
-		case SADB_ADD:
-		case SADB_DELETE:
-		case SADB_GET:
-		case SADB_ACQUIRE:
-		case SADB_EXPIRE:
-			ipseclog((LOG_DEBUG, "%s: must specify satype "
-			    "when msg type=%u.\n", __func__,
-			    msg->sadb_msg_type));
+	/* We use satype as scope mask for spddump */
+	if (msg->sadb_msg_type == SADB_X_SPDDUMP) {
+		switch (msg->sadb_msg_satype) {
+		case IPSEC_POLICYSCOPE_ANY:
+		case IPSEC_POLICYSCOPE_GLOBAL:
+		case IPSEC_POLICYSCOPE_IFNET:
+		case IPSEC_POLICYSCOPE_PCB:
+			break;
+		default:
+			ipseclog((LOG_DEBUG, "%s: illegal satype=%u\n",
+			    __func__, msg->sadb_msg_type));
 			PFKEYSTAT_INC(out_invsatype);
 			error = EINVAL;
 			goto senderror;
 		}
-		break;
-	case SADB_SATYPE_AH:
-	case SADB_SATYPE_ESP:
-	case SADB_X_SATYPE_IPCOMP:
-	case SADB_X_SATYPE_TCPSIGNATURE:
-		switch (msg->sadb_msg_type) {
-		case SADB_X_SPDADD:
-		case SADB_X_SPDDELETE:
-		case SADB_X_SPDGET:
-		case SADB_X_SPDDUMP:
-		case SADB_X_SPDFLUSH:
-		case SADB_X_SPDSETIDX:
-		case SADB_X_SPDUPDATE:
-		case SADB_X_SPDDELETE2:
-			ipseclog((LOG_DEBUG, "%s: illegal satype=%u\n",
-				__func__, msg->sadb_msg_type));
+	} else {
+		switch (msg->sadb_msg_satype) { /* check SA type */
+		case SADB_SATYPE_UNSPEC:
+			switch (msg->sadb_msg_type) {
+			case SADB_GETSPI:
+			case SADB_UPDATE:
+			case SADB_ADD:
+			case SADB_DELETE:
+			case SADB_GET:
+			case SADB_ACQUIRE:
+			case SADB_EXPIRE:
+				ipseclog((LOG_DEBUG, "%s: must specify satype "
+				    "when msg type=%u.\n", __func__,
+				    msg->sadb_msg_type));
+				PFKEYSTAT_INC(out_invsatype);
+				error = EINVAL;
+				goto senderror;
+			}
+			break;
+		case SADB_SATYPE_AH:
+		case SADB_SATYPE_ESP:
+		case SADB_X_SATYPE_IPCOMP:
+		case SADB_X_SATYPE_TCPSIGNATURE:
+			switch (msg->sadb_msg_type) {
+			case SADB_X_SPDADD:
+			case SADB_X_SPDDELETE:
+			case SADB_X_SPDGET:
+			case SADB_X_SPDFLUSH:
+			case SADB_X_SPDSETIDX:
+			case SADB_X_SPDUPDATE:
+			case SADB_X_SPDDELETE2:
+				ipseclog((LOG_DEBUG, "%s: illegal satype=%u\n",
+				    __func__, msg->sadb_msg_type));
+				PFKEYSTAT_INC(out_invsatype);
+				error = EINVAL;
+				goto senderror;
+			}
+			break;
+		case SADB_SATYPE_RSVP:
+		case SADB_SATYPE_OSPFV2:
+		case SADB_SATYPE_RIPV2:
+		case SADB_SATYPE_MIP:
+			ipseclog((LOG_DEBUG, "%s: type %u isn't supported.\n",
+			    __func__, msg->sadb_msg_satype));
+			PFKEYSTAT_INC(out_invsatype);
+			error = EOPNOTSUPP;
+			goto senderror;
+		case 1:	/* XXX: What does it do? */
+			if (msg->sadb_msg_type == SADB_X_PROMISC)
+				break;
+			/*FALLTHROUGH*/
+		default:
+			ipseclog((LOG_DEBUG, "%s: invalid type %u is passed.\n",
+			    __func__, msg->sadb_msg_satype));
 			PFKEYSTAT_INC(out_invsatype);
 			error = EINVAL;
 			goto senderror;
 		}
-		break;
-	case SADB_SATYPE_RSVP:
-	case SADB_SATYPE_OSPFV2:
-	case SADB_SATYPE_RIPV2:
-	case SADB_SATYPE_MIP:
-		ipseclog((LOG_DEBUG, "%s: type %u isn't supported.\n",
-			__func__, msg->sadb_msg_satype));
-		PFKEYSTAT_INC(out_invsatype);
-		error = EOPNOTSUPP;
-		goto senderror;
-	case 1:	/* XXX: What does it do? */
-		if (msg->sadb_msg_type == SADB_X_PROMISC)
-			break;
-		/*FALLTHROUGH*/
-	default:
-		ipseclog((LOG_DEBUG, "%s: invalid type %u is passed.\n",
-			__func__, msg->sadb_msg_satype));
-		PFKEYSTAT_INC(out_invsatype);
-		error = EINVAL;
-		goto senderror;
 	}
 
 	/* check field of upper layer protocol and address family */
@@ -7546,7 +7851,7 @@ key_parse(m, so)
 		 */
 	}
 
-	if (msg->sadb_msg_type >= sizeof(key_typesw)/sizeof(key_typesw[0]) ||
+	if (msg->sadb_msg_type >= nitems(key_typesw) ||
 	    key_typesw[msg->sadb_msg_type] == NULL) {
 		PFKEYSTAT_INC(out_invmsgtype);
 		error = EINVAL;
@@ -7561,10 +7866,7 @@ senderror:
 }
 
 static int
-key_senderror(so, m, code)
-	struct socket *so;
-	struct mbuf *m;
-	int code;
+key_senderror(struct socket *so, struct mbuf *m, int code)
 {
 	struct sadb_msg *msg;
 
@@ -7582,9 +7884,7 @@ key_senderror(so, m, code)
  * XXX larger-than-MCLBYTES extension?
  */
 static int
-key_align(m, mhp)
-	struct mbuf *m;
-	struct sadb_msghdr *mhp;
+key_align(struct mbuf *m, struct sadb_msghdr *mhp)
 {
 	struct mbuf *n;
 	struct sadb_ext *ext;
@@ -7633,14 +7933,15 @@ key_align(m, mhp)
 		case SADB_EXT_SPIRANGE:
 		case SADB_X_EXT_POLICY:
 		case SADB_X_EXT_SA2:
-#ifdef IPSEC_NAT_T
 		case SADB_X_EXT_NAT_T_TYPE:
 		case SADB_X_EXT_NAT_T_SPORT:
 		case SADB_X_EXT_NAT_T_DPORT:
 		case SADB_X_EXT_NAT_T_OAI:
 		case SADB_X_EXT_NAT_T_OAR:
 		case SADB_X_EXT_NAT_T_FRAG:
-#endif
+		case SADB_X_EXT_SA_REPLAY:
+		case SADB_X_EXT_NEW_ADDRESS_SRC:
+		case SADB_X_EXT_NEW_ADDRESS_DST:
 			/* duplicate check */
 			/*
 			 * XXX Are there duplication payloads of either
@@ -7692,9 +7993,7 @@ key_align(m, mhp)
 }
 
 static int
-key_validate_ext(ext, len)
-	const struct sadb_ext *ext;
-	int len;
+key_validate_ext(const struct sadb_ext *ext, int len)
 {
 	const struct sockaddr *sa;
 	enum { NONE, ADDR } checktype = NONE;
@@ -7705,8 +8004,8 @@ key_validate_ext(ext, len)
 		return EINVAL;
 
 	/* if it does not match minimum/maximum length, bail */
-	if (ext->sadb_ext_type >= sizeof(minsize) / sizeof(minsize[0]) ||
-	    ext->sadb_ext_type >= sizeof(maxsize) / sizeof(maxsize[0]))
+	if (ext->sadb_ext_type >= nitems(minsize) ||
+	    ext->sadb_ext_type >= nitems(maxsize))
 		return EINVAL;
 	if (!minsize[ext->sadb_ext_type] || len < minsize[ext->sadb_ext_type])
 		return EINVAL;
@@ -7718,6 +8017,10 @@ key_validate_ext(ext, len)
 	case SADB_EXT_ADDRESS_SRC:
 	case SADB_EXT_ADDRESS_DST:
 	case SADB_EXT_ADDRESS_PROXY:
+	case SADB_X_EXT_NAT_T_OAI:
+	case SADB_X_EXT_NAT_T_OAR:
+	case SADB_X_EXT_NEW_ADDRESS_SRC:
+	case SADB_X_EXT_NEW_ADDRESS_DST:
 		baselen = PFKEY_ALIGN8(sizeof(struct sadb_address));
 		checktype = ADDR;
 		break;
@@ -7755,10 +8058,24 @@ key_init(void)
 {
 	int i;
 
-	for (i = 0; i < IPSEC_DIR_MAX; i++)
-		LIST_INIT(&V_sptree[i]);
+	for (i = 0; i < IPSEC_DIR_MAX; i++) {
+		TAILQ_INIT(&V_sptree[i]);
+		TAILQ_INIT(&V_sptree_ifnet[i]);
+	}
+
+	V_key_lft_zone = uma_zcreate("IPsec SA lft_c",
+	    sizeof(uint64_t) * 2, NULL, NULL, NULL, NULL,
+	    UMA_ALIGN_PTR, UMA_ZONE_PCPU);
 
-	LIST_INIT(&V_sahtree);
+	TAILQ_INIT(&V_sahtree);
+	V_sphashtbl = hashinit(SPHASH_NHASH, M_IPSEC_SP, &V_sphash_mask);
+	V_savhashtbl = hashinit(SAVHASH_NHASH, M_IPSEC_SA, &V_savhash_mask);
+	V_sahaddrhashtbl = hashinit(SAHHASH_NHASH, M_IPSEC_SAH,
+	    &V_sahaddrhash_mask);
+	V_acqaddrhashtbl = hashinit(ACQHASH_NHASH, M_IPSEC_SAQ,
+	    &V_acqaddrhash_mask);
+	V_acqseqhashtbl = hashinit(ACQHASH_NHASH, M_IPSEC_SAQ,
+	    &V_acqseqhash_mask);
 
 	for (i = 0; i <= SADB_SATYPE_MAX; i++)
 		LIST_INIT(&V_regtree[i]);
@@ -7766,13 +8083,10 @@ key_init(void)
 	LIST_INIT(&V_acqtree);
 	LIST_INIT(&V_spacqtree);
 
-	/* system default */
-	V_ip4_def_policy.policy = IPSEC_POLICY_NONE;
-	V_ip4_def_policy.refcnt++;	/*never reclaim this*/
-
 	if (!IS_DEFAULT_VNET(curvnet))
 		return;
 
+	XFORMS_LOCK_INIT();
 	SPTREE_LOCK_INIT();
 	REGTREE_LOCK_INIT();
 	SAHTREE_LOCK_INIT();
@@ -7780,48 +8094,71 @@ key_init(void)
 	SPACQ_LOCK_INIT();
 
 #ifndef IPSEC_DEBUG2
-	timeout((void *)key_timehandler, (void *)0, hz);
+	callout_init(&key_timer, 1);
+	callout_reset(&key_timer, hz, key_timehandler, NULL);
 #endif /*IPSEC_DEBUG2*/
 
 	/* initialize key statistics */
 	keystat.getspi_count = 1;
 
-	printf("IPsec: Initialized Security Association Processing.\n");
+	if (bootverbose)
+		printf("IPsec: Initialized Security Association Processing.\n");
 }
 
 #ifdef VIMAGE
 void
 key_destroy(void)
 {
+	struct secashead_queue sahdrainq;
+	struct secpolicy_queue drainq;
 	struct secpolicy *sp, *nextsp;
 	struct secacq *acq, *nextacq;
 	struct secspacq *spacq, *nextspacq;
-	struct secashead *sah, *nextsah;
+	struct secashead *sah;
+	struct secasvar *sav;
 	struct secreg *reg;
 	int i;
 
-	SPTREE_LOCK();
+	/*
+	 * XXX: can we just call free() for each object without
+	 * walking through safe way with releasing references?
+	 */
+	TAILQ_INIT(&drainq);
+	SPTREE_WLOCK();
 	for (i = 0; i < IPSEC_DIR_MAX; i++) {
-		for (sp = LIST_FIRST(&V_sptree[i]); 
-		    sp != NULL; sp = nextsp) {
-			nextsp = LIST_NEXT(sp, chain);
-			if (__LIST_CHAINED(sp)) {
-				LIST_REMOVE(sp, chain);
-				free(sp, M_IPSEC_SP);
-			}
+		TAILQ_CONCAT(&drainq, &V_sptree[i], chain);
+		TAILQ_CONCAT(&drainq, &V_sptree_ifnet[i], chain);
+	}
+	SPTREE_WUNLOCK();
+	sp = TAILQ_FIRST(&drainq);
+	while (sp != NULL) {
+		nextsp = TAILQ_NEXT(sp, chain);
+		key_freesp(&sp);
+		sp = nextsp;
+	}
+
+	TAILQ_INIT(&sahdrainq);
+	SAHTREE_WLOCK();
+	TAILQ_CONCAT(&sahdrainq, &V_sahtree, chain);
+	for (i = 0; i < V_savhash_mask + 1; i++)
+		LIST_INIT(&V_savhashtbl[i]);
+	for (i = 0; i < V_sahaddrhash_mask + 1; i++)
+		LIST_INIT(&V_sahaddrhashtbl[i]);
+	TAILQ_FOREACH(sah, &sahdrainq, chain) {
+		sah->state = SADB_SASTATE_DEAD;
+		TAILQ_FOREACH(sav, &sah->savtree_larval, chain) {
+			sav->state = SADB_SASTATE_DEAD;
 		}
-	}
-	SPTREE_UNLOCK();
-
-	SAHTREE_LOCK();
-	for (sah = LIST_FIRST(&V_sahtree); sah != NULL; sah = nextsah) {
-		nextsah = LIST_NEXT(sah, chain);
-		if (__LIST_CHAINED(sah)) {
-			LIST_REMOVE(sah, chain);
-			free(sah, M_IPSEC_SAH);
+		TAILQ_FOREACH(sav, &sah->savtree_alive, chain) {
+			sav->state = SADB_SASTATE_DEAD;
 		}
 	}
-	SAHTREE_UNLOCK();
+	SAHTREE_WUNLOCK();
+
+	key_freesah_flushed(&sahdrainq);
+	hashdestroy(V_sphashtbl, M_IPSEC_SP, V_sphash_mask);
+	hashdestroy(V_savhashtbl, M_IPSEC_SA, V_savhash_mask);
+	hashdestroy(V_sahaddrhashtbl, M_IPSEC_SAH, V_sahaddrhash_mask);
 
 	REGTREE_LOCK();
 	for (i = 0; i <= SADB_SATYPE_MAX; i++) {
@@ -7836,12 +8173,12 @@ key_destroy(void)
 	REGTREE_UNLOCK();
 
 	ACQ_LOCK();
-	for (acq = LIST_FIRST(&V_acqtree); acq != NULL; acq = nextacq) {
+	acq = LIST_FIRST(&V_acqtree);
+	while (acq != NULL) {
 		nextacq = LIST_NEXT(acq, chain);
-		if (__LIST_CHAINED(acq)) {
-			LIST_REMOVE(acq, chain);
-			free(acq, M_IPSEC_SAQ);
-		}
+		LIST_REMOVE(acq, chain);
+		free(acq, M_IPSEC_SAQ);
+		acq = nextacq;
 	}
 	ACQ_UNLOCK();
 
@@ -7855,56 +8192,31 @@ key_destroy(void)
 		}
 	}
 	SPACQ_UNLOCK();
+	hashdestroy(V_acqaddrhashtbl, M_IPSEC_SAQ, V_acqaddrhash_mask);
+	hashdestroy(V_acqseqhashtbl, M_IPSEC_SAQ, V_acqseqhash_mask);
+	uma_zdestroy(V_key_lft_zone);
 }
 #endif
 
-/*
- * XXX: maybe This function is called after INBOUND IPsec processing.
- *
- * Special check for tunnel-mode packets.
- * We must make some checks for consistency between inner and outer IP header.
- *
- * xxx more checks to be provided
- */
-int
-key_checktunnelsanity(sav, family, src, dst)
-	struct secasvar *sav;
-	u_int family;
-	caddr_t src;
-	caddr_t dst;
-{
-	IPSEC_ASSERT(sav->sah != NULL, ("null SA header"));
-
-	/* XXX: check inner IP header */
-
-	return 1;
-}
-
 /* record data transfer on SA, and update timestamps */
 void
-key_sa_recordxfer(sav, m)
-	struct secasvar *sav;
-	struct mbuf *m;
+key_sa_recordxfer(struct secasvar *sav, struct mbuf *m)
 {
 	IPSEC_ASSERT(sav != NULL, ("Null secasvar"));
 	IPSEC_ASSERT(m != NULL, ("Null mbuf"));
-	if (!sav->lft_c)
-		return;
 
 	/*
 	 * XXX Currently, there is a difference of bytes size
 	 * between inbound and outbound processing.
 	 */
-	sav->lft_c->bytes += m->m_pkthdr.len;
-	/* to check bytes lifetime is done in key_timehandler(). */
+	counter_u64_add(sav->lft_c_bytes, m->m_pkthdr.len);
 
 	/*
 	 * We use the number of packets as the unit of
 	 * allocations.  We increment the variable
 	 * whenever {esp,ah}_{in,out}put is called.
 	 */
-	sav->lft_c->allocations++;
-	/* XXX check for expires? */
+	counter_u64_add(sav->lft_c_allocations, 1);
 
 	/*
 	 * NOTE: We record CURRENT usetime by using wall clock,
@@ -7917,92 +8229,8 @@ key_sa_recordxfer(sav, m)
 	 *	<--------------> HARD
 	 *	<-----> SOFT
 	 */
-	sav->lft_c->usetime = time_second;
-	/* XXX check for expires? */
-
-	return;
-}
-
-/* dumb version */
-void
-key_sa_routechange(dst)
-	struct sockaddr *dst;
-{
-	struct secashead *sah;
-	struct route *ro;
-
-	SAHTREE_LOCK();
-	LIST_FOREACH(sah, &V_sahtree, chain) {
-		ro = &sah->route_cache.sa_route;
-		if (ro->ro_rt && dst->sa_len == ro->ro_dst.sa_len
-		 && bcmp(dst, &ro->ro_dst, dst->sa_len) == 0) {
-			RTFREE(ro->ro_rt);
-			ro->ro_rt = (struct rtentry *)NULL;
-		}
-	}
-	SAHTREE_UNLOCK();
-}
-
-static void
-key_sa_chgstate(struct secasvar *sav, u_int8_t state)
-{
-	IPSEC_ASSERT(sav != NULL, ("NULL sav"));
-	SAHTREE_LOCK_ASSERT();
-
-	if (sav->state != state) {
-		if (__LIST_CHAINED(sav))
-			LIST_REMOVE(sav, chain);
-		sav->state = state;
-		LIST_INSERT_HEAD(&sav->sah->savtree[state], sav, chain);
-	}
-}
-
-void
-key_sa_stir_iv(sav)
-	struct secasvar *sav;
-{
-
-	IPSEC_ASSERT(sav->iv != NULL, ("null IV"));
-	key_randomfill(sav->iv, sav->ivlen);
-}
-
-/* XXX too much? */
-static struct mbuf *
-key_alloc_mbuf(l)
-	int l;
-{
-	struct mbuf *m = NULL, *n;
-	int len, t;
-
-	len = l;
-	while (len > 0) {
-		MGET(n, M_DONTWAIT, MT_DATA);
-		if (n && len > MLEN)
-			MCLGET(n, M_DONTWAIT);
-		if (!n) {
-			m_freem(m);
-			return NULL;
-		}
-
-		n->m_next = NULL;
-		n->m_len = 0;
-		n->m_len = M_TRAILINGSPACE(n);
-		/* use the bottom of mbuf, hoping we can prepend afterwards */
-		if (n->m_len > len) {
-			t = (n->m_len - len) & ~(sizeof(long) - 1);
-			n->m_data += t;
-			n->m_len = len;
-		}
-
-		len -= n->m_len;
-
-		if (m)
-			m_cat(m, n);
-		else
-			m = n;
-	}
-
-	return m;
+	if (sav->firstused == 0)
+		sav->firstused = time_second;
 }
 
 /*
@@ -8019,7 +8247,7 @@ key_alloc_mbuf(l)
  */
 
 static struct mbuf *
-key_setkey(struct seckey *src, u_int16_t exttype) 
+key_setkey(struct seckey *src, uint16_t exttype) 
 {
 	struct mbuf *m;
 	struct sadb_key *p;
@@ -8029,9 +8257,11 @@ key_setkey(struct seckey *src, u_int16_t exttype)
 		return NULL;
 
 	len = PFKEY_ALIGN8(sizeof(struct sadb_key) + _KEYLEN(src));
-	m = key_alloc_mbuf(len);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
 	if (m == NULL)
 		return NULL;
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_key *);
 	bzero(p, len);
 	p->sadb_key_len = PFKEY_UNIT64(len);
@@ -8057,7 +8287,7 @@ key_setkey(struct seckey *src, u_int16_t exttype)
  */
 
 static struct mbuf *
-key_setlifetime(struct seclifetime *src, u_int16_t exttype)
+key_setlifetime(struct seclifetime *src, uint16_t exttype)
 {
 	struct mbuf *m = NULL;
 	struct sadb_lifetime *p;
@@ -8066,9 +8296,11 @@ key_setlifetime(struct seclifetime *src, u_int16_t exttype)
 	if (src == NULL)
 		return NULL;
 
-	m = key_alloc_mbuf(len);
+	m = m_get2(len, M_NOWAIT, MT_DATA, 0);
 	if (m == NULL)
 		return m;
+	m_align(m, len);
+	m->m_len = len;
 	p = mtod(m, struct sadb_lifetime *);
 
 	bzero(p, len);
@@ -8082,3 +8314,104 @@ key_setlifetime(struct seclifetime *src, u_int16_t exttype)
 	return m;
 
 }
+
+const struct enc_xform *
+enc_algorithm_lookup(int alg)
+{
+	int i;
+
+	for (i = 0; i < nitems(supported_ealgs); i++)
+		if (alg == supported_ealgs[i].sadb_alg)
+			return (supported_ealgs[i].xform);
+	return (NULL);
+}
+
+const struct auth_hash *
+auth_algorithm_lookup(int alg)
+{
+	int i;
+
+	for (i = 0; i < nitems(supported_aalgs); i++)
+		if (alg == supported_aalgs[i].sadb_alg)
+			return (supported_aalgs[i].xform);
+	return (NULL);
+}
+
+const struct comp_algo *
+comp_algorithm_lookup(int alg)
+{
+	int i;
+
+	for (i = 0; i < nitems(supported_calgs); i++)
+		if (alg == supported_calgs[i].sadb_alg)
+			return (supported_calgs[i].xform);
+	return (NULL);
+}
+
+/*
+ * Register a transform.
+ */
+static int
+xform_register(struct xformsw* xsp)
+{
+	struct xformsw *entry;
+
+	XFORMS_LOCK();
+	LIST_FOREACH(entry, &xforms, chain) {
+		if (entry->xf_type == xsp->xf_type) {
+			XFORMS_UNLOCK();
+			return (EEXIST);
+		}
+	}
+	LIST_INSERT_HEAD(&xforms, xsp, chain);
+	XFORMS_UNLOCK();
+	return (0);
+}
+
+void
+xform_attach(void *data)
+{
+	struct xformsw *xsp = (struct xformsw *)data;
+
+	if (xform_register(xsp) != 0)
+		printf("%s: failed to register %s xform\n", __func__,
+		    xsp->xf_name);
+}
+
+void
+xform_detach(void *data)
+{
+	struct xformsw *xsp = (struct xformsw *)data;
+
+	XFORMS_LOCK();
+	LIST_REMOVE(xsp, chain);
+	XFORMS_UNLOCK();
+
+	/* Delete all SAs related to this xform. */
+	key_delete_xform(xsp);
+}
+
+/*
+ * Initialize transform support in an sav.
+ */
+static int
+xform_init(struct secasvar *sav, u_short xftype)
+{
+	struct xformsw *entry;
+	int ret;
+
+	IPSEC_ASSERT(sav->tdb_xform == NULL,
+	    ("tdb_xform is already initialized"));
+
+	ret = EINVAL;
+	XFORMS_LOCK();
+	LIST_FOREACH(entry, &xforms, chain) {
+	    if (entry->xf_type == xftype) {
+		    ret = (*entry->xf_init)(sav, entry);
+		    break;
+	    }
+	}
+	XFORMS_UNLOCK();
+	return (ret);
+}
+
diff --git a/freebsd/sys/netipsec/key.h b/freebsd/sys/netipsec/key.h
index f246dbcf..a646832e 100644
--- a/freebsd/sys/netipsec/key.h
+++ b/freebsd/sys/netipsec/key.h
@@ -37,7 +37,6 @@
 
 struct secpolicy;
 struct secpolicyindex;
-struct ipsecrequest;
 struct secasvar;
 struct sockaddr;
 struct socket;
@@ -46,74 +45,44 @@ struct sadb_x_policy;
 struct secasindex;
 union sockaddr_union;
 
-extern void key_addref(struct secpolicy *sp);
-extern	int key_havesp(u_int dir);
-extern struct secpolicy *key_allocsp(struct secpolicyindex *, u_int,
-	const char*, int);
-extern struct secpolicy *key_allocsp2(u_int32_t spi, union sockaddr_union *dst,
-	u_int8_t proto, u_int dir, const char*, int);
-extern struct secpolicy *key_newsp(const char*, int);
-#if 0
-extern struct secpolicy *key_gettunnel(const struct sockaddr *,
-	const struct sockaddr *, const struct sockaddr *,
-	const struct sockaddr *, const char*, int);
-#endif
-/* NB: prepend with _ for KAME IPv6 compatbility */
-extern void _key_freesp(struct secpolicy **, const char*, int);
-
-#define	KEY_ALLOCSP(spidx, dir)					\
-	key_allocsp(spidx, dir, __FILE__, __LINE__)
-#define	KEY_ALLOCSP2(spi, dst, proto, dir)			\
-	key_allocsp2(spi, dst, proto, dir, __FILE__, __LINE__)
-#define	KEY_NEWSP()						\
-	key_newsp(__FILE__, __LINE__)
-#if 0
-#define	KEY_GETTUNNEL(osrc, odst, isrc, idst)			\
-	key_gettunnel(osrc, odst, isrc, idst, __FILE__, __LINE__)
-#endif
-#define	KEY_FREESP(spp)						\
-	_key_freesp(spp, __FILE__, __LINE__)
+struct secpolicy *key_newsp(void);
+struct secpolicy *key_allocsp(struct secpolicyindex *, u_int);
+struct secpolicy *key_msg2sp(struct sadb_x_policy *, size_t, int *);
+int key_sp2msg(struct secpolicy *, void *, size_t *);
+void key_addref(struct secpolicy *);
+void key_freesp(struct secpolicy **);
+int key_spdacquire(struct secpolicy *);
+int key_havesp(u_int);
+void key_bumpspgen(void);
+uint32_t key_getspgen(void);
+uint32_t key_newreqid(void);
 
-extern struct secasvar *key_allocsa(union sockaddr_union *, u_int, u_int32_t,
-	const char*, int);
-extern void key_addrefsa(struct secasvar *, const char*, int);
-extern void key_freesav(struct secasvar **, const char*, int);
+struct secasvar *key_allocsa(union sockaddr_union *, uint8_t, uint32_t);
+struct secasvar *key_allocsa_tunnel(union sockaddr_union *,
+    union sockaddr_union *, uint8_t);
+struct secasvar *key_allocsa_policy(struct secpolicy *,
+    const struct secasindex *, int *);
+struct secasvar *key_allocsa_tcpmd5(struct secasindex *);
+void key_freesav(struct secasvar **);
 
-#define	KEY_ALLOCSA(dst, proto, spi)				\
-	key_allocsa(dst, proto, spi, __FILE__, __LINE__)
-#define	KEY_ADDREFSA(sav)					\
-	key_addrefsa(sav, __FILE__, __LINE__)
-#define	KEY_FREESAV(psav)					\
-	key_freesav(psav, __FILE__, __LINE__)
+int key_sockaddrcmp(const struct sockaddr *, const struct sockaddr *, int);
+int key_sockaddrcmp_withmask(const struct sockaddr *, const struct sockaddr *,
+    size_t);
 
-extern void key_freeso __P((struct socket *));
-extern int key_checktunnelsanity __P((struct secasvar *, u_int,
-					caddr_t, caddr_t));
-extern int key_checkrequest
-	__P((struct ipsecrequest *isr, const struct secasindex *));
+int key_register_ifnet(struct secpolicy **, u_int);
+void key_unregister_ifnet(struct secpolicy **, u_int);
 
-extern struct secpolicy *key_msg2sp __P((struct sadb_x_policy *,
-	size_t, int *));
-extern struct mbuf *key_sp2msg __P((struct secpolicy *));
-extern int key_ismyaddr __P((struct sockaddr *));
-extern int key_spdacquire __P((struct secpolicy *));
-extern void key_timehandler __P((void));
-extern u_long key_random __P((void));
-extern void key_randomfill __P((void *, size_t));
-extern void key_freereg __P((struct socket *));
-extern int key_parse __P((struct mbuf *, struct socket *));
-extern void key_init __P((void));
+extern u_long key_random(void);
+extern void key_randomfill(void *, size_t);
+extern void key_freereg(struct socket *);
+extern int key_parse(struct mbuf *, struct socket *);
+extern void key_init(void);
 #ifdef VIMAGE
 extern void key_destroy(void);
 #endif
-extern void key_sa_recordxfer __P((struct secasvar *, struct mbuf *));
-extern void key_sa_routechange __P((struct sockaddr *));
-extern void key_sa_stir_iv __P((struct secasvar *));
-#ifdef IPSEC_NAT_T
-u_int16_t key_portfromsaddr(struct sockaddr *);
-#define	KEY_PORTFROMSADDR(saddr)				\
-	key_portfromsaddr((struct sockaddr *)(saddr))
-#endif
+extern void key_sa_recordxfer(struct secasvar *, struct mbuf *);
+uint16_t key_portfromsaddr(struct sockaddr *);
+void key_porttosaddr(struct sockaddr *, uint16_t port);
 
 #ifdef MALLOC_DECLARE
 MALLOC_DECLARE(M_IPSEC_SA);
diff --git a/freebsd/sys/netipsec/key_debug.c b/freebsd/sys/netipsec/key_debug.c
index 0b54ffab..51ac1fdc 100644
--- a/freebsd/sys/netipsec/key_debug.c
+++ b/freebsd/sys/netipsec/key_debug.c
@@ -38,16 +38,17 @@
 #include <rtems/bsd/local/opt_ipsec.h>
 #endif
 
-#include <sys/types.h>
 #include <rtems/bsd/sys/param.h>
 #ifdef _KERNEL
 #include <sys/systm.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/malloc.h>
 #include <sys/mbuf.h>
+#include <sys/mutex.h>
 #include <sys/queue.h>
 #endif
 #include <sys/socket.h>
 
-#include <net/route.h>
 #include <net/vnet.h>
 
 #include <netipsec/key_var.h>
@@ -57,6 +58,7 @@
 #include <netipsec/ipsec.h>
 #ifdef _KERNEL
 #include <netipsec/keydb.h>
+#include <netipsec/xform.h>
 #endif
 
 #ifndef _KERNEL
@@ -65,17 +67,17 @@
 #include <stdlib.h>
 #endif /* !_KERNEL */
 
-static void kdebug_sadb_prop __P((struct sadb_ext *));
-static void kdebug_sadb_identity __P((struct sadb_ext *));
-static void kdebug_sadb_supported __P((struct sadb_ext *));
-static void kdebug_sadb_lifetime __P((struct sadb_ext *));
-static void kdebug_sadb_sa __P((struct sadb_ext *));
-static void kdebug_sadb_address __P((struct sadb_ext *));
-static void kdebug_sadb_key __P((struct sadb_ext *));
-static void kdebug_sadb_x_sa2 __P((struct sadb_ext *));
+static void kdebug_sadb_prop(struct sadb_ext *);
+static void kdebug_sadb_identity(struct sadb_ext *);
+static void kdebug_sadb_supported(struct sadb_ext *);
+static void kdebug_sadb_lifetime(struct sadb_ext *);
+static void kdebug_sadb_sa(struct sadb_ext *);
+static void kdebug_sadb_address(struct sadb_ext *);
+static void kdebug_sadb_key(struct sadb_ext *);
+static void kdebug_sadb_x_sa2(struct sadb_ext *);
 
 #ifdef _KERNEL
-static void kdebug_secreplay __P((struct secreplay *));
+static void kdebug_secreplay(struct secreplay *);
 #endif
 
 #ifndef _KERNEL
@@ -86,8 +88,7 @@ static void kdebug_secreplay __P((struct secreplay *));
 
 /* %%%: about struct sadb_msg */
 void
-kdebug_sadb(base)
-	struct sadb_msg *base;
+kdebug_sadb(struct sadb_msg *base)
 {
 	struct sadb_ext *ext;
 	int tlen, extlen;
@@ -175,8 +176,7 @@ kdebug_sadb(base)
 }
 
 static void
-kdebug_sadb_prop(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_prop(struct sadb_ext *ext)
 {
 	struct sadb_prop *prop = (struct sadb_prop *)ext;
 	struct sadb_comb *comb;
@@ -225,8 +225,7 @@ kdebug_sadb_prop(ext)
 }
 
 static void
-kdebug_sadb_identity(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_identity(struct sadb_ext *ext)
 {
 	struct sadb_ident *id = (struct sadb_ident *)ext;
 	int len;
@@ -268,8 +267,7 @@ kdebug_sadb_identity(ext)
 }
 
 static void
-kdebug_sadb_supported(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_supported(struct sadb_ext *ext)
 {
 	struct sadb_supported *sup = (struct sadb_supported *)ext;
 	struct sadb_alg *alg;
@@ -295,8 +293,7 @@ kdebug_sadb_supported(ext)
 }
 
 static void
-kdebug_sadb_lifetime(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_lifetime(struct sadb_ext *ext)
 {
 	struct sadb_lifetime *lft = (struct sadb_lifetime *)ext;
 
@@ -315,8 +312,7 @@ kdebug_sadb_lifetime(ext)
 }
 
 static void
-kdebug_sadb_sa(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_sa(struct sadb_ext *ext)
 {
 	struct sadb_sa *sa = (struct sadb_sa *)ext;
 
@@ -334,8 +330,7 @@ kdebug_sadb_sa(ext)
 }
 
 static void
-kdebug_sadb_address(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_address(struct sadb_ext *ext)
 {
 	struct sadb_address *addr = (struct sadb_address *)ext;
 
@@ -354,8 +349,7 @@ kdebug_sadb_address(ext)
 }
 
 static void
-kdebug_sadb_key(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_key(struct sadb_ext *ext)
 {
 	struct sadb_key *key = (struct sadb_key *)ext;
 
@@ -383,8 +377,7 @@ kdebug_sadb_key(ext)
 }
 
 static void
-kdebug_sadb_x_sa2(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_x_sa2(struct sadb_ext *ext)
 {
 	struct sadb_x_sa2 *sa2 = (struct sadb_x_sa2 *)ext;
 
@@ -402,8 +395,7 @@ kdebug_sadb_x_sa2(ext)
 }
 
 void
-kdebug_sadb_x_policy(ext)
-	struct sadb_ext *ext;
+kdebug_sadb_x_policy(struct sadb_ext *ext)
 {
 	struct sadb_x_policy *xpl = (struct sadb_x_policy *)ext;
 	struct sockaddr *addr;
@@ -469,185 +461,304 @@ kdebug_sadb_x_policy(ext)
 
 #ifdef _KERNEL
 /* %%%: about SPD and SAD */
-void
-kdebug_secpolicy(sp)
-	struct secpolicy *sp;
+const char*
+kdebug_secpolicy_state(u_int state)
 {
-	/* sanity check */
-	if (sp == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
 
-	printf("secpolicy{ refcnt=%u state=%u policy=%u\n",
-		sp->refcnt, sp->state, sp->policy);
+	switch (state) {
+	case IPSEC_SPSTATE_DEAD:
+		return ("dead");
+	case IPSEC_SPSTATE_LARVAL:
+		return ("larval");
+	case IPSEC_SPSTATE_ALIVE:
+		return ("alive");
+	case IPSEC_SPSTATE_PCB:
+		return ("pcb");
+	case IPSEC_SPSTATE_IFNET:
+		return ("ifnet");
+	}
+	return ("unknown");
+}
 
-	kdebug_secpolicyindex(&sp->spidx);
+const char*
+kdebug_secpolicy_policy(u_int policy)
+{
 
-	switch (sp->policy) {
+	switch (policy) {
 	case IPSEC_POLICY_DISCARD:
-		printf("  type=discard }\n");
-		break;
+		return ("discard");
 	case IPSEC_POLICY_NONE:
-		printf("  type=none }\n");
-		break;
+		return ("none");
 	case IPSEC_POLICY_IPSEC:
-	    {
-		struct ipsecrequest *isr;
-		for (isr = sp->req; isr != NULL; isr = isr->next) {
-
-			printf("  level=%u\n", isr->level);
-			kdebug_secasindex(&isr->saidx);
-
-			if (isr->sav != NULL)
-				kdebug_secasv(isr->sav);
-		}
-		printf("  }\n");
-	    }
-		break;
-	case IPSEC_POLICY_BYPASS:
-		printf("  type=bypass }\n");
-		break;
+		return ("ipsec");
 	case IPSEC_POLICY_ENTRUST:
-		printf("  type=entrust }\n");
-		break;
-	default:
-		printf("%s: Invalid policy found. %d\n", __func__, sp->policy);
-		break;
+		return ("entrust");
+	case IPSEC_POLICY_BYPASS:
+		return ("bypass");
 	}
-
-	return;
+	return ("unknown");
 }
 
-void
-kdebug_secpolicyindex(spidx)
-	struct secpolicyindex *spidx;
+const char*
+kdebug_secpolicyindex_dir(u_int dir)
 {
-	/* sanity check */
-	if (spidx == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
 
-	printf("secpolicyindex{ dir=%u prefs=%u prefd=%u ul_proto=%u\n",
-		spidx->dir, spidx->prefs, spidx->prefd, spidx->ul_proto);
+	switch (dir) {
+	case IPSEC_DIR_ANY:
+		return ("any");
+	case IPSEC_DIR_INBOUND:
+		return ("in");
+	case IPSEC_DIR_OUTBOUND:
+		return ("out");
+	}
+	return ("unknown");
+}
 
-	ipsec_hexdump((caddr_t)&spidx->src,
-		((struct sockaddr *)&spidx->src)->sa_len);
-	printf("\n");
-	ipsec_hexdump((caddr_t)&spidx->dst,
-		((struct sockaddr *)&spidx->dst)->sa_len);
-	printf("}\n");
+const char*
+kdebug_ipsecrequest_level(u_int level)
+{
 
-	return;
+	switch (level) {
+	case IPSEC_LEVEL_DEFAULT:
+		return ("default");
+	case IPSEC_LEVEL_USE:
+		return ("use");
+	case IPSEC_LEVEL_REQUIRE:
+		return ("require");
+	case IPSEC_LEVEL_UNIQUE:
+		return ("unique");
+	}
+	return ("unknown");
 }
 
-void
-kdebug_secasindex(saidx)
-	struct secasindex *saidx;
+const char*
+kdebug_secasindex_mode(u_int mode)
 {
-	/* sanity check */
-	if (saidx == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
 
-	printf("secasindex{ mode=%u proto=%u\n",
-		saidx->mode, saidx->proto);
+	switch (mode) {
+	case IPSEC_MODE_ANY:
+		return ("any");
+	case IPSEC_MODE_TRANSPORT:
+		return ("transport");
+	case IPSEC_MODE_TUNNEL:
+		return ("tunnel");
+	case IPSEC_MODE_TCPMD5:
+		return ("tcp-md5");
+	}
+	return ("unknown");
+}
 
-	ipsec_hexdump((caddr_t)&saidx->src,
-		((struct sockaddr *)&saidx->src)->sa_len);
-	printf("\n");
-	ipsec_hexdump((caddr_t)&saidx->dst,
-		((struct sockaddr *)&saidx->dst)->sa_len);
-	printf("\n");
+const char*
+kdebug_secasv_state(u_int state)
+{
 
-	return;
+	switch (state) {
+	case SADB_SASTATE_LARVAL:
+		return ("larval");
+	case SADB_SASTATE_MATURE:
+		return ("mature");
+	case SADB_SASTATE_DYING:
+		return ("dying");
+	case SADB_SASTATE_DEAD:
+		return ("dead");
+	}
+	return ("unknown");
 }
 
-static void
-kdebug_sec_lifetime(struct seclifetime *lft)
+static char*
+kdebug_port2str(const struct sockaddr *sa, char *buf, size_t len)
 {
-	/* sanity check */
-	if (lft == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
-
-	printf("sec_lifetime{ alloc=%u, bytes=%u\n",
-		lft->allocations, (u_int32_t)lft->bytes);
-	printf("  addtime=%u, usetime=%u }\n",
-		(u_int32_t)lft->addtime, (u_int32_t)lft->usetime);
+	uint16_t port;
 
-	return;
+	IPSEC_ASSERT(sa != NULL, ("null sa"));
+	switch (sa->sa_family) {
+#ifdef INET
+	case AF_INET:
+		port = ntohs(((const struct sockaddr_in *)sa)->sin_port);
+		break;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		port = ntohs(((const struct sockaddr_in6 *)sa)->sin6_port);
+		break;
+#endif
+	default:
+		port = 0;
+	}
+	if (port == 0)
+		return ("*");
+	snprintf(buf, len, "%u", port);
+	return (buf);
 }
 
 void
-kdebug_secasv(sav)
-	struct secasvar *sav;
+kdebug_secpolicy(struct secpolicy *sp)
 {
-	/* sanity check */
-	if (sav == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
+	u_int idx;
+
+	IPSEC_ASSERT(sp != NULL, ("null sp"));
+	printf("SP { refcnt=%u id=%u priority=%u state=%s policy=%s\n",
+	    sp->refcnt, sp->id, sp->priority,
+	    kdebug_secpolicy_state(sp->state),
+	    kdebug_secpolicy_policy(sp->policy));
+	kdebug_secpolicyindex(&sp->spidx, "  ");
+	for (idx = 0; idx < sp->tcount; idx++) {
+		printf("  req[%u]{ level=%s ", idx,
+		    kdebug_ipsecrequest_level(sp->req[idx]->level));
+		kdebug_secasindex(&sp->req[idx]->saidx, NULL);
+		printf("  }\n");
+	}
+	printf("}\n");
+}
 
-	printf("secas{");
-	kdebug_secasindex(&sav->sah->saidx);
+void
+kdebug_secpolicyindex(struct secpolicyindex *spidx, const char *indent)
+{
+	char buf[IPSEC_ADDRSTRLEN];
+
+	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
+	if (indent != NULL)
+		printf("%s", indent);
+	printf("spidx { dir=%s ul_proto=",
+	    kdebug_secpolicyindex_dir(spidx->dir));
+	if (spidx->ul_proto == IPSEC_ULPROTO_ANY)
+		printf("* ");
+	else
+		printf("%u ", spidx->ul_proto);
+	printf("%s/%u -> ", ipsec_address(&spidx->src, buf, sizeof(buf)),
+	    spidx->prefs);
+	printf("%s/%u }\n", ipsec_address(&spidx->dst, buf, sizeof(buf)),
+	    spidx->prefd);
+}
 
-	printf("  refcnt=%u state=%u auth=%u enc=%u\n",
-	    sav->refcnt, sav->state, sav->alg_auth, sav->alg_enc);
-	printf("  spi=%u flags=%u\n",
-	    (u_int32_t)ntohl(sav->spi), sav->flags);
+void
+kdebug_secasindex(const struct secasindex *saidx, const char *indent)
+{
+	char buf[IPSEC_ADDRSTRLEN], port[6];
+
+	IPSEC_ASSERT(saidx != NULL, ("null saidx"));
+	if (indent != NULL)
+		printf("%s", indent);
+	printf("saidx { mode=%s proto=%u reqid=%u ",
+	    kdebug_secasindex_mode(saidx->mode), saidx->proto, saidx->reqid);
+	printf("%s:%s -> ", ipsec_address(&saidx->src, buf, sizeof(buf)),
+	    kdebug_port2str(&saidx->src.sa, port, sizeof(port)));
+	printf("%s:%s }\n", ipsec_address(&saidx->dst, buf, sizeof(buf)),
+	    kdebug_port2str(&saidx->dst.sa, port, sizeof(port)));
+}
 
-	if (sav->key_auth != NULL)
-		kdebug_sadb_key((struct sadb_ext *)sav->key_auth);
-	if (sav->key_enc != NULL)
-		kdebug_sadb_key((struct sadb_ext *)sav->key_enc);
-	if (sav->iv != NULL) {
-		printf("  iv=");
-		ipsec_hexdump(sav->iv, sav->ivlen ? sav->ivlen : 8);
-		printf("\n");
-	}
+static void
+kdebug_sec_lifetime(struct seclifetime *lft, const char *indent)
+{
 
-	if (sav->replay != NULL)
-		kdebug_secreplay(sav->replay);
-	if (sav->lft_c != NULL)
-		kdebug_sec_lifetime(sav->lft_c);
-	if (sav->lft_h != NULL)
-		kdebug_sec_lifetime(sav->lft_h);
-	if (sav->lft_s != NULL)
-		kdebug_sec_lifetime(sav->lft_s);
+	IPSEC_ASSERT(lft != NULL, ("null lft"));
+	if (indent != NULL)
+		printf("%s", indent);
+	printf("lifetime { alloc=%u, bytes=%ju addtime=%ju usetime=%ju }\n",
+	    lft->allocations, (uintmax_t)lft->bytes, (uintmax_t)lft->addtime,
+	    (uintmax_t)lft->usetime);
+}
 
-#ifdef notyet
-	/* XXX: misc[123] ? */
-#endif
+void
+kdebug_secash(struct secashead *sah, const char *indent)
+{
 
-	return;
+	IPSEC_ASSERT(sah != NULL, ("null sah"));
+	if (indent != NULL)
+		printf("%s", indent);
+	printf("SAH { refcnt=%u state=%s\n", sah->refcnt,
+	    kdebug_secasv_state(sah->state));
+	if (indent != NULL)
+		printf("%s", indent);
+	kdebug_secasindex(&sah->saidx, indent);
+	if (indent != NULL)
+		printf("%s", indent);
+	printf("}\n");
 }
 
 static void
-kdebug_secreplay(rpl)
-	struct secreplay *rpl;
+kdebug_secreplay(struct secreplay *rpl)
 {
 	int len, l;
 
-	/* sanity check */
-	if (rpl == NULL)
-		panic("%s: NULL pointer was passed.\n", __func__);
-
-	printf(" secreplay{ count=%u wsize=%u seq=%u lastseq=%u",
-	    rpl->count, rpl->wsize, rpl->seq, rpl->lastseq);
+	IPSEC_ASSERT(rpl != NULL, ("null rpl"));
+	printf(" secreplay{ count=%u bitmap_size=%u wsize=%u seq=%u lastseq=%u",
+	    rpl->count, rpl->bitmap_size, rpl->wsize, rpl->seq, rpl->lastseq);
 
 	if (rpl->bitmap == NULL) {
-		printf(" }\n");
+		printf("  }\n");
 		return;
 	}
 
-	printf("\n   bitmap { ");
-
-	for (len = 0; len < rpl->wsize; len++) {
+	printf("\n    bitmap { ");
+	for (len = 0; len < rpl->bitmap_size*4; len++) {
 		for (l = 7; l >= 0; l--)
 			printf("%u", (((rpl->bitmap)[len] >> l) & 1) ? 1 : 0);
 	}
-	printf(" }\n");
+	printf("    }\n");
+}
 
-	return;
+static void
+kdebug_secnatt(struct secnatt *natt)
+{
+	char buf[IPSEC_ADDRSTRLEN];
+
+	IPSEC_ASSERT(natt != NULL, ("null natt"));
+	printf("  natt{ sport=%u dport=%u ", ntohs(natt->sport),
+	    ntohs(natt->dport));
+	if (natt->flags & IPSEC_NATT_F_OAI)
+		printf("oai=%s ", ipsec_address(&natt->oai, buf, sizeof(buf)));
+	if (natt->flags & IPSEC_NATT_F_OAR)
+		printf("oar=%s ", ipsec_address(&natt->oar, buf, sizeof(buf)));
+	printf("}\n");
+}
+
+void
+kdebug_secasv(struct secasvar *sav)
+{
+	struct seclifetime lft_c;
+
+	IPSEC_ASSERT(sav != NULL, ("null sav"));
+
+	printf("SA { refcnt=%u spi=%u seq=%u pid=%u flags=0x%x state=%s\n",
+	    sav->refcnt, ntohl(sav->spi), sav->seq, (uint32_t)sav->pid,
+	    sav->flags, kdebug_secasv_state(sav->state));
+	kdebug_secash(sav->sah, "  ");
+
+	lft_c.addtime = sav->created;
+	lft_c.allocations = (uint32_t)counter_u64_fetch(
+	    sav->lft_c_allocations);
+	lft_c.bytes = counter_u64_fetch(sav->lft_c_bytes);
+	lft_c.usetime = sav->firstused;
+	kdebug_sec_lifetime(&lft_c, "  c_");
+	if (sav->lft_h != NULL)
+		kdebug_sec_lifetime(sav->lft_h, "  h_");
+	if (sav->lft_s != NULL)
+		kdebug_sec_lifetime(sav->lft_s, "  s_");
+
+	if (sav->tdb_authalgxform != NULL)
+		printf("  alg_auth=%s\n", sav->tdb_authalgxform->name);
+	if (sav->key_auth != NULL)
+		KEYDBG(DUMP,
+		    kdebug_sadb_key((struct sadb_ext *)sav->key_auth));
+	if (sav->tdb_encalgxform != NULL)
+		printf("  alg_enc=%s\n", sav->tdb_encalgxform->name);
+	if (sav->key_enc != NULL)
+		KEYDBG(DUMP,
+		    kdebug_sadb_key((struct sadb_ext *)sav->key_enc));
+	if (sav->natt != NULL)
+		kdebug_secnatt(sav->natt);
+	if (sav->replay != NULL) {
+		KEYDBG(DUMP,
+		    SECASVAR_LOCK(sav);
+		    kdebug_secreplay(sav->replay);
+		    SECASVAR_UNLOCK(sav));
+	}
+	printf("}\n");
 }
 
 void
-kdebug_mbufhdr(m)
-	struct mbuf *m;
+kdebug_mbufhdr(const struct mbuf *m)
 {
 	/* sanity check */
 	if (m == NULL)
@@ -665,19 +776,18 @@ kdebug_mbufhdr(m)
 
 	if (m->m_flags & M_EXT) {
 		printf("  m_ext{ ext_buf:%p ext_free:%p "
-		       "ext_size:%u ref_cnt:%p }\n",
+		       "ext_size:%u ext_cnt:%p }\n",
 			m->m_ext.ext_buf, m->m_ext.ext_free,
-			m->m_ext.ext_size, m->m_ext.ref_cnt);
+			m->m_ext.ext_size, m->m_ext.ext_cnt);
 	}
 
 	return;
 }
 
 void
-kdebug_mbuf(m0)
-	struct mbuf *m0;
+kdebug_mbuf(const struct mbuf *m0)
 {
-	struct mbuf *m = m0;
+	const struct mbuf *m = m0;
 	int i, j;
 
 	for (j = 0; m; m = m->m_next) {
@@ -688,7 +798,7 @@ kdebug_mbuf(m0)
 				printf("\n");
 			if (i % 4 == 0)
 				printf(" ");
-			printf("%02x", mtod(m, u_char *)[i]);
+			printf("%02x", mtod(m, const u_char *)[i]);
 			j++;
 		}
 		printf("\n");
@@ -696,11 +806,51 @@ kdebug_mbuf(m0)
 
 	return;
 }
+
+/* Return a printable string for the address. */
+char *
+ipsec_address(const union sockaddr_union* sa, char *buf, socklen_t size)
+{
+
+	switch (sa->sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		return (inet_ntop(AF_INET, &sa->sin.sin_addr, buf, size));
+#endif /* INET */
+#ifdef INET6
+	case AF_INET6:
+		if (IN6_IS_SCOPE_LINKLOCAL(&sa->sin6.sin6_addr)) {
+			snprintf(buf, size, "%s%%%u", inet_ntop(AF_INET6,
+			    &sa->sin6.sin6_addr, buf, size),
+			    sa->sin6.sin6_scope_id);
+			return (buf);
+		} else
+			return (inet_ntop(AF_INET6, &sa->sin6.sin6_addr,
+			    buf, size));
+#endif /* INET6 */
+	case 0:
+		return ("*");
+	default:
+		return ("(unknown address family)");
+	}
+}
+
+char *
+ipsec_sa2str(struct secasvar *sav, char *buf, size_t size)
+{
+	char sbuf[IPSEC_ADDRSTRLEN], dbuf[IPSEC_ADDRSTRLEN];
+
+	snprintf(buf, size, "SA(SPI=%08lx src=%s dst=%s)",
+	    (u_long)ntohl(sav->spi),
+	    ipsec_address(&sav->sah->saidx.src, sbuf, sizeof(sbuf)),
+	    ipsec_address(&sav->sah->saidx.dst, dbuf, sizeof(dbuf)));
+	return (buf);
+}
+
 #endif /* _KERNEL */
 
 void
-kdebug_sockaddr(addr)
-	struct sockaddr *addr;
+kdebug_sockaddr(struct sockaddr *addr)
 {
 	struct sockaddr_in *sin4;
 #ifdef INET6
@@ -738,9 +888,7 @@ kdebug_sockaddr(addr)
 }
 
 void
-ipsec_bindump(buf, len)
-	caddr_t buf;
-	int len;
+ipsec_bindump(caddr_t buf, int len)
 {
 	int i;
 
@@ -752,9 +900,7 @@ ipsec_bindump(buf, len)
 
 
 void
-ipsec_hexdump(buf, len)
-	caddr_t buf;
-	int len;
+ipsec_hexdump(caddr_t buf, int len)
 {
 	int i;
 
diff --git a/freebsd/sys/netipsec/key_debug.h b/freebsd/sys/netipsec/key_debug.h
index 1a3782b1..18150b53 100644
--- a/freebsd/sys/netipsec/key_debug.h
+++ b/freebsd/sys/netipsec/key_debug.h
@@ -53,37 +53,50 @@
 #define KEYDEBUG_IPSEC_DATA	(KEYDEBUG_IPSEC | KEYDEBUG_DATA)
 #define KEYDEBUG_IPSEC_DUMP	(KEYDEBUG_IPSEC | KEYDEBUG_DUMP)
 
-#define KEYDEBUG(lev,arg) \
-	do { if ((V_key_debug_level & (lev)) == (lev)) { arg; } } while (0)
+#define KEYDBG(lev, arg)	\
+    if ((V_key_debug_level & (KEYDEBUG_ ## lev)) == (KEYDEBUG_ ## lev)) { \
+	    arg;		\
+    }
 
-VNET_DECLARE(u_int32_t, key_debug_level);
+VNET_DECLARE(uint32_t, key_debug_level);
 #define	V_key_debug_level	VNET(key_debug_level)
 #endif /*_KERNEL*/
 
 struct sadb_msg;
 struct sadb_ext;
-extern void kdebug_sadb __P((struct sadb_msg *));
-extern void kdebug_sadb_x_policy __P((struct sadb_ext *));
+extern void kdebug_sadb(struct sadb_msg *);
+extern void kdebug_sadb_x_policy(struct sadb_ext *);
 
 #ifdef _KERNEL
 struct secpolicy;
 struct secpolicyindex;
 struct secasindex;
+struct secashead;
 struct secasvar;
 struct secreplay;
 struct mbuf;
-extern void kdebug_secpolicy __P((struct secpolicy *));
-extern void kdebug_secpolicyindex __P((struct secpolicyindex *));
-extern void kdebug_secasindex __P((struct secasindex *));
-extern void kdebug_secasv __P((struct secasvar *));
-extern void kdebug_mbufhdr __P((struct mbuf *));
-extern void kdebug_mbuf __P((struct mbuf *));
+union sockaddr_union;
+const char* kdebug_secpolicy_state(u_int);
+const char* kdebug_secpolicy_policy(u_int);
+const char* kdebug_secpolicyindex_dir(u_int);
+const char* kdebug_ipsecrequest_level(u_int);
+const char* kdebug_secasindex_mode(u_int);
+const char* kdebug_secasv_state(u_int);
+void kdebug_secpolicy(struct secpolicy *);
+void kdebug_secpolicyindex(struct secpolicyindex *, const char *);
+void kdebug_secasindex(const struct secasindex *, const char *);
+void kdebug_secash(struct secashead *, const char *);
+void kdebug_secasv(struct secasvar *);
+void kdebug_mbufhdr(const struct mbuf *);
+void kdebug_mbuf(const struct mbuf *);
+char *ipsec_address(const union sockaddr_union *, char *, socklen_t);
+char *ipsec_sa2str(struct secasvar *, char *, size_t);
 #endif /*_KERNEL*/
 
 struct sockaddr;
-extern void kdebug_sockaddr __P((struct sockaddr *));
+extern void kdebug_sockaddr(struct sockaddr *);
 
-extern void ipsec_hexdump __P((caddr_t, int));
-extern void ipsec_bindump __P((caddr_t, int));
+extern void ipsec_hexdump(caddr_t, int);
+extern void ipsec_bindump(caddr_t, int);
 
 #endif /* _NETIPSEC_KEY_DEBUG_H_ */
diff --git a/freebsd/sys/netipsec/key_var.h b/freebsd/sys/netipsec/key_var.h
index edf232d8..ecef2360 100644
--- a/freebsd/sys/netipsec/key_var.h
+++ b/freebsd/sys/netipsec/key_var.h
@@ -46,23 +46,6 @@
 #define KEYCTL_ESP_AUTH			10
 #define KEYCTL_AH_KEYMIN		11
 #define KEYCTL_PREFERED_OLDSA		12
-#define KEYCTL_MAXID			13
-
-#define KEYCTL_NAMES { \
-	{ 0, 0 }, \
-	{ "debug", CTLTYPE_INT }, \
-	{ "spi_try", CTLTYPE_INT }, \
-	{ "spi_min_value", CTLTYPE_INT }, \
-	{ "spi_max_value", CTLTYPE_INT }, \
-	{ "random_int", CTLTYPE_INT }, \
-	{ "larval_lifetime", CTLTYPE_INT }, \
-	{ "blockacq_count", CTLTYPE_INT }, \
-	{ "blockacq_lifetime", CTLTYPE_INT }, \
-	{ "esp_keymin", CTLTYPE_INT }, \
-	{ "esp_auth", CTLTYPE_INT }, \
-	{ "ah_keymin", CTLTYPE_INT }, \
-	{ "prefered_oldsa", CTLTYPE_INT }, \
-}
 
 #ifdef _KERNEL
 #define _ARRAYLEN(p) (sizeof(p)/sizeof(p[0]))
diff --git a/freebsd/sys/netipsec/keydb.h b/freebsd/sys/netipsec/keydb.h
index 7494f5f4..e3650146 100644
--- a/freebsd/sys/netipsec/keydb.h
+++ b/freebsd/sys/netipsec/keydb.h
@@ -34,6 +34,9 @@
 #define _NETIPSEC_KEYDB_H_
 
 #ifdef _KERNEL
+#include <sys/counter.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/mutex.h>
 
 #include <netipsec/key_var.h>
 
@@ -54,9 +57,9 @@ union sockaddr_union {
 struct secasindex {
 	union sockaddr_union src;	/* source address for SA */
 	union sockaddr_union dst;	/* destination address for SA */
-	u_int16_t proto;		/* IPPROTO_ESP or IPPROTO_AH */
-	u_int8_t mode;			/* mode of protocol, see ipsec.h */
-	u_int32_t reqid;		/* reqid id who owned this SA */
+	uint8_t proto;			/* IPPROTO_ESP or IPPROTO_AH */
+	uint8_t mode;			/* mode of protocol, see ipsec.h */
+	uint32_t reqid;			/* reqid id who owned this SA */
 					/* see IPSEC_MANUAL_REQID_MAX. */
 };
 
@@ -85,15 +88,23 @@ struct seclifetime {
 	u_int64_t usetime;
 };
 
-union sa_route_union {
-	struct route		sa_route;
-	struct route		sin_route;	/* Duplicate for consistency. */
-	struct route_in6	sin6_route;
+struct secnatt {
+	union sockaddr_union oai;	/* original addresses of initiator */
+	union sockaddr_union oar;	/* original address of responder */
+	uint16_t sport;			/* source port */
+	uint16_t dport;			/* destination port */
+	uint16_t cksum;			/* checksum delta */
+	uint16_t flags;
+#define	IPSEC_NATT_F_OAI	0x0001
+#define	IPSEC_NATT_F_OAR	0x0002
 };
 
 /* Security Association Data Base */
+TAILQ_HEAD(secasvar_queue, secasvar);
 struct secashead {
-	LIST_ENTRY(secashead) chain;
+	TAILQ_ENTRY(secashead) chain;
+	LIST_ENTRY(secashead) addrhash;	/* hash by sproto+src+dst addresses */
+	LIST_ENTRY(secashead) drainq;	/* used ONLY by flush callout */
 
 	struct secasindex saidx;
 
@@ -101,12 +112,10 @@ struct secashead {
 	struct secident *identd;	/* destination identity */
 					/* XXX I don't know how to use them. */
 
-	u_int8_t state;			/* MATURE or DEAD. */
-	LIST_HEAD(_satree, secasvar) savtree[SADB_SASTATE_MAX+1];
-					/* SA chain */
-					/* The first of this list is newer SA */
-
-	union sa_route_union route_cache;
+	volatile u_int refcnt;		/* reference count */
+	uint8_t state;			/* MATURE or DEAD. */
+	struct secasvar_queue savtree_alive;	/* MATURE and DYING SA */
+	struct secasvar_queue savtree_larval;	/* LARVAL SA */
 };
 
 struct xformsw;
@@ -114,72 +123,89 @@ struct enc_xform;
 struct auth_hash;
 struct comp_algo;
 
-/* Security Association */
+/*
+ * Security Association
+ *
+ * For INBOUND packets we do SA lookup using SPI, thus only SPIHASH is used.
+ * For OUTBOUND packets there may be several SA suitable for packet.
+ * We use key_preferred_oldsa variable to choose better SA. First of we do
+ * lookup for suitable SAH using packet's saidx. Then we use SAH's savtree
+ * to search better candidate. The newer SA (by created time) are placed
+ * in the beginning of the savtree list. There is no preference between
+ * DYING and MATURE.
+ *
+ * NB: Fields with a tdb_ prefix are part of the "glue" used
+ *     to interface to the OpenBSD crypto support.  This was done
+ *     to distinguish this code from the mainline KAME code.
+ * NB: Fields are sorted on the basis of the frequency of changes, i.e.
+ *     constants and unchangeable fields are going first.
+ * NB: if you want to change this structure, check that this will not break
+ *     key_updateaddresses().
+ */
 struct secasvar {
-	LIST_ENTRY(secasvar) chain;
-	struct mtx lock;		/* update/access lock */
-
-	u_int refcnt;			/* reference count */
-	u_int8_t state;			/* Status of this Association */
-
-	u_int8_t alg_auth;		/* Authentication Algorithm Identifier*/
-	u_int8_t alg_enc;		/* Cipher Algorithm Identifier */
-	u_int8_t alg_comp;		/* Compression Algorithm Identifier */
-	u_int32_t spi;			/* SPI Value, network byte order */
-	u_int32_t flags;		/* holder for SADB_KEY_FLAGS */
+	uint32_t spi;			/* SPI Value, network byte order */
+	uint32_t flags;			/* holder for SADB_KEY_FLAGS */
+	uint32_t seq;			/* sequence number */
+	pid_t pid;			/* message's pid */
+	u_int ivlen;			/* length of IV */
 
+	struct secashead *sah;		/* back pointer to the secashead */
 	struct seckey *key_auth;	/* Key for Authentication */
 	struct seckey *key_enc;	        /* Key for Encryption */
-	caddr_t iv;			/* Initilization Vector */
-	u_int ivlen;			/* length of IV */
-	void *sched;			/* intermediate encryption key */
-	size_t schedlen;
-
 	struct secreplay *replay;	/* replay prevention */
-	time_t created;			/* for lifetime */
-
-	struct seclifetime *lft_c;	/* CURRENT lifetime, it's constant. */
+	struct secnatt *natt;		/* NAT-T config */
+	struct mtx *lock;		/* update/access lock */
+
+	const struct xformsw *tdb_xform;	/* transform */
+	const struct enc_xform *tdb_encalgxform;/* encoding algorithm */
+	const struct auth_hash *tdb_authalgxform;/* authentication algorithm */
+	const struct comp_algo *tdb_compalgxform;/* compression algorithm */
+	uint64_t tdb_cryptoid;		/* crypto session id */
+
+	uint8_t alg_auth;		/* Authentication Algorithm Identifier*/
+	uint8_t alg_enc;		/* Cipher Algorithm Identifier */
+	uint8_t alg_comp;		/* Compression Algorithm Identifier */
+	uint8_t state;			/* Status of this SA (pfkeyv2.h) */
+
+	counter_u64_t lft_c;		/* CURRENT lifetime */
+#define	lft_c_allocations	lft_c
+#define	lft_c_bytes		lft_c + 1
 	struct seclifetime *lft_h;	/* HARD lifetime */
 	struct seclifetime *lft_s;	/* SOFT lifetime */
 
-	u_int32_t seq;			/* sequence number */
-	pid_t pid;			/* message's pid */
+	uint64_t created;		/* time when SA was created */
+	uint64_t firstused;		/* time when SA was first used */
 
-	struct secashead *sah;		/* back pointer to the secashead */
+	TAILQ_ENTRY(secasvar) chain;
+	LIST_ENTRY(secasvar) spihash;
+	LIST_ENTRY(secasvar) drainq;	/* used ONLY by flush callout */
 
-	/*
-	 * NB: Fields with a tdb_ prefix are part of the "glue" used
-	 *     to interface to the OpenBSD crypto support.  This was done
-	 *     to distinguish this code from the mainline KAME code.
-	 */
-	struct xformsw *tdb_xform;	/* transform */
-	struct enc_xform *tdb_encalgxform;	/* encoding algorithm */
-	struct auth_hash *tdb_authalgxform;	/* authentication algorithm */
-	struct comp_algo *tdb_compalgxform;	/* compression algorithm */
-	u_int64_t tdb_cryptoid;		/* crypto session id */
-
-	/*
-	 * NAT-Traversal.
-	 */
-	u_int16_t natt_type;		/* IKE/ESP-marker in output. */
-	u_int16_t natt_esp_frag_len;	/* MTU for payload fragmentation. */
+	uint64_t cntr;			/* counter for GCM and CTR */
+	volatile u_int refcnt;		/* reference count */
 };
 
-#define	SECASVAR_LOCK_INIT(_sav) \
-	mtx_init(&(_sav)->lock, "ipsec association", NULL, MTX_DEF)
-#define	SECASVAR_LOCK(_sav)		mtx_lock(&(_sav)->lock)
-#define	SECASVAR_UNLOCK(_sav)		mtx_unlock(&(_sav)->lock)
-#define	SECASVAR_LOCK_DESTROY(_sav)	mtx_destroy(&(_sav)->lock)
-#define	SECASVAR_LOCK_ASSERT(_sav)	mtx_assert(&(_sav)->lock, MA_OWNED)
-
-/* replay prevention */
+#define	SECASVAR_LOCK(_sav)		mtx_lock((_sav)->lock)
+#define	SECASVAR_UNLOCK(_sav)		mtx_unlock((_sav)->lock)
+#define	SECASVAR_LOCK_ASSERT(_sav)	mtx_assert((_sav)->lock, MA_OWNED)
+#define	SAV_ISGCM(_sav)							\
+			((_sav)->alg_enc == SADB_X_EALG_AESGCM8 ||	\
+			(_sav)->alg_enc == SADB_X_EALG_AESGCM12 ||	\
+			(_sav)->alg_enc == SADB_X_EALG_AESGCM16)
+#define	SAV_ISCTR(_sav) ((_sav)->alg_enc == SADB_X_EALG_AESCTR)
+#define SAV_ISCTRORGCM(_sav)	(SAV_ISCTR((_sav)) || SAV_ISGCM((_sav)))
+
+/* Replay prevention, protected by SECASVAR_LOCK:
+ *  (m) locked by mtx
+ *  (c) read only except during creation / free
+ */
 struct secreplay {
-	u_int32_t count;
-	u_int wsize;		/* window size, i.g. 4 bytes */
-	u_int32_t seq;		/* used by sender */
-	u_int32_t lastseq;	/* used by receiver */
-	caddr_t bitmap;		/* used by receiver */
-	int overflow;		/* overflow flag */
+	u_int32_t count;	/* (m) */
+	u_int wsize;		/* (c) window size, i.g. 4 bytes */
+	u_int32_t seq;		/* (m) used by sender */
+	u_int32_t lastseq;	/* (m) used by receiver */
+	u_int32_t *bitmap;	/* (m) used by receiver */
+	u_int bitmap_size;	/* (c) size of the bitmap array */
+	int overflow;		/* (m) overflow flag */
 };
 
 /* socket table due to send PF_KEY messages. */
@@ -192,36 +218,15 @@ struct secreg {
 /* acquiring list table. */
 struct secacq {
 	LIST_ENTRY(secacq) chain;
+	LIST_ENTRY(secacq) addrhash;
+	LIST_ENTRY(secacq) seqhash;
 
 	struct secasindex saidx;
-
-	u_int32_t seq;		/* sequence number */
+	uint32_t seq;		/* sequence number */
 	time_t created;		/* for lifetime */
 	int count;		/* for lifetime */
 };
 
-/* Sensitivity Level Specification */
-/* nothing */
-
-#define SADB_KILL_INTERVAL	600	/* six seconds */
-
-/* secpolicy */
-extern struct secpolicy *keydb_newsecpolicy __P((void));
-extern void keydb_delsecpolicy __P((struct secpolicy *));
-/* secashead */
-extern struct secashead *keydb_newsecashead __P((void));
-extern void keydb_delsecashead __P((struct secashead *));
-/* secasvar */
-extern struct secasvar *keydb_newsecasvar __P((void));
-extern void keydb_refsecasvar __P((struct secasvar *));
-extern void keydb_freesecasvar __P((struct secasvar *));
-/* secreplay */
-extern struct secreplay *keydb_newsecreplay __P((size_t));
-extern void keydb_delsecreplay __P((struct secreplay *));
-/* secreg */
-extern struct secreg *keydb_newsecreg __P((void));
-extern void keydb_delsecreg __P((struct secreg *));
-
 #endif /* _KERNEL */
 
 #endif /* _NETIPSEC_KEYDB_H_ */
diff --git a/freebsd/sys/netipsec/keysock.c b/freebsd/sys/netipsec/keysock.c
index 29e23bf3..21430347 100644
--- a/freebsd/sys/netipsec/keysock.c
+++ b/freebsd/sys/netipsec/keysock.c
@@ -54,9 +54,8 @@
 #include <sys/systm.h>
 
 #include <net/if.h>
-#include <net/raw_cb.h>
-#include <net/route.h>
 #include <net/vnet.h>
+#include <net/raw_cb.h>
 
 #include <netinet/in.h>
 
@@ -77,20 +76,25 @@ static VNET_DEFINE(struct key_cb, key_cb);
 
 static struct sockaddr key_src = { 2, PF_KEY, };
 
-static int key_sendup0 __P((struct rawcb *, struct mbuf *, int));
+static int key_sendup0(struct rawcb *, struct mbuf *, int);
+
+VNET_PCPUSTAT_DEFINE(struct pfkeystat, pfkeystat);
+VNET_PCPUSTAT_SYSINIT(pfkeystat);
 
-VNET_DEFINE(struct pfkeystat, pfkeystat);
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(pfkeystat);
+#endif /* VIMAGE */
 
 /*
  * key_output()
  */
 int
-key_output(struct mbuf *m, struct socket *so)
+key_output(struct mbuf *m, struct socket *so, ...)
 {
 	struct sadb_msg *msg;
 	int len, error = 0;
 
-	if (m == 0)
+	if (m == NULL)
 		panic("%s: NULL pointer was passed.\n", __func__);
 
 	PFKEYSTAT_INC(out_total);
@@ -104,7 +108,7 @@ key_output(struct mbuf *m, struct socket *so)
 	}
 
 	if (m->m_len < sizeof(struct sadb_msg)) {
-		if ((m = m_pullup(m, sizeof(struct sadb_msg))) == 0) {
+		if ((m = m_pullup(m, sizeof(struct sadb_msg))) == NULL) {
 			PFKEYSTAT_INC(out_nomem);
 			error = ENOBUFS;
 			goto end;
@@ -113,7 +117,7 @@ key_output(struct mbuf *m, struct socket *so)
 
 	M_ASSERTPKTHDR(m);
 
-	KEYDEBUG(KEYDEBUG_KEY_DUMP, kdebug_mbuf(m));
+	KEYDBG(KEY_DUMP, kdebug_mbuf(m));
 
 	msg = mtod(m, struct sadb_msg *);
 	PFKEYSTAT_INC(out_msgtype[msg->sadb_msg_type]);
@@ -135,26 +139,18 @@ end:
  * send message to the socket.
  */
 static int
-key_sendup0(rp, m, promisc)
-	struct rawcb *rp;
-	struct mbuf *m;
-	int promisc;
+key_sendup0(struct rawcb *rp, struct mbuf *m, int promisc)
 {
 	int error;
 
 	if (promisc) {
 		struct sadb_msg *pmsg;
 
-		M_PREPEND(m, sizeof(struct sadb_msg), M_DONTWAIT);
-		if (m && m->m_len < sizeof(struct sadb_msg))
-			m = m_pullup(m, sizeof(struct sadb_msg));
-		if (!m) {
+		M_PREPEND(m, sizeof(struct sadb_msg), M_NOWAIT);
+		if (m == NULL) {
 			PFKEYSTAT_INC(in_nomem);
-			m_freem(m);
-			return ENOBUFS;
+			return (ENOBUFS);
 		}
-		m->m_pkthdr.len += sizeof(*pmsg);
-
 		pmsg = mtod(m, struct sadb_msg *);
 		bzero(pmsg, sizeof(*pmsg));
 		pmsg->sadb_msg_version = PF_KEY_V2;
@@ -178,22 +174,18 @@ key_sendup0(rp, m, promisc)
 
 /* XXX this interface should be obsoleted. */
 int
-key_sendup(so, msg, len, target)
-	struct socket *so;
-	struct sadb_msg *msg;
-	u_int len;
-	int target;	/*target of the resulting message*/
+key_sendup(struct socket *so, struct sadb_msg *msg, u_int len, int target)
 {
 	struct mbuf *m, *n, *mprev;
 	int tlen;
 
 	/* sanity check */
-	if (so == 0 || msg == 0)
+	if (so == NULL || msg == NULL)
 		panic("%s: NULL pointer was passed.\n", __func__);
 
-	KEYDEBUG(KEYDEBUG_KEY_DUMP,
-		printf("%s: \n", __func__);
-		kdebug_sadb(msg));
+	KEYDBG(KEY_DUMP,
+	    printf("%s: \n", __func__);
+	    kdebug_sadb(msg));
 
 	/*
 	 * we increment statistics here, just in case we have ENOBUFS
@@ -216,14 +208,14 @@ key_sendup(so, msg, len, target)
 	m = mprev = NULL;
 	while (tlen > 0) {
 		if (tlen == len) {
-			MGETHDR(n, M_DONTWAIT, MT_DATA);
+			MGETHDR(n, M_NOWAIT, MT_DATA);
 			if (n == NULL) {
 				PFKEYSTAT_INC(in_nomem);
 				return ENOBUFS;
 			}
 			n->m_len = MHLEN;
 		} else {
-			MGET(n, M_DONTWAIT, MT_DATA);
+			MGET(n, M_NOWAIT, MT_DATA);
 			if (n == NULL) {
 				PFKEYSTAT_INC(in_nomem);
 				return ENOBUFS;
@@ -231,8 +223,7 @@ key_sendup(so, msg, len, target)
 			n->m_len = MLEN;
 		}
 		if (tlen >= MCLBYTES) {	/*XXX better threshold? */
-			MCLGET(n, M_DONTWAIT);
-			if ((n->m_flags & M_EXT) == 0) {
+			if (!(MCLGET(n, M_NOWAIT))) {
 				m_free(n);
 				m_freem(m);
 				PFKEYSTAT_INC(in_nomem);
@@ -267,10 +258,7 @@ key_sendup(so, msg, len, target)
 
 /* so can be NULL if target != KEY_SENDUP_ONE */
 int
-key_sendup_mbuf(so, m, target)
-	struct socket *so;
-	struct mbuf *m;
-	int target;
+key_sendup_mbuf(struct socket *so, struct mbuf *m, int target)
 {
 	struct mbuf *n;
 	struct keycb *kp;
@@ -315,7 +303,7 @@ key_sendup_mbuf(so, m, target)
 		 * (based on pf_key@inner.net message on 14 Oct 1998)
 		 */
 		if (((struct keycb *)rp)->kp_promisc) {
-			if ((n = m_copy(m, 0, (int)M_COPYALL)) != NULL) {
+			if ((n = m_copym(m, 0, M_COPYALL, M_NOWAIT)) != NULL) {
 				(void)key_sendup0(rp, n, 1);
 				n = NULL;
 			}
@@ -345,7 +333,7 @@ key_sendup_mbuf(so, m, target)
 		if (!sendup)
 			continue;
 
-		if ((n = m_copy(m, 0, (int)M_COPYALL)) == NULL) {
+		if ((n = m_copym(m, 0, M_COPYALL, M_NOWAIT)) == NULL) {
 			m_freem(m);
 			PFKEYSTAT_INC(in_nomem);
 			mtx_unlock(&rawcb_mtx);
@@ -402,7 +390,7 @@ key_attach(struct socket *so, int proto, struct thread *td)
 
 	/* XXX */
 	kp = malloc(sizeof *kp, M_PCB, M_WAITOK | M_ZERO); 
-	if (kp == 0)
+	if (kp == NULL)
 		return ENOBUFS;
 
 	so->so_pcb = (caddr_t)kp;
@@ -578,7 +566,7 @@ struct domain keydomain = {
 	.dom_destroy =		key_destroy,
 #endif
 	.dom_protosw =		keysw,
-	.dom_protoswNPROTOSW =	&keysw[sizeof(keysw)/sizeof(keysw[0])]
+	.dom_protoswNPROTOSW =	&keysw[nitems(keysw)]
 };
 
 VNET_DOMAIN_SET(key);
diff --git a/freebsd/sys/netipsec/keysock.h b/freebsd/sys/netipsec/keysock.h
index 6039dbba..8fbf4a02 100644
--- a/freebsd/sys/netipsec/keysock.h
+++ b/freebsd/sys/netipsec/keysock.h
@@ -36,26 +36,26 @@
 /* statistics for pfkey socket */
 struct pfkeystat {
 	/* kernel -> userland */
-	u_quad_t out_total;		/* # of total calls */
-	u_quad_t out_bytes;		/* total bytecount */
-	u_quad_t out_msgtype[256];	/* message type histogram */
-	u_quad_t out_invlen;		/* invalid length field */
-	u_quad_t out_invver;		/* invalid version field */
-	u_quad_t out_invmsgtype;	/* invalid message type field */
-	u_quad_t out_tooshort;		/* msg too short */
-	u_quad_t out_nomem;		/* memory allocation failure */
-	u_quad_t out_dupext;		/* duplicate extension */
-	u_quad_t out_invexttype;	/* invalid extension type */
-	u_quad_t out_invsatype;		/* invalid sa type */
-	u_quad_t out_invaddr;		/* invalid address extension */
+	uint64_t out_total;		/* # of total calls */
+	uint64_t out_bytes;		/* total bytecount */
+	uint64_t out_msgtype[256];	/* message type histogram */
+	uint64_t out_invlen;		/* invalid length field */
+	uint64_t out_invver;		/* invalid version field */
+	uint64_t out_invmsgtype;	/* invalid message type field */
+	uint64_t out_tooshort;		/* msg too short */
+	uint64_t out_nomem;		/* memory allocation failure */
+	uint64_t out_dupext;		/* duplicate extension */
+	uint64_t out_invexttype;	/* invalid extension type */
+	uint64_t out_invsatype;		/* invalid sa type */
+	uint64_t out_invaddr;		/* invalid address extension */
 	/* userland -> kernel */
-	u_quad_t in_total;		/* # of total calls */
-	u_quad_t in_bytes;		/* total bytecount */
-	u_quad_t in_msgtype[256];	/* message type histogram */
-	u_quad_t in_msgtarget[3];	/* one/all/registered */
-	u_quad_t in_nomem;		/* memory allocation failure */
+	uint64_t in_total;		/* # of total calls */
+	uint64_t in_bytes;		/* total bytecount */
+	uint64_t in_msgtype[256];	/* message type histogram */
+	uint64_t in_msgtarget[3];	/* one/all/registered */
+	uint64_t in_nomem;		/* memory allocation failure */
 	/* others */
-	u_quad_t sockerr;		/* # of socket related errors */
+	uint64_t sockerr;		/* # of socket related errors */
 };
 
 #define KEY_SENDUP_ONE		0
@@ -63,23 +63,25 @@ struct pfkeystat {
 #define KEY_SENDUP_REGISTERED	2
 
 #ifdef _KERNEL
+#include <sys/counter.h>
+
 struct keycb {
 	struct rawcb kp_raw;	/* rawcb */
 	int kp_promisc;		/* promiscuous mode */
 	int kp_registered;	/* registered socket */
 };
 
-VNET_DECLARE(struct pfkeystat, pfkeystat);
-#define	PFKEYSTAT_ADD(name, val)	V_pfkeystat.name += (val)
+VNET_PCPUSTAT_DECLARE(struct pfkeystat, pfkeystat);
+#define	PFKEYSTAT_ADD(name, val)	\
+    VNET_PCPUSTAT_ADD(struct pfkeystat, pfkeystat, name, (val))
 #define	PFKEYSTAT_INC(name)		PFKEYSTAT_ADD(name, 1)
-#define	V_pfkeystat		VNET(pfkeystat)
 
-extern int key_output(struct mbuf *m, struct socket *so);
-extern int key_usrreq __P((struct socket *,
-	int, struct mbuf *, struct mbuf *, struct mbuf *));
+extern int key_output(struct mbuf *m, struct socket *so, ...);
+extern int key_usrreq(struct socket *, int, struct mbuf *,
+    struct mbuf *, struct mbuf *);
 
-extern int key_sendup __P((struct socket *, struct sadb_msg *, u_int, int));
-extern int key_sendup_mbuf __P((struct socket *, struct mbuf *, int));
+extern int key_sendup(struct socket *, struct sadb_msg *, u_int, int);
+extern int key_sendup_mbuf(struct socket *, struct mbuf *, int);
 #endif /* _KERNEL */
 
 #endif /*_NETIPSEC_KEYSOCK_H_*/
diff --git a/freebsd/sys/netipsec/subr_ipsec.c b/freebsd/sys/netipsec/subr_ipsec.c
new file mode 100644
index 00000000..ff830564
--- /dev/null
+++ b/freebsd/sys/netipsec/subr_ipsec.c
@@ -0,0 +1,356 @@
+#include <machine/rtems-bsd-kernel-space.h>
+
+/*-
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <rtems/bsd/local/opt_inet.h>
+#include <rtems/bsd/local/opt_inet6.h>
+#include <rtems/bsd/local/opt_ipsec.h>
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <rtems/bsd/sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/module.h>
+#include <sys/priv.h>
+#include <sys/socket.h>
+#include <sys/sockopt.h>
+#include <sys/syslog.h>
+#include <sys/proc.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+
+#include <netipsec/ipsec_support.h>
+#include <netipsec/ipsec.h>
+#include <netipsec/ipsec6.h>
+#include <netipsec/key.h>
+#include <netipsec/key_debug.h>
+
+#include <machine/atomic.h>
+/*
+ * This file is build in the kernel only when 'options IPSEC' or
+ * 'options IPSEC_SUPPORT' is enabled.
+ */
+
+#ifdef INET
+void
+ipsec4_setsockaddrs(const struct mbuf *m, union sockaddr_union *src,
+    union sockaddr_union *dst)
+{
+	static const struct sockaddr_in template = {
+		sizeof (struct sockaddr_in),
+		AF_INET,
+		0, { 0 }, { 0, 0, 0, 0, 0, 0, 0, 0 }
+	};
+
+	src->sin = template;
+	dst->sin = template;
+
+	if (m->m_len < sizeof (struct ip)) {
+		m_copydata(m, offsetof(struct ip, ip_src),
+			   sizeof (struct  in_addr),
+			   (caddr_t) &src->sin.sin_addr);
+		m_copydata(m, offsetof(struct ip, ip_dst),
+			   sizeof (struct  in_addr),
+			   (caddr_t) &dst->sin.sin_addr);
+	} else {
+		const struct ip *ip = mtod(m, const struct ip *);
+		src->sin.sin_addr = ip->ip_src;
+		dst->sin.sin_addr = ip->ip_dst;
+	}
+}
+#endif
+#ifdef INET6
+void
+ipsec6_setsockaddrs(const struct mbuf *m, union sockaddr_union *src,
+    union sockaddr_union *dst)
+{
+	struct ip6_hdr ip6buf;
+	const struct ip6_hdr *ip6;
+
+	if (m->m_len >= sizeof(*ip6))
+		ip6 = mtod(m, const struct ip6_hdr *);
+	else {
+		m_copydata(m, 0, sizeof(ip6buf), (caddr_t)&ip6buf);
+		ip6 = &ip6buf;
+	}
+
+	bzero(&src->sin6, sizeof(struct sockaddr_in6));
+	src->sin6.sin6_family = AF_INET6;
+	src->sin6.sin6_len = sizeof(struct sockaddr_in6);
+	bcopy(&ip6->ip6_src, &src->sin6.sin6_addr, sizeof(ip6->ip6_src));
+	if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
+		src->sin6.sin6_addr.s6_addr16[1] = 0;
+		src->sin6.sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
+	}
+
+	bzero(&dst->sin6, sizeof(struct sockaddr_in6));
+	dst->sin6.sin6_family = AF_INET6;
+	dst->sin6.sin6_len = sizeof(struct sockaddr_in6);
+	bcopy(&ip6->ip6_dst, &dst->sin6.sin6_addr, sizeof(ip6->ip6_dst));
+	if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
+		dst->sin6.sin6_addr.s6_addr16[1] = 0;
+		dst->sin6.sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
+	}
+}
+#endif
+
+#ifdef IPSEC_SUPPORT
+/*
+ * IPSEC_SUPPORT - loading of ipsec.ko and tcpmd5.ko is supported.
+ * IPSEC + IPSEC_SUPPORT - loading tcpmd5.ko is supported.
+ * IPSEC + TCP_SIGNATURE - all is build in the kernel, do not build
+ *   IPSEC_SUPPORT.
+ */
+#if !defined(IPSEC) || !defined(TCP_SIGNATURE)
+#define	IPSEC_MODULE_INCR	2
+static int
+ipsec_kmod_enter(volatile u_int *cntr)
+{
+	u_int old, new;
+
+	do {
+		old = *cntr;
+		if ((old & IPSEC_MODULE_ENABLED) == 0)
+			return (ENXIO);
+		new = old + IPSEC_MODULE_INCR;
+	} while(atomic_cmpset_acq_int(cntr, old, new) == 0);
+	return (0);
+}
+
+static void
+ipsec_kmod_exit(volatile u_int *cntr)
+{
+	u_int old, new;
+
+	do {
+		old = *cntr;
+		new = old - IPSEC_MODULE_INCR;
+	} while (atomic_cmpset_rel_int(cntr, old, new) == 0);
+}
+
+static void
+ipsec_kmod_drain(volatile u_int *cntr)
+{
+	u_int old, new;
+
+	do {
+		old = *cntr;
+		new = old & ~IPSEC_MODULE_ENABLED;
+	} while (atomic_cmpset_acq_int(cntr, old, new) == 0);
+	while (atomic_cmpset_int(cntr, 0, 0) == 0)
+		pause("ipsecd", hz/2);
+}
+
+#define	METHOD_DECL(...)	__VA_ARGS__
+#define	METHOD_ARGS(...)	__VA_ARGS__
+#define	IPSEC_KMOD_METHOD(type, name, sc, method, decl, args)		\
+type name (decl)							\
+{									\
+	type ret = (type)ipsec_kmod_enter(&sc->enabled);		\
+	if (ret == 0) {							\
+		ret = (*sc->methods->method)(args);			\
+		ipsec_kmod_exit(&sc->enabled);				\
+	}								\
+	return (ret);							\
+}
+
+static int
+ipsec_support_modevent(module_t mod, int type, void *data)
+{
+
+	switch (type) {
+	case MOD_LOAD:
+		return (0);
+	case MOD_UNLOAD:
+		return (EBUSY);
+	default:
+		return (EOPNOTSUPP);
+	}
+}
+
+static moduledata_t ipsec_support_mod = {
+	"ipsec_support",
+	ipsec_support_modevent,
+	0
+};
+DECLARE_MODULE(ipsec_support, ipsec_support_mod, SI_SUB_PROTO_DOMAIN,
+    SI_ORDER_ANY);
+MODULE_VERSION(ipsec_support, 1);
+#endif /* !IPSEC || !TCP_SIGNATURE */
+
+#ifndef TCP_SIGNATURE
+/* Declare TCP-MD5 support as kernel module. */
+static struct tcpmd5_support tcpmd5_ipsec = {
+	.enabled = 0,
+	.methods = NULL
+};
+struct tcpmd5_support * const tcp_ipsec_support = &tcpmd5_ipsec;
+
+IPSEC_KMOD_METHOD(int, tcpmd5_kmod_input, sc,
+    input, METHOD_DECL(struct tcpmd5_support * const sc, struct mbuf *m,
+	struct tcphdr *th, u_char *buf), METHOD_ARGS(m, th, buf)
+)
+
+IPSEC_KMOD_METHOD(int, tcpmd5_kmod_output, sc,
+    output, METHOD_DECL(struct tcpmd5_support * const sc, struct mbuf *m,
+	struct tcphdr *th, u_char *buf), METHOD_ARGS(m, th, buf)
+)
+
+IPSEC_KMOD_METHOD(int, tcpmd5_kmod_pcbctl, sc,
+    pcbctl, METHOD_DECL(struct tcpmd5_support * const sc, struct inpcb *inp,
+	struct sockopt *sopt), METHOD_ARGS(inp, sopt)
+)
+
+void
+tcpmd5_support_enable(const struct tcpmd5_methods * const methods)
+{
+
+	KASSERT(tcp_ipsec_support->enabled == 0, ("TCP-MD5 already enabled"));
+	tcp_ipsec_support->methods = methods;
+	tcp_ipsec_support->enabled |= IPSEC_MODULE_ENABLED;
+}
+
+void
+tcpmd5_support_disable(void)
+{
+
+	if (tcp_ipsec_support->enabled & IPSEC_MODULE_ENABLED) {
+		ipsec_kmod_drain(&tcp_ipsec_support->enabled);
+		tcp_ipsec_support->methods = NULL;
+	}
+}
+#endif /* !TCP_SIGNATURE */
+
+#ifndef IPSEC
+/*
+ * IPsec support is build as kernel module.
+ */
+#ifdef INET
+static struct ipsec_support ipv4_ipsec = {
+	.enabled = 0,
+	.methods = NULL
+};
+struct ipsec_support * const ipv4_ipsec_support = &ipv4_ipsec;
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_udp_input, sc,
+    udp_input, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m,
+	int off, int af), METHOD_ARGS(m, off, af)
+)
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_udp_pcbctl, sc,
+    udp_pcbctl, METHOD_DECL(struct ipsec_support * const sc, struct inpcb *inp,
+	struct sockopt *sopt), METHOD_ARGS(inp, sopt)
+)
+#endif
+
+#ifdef INET6
+static struct ipsec_support ipv6_ipsec = {
+	.enabled = 0,
+	.methods = NULL
+};
+struct ipsec_support * const ipv6_ipsec_support = &ipv6_ipsec;
+#endif
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_input, sc,
+    input, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m,
+	int offset, int proto), METHOD_ARGS(m, offset, proto)
+)
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_check_policy, sc,
+    check_policy, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m,
+	struct inpcb *inp), METHOD_ARGS(m, inp)
+)
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_forward, sc,
+    forward, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m),
+    (m)
+)
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_output, sc,
+    output, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m,
+	struct inpcb *inp), METHOD_ARGS(m, inp)
+)
+
+IPSEC_KMOD_METHOD(int, ipsec_kmod_pcbctl, sc,
+    pcbctl, METHOD_DECL(struct ipsec_support * const sc, struct inpcb *inp,
+	struct sockopt *sopt), METHOD_ARGS(inp, sopt)
+)
+
+IPSEC_KMOD_METHOD(size_t, ipsec_kmod_hdrsize, sc,
+    hdrsize, METHOD_DECL(struct ipsec_support * const sc, struct inpcb *inp),
+    (inp)
+)
+
+static IPSEC_KMOD_METHOD(int, ipsec_kmod_caps, sc,
+    capability, METHOD_DECL(struct ipsec_support * const sc, struct mbuf *m,
+	u_int cap), METHOD_ARGS(m, cap)
+)
+
+int
+ipsec_kmod_capability(struct ipsec_support * const sc, struct mbuf *m,
+    u_int cap)
+{
+
+	/*
+	 * Since PF_KEY is build in the kernel, we can directly
+	 * call key_havesp() without additional synchronizations.
+	 */
+	if (cap == IPSEC_CAP_OPERABLE)
+		return (key_havesp(IPSEC_DIR_INBOUND) != 0 ||
+		    key_havesp(IPSEC_DIR_OUTBOUND) != 0);
+	return (ipsec_kmod_caps(sc, m, cap));
+}
+
+void
+ipsec_support_enable(struct ipsec_support * const sc,
+    const struct ipsec_methods * const methods)
+{
+
+	KASSERT(sc->enabled == 0, ("IPsec already enabled"));
+	sc->methods = methods;
+	sc->enabled |= IPSEC_MODULE_ENABLED;
+}
+
+void
+ipsec_support_disable(struct ipsec_support * const sc)
+{
+
+	if (sc->enabled & IPSEC_MODULE_ENABLED) {
+		ipsec_kmod_drain(&sc->enabled);
+		sc->methods = NULL;
+	}
+}
+#endif /* !IPSEC */
+#endif /* IPSEC_SUPPORT */
diff --git a/freebsd/sys/netipsec/udpencap.c b/freebsd/sys/netipsec/udpencap.c
new file mode 100644
index 00000000..fd2ca444
--- /dev/null
+++ b/freebsd/sys/netipsec/udpencap.c
@@ -0,0 +1,299 @@
+#include <machine/rtems-bsd-kernel-space.h>
+
+/*-
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <rtems/bsd/local/opt_inet.h>
+#include <rtems/bsd/local/opt_ipsec.h>
+
+#include <rtems/bsd/sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/mbuf.h>
+#include <sys/protosw.h>
+#include <sys/socket.h>
+#include <sys/sockopt.h>
+#include <sys/sysctl.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/in_systm.h>
+#include <netinet/ip.h>
+#include <netinet/ip_var.h>
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
+#include <netinet/udp_var.h>
+
+#include <net/vnet.h>
+
+#include <netipsec/ipsec.h>
+#include <netipsec/esp.h>
+#include <netipsec/esp_var.h>
+#include <netipsec/xform.h>
+
+#include <netipsec/key.h>
+#include <netipsec/key_debug.h>
+#include <netipsec/ipsec_support.h>
+#include <machine/in_cksum.h>
+
+/*
+ * Handle UDP_ENCAP socket option. Always return with released INP_WLOCK.
+ */
+int
+udp_ipsec_pcbctl(struct inpcb *inp, struct sockopt *sopt)
+{
+	struct udpcb *up;
+	int error, optval;
+
+	INP_WLOCK_ASSERT(inp);
+	if (sopt->sopt_name != UDP_ENCAP) {
+		INP_WUNLOCK(inp);
+		return (ENOPROTOOPT);
+	}
+
+	up = intoudpcb(inp);
+	if (sopt->sopt_dir == SOPT_GET) {
+		if (up->u_flags & UF_ESPINUDP)
+			optval = UDP_ENCAP_ESPINUDP;
+		else
+			optval = 0;
+		INP_WUNLOCK(inp);
+		return (sooptcopyout(sopt, &optval, sizeof(optval)));
+	}
+	INP_WUNLOCK(inp);
+
+	error = sooptcopyin(sopt, &optval, sizeof(optval), sizeof(optval));
+	if (error != 0)
+		return (error);
+
+	INP_WLOCK(inp);
+	switch (optval) {
+	case 0:
+		up->u_flags &= ~UF_ESPINUDP;
+		break;
+	case UDP_ENCAP_ESPINUDP:
+		up->u_flags |= UF_ESPINUDP;
+		break;
+	default:
+		error = EINVAL;
+	}
+	INP_WUNLOCK(inp);
+	return (error);
+}
+
+/*
+ * Potentially decap ESP in UDP frame.  Check for an ESP header.
+ * If present, strip the UDP header and push the result through IPSec.
+ *
+ * Returns error if mbuf consumed and/or processed, otherwise 0.
+ */
+int
+udp_ipsec_input(struct mbuf *m, int off, int af)
+{
+	union sockaddr_union dst;
+	struct secasvar *sav;
+	struct udphdr *udp;
+	struct ip *ip;
+	uint32_t spi;
+	int error, hlen;
+
+	/*
+	 * Just return if packet doesn't have enough data.
+	 * We need at least [IP header + UDP header + ESP header].
+	 * NAT-Keepalive packet has only one byte of payload, so it
+	 * by default will not be processed.
+	 */
+	if (m->m_pkthdr.len < off + sizeof(struct esp))
+		return (0);
+
+	m_copydata(m, off, sizeof(uint32_t), (caddr_t)&spi);
+	if (spi == 0)	/* Non-ESP marker. */
+		return (0);
+
+	/*
+	 * Find SA and check that it is configured for UDP
+	 * encapsulation.
+	 */
+	bzero(&dst, sizeof(dst));
+	dst.sa.sa_family = af;
+	switch (af) {
+#ifdef INET
+	case AF_INET:
+		dst.sin.sin_len = sizeof(struct sockaddr_in);
+		ip = mtod(m, struct ip *);
+		ip->ip_p = IPPROTO_ESP;
+		off = offsetof(struct ip, ip_p);
+		hlen = ip->ip_hl << 2;
+		dst.sin.sin_addr = ip->ip_dst;
+		break;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		/* Not yet */
+		/* FALLTHROUGH */
+#endif
+	default:
+		ESPSTAT_INC(esps_nopf);
+		m_freem(m);
+		return (EPFNOSUPPORT);
+	}
+
+	sav = key_allocsa(&dst, IPPROTO_ESP, spi);
+	if (sav == NULL) {
+		ESPSTAT_INC(esps_notdb);
+		m_freem(m);
+		return (ENOENT);
+	}
+	udp = mtodo(m, hlen);
+	if (sav->natt == NULL ||
+	    sav->natt->sport != udp->uh_sport ||
+	    sav->natt->dport != udp->uh_dport) {
+		/* XXXAE: should we check source address? */
+		ESPSTAT_INC(esps_notdb);
+		key_freesav(&sav);
+		m_freem(m);
+		return (ENOENT);
+	}
+	/*
+	 * Remove the UDP header
+	 * Before:
+	 *   <--- off --->
+	 *   +----+------+-----+
+	 *   | IP |  UDP | ESP |
+	 *   +----+------+-----+
+	 *        <-skip->
+	 * After:
+	 *          +----+-----+
+	 *          | IP | ESP |
+	 *          +----+-----+
+	 *   <-skip->
+	 */
+	m_striphdr(m, hlen, sizeof(*udp));
+	/*
+	 * We cannot yet update the cksums so clear any h/w cksum flags
+	 * as they are no longer valid.
+	 */
+	if (m->m_pkthdr.csum_flags & CSUM_DATA_VALID)
+		m->m_pkthdr.csum_flags &= ~(CSUM_DATA_VALID | CSUM_PSEUDO_HDR);
+	/*
+	 * We can update ip_len and ip_sum here, but ipsec4_input_cb()
+	 * will do this anyway, so don't touch them here.
+	 */
+	ESPSTAT_INC(esps_input);
+	error = (*sav->tdb_xform->xf_input)(m, sav, hlen, off);
+	if (error != 0)
+		key_freesav(&sav);
+
+	return (EINPROGRESS);	/* Consumed by IPsec. */
+}
+
+int
+udp_ipsec_output(struct mbuf *m, struct secasvar *sav)
+{
+	struct udphdr *udp;
+	struct mbuf *n;
+	struct ip *ip;
+	int hlen, off;
+
+	IPSEC_ASSERT(sav->natt != NULL, ("UDP encapsulation isn't required."));
+
+	if (sav->sah->saidx.dst.sa.sa_family == AF_INET6)
+		return (EAFNOSUPPORT);
+
+	ip = mtod(m, struct ip *);
+	hlen = ip->ip_hl << 2;
+	n = m_makespace(m, hlen, sizeof(*udp), &off);
+	if (n == NULL) {
+		DPRINTF(("%s: m_makespace for udphdr failed\n", __func__));
+		return (ENOBUFS);
+	}
+
+	udp = mtodo(n, off);
+	udp->uh_dport = sav->natt->dport;
+	udp->uh_sport = sav->natt->sport;
+	udp->uh_sum = 0;
+	udp->uh_ulen = htons(m->m_pkthdr.len - hlen);
+
+	ip = mtod(m, struct ip *);
+	ip->ip_len = htons(m->m_pkthdr.len);
+	ip->ip_p = IPPROTO_UDP;
+	return (0);
+}
+
+void
+udp_ipsec_adjust_cksum(struct mbuf *m, struct secasvar *sav, int proto,
+    int skip)
+{
+	struct ip *ip;
+	uint16_t cksum, off;
+
+	IPSEC_ASSERT(sav->natt != NULL, ("NAT-T isn't required"));
+	IPSEC_ASSERT(proto == IPPROTO_UDP || proto == IPPROTO_TCP,
+	    ("unexpected protocol %u", proto));
+
+	if (proto == IPPROTO_UDP)
+		off = offsetof(struct udphdr, uh_sum);
+	else
+		off = offsetof(struct tcphdr, th_sum);
+
+	if (V_natt_cksum_policy == 0) {	/* auto */
+		if (sav->natt->cksum != 0) {
+			/* Incrementally recompute. */
+			m_copydata(m, skip + off, sizeof(cksum),
+			    (caddr_t)&cksum);
+			/* Do not adjust UDP checksum if it is zero. */
+			if (proto == IPPROTO_UDP && cksum == 0)
+				return;
+			cksum = in_addword(cksum, sav->natt->cksum);
+		} else {
+			/* No OA from IKEd. */
+			if (proto == IPPROTO_TCP) {
+				/* Ignore for TCP. */
+				m->m_pkthdr.csum_data = 0xffff;
+				m->m_pkthdr.csum_flags |= (CSUM_DATA_VALID |
+				    CSUM_PSEUDO_HDR);
+				return;
+			}
+			cksum = 0; /* Reset for UDP. */
+		}
+		m_copyback(m, skip + off, sizeof(cksum), (caddr_t)&cksum);
+	} else { /* Fully recompute */
+		ip = mtod(m, struct ip *);
+		cksum = in_pseudo(ip->ip_src.s_addr, ip->ip_dst.s_addr,
+		    htons(m->m_pkthdr.len - skip + proto));
+		m_copyback(m, skip + off, sizeof(cksum), (caddr_t)&cksum);
+		m->m_pkthdr.csum_flags =
+		    (proto == IPPROTO_UDP) ? CSUM_UDP: CSUM_TCP;
+		m->m_pkthdr.csum_data = off;
+		in_delayed_cksum(m);
+		m->m_pkthdr.csum_flags &= ~CSUM_DELAY_DATA;
+	}
+}
+
diff --git a/freebsd/sys/netipsec/xform.h b/freebsd/sys/netipsec/xform.h
index e389cab3..8e6f8bdb 100644
--- a/freebsd/sys/netipsec/xform.h
+++ b/freebsd/sys/netipsec/xform.h
@@ -42,6 +42,7 @@
 #define _NETIPSEC_XFORM_H_
 
 #include <sys/types.h>
+#include <sys/queue.h>
 #include <netinet/in.h>
 #include <opencrypto/xform.h>
 
@@ -49,83 +50,68 @@
 #define	AH_HMAC_MAXHASHLEN	(SHA2_512_HASH_LEN/2)	/* Keep this updated */
 #define	AH_HMAC_INITIAL_RPL	1	/* replay counter initial value */
 
+#ifdef _KERNEL
+struct secpolicy;
+struct secasvar;
+
 /*
  * Packet tag assigned on completion of IPsec processing; used
- * to speedup processing when/if the packet comes back for more
- * processing.
+ * to speedup security policy checking for INBOUND packets.
  */
-struct tdb_ident {
-	u_int32_t spi;
-	union sockaddr_union dst;
-	u_int8_t proto;
-	/* Cache those two for enc(4) in xform_ipip. */
-	u_int8_t alg_auth;
-	u_int8_t alg_enc;
+struct xform_history {
+	union sockaddr_union	dst;		/* destination address */
+	uint32_t		spi;		/* Security Parameters Index */
+	uint8_t			proto;		/* IPPROTO_ESP or IPPROTO_AH */
+	uint8_t			mode;		/* transport or tunnel */
 };
 
 /*
  * Opaque data structure hung off a crypto operation descriptor.
  */
-struct tdb_crypto {
-	struct ipsecrequest	*tc_isr;	/* ipsec request state */
-	u_int32_t		tc_spi;		/* associated SPI */
-	union sockaddr_union	tc_dst;		/* dst addr of packet */
-	u_int8_t		tc_proto;	/* current protocol, e.g. AH */
-	u_int8_t		tc_nxt;		/* next protocol, e.g. IPV4 */
-	int			tc_protoff;	/* current protocol offset */
-	int			tc_skip;	/* data offset */
-	caddr_t			tc_ptr;		/* associated crypto data */
-	struct secasvar 	*tc_sav;	/* related SA */
+struct xform_data {
+	struct secpolicy	*sp;		/* security policy */
+	struct secasvar		*sav;		/* related SA */
+	uint64_t		cryptoid;	/* used crypto session id */
+	u_int			idx;		/* IPsec request index */
+	int			protoff;	/* current protocol offset */
+	int			skip;		/* data offset */
+	uint8_t			nxt;		/* next protocol, e.g. IPV4 */
 };
 
-struct secasvar;
-struct ipescrequest;
-
-struct xformsw {
-	u_short	xf_type;		/* xform ID */
-#define	XF_IP4		1	/* IP inside IP */
+#define	XF_IP4		1	/* unused */
 #define	XF_AH		2	/* AH */
 #define	XF_ESP		3	/* ESP */
 #define	XF_TCPSIGNATURE	5	/* TCP MD5 Signature option, RFC 2358 */
 #define	XF_IPCOMP	6	/* IPCOMP */
-	u_short	xf_flags;
-#define	XFT_AUTH	0x0001
-#define	XFT_CONF	0x0100
-#define	XFT_COMP	0x1000
-	char	*xf_name;			/* human-readable name */
+
+struct xformsw {
+	u_short	xf_type;		/* xform ID */
+	char	*xf_name;		/* human-readable name */
 	int	(*xf_init)(struct secasvar*, struct xformsw*);	/* setup */
 	int	(*xf_zeroize)(struct secasvar*);		/* cleanup */
 	int	(*xf_input)(struct mbuf*, struct secasvar*,	/* input */
 			int, int);
-	int	(*xf_output)(struct mbuf*,	       		/* output */
-			struct ipsecrequest *, struct mbuf **, int, int);
-	struct xformsw *xf_next;		/* list of registered xforms */
+	int	(*xf_output)(struct mbuf*,			/* output */
+	    struct secpolicy *, struct secasvar *, u_int, int, int);
+	LIST_ENTRY(xformsw)	chain;
 };
 
-#ifdef _KERNEL
-extern void xform_register(struct xformsw*);
-extern int xform_init(struct secasvar *sav, int xftype);
+const struct enc_xform * enc_algorithm_lookup(int);
+const struct auth_hash * auth_algorithm_lookup(int);
+const struct comp_algo * comp_algorithm_lookup(int);
 
-struct cryptoini;
-
-/* XF_IP4 */
-extern	int ip4_input6(struct mbuf **m, int *offp, int proto);
-extern	void ip4_input(struct mbuf *m, int);
-extern	int ipip_output(struct mbuf *, struct ipsecrequest *,
-			struct mbuf **, int, int);
+void xform_attach(void *);
+void xform_detach(void *);
 
+struct cryptoini;
 /* XF_AH */
+int xform_ah_authsize(const struct auth_hash *);
 extern int ah_init0(struct secasvar *, struct xformsw *, struct cryptoini *);
 extern int ah_zeroize(struct secasvar *sav);
-extern struct auth_hash *ah_algorithm_lookup(int alg);
 extern size_t ah_hdrsiz(struct secasvar *);
 
 /* XF_ESP */
-extern struct enc_xform *esp_algorithm_lookup(int alg);
 extern size_t esp_hdrsiz(struct secasvar *sav);
 
-/* XF_COMP */
-extern struct comp_algo *ipcomp_algorithm_lookup(int alg);
-
 #endif /* _KERNEL */
 #endif /* _NETIPSEC_XFORM_H_ */
diff --git a/freebsd/sys/netipsec/xform_ah.c b/freebsd/sys/netipsec/xform_ah.c
index f1304c24..5dd41282 100644
--- a/freebsd/sys/netipsec/xform_ah.c
+++ b/freebsd/sys/netipsec/xform_ah.c
@@ -47,6 +47,8 @@
 #include <sys/socket.h>
 #include <sys/syslog.h>
 #include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
+#include <sys/mutex.h>
 #include <sys/sysctl.h>
 
 #include <net/if.h>
@@ -58,7 +60,6 @@
 #include <netinet/ip_ecn.h>
 #include <netinet/ip6.h>
 
-#include <net/route.h>
 #include <netipsec/ipsec.h>
 #include <netipsec/ah.h>
 #include <netipsec/ah_var.h>
@@ -83,24 +84,29 @@
 	(((sav)->flags & SADB_X_EXT_OLD) ? \
 		sizeof (struct ah) : sizeof (struct ah) + sizeof (u_int32_t))
 /* 
- * Return authenticator size in bytes.  The old protocol is known
- * to use a fixed 16-byte authenticator.  The new algorithm use 12-byte
- * authenticator.
+ * Return authenticator size in bytes, based on a field in the
+ * algorithm descriptor.
  */
-#define	AUTHSIZE(sav)	ah_authsize(sav)
+#define	AUTHSIZE(sav)	((sav->flags & SADB_X_EXT_OLD) ? 16 :	\
+			 xform_ah_authsize((sav)->tdb_authalgxform))
 
 VNET_DEFINE(int, ah_enable) = 1;	/* control flow of packets with AH */
 VNET_DEFINE(int, ah_cleartos) = 1;	/* clear ip_tos when doing AH calc */
-VNET_DEFINE(struct ahstat, ahstat);
+VNET_PCPUSTAT_DEFINE(struct ahstat, ahstat);
+VNET_PCPUSTAT_SYSINIT(ahstat);
+
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(ahstat);
+#endif /* VIMAGE */
 
 #ifdef INET
 SYSCTL_DECL(_net_inet_ah);
-SYSCTL_VNET_INT(_net_inet_ah, OID_AUTO,
-	ah_enable,	CTLFLAG_RW,	&VNET_NAME(ah_enable),	0, "");
-SYSCTL_VNET_INT(_net_inet_ah, OID_AUTO,
-	ah_cleartos,	CTLFLAG_RW,	&VNET_NAME(ah_cleartos), 0, "");
-SYSCTL_VNET_STRUCT(_net_inet_ah, IPSECCTL_STATS,
-	stats,		CTLFLAG_RD,	&VNET_NAME(ahstat), ahstat, "");
+SYSCTL_INT(_net_inet_ah, OID_AUTO, ah_enable,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ah_enable), 0, "");
+SYSCTL_INT(_net_inet_ah, OID_AUTO, ah_cleartos,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ah_cleartos), 0, "");
+SYSCTL_VNET_PCPUSTAT(_net_inet_ah, IPSECCTL_STATS, stats, struct ahstat,
+    ahstat, "AH statistics (struct ahstat, netipsec/ah_var.h)");
 #endif
 
 static unsigned char ipseczeroes[256];	/* larger than an ip6 extension hdr */
@@ -108,56 +114,33 @@ static unsigned char ipseczeroes[256];	/* larger than an ip6 extension hdr */
 static int ah_input_cb(struct cryptop*);
 static int ah_output_cb(struct cryptop*);
 
-static int
-ah_authsize(struct secasvar *sav)
+int
+xform_ah_authsize(const struct auth_hash *esph)
 {
+	int alen;
 
-	IPSEC_ASSERT(sav != NULL, ("%s: sav == NULL", __func__));
+	if (esph == NULL)
+		return 0;
 
-	if (sav->flags & SADB_X_EXT_OLD)
-		return 16;
+	switch (esph->type) {
+	case CRYPTO_SHA2_256_HMAC:
+	case CRYPTO_SHA2_384_HMAC:
+	case CRYPTO_SHA2_512_HMAC:
+		alen = esph->hashsize / 2;	/* RFC4868 2.3 */
+		break;
+
+	case CRYPTO_AES_128_NIST_GMAC:
+	case CRYPTO_AES_192_NIST_GMAC:
+	case CRYPTO_AES_256_NIST_GMAC:
+		alen = esph->hashsize;
+		break;
 
-	switch (sav->alg_auth) {
-	case SADB_X_AALG_SHA2_256:
-		return 16;
-	case SADB_X_AALG_SHA2_384:
-		return 24;
-	case SADB_X_AALG_SHA2_512:
-		return 32;
 	default:
-		return AH_HMAC_HASHLEN;
-	}
-	/* NOTREACHED */
-}
-/*
- * NB: this is public for use by the PF_KEY support.
- */
-struct auth_hash *
-ah_algorithm_lookup(int alg)
-{
-	if (alg > SADB_AALG_MAX)
-		return NULL;
-	switch (alg) {
-	case SADB_X_AALG_NULL:
-		return &auth_hash_null;
-	case SADB_AALG_MD5HMAC:
-		return &auth_hash_hmac_md5;
-	case SADB_AALG_SHA1HMAC:
-		return &auth_hash_hmac_sha1;
-	case SADB_X_AALG_RIPEMD160HMAC:
-		return &auth_hash_hmac_ripemd_160;
-	case SADB_X_AALG_MD5:
-		return &auth_hash_key_md5;
-	case SADB_X_AALG_SHA:
-		return &auth_hash_key_sha1;
-	case SADB_X_AALG_SHA2_256:
-		return &auth_hash_hmac_sha2_256;
-	case SADB_X_AALG_SHA2_384:
-		return &auth_hash_hmac_sha2_384;
-	case SADB_X_AALG_SHA2_512:
-		return &auth_hash_hmac_sha2_512;
+		alen = AH_HMAC_HASHLEN;
+		break;
 	}
-	return NULL;
+
+	return alen;
 }
 
 size_t
@@ -184,10 +167,10 @@ ah_hdrsiz(struct secasvar *sav)
 int
 ah_init0(struct secasvar *sav, struct xformsw *xsp, struct cryptoini *cria)
 {
-	struct auth_hash *thash;
+	const struct auth_hash *thash;
 	int keylen;
 
-	thash = ah_algorithm_lookup(sav->alg_auth);
+	thash = auth_algorithm_lookup(sav->alg_auth);
 	if (thash == NULL) {
 		DPRINTF(("%s: unsupported authentication algorithm %u\n",
 			__func__, sav->alg_auth));
@@ -307,23 +290,10 @@ ah_massage_headers(struct mbuf **m0, int proto, int skip, int alg, int out)
 		ip->ip_ttl = 0;
 		ip->ip_sum = 0;
 
-		/*
-		 * On input, fix ip_len which has been byte-swapped
-		 * at ip_input().
-		 */
-		if (!out) {
-			ip->ip_len = htons(ip->ip_len + skip);
-
-			if (alg == CRYPTO_MD5_KPDK || alg == CRYPTO_SHA1_KPDK)
-				ip->ip_off = htons(ip->ip_off & IP_DF);
-			else
-				ip->ip_off = 0;
-		} else {
-			if (alg == CRYPTO_MD5_KPDK || alg == CRYPTO_SHA1_KPDK)
-				ip->ip_off = htons(ntohs(ip->ip_off) & IP_DF);
-			else
-				ip->ip_off = 0;
-		}
+		if (alg == CRYPTO_MD5_KPDK || alg == CRYPTO_SHA1_KPDK)
+			ip->ip_off &= htons(IP_DF);
+		else
+			ip->ip_off = htons(0);
 
 		ptr = mtod(m, unsigned char *) + sizeof(struct ip);
 
@@ -576,15 +546,14 @@ ah_massage_headers(struct mbuf **m0, int proto, int skip, int alg, int out)
 static int
 ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 {
-	struct auth_hash *ahx;
-	struct tdb_ident *tdbi;
-	struct tdb_crypto *tc;
-	struct m_tag *mtag;
-	struct newah *ah;
-	int hl, rplen, authsize;
-
+	char buf[128];
+	const struct auth_hash *ahx;
 	struct cryptodesc *crda;
 	struct cryptop *crp;
+	struct xform_data *xd;
+	struct newah *ah;
+	uint64_t cryptoid;
+	int hl, rplen, authsize, error;
 
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	IPSEC_ASSERT(sav->key_auth != NULL, ("null authentication key"));
@@ -604,13 +573,18 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	}
 
 	/* Check replay window, if applicable. */
-	if (sav->replay && !ipsec_chkreplay(ntohl(ah->ah_seq), sav)) {
+	SECASVAR_LOCK(sav);
+	if (sav->replay != NULL && sav->replay->wsize != 0 &&
+	    ipsec_chkreplay(ntohl(ah->ah_seq), sav) == 0) {
+		SECASVAR_UNLOCK(sav);
 		AHSTAT_INC(ahs_replay);
 		DPRINTF(("%s: packet replay failure: %s\n", __func__,
-			  ipsec_logsastr(sav)));
+		    ipsec_sa2str(sav, buf, sizeof(buf))));
 		m_freem(m);
-		return ENOBUFS;
+		return (EACCES);
 	}
+	cryptoid = sav->tdb_cryptoid;
+	SECASVAR_UNLOCK(sav);
 
 	/* Verify AH header length. */
 	hl = ah->ah_len * sizeof (u_int32_t);
@@ -618,10 +592,10 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	authsize = AUTHSIZE(sav);
 	if (hl != authsize + rplen - sizeof (struct ah)) {
 		DPRINTF(("%s: bad authenticator length %u (expecting %lu)"
-			" for packet in SA %s/%08lx\n", __func__,
-			hl, (u_long) (authsize + rplen - sizeof (struct ah)),
-			ipsec_address(&sav->sah->saidx.dst),
-			(u_long) ntohl(sav->spi)));
+		    " for packet in SA %s/%08lx\n", __func__, hl,
+		    (u_long) (authsize + rplen - sizeof (struct ah)),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		AHSTAT_INC(ahs_badauthl);
 		m_freem(m);
 		return EACCES;
@@ -631,7 +605,8 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	/* Get crypto descriptors. */
 	crp = crypto_getreq(1);
 	if (crp == NULL) {
-		DPRINTF(("%s: failed to acquire crypto descriptor\n",__func__));
+		DPRINTF(("%s: failed to acquire crypto descriptor\n",
+		    __func__));
 		AHSTAT_INC(ahs_crypto);
 		m_freem(m);
 		return ENOBUFS;
@@ -649,58 +624,35 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	crda->crd_klen = _KEYBITS(sav->key_auth);
 	crda->crd_key = sav->key_auth->key_data;
 
-	/* Find out if we've already done crypto. */
-	for (mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, NULL);
-	     mtag != NULL;
-	     mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, mtag)) {
-		tdbi = (struct tdb_ident *) (mtag + 1);
-		if (tdbi->proto == sav->sah->saidx.proto &&
-		    tdbi->spi == sav->spi &&
-		    !bcmp(&tdbi->dst, &sav->sah->saidx.dst,
-			  sizeof (union sockaddr_union)))
-			break;
-	}
-
 	/* Allocate IPsec-specific opaque crypto info. */
-	if (mtag == NULL) {
-		tc = (struct tdb_crypto *) malloc(sizeof (struct tdb_crypto) +
-			skip + rplen + authsize, M_XDATA, M_NOWAIT|M_ZERO);
-	} else {
-		/* Hash verification has already been done successfully. */
-		tc = (struct tdb_crypto *) malloc(sizeof (struct tdb_crypto),
-						    M_XDATA, M_NOWAIT|M_ZERO);
-	}
-	if (tc == NULL) {
-		DPRINTF(("%s: failed to allocate tdb_crypto\n", __func__));
+	xd = malloc(sizeof(*xd) + skip + rplen + authsize, M_XDATA,
+	    M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
+		DPRINTF(("%s: failed to allocate xform_data\n", __func__));
 		AHSTAT_INC(ahs_crypto);
 		crypto_freereq(crp);
 		m_freem(m);
 		return ENOBUFS;
 	}
 
-	/* Only save information if crypto processing is needed. */
-	if (mtag == NULL) {
-		int error;
+	/*
+	 * Save the authenticator, the skipped portion of the packet,
+	 * and the AH header.
+	 */
+	m_copydata(m, 0, skip + rplen + authsize, (caddr_t)(xd + 1));
 
-		/*
-		 * Save the authenticator, the skipped portion of the packet,
-		 * and the AH header.
-		 */
-		m_copydata(m, 0, skip + rplen + authsize, (caddr_t)(tc+1));
-
-		/* Zeroize the authenticator on the packet. */
-		m_copyback(m, skip + rplen, authsize, ipseczeroes);
-
-		/* "Massage" the packet headers for crypto processing. */
-		error = ah_massage_headers(&m, sav->sah->saidx.dst.sa.sa_family,
-		    skip, ahx->type, 0);
-		if (error != 0) {
-			/* NB: mbuf is free'd by ah_massage_headers */
-			AHSTAT_INC(ahs_hdrops);
-			free(tc, M_XDATA);
-			crypto_freereq(crp);
-			return error;
-		}
+	/* Zeroize the authenticator on the packet. */
+	m_copyback(m, skip + rplen, authsize, ipseczeroes);
+
+	/* "Massage" the packet headers for crypto processing. */
+	error = ah_massage_headers(&m, sav->sah->saidx.dst.sa.sa_family,
+	    skip, ahx->type, 0);
+	if (error != 0) {
+		/* NB: mbuf is free'd by ah_massage_headers */
+		AHSTAT_INC(ahs_hdrops);
+		free(xd, M_XDATA);
+		crypto_freereq(crp);
+		return (error);
 	}
 
 	/* Crypto operation descriptor. */
@@ -708,24 +660,16 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = ah_input_cb;
-	crp->crp_sid = sav->tdb_cryptoid;
-	crp->crp_opaque = (caddr_t) tc;
+	crp->crp_sid = cryptoid;
+	crp->crp_opaque = (caddr_t) xd;
 
 	/* These are passed as-is to the callback. */
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = sav->sah->saidx.dst;
-	tc->tc_proto = sav->sah->saidx.proto;
-	tc->tc_nxt = ah->ah_nxt;
-	tc->tc_protoff = protoff;
-	tc->tc_skip = skip;
-	tc->tc_ptr = (caddr_t) mtag; /* Save the mtag we've identified. */
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
-
-	if (mtag == NULL)
-		return crypto_dispatch(crp);
-	else
-		return ah_input_cb(crp);
+	xd->sav = sav;
+	xd->nxt = ah->ah_nxt;
+	xd->protoff = protoff;
+	xd->skip = skip;
+	xd->cryptoid = cryptoid;
+	return (crypto_dispatch(crp));
 }
 
 /*
@@ -734,49 +678,43 @@ ah_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 static int
 ah_input_cb(struct cryptop *crp)
 {
-	int rplen, error, skip, protoff;
+	char buf[IPSEC_ADDRSTRLEN];
 	unsigned char calc[AH_ALEN_MAX];
+	const struct auth_hash *ahx;
 	struct mbuf *m;
 	struct cryptodesc *crd;
-	struct auth_hash *ahx;
-	struct tdb_crypto *tc;
-	struct m_tag *mtag;
+	struct xform_data *xd;
 	struct secasvar *sav;
 	struct secasindex *saidx;
-	u_int8_t nxt;
 	caddr_t ptr;
-	int authsize;
+	uint64_t cryptoid;
+	int authsize, rplen, error, skip, protoff;
+	uint8_t nxt;
 
 	crd = crp->crp_desc;
-
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque crypto data area!"));
-	skip = tc->tc_skip;
-	nxt = tc->tc_nxt;
-	protoff = tc->tc_protoff;
-	mtag = (struct m_tag *) tc->tc_ptr;
 	m = (struct mbuf *) crp->crp_buf;
-
-	sav = tc->tc_sav;
-	IPSEC_ASSERT(sav != NULL, ("null SA!"));
-
+	xd = (struct xform_data *) crp->crp_opaque;
+	sav = xd->sav;
+	skip = xd->skip;
+	nxt = xd->nxt;
+	protoff = xd->protoff;
+	cryptoid = xd->cryptoid;
 	saidx = &sav->sah->saidx;
 	IPSEC_ASSERT(saidx->dst.sa.sa_family == AF_INET ||
 		saidx->dst.sa.sa_family == AF_INET6,
 		("unexpected protocol family %u", saidx->dst.sa.sa_family));
 
-	ahx = (struct auth_hash *) sav->tdb_authalgxform;
+	ahx = sav->tdb_authalgxform;
 
 	/* Check for crypto errors. */
 	if (crp->crp_etype) {
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			error = crypto_dispatch(crp);
-			return error;
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
-
 		AHSTAT_INC(ahs_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
 		error = crp->crp_etype;
@@ -802,35 +740,23 @@ ah_input_cb(struct cryptop *crp)
 	/* Copy authenticator off the packet. */
 	m_copydata(m, skip + rplen, authsize, calc);
 
-	/*
-	 * If we have an mtag, we don't need to verify the authenticator --
-	 * it has been verified by an IPsec-aware NIC.
-	 */
-	if (mtag == NULL) {
-		ptr = (caddr_t) (tc + 1);
-
-		/* Verify authenticator. */
-		if (bcmp(ptr + skip + rplen, calc, authsize)) {
-			DPRINTF(("%s: authentication hash mismatch for packet "
-			    "in SA %s/%08lx\n", __func__,
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-			AHSTAT_INC(ahs_badauth);
-			error = EACCES;
-			goto bad;
-		}
-
-		/* Fix the Next Protocol field. */
-		((u_int8_t *) ptr)[protoff] = nxt;
-
-		/* Copyback the saved (uncooked) network headers. */
-		m_copyback(m, 0, skip, ptr);
-	} else {
-		/* Fix the Next Protocol field. */
-		m_copyback(m, protoff, sizeof(u_int8_t), &nxt);
+	/* Verify authenticator. */
+	ptr = (caddr_t) (xd + 1);
+	if (timingsafe_bcmp(ptr + skip + rplen, calc, authsize)) {
+		DPRINTF(("%s: authentication hash mismatch for packet "
+		    "in SA %s/%08lx\n", __func__,
+		    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
+		AHSTAT_INC(ahs_badauth);
+		error = EACCES;
+		goto bad;
 	}
+	/* Fix the Next Protocol field. */
+	((uint8_t *) ptr)[protoff] = nxt;
 
-	free(tc, M_XDATA), tc = NULL;			/* No longer needed */
+	/* Copyback the saved (uncooked) network headers. */
+	m_copyback(m, 0, skip, ptr);
+	free(xd, M_XDATA), xd = NULL;			/* No longer needed */
 
 	/*
 	 * Header is now authenticated.
@@ -845,11 +771,14 @@ ah_input_cb(struct cryptop *crp)
 
 		m_copydata(m, skip + offsetof(struct newah, ah_seq),
 			   sizeof (seq), (caddr_t) &seq);
+		SECASVAR_LOCK(sav);
 		if (ipsec_updatereplay(ntohl(seq), sav)) {
+			SECASVAR_UNLOCK(sav);
 			AHSTAT_INC(ahs_replay);
-			error = ENOBUFS;			/*XXX as above*/
+			error = EACCES;
 			goto bad;
 		}
+		SECASVAR_UNLOCK(sav);
 	}
 
 	/*
@@ -858,8 +787,8 @@ ah_input_cb(struct cryptop *crp)
 	error = m_striphdr(m, skip, rplen + authsize);
 	if (error) {
 		DPRINTF(("%s: mangled mbuf chain for SA %s/%08lx\n", __func__,
-		    ipsec_address(&saidx->dst), (u_long) ntohl(sav->spi)));
-
+		    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		AHSTAT_INC(ahs_hdrops);
 		goto bad;
 	}
@@ -867,56 +796,50 @@ ah_input_cb(struct cryptop *crp)
 	switch (saidx->dst.sa.sa_family) {
 #ifdef INET6
 	case AF_INET6:
-		error = ipsec6_common_input_cb(m, sav, skip, protoff, mtag);
+		error = ipsec6_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 #ifdef INET
 	case AF_INET:
-		error = ipsec4_common_input_cb(m, sav, skip, protoff, mtag);
+		error = ipsec4_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 	default:
 		panic("%s: Unexpected address family: %d saidx=%p", __func__,
 		    saidx->dst.sa.sa_family, saidx);
 	}
-
-	KEY_FREESAV(&sav);
 	return error;
 bad:
 	if (sav)
-		KEY_FREESAV(&sav);
+		key_freesav(&sav);
 	if (m != NULL)
 		m_freem(m);
-	if (tc != NULL)
-		free(tc, M_XDATA);
+	if (xd != NULL)
+		free(xd, M_XDATA);
 	if (crp != NULL)
 		crypto_freereq(crp);
 	return error;
 }
 
 /*
- * AH output routine, called by ipsec[46]_process_packet().
+ * AH output routine, called by ipsec[46]_perform_request().
  */
 static int
-ah_output(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	struct mbuf **mp,
-	int skip,
-	int protoff)
+ah_output(struct mbuf *m, struct secpolicy *sp, struct secasvar *sav,
+    u_int idx, int skip, int protoff)
 {
-	struct secasvar *sav;
-	struct auth_hash *ahx;
+	char buf[IPSEC_ADDRSTRLEN];
+	const struct auth_hash *ahx;
 	struct cryptodesc *crda;
-	struct tdb_crypto *tc;
+	struct xform_data *xd;
 	struct mbuf *mi;
 	struct cryptop *crp;
-	u_int16_t iplen;
-	int error, rplen, authsize, maxpacketsize, roff;
-	u_int8_t prot;
 	struct newah *ah;
+	uint64_t cryptoid;
+	uint16_t iplen;
+	int error, rplen, authsize, maxpacketsize, roff;
+	uint8_t prot;
 
-	sav = isr->sav;
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	ahx = sav->tdb_authalgxform;
 	IPSEC_ASSERT(ahx != NULL, ("null authentication xform"));
@@ -942,7 +865,7 @@ ah_output(
 		DPRINTF(("%s: unknown/unsupported protocol family %u, "
 		    "SA %s/%08lx\n", __func__,
 		    sav->sah->saidx.dst.sa.sa_family,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi)));
 		AHSTAT_INC(ahs_nopf);
 		error = EPFNOSUPPORT;
@@ -952,7 +875,7 @@ ah_output(
 	if (rplen + authsize + m->m_pkthdr.len > maxpacketsize) {
 		DPRINTF(("%s: packet in SA %s/%08lx got too big "
 		    "(len %u, max len %u)\n", __func__,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi),
 		    rplen + authsize + m->m_pkthdr.len, maxpacketsize));
 		AHSTAT_INC(ahs_toobig);
@@ -966,7 +889,7 @@ ah_output(
 	m = m_unshare(m, M_NOWAIT);
 	if (m == NULL) {
 		DPRINTF(("%s: cannot clone mbuf chain, SA %s/%08lx\n", __func__,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi)));
 		AHSTAT_INC(ahs_hdrops);
 		error = ENOBUFS;
@@ -979,7 +902,7 @@ ah_output(
 		DPRINTF(("%s: failed to inject %u byte AH header for SA "
 		    "%s/%08lx\n", __func__,
 		    rplen + authsize,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi)));
 		AHSTAT_INC(ahs_hdrops);		/*XXX differs from openbsd */
 		error = ENOBUFS;
@@ -1002,15 +925,16 @@ ah_output(
 	m_copyback(m, skip + rplen, authsize, ipseczeroes);
 
 	/* Insert packet replay counter, as requested.  */
+	SECASVAR_LOCK(sav);
 	if (sav->replay) {
 		if (sav->replay->count == ~0 &&
 		    (sav->flags & SADB_X_EXT_CYCSEQ) == 0) {
+			SECASVAR_UNLOCK(sav);
 			DPRINTF(("%s: replay counter wrapped for SA %s/%08lx\n",
-				__func__,
-				ipsec_address(&sav->sah->saidx.dst),
-				(u_long) ntohl(sav->spi)));
+			    __func__, ipsec_address(&sav->sah->saidx.dst, buf,
+			    sizeof(buf)), (u_long) ntohl(sav->spi)));
 			AHSTAT_INC(ahs_wrap);
-			error = EINVAL;
+			error = EACCES;
 			goto bad;
 		}
 #ifdef REGRESSION
@@ -1020,6 +944,8 @@ ah_output(
 			sav->replay->count++;
 		ah->ah_seq = htonl(sav->replay->count);
 	}
+	cryptoid = sav->tdb_cryptoid;
+	SECASVAR_UNLOCK(sav);
 
 	/* Get crypto descriptors. */
 	crp = crypto_getreq(1);
@@ -1032,7 +958,6 @@ ah_output(
 	}
 
 	crda = crp->crp_desc;
-
 	crda->crd_skip = 0;
 	crda->crd_inject = skip + rplen;
 	crda->crd_len = m->m_pkthdr.len;
@@ -1043,18 +968,18 @@ ah_output(
 	crda->crd_klen = _KEYBITS(sav->key_auth);
 
 	/* Allocate IPsec-specific opaque crypto info. */
-	tc = (struct tdb_crypto *) malloc(
-		sizeof(struct tdb_crypto) + skip, M_XDATA, M_NOWAIT|M_ZERO);
-	if (tc == NULL) {
+	xd =  malloc(sizeof(struct xform_data) + skip, M_XDATA,
+	    M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
 		crypto_freereq(crp);
-		DPRINTF(("%s: failed to allocate tdb_crypto\n", __func__));
+		DPRINTF(("%s: failed to allocate xform_data\n", __func__));
 		AHSTAT_INC(ahs_crypto);
 		error = ENOBUFS;
 		goto bad;
 	}
 
 	/* Save the skipped portion of the packet. */
-	m_copydata(m, 0, skip, (caddr_t) (tc + 1));
+	m_copydata(m, 0, skip, (caddr_t) (xd + 1));
 
 	/*
 	 * Fix IP header length on the header used for
@@ -1064,7 +989,7 @@ ah_output(
 	switch (sav->sah->saidx.dst.sa.sa_family) {
 #ifdef INET
 	case AF_INET:
-		bcopy(((caddr_t)(tc + 1)) +
+		bcopy(((caddr_t)(xd + 1)) +
 		    offsetof(struct ip, ip_len),
 		    (caddr_t) &iplen, sizeof(u_int16_t));
 		iplen = htons(ntohs(iplen) + rplen + authsize);
@@ -1075,29 +1000,29 @@ ah_output(
 
 #ifdef INET6
 	case AF_INET6:
-		bcopy(((caddr_t)(tc + 1)) +
+		bcopy(((caddr_t)(xd + 1)) +
 		    offsetof(struct ip6_hdr, ip6_plen),
-		    (caddr_t) &iplen, sizeof(u_int16_t));
+		    (caddr_t) &iplen, sizeof(uint16_t));
 		iplen = htons(ntohs(iplen) + rplen + authsize);
 		m_copyback(m, offsetof(struct ip6_hdr, ip6_plen),
-		    sizeof(u_int16_t), (caddr_t) &iplen);
+		    sizeof(uint16_t), (caddr_t) &iplen);
 		break;
 #endif /* INET6 */
 	}
 
 	/* Fix the Next Header field in saved header. */
-	((u_int8_t *) (tc + 1))[protoff] = IPPROTO_AH;
+	((uint8_t *) (xd + 1))[protoff] = IPPROTO_AH;
 
 	/* Update the Next Protocol field in the IP header. */
 	prot = IPPROTO_AH;
-	m_copyback(m, protoff, sizeof(u_int8_t), (caddr_t) &prot);
+	m_copyback(m, protoff, sizeof(uint8_t), (caddr_t) &prot);
 
 	/* "Massage" the packet headers for crypto processing. */
 	error = ah_massage_headers(&m, sav->sah->saidx.dst.sa.sa_family,
 			skip, ahx->type, 1);
 	if (error != 0) {
 		m = NULL;	/* mbuf was free'd by ah_massage_headers. */
-		free(tc, M_XDATA);
+		free(xd, M_XDATA);
 		crypto_freereq(crp);
 		goto bad;
 	}
@@ -1107,18 +1032,15 @@ ah_output(
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = ah_output_cb;
-	crp->crp_sid = sav->tdb_cryptoid;
-	crp->crp_opaque = (caddr_t) tc;
+	crp->crp_sid = cryptoid;
+	crp->crp_opaque = (caddr_t) xd;
 
 	/* These are passed as-is to the callback. */
-	tc->tc_isr = isr;
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = sav->sah->saidx.dst;
-	tc->tc_proto = sav->sah->saidx.proto;
-	tc->tc_skip = skip;
-	tc->tc_protoff = protoff;
+	xd->sp = sp;
+	xd->sav = sav;
+	xd->skip = skip;
+	xd->idx = idx;
+	xd->cryptoid = cryptoid;
 
 	return crypto_dispatch(crp);
 bad:
@@ -1133,46 +1055,37 @@ bad:
 static int
 ah_output_cb(struct cryptop *crp)
 {
-	int skip, protoff, error;
-	struct tdb_crypto *tc;
-	struct ipsecrequest *isr;
+	struct xform_data *xd;
+	struct secpolicy *sp;
 	struct secasvar *sav;
 	struct mbuf *m;
+	uint64_t cryptoid;
 	caddr_t ptr;
-	int err;
+	u_int idx;
+	int skip, error;
 
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque data area!"));
-	skip = tc->tc_skip;
-	protoff = tc->tc_protoff;
-	ptr = (caddr_t) (tc + 1);
 	m = (struct mbuf *) crp->crp_buf;
-
-	isr = tc->tc_isr;
-	IPSECREQUEST_LOCK(isr);
-	sav = tc->tc_sav;
-	/* With the isr lock released SA pointer can be updated. */
-	if (sav != isr->sav) {
-		AHSTAT_INC(ahs_notdb);
-		DPRINTF(("%s: SA expired while in crypto\n", __func__));
-		error = ENOBUFS;		/*XXX*/
-		goto bad;
-	}
+	xd = (struct xform_data *) crp->crp_opaque;
+	sp = xd->sp;
+	sav = xd->sav;
+	skip = xd->skip;
+	idx = xd->idx;
+	cryptoid = xd->cryptoid;
+	ptr = (caddr_t) (xd + 1);
 
 	/* Check for crypto errors. */
 	if (crp->crp_etype) {
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			IPSECREQUEST_UNLOCK(isr);
-			error = crypto_dispatch(crp);
-			return error;
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
-
 		AHSTAT_INC(ahs_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
 		error = crp->crp_etype;
+		m_freem(m);
 		goto bad;
 	}
 
@@ -1183,18 +1096,15 @@ ah_output_cb(struct cryptop *crp)
 		error = EINVAL;
 		goto bad;
 	}
-	AHSTAT_INC(ahs_hist[sav->alg_auth]);
-
 	/*
 	 * Copy original headers (with the new protocol number) back
 	 * in place.
 	 */
 	m_copyback(m, 0, skip, ptr);
 
-	/* No longer needed. */
-	free(tc, M_XDATA);
+	free(xd, M_XDATA);
 	crypto_freereq(crp);
-
+	AHSTAT_INC(ahs_hist[sav->alg_auth]);
 #ifdef REGRESSION
 	/* Emulate man-in-the-middle attack when ipsec_integrity is TRUE. */
 	if (V_ipsec_integrity) {
@@ -1210,31 +1120,26 @@ ah_output_cb(struct cryptop *crp)
 #endif
 
 	/* NB: m is reclaimed by ipsec_process_done. */
-	err = ipsec_process_done(m, isr);
-	KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
-	return err;
+	error = ipsec_process_done(m, sp, sav, idx);
+	return (error);
 bad:
-	if (sav)
-		KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
-	if (m)
-		m_freem(m);
-	free(tc, M_XDATA);
+	free(xd, M_XDATA);
 	crypto_freereq(crp);
-	return error;
+	key_freesav(&sav);
+	key_freesp(&sp);
+	return (error);
 }
 
 static struct xformsw ah_xformsw = {
-	XF_AH,		XFT_AUTH,	"IPsec AH",
-	ah_init,	ah_zeroize,	ah_input,	ah_output,
+	.xf_type =	XF_AH,
+	.xf_name =	"IPsec AH",
+	.xf_init =	ah_init,
+	.xf_zeroize =	ah_zeroize,
+	.xf_input =	ah_input,
+	.xf_output =	ah_output,
 };
 
-static void
-ah_attach(void)
-{
-
-	xform_register(&ah_xformsw);
-}
-
-SYSINIT(ah_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE, ah_attach, NULL);
+SYSINIT(ah_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    xform_attach, &ah_xformsw);
+SYSUNINIT(ah_xform_uninit, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    xform_detach, &ah_xformsw);
diff --git a/freebsd/sys/netipsec/xform_esp.c b/freebsd/sys/netipsec/xform_esp.c
index 20790d0d..bf98dc03 100644
--- a/freebsd/sys/netipsec/xform_esp.c
+++ b/freebsd/sys/netipsec/xform_esp.c
@@ -46,8 +46,12 @@
 #include <sys/socket.h>
 #include <sys/syslog.h>
 #include <sys/kernel.h>
+#include <rtems/bsd/sys/lock.h>
 #include <sys/random.h>
+#include <sys/mutex.h>
 #include <sys/sysctl.h>
+#include <sys/mutex.h>
+#include <machine/atomic.h>
 
 #include <net/if.h>
 #include <net/vnet.h>
@@ -58,7 +62,6 @@
 #include <netinet/ip_ecn.h>
 #include <netinet/ip6.h>
 
-#include <net/route.h>
 #include <netipsec/ipsec.h>
 #include <netipsec/ah.h>
 #include <netipsec/ah_var.h>
@@ -79,50 +82,23 @@
 #include <opencrypto/xform.h>
 
 VNET_DEFINE(int, esp_enable) = 1;
-VNET_DEFINE(struct espstat, espstat);
+VNET_PCPUSTAT_DEFINE(struct espstat, espstat);
+VNET_PCPUSTAT_SYSINIT(espstat);
 
-SYSCTL_DECL(_net_inet_esp);
-SYSCTL_VNET_INT(_net_inet_esp, OID_AUTO,
-	esp_enable,	CTLFLAG_RW,	&VNET_NAME(esp_enable),	0, "");
-SYSCTL_VNET_STRUCT(_net_inet_esp, IPSECCTL_STATS,
-	stats,		CTLFLAG_RD,	&VNET_NAME(espstat),	espstat, "");
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(espstat);
+#endif /* VIMAGE */
 
-static VNET_DEFINE(int, esp_max_ivlen);	/* max iv length over all algorithms */
-#define	V_esp_max_ivlen	VNET(esp_max_ivlen)
+SYSCTL_DECL(_net_inet_esp);
+SYSCTL_INT(_net_inet_esp, OID_AUTO, esp_enable,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(esp_enable), 0, "");
+SYSCTL_VNET_PCPUSTAT(_net_inet_esp, IPSECCTL_STATS, stats,
+    struct espstat, espstat,
+    "ESP statistics (struct espstat, netipsec/esp_var.h");
 
 static int esp_input_cb(struct cryptop *op);
 static int esp_output_cb(struct cryptop *crp);
 
-/*
- * NB: this is public for use by the PF_KEY support.
- * NB: if you add support here; be sure to add code to esp_attach below!
- */
-struct enc_xform *
-esp_algorithm_lookup(int alg)
-{
-	if (alg >= ESP_ALG_MAX)
-		return NULL;
-	switch (alg) {
-	case SADB_EALG_DESCBC:
-		return &enc_xform_des;
-	case SADB_EALG_3DESCBC:
-		return &enc_xform_3des;
-	case SADB_X_EALG_AES:
-		return &enc_xform_rijndael128;
-	case SADB_X_EALG_BLOWFISHCBC:
-		return &enc_xform_blf;
-	case SADB_X_EALG_CAST128CBC:
-		return &enc_xform_cast5;
-	case SADB_X_EALG_SKIPJACK:
-		return &enc_xform_skipjack;
-	case SADB_EALG_NULL:
-		return &enc_xform_null;
-	case SADB_X_EALG_CAMELLIACBC:
-		return &enc_xform_camellia;
-	}
-	return NULL;
-}
-
 size_t
 esp_hdrsiz(struct secasvar *sav)
 {
@@ -149,7 +125,7 @@ esp_hdrsiz(struct secasvar *sav)
 		 * + sizeof (next header field)
 		 * + max icv supported.
 		 */
-		size = sizeof (struct newesp) + V_esp_max_ivlen + 9 + 16;
+		size = sizeof (struct newesp) + EALG_MAX_BLOCK_LEN + 9 + 16;
 	}
 	return size;
 }
@@ -160,12 +136,12 @@ esp_hdrsiz(struct secasvar *sav)
 static int
 esp_init(struct secasvar *sav, struct xformsw *xsp)
 {
-	struct enc_xform *txform;
+	const struct enc_xform *txform;
 	struct cryptoini cria, crie;
 	int keylen;
 	int error;
 
-	txform = esp_algorithm_lookup(sav->alg_enc);
+	txform = enc_algorithm_lookup(sav->alg_enc);
 	if (txform == NULL) {
 		DPRINTF(("%s: unsupported encryption algorithm %d\n",
 			__func__, sav->alg_enc));
@@ -176,12 +152,14 @@ esp_init(struct secasvar *sav, struct xformsw *xsp)
 			 __func__, txform->name));
 		return EINVAL;
 	}
-	if ((sav->flags&(SADB_X_EXT_OLD|SADB_X_EXT_IV4B)) == SADB_X_EXT_IV4B) {
+	if ((sav->flags & (SADB_X_EXT_OLD | SADB_X_EXT_IV4B)) ==
+	    SADB_X_EXT_IV4B) {
 		DPRINTF(("%s: 4-byte IV not supported with protocol\n",
 			__func__));
 		return EINVAL;
 	}
-	keylen = _KEYLEN(sav->key_enc);
+	/* subtract off the salt, RFC4106, 8.1 and RFC3686, 5.1 */
+	keylen = _KEYLEN(sav->key_enc) - SAV_ISCTRORGCM(sav) * 4;
 	if (txform->minkey > keylen || keylen > txform->maxkey) {
 		DPRINTF(("%s: invalid key length %u, must be in the range "
 			"[%u..%u] for algorithm %s\n", __func__,
@@ -190,19 +168,10 @@ esp_init(struct secasvar *sav, struct xformsw *xsp)
 		return EINVAL;
 	}
 
-	/*
-	 * NB: The null xform needs a non-zero blocksize to keep the
-	 *      crypto code happy but if we use it to set ivlen then
-	 *      the ESP header will be processed incorrectly.  The
-	 *      compromise is to force it to zero here.
-	 */
-	sav->ivlen = (txform == &enc_xform_null ? 0 : txform->blocksize);
-	sav->iv = (caddr_t) malloc(sav->ivlen, M_XDATA, M_WAITOK);
-	if (sav->iv == NULL) {
-		DPRINTF(("%s: no memory for IV\n", __func__));
-		return EINVAL;
-	}
-	key_randomfill(sav->iv, sav->ivlen);	/*XXX*/
+	if (SAV_ISCTRORGCM(sav))
+		sav->ivlen = 8;	/* RFC4106 3.1 and RFC3686 3.1 */
+	else
+		sav->ivlen = txform->ivsize;
 
 	/*
 	 * Setup AH-related state.
@@ -217,12 +186,42 @@ esp_init(struct secasvar *sav, struct xformsw *xsp)
 	sav->tdb_xform = xsp;
 	sav->tdb_encalgxform = txform;
 
+	/*
+	 * Whenever AES-GCM is used for encryption, one
+	 * of the AES authentication algorithms is chosen
+	 * as well, based on the key size.
+	 */
+	if (sav->alg_enc == SADB_X_EALG_AESGCM16) {
+		switch (keylen) {
+		case AES_128_GMAC_KEY_LEN:
+			sav->alg_auth = SADB_X_AALG_AES128GMAC;
+			sav->tdb_authalgxform = &auth_hash_nist_gmac_aes_128;
+			break;
+		case AES_192_GMAC_KEY_LEN:
+			sav->alg_auth = SADB_X_AALG_AES192GMAC;
+			sav->tdb_authalgxform = &auth_hash_nist_gmac_aes_192;
+			break;
+		case AES_256_GMAC_KEY_LEN:
+			sav->alg_auth = SADB_X_AALG_AES256GMAC;
+			sav->tdb_authalgxform = &auth_hash_nist_gmac_aes_256;
+			break;
+		default:
+			DPRINTF(("%s: invalid key length %u"
+				 "for algorithm %s\n", __func__,
+				 keylen, txform->name));
+			return EINVAL;
+		}
+		bzero(&cria, sizeof(cria));
+		cria.cri_alg = sav->tdb_authalgxform->type;
+		cria.cri_key = sav->key_enc->key_data;
+		cria.cri_klen = _KEYBITS(sav->key_enc) - SAV_ISGCM(sav) * 32;
+	}
+
 	/* Initialize crypto session. */
-	bzero(&crie, sizeof (crie));
+	bzero(&crie, sizeof(crie));
 	crie.cri_alg = sav->tdb_encalgxform->type;
-	crie.cri_klen = _KEYBITS(sav->key_enc);
 	crie.cri_key = sav->key_enc->key_data;
-	/* XXX Rounds ? */
+	crie.cri_klen = _KEYBITS(sav->key_enc) - SAV_ISCTRORGCM(sav) * 32;
 
 	if (sav->tdb_authalgxform && sav->tdb_encalgxform) {
 		/* init both auth & enc */
@@ -255,10 +254,6 @@ esp_zeroize(struct secasvar *sav)
 
 	if (sav->key_enc)
 		bzero(sav->key_enc->key_data, _KEYLEN(sav->key_enc));
-	if (sav->iv) {
-		free(sav->iv, M_XDATA);
-		sav->iv = NULL;
-	}
 	sav->tdb_encalgxform = NULL;
 	sav->tdb_xform = NULL;
 	return error;
@@ -270,16 +265,16 @@ esp_zeroize(struct secasvar *sav)
 static int
 esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 {
-	struct auth_hash *esph;
-	struct enc_xform *espx;
-	struct tdb_ident *tdbi;
-	struct tdb_crypto *tc;
-	int plen, alen, hlen;
-	struct m_tag *mtag;
-	struct newesp *esp;
-
+	char buf[128];
+	const struct auth_hash *esph;
+	const struct enc_xform *espx;
+	struct xform_data *xd;
 	struct cryptodesc *crde;
 	struct cryptop *crp;
+	struct newesp *esp;
+	uint8_t *ivp;
+	uint64_t cryptoid;
+	int plen, alen, hlen;
 
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	IPSEC_ASSERT(sav->tdb_encalgxform != NULL, ("null encoding xform"));
@@ -292,32 +287,19 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 		m_freem(m);
 		return EINVAL;
 	}
-
 	/* XXX don't pullup, just copy header */
 	IP6_EXTHDR_GET(esp, struct newesp *, m, skip, sizeof (struct newesp));
 
 	esph = sav->tdb_authalgxform;
 	espx = sav->tdb_encalgxform;
 
-	/* Determine the ESP header length */
+	/* Determine the ESP header and auth length */
 	if (sav->flags & SADB_X_EXT_OLD)
 		hlen = sizeof (struct esp) + sav->ivlen;
 	else
 		hlen = sizeof (struct newesp) + sav->ivlen;
-	/* Authenticator hash size */
-	if (esph != NULL) {
-		switch (esph->type) {
-		case CRYPTO_SHA2_256_HMAC:
-		case CRYPTO_SHA2_384_HMAC:
-		case CRYPTO_SHA2_512_HMAC:
-			alen = esph->hashsize/2;
-			break;
-		default:
-			alen = AH_HMAC_HASHLEN;
-			break;
-		}
-	}else
-		alen = 0;
+
+	alen = xform_ah_authsize(esph);
 
 	/*
 	 * Verify payload length is multiple of encryption algorithm
@@ -330,10 +312,9 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	plen = m->m_pkthdr.len - (skip + hlen + alen);
 	if ((plen & (espx->blocksize - 1)) || (plen <= 0)) {
 		DPRINTF(("%s: payload of %d octets not a multiple of %d octets,"
-		    "  SA %s/%08lx\n", __func__,
-		    plen, espx->blocksize,
-		    ipsec_address(&sav->sah->saidx.dst),
-		    (u_long) ntohl(sav->spi)));
+		    "  SA %s/%08lx\n", __func__, plen, espx->blocksize,
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
+		    (u_long)ntohl(sav->spi)));
 		ESPSTAT_INC(esps_badilen);
 		m_freem(m);
 		return EINVAL;
@@ -342,29 +323,23 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	/*
 	 * Check sequence number.
 	 */
-	if (esph && sav->replay && !ipsec_chkreplay(ntohl(esp->esp_seq), sav)) {
-		DPRINTF(("%s: packet replay check for %s\n", __func__,
-		    ipsec_logsastr(sav)));	/*XXX*/
-		ESPSTAT_INC(esps_replay);
-		m_freem(m);
-		return ENOBUFS;		/*XXX*/
+	SECASVAR_LOCK(sav);
+	if (esph != NULL && sav->replay != NULL && sav->replay->wsize != 0) {
+		if (ipsec_chkreplay(ntohl(esp->esp_seq), sav) == 0) {
+			SECASVAR_UNLOCK(sav);
+			DPRINTF(("%s: packet replay check for %s\n", __func__,
+			    ipsec_sa2str(sav, buf, sizeof(buf))));
+			ESPSTAT_INC(esps_replay);
+			m_freem(m);
+			return (EACCES);
+		}
 	}
+	cryptoid = sav->tdb_cryptoid;
+	SECASVAR_UNLOCK(sav);
 
 	/* Update the counters */
 	ESPSTAT_ADD(esps_ibytes, m->m_pkthdr.len - (skip + hlen + alen));
 
-	/* Find out if we've already done crypto */
-	for (mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, NULL);
-	     mtag != NULL;
-	     mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, mtag)) {
-		tdbi = (struct tdb_ident *) (mtag + 1);
-		if (tdbi->proto == sav->sah->saidx.proto &&
-		    tdbi->spi == sav->spi &&
-		    !bcmp(&tdbi->dst, &sav->sah->saidx.dst,
-			  sizeof(union sockaddr_union)))
-			break;
-	}
-
 	/* Get crypto descriptors */
 	crp = crypto_getreq(esph && espx ? 2 : 1);
 	if (crp == NULL) {
@@ -376,40 +351,33 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	}
 
 	/* Get IPsec-specific opaque pointer */
-	if (esph == NULL || mtag != NULL)
-		tc = (struct tdb_crypto *) malloc(sizeof(struct tdb_crypto),
-		    M_XDATA, M_NOWAIT|M_ZERO);
-	else
-		tc = (struct tdb_crypto *) malloc(sizeof(struct tdb_crypto) + alen,
-		    M_XDATA, M_NOWAIT|M_ZERO);
-	if (tc == NULL) {
-		crypto_freereq(crp);
-		DPRINTF(("%s: failed to allocate tdb_crypto\n", __func__));
+	xd = malloc(sizeof(*xd) + alen, M_XDATA, M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
+		DPRINTF(("%s: failed to allocate xform_data\n", __func__));
 		ESPSTAT_INC(esps_crypto);
+		crypto_freereq(crp);
 		m_freem(m);
 		return ENOBUFS;
 	}
 
-	tc->tc_ptr = (caddr_t) mtag;
-
-	if (esph) {
+	if (esph != NULL) {
 		struct cryptodesc *crda = crp->crp_desc;
 
 		IPSEC_ASSERT(crda != NULL, ("null ah crypto descriptor"));
 
 		/* Authentication descriptor */
 		crda->crd_skip = skip;
-		crda->crd_len = m->m_pkthdr.len - (skip + alen);
+		if (SAV_ISGCM(sav))
+			crda->crd_len = 8;	/* RFC4106 5, SPI + SN */
+		else
+			crda->crd_len = m->m_pkthdr.len - (skip + alen);
 		crda->crd_inject = m->m_pkthdr.len - alen;
 
 		crda->crd_alg = esph->type;
-		crda->crd_key = sav->key_auth->key_data;
-		crda->crd_klen = _KEYBITS(sav->key_auth);
 
 		/* Copy the authenticator */
-		if (mtag == NULL)
-			m_copydata(m, m->m_pkthdr.len - alen, alen,
-				   (caddr_t) (tc + 1));
+		m_copydata(m, m->m_pkthdr.len - alen, alen,
+		    (caddr_t) (xd + 1));
 
 		/* Chain authentication request */
 		crde = crda->crd_next;
@@ -422,35 +390,43 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = esp_input_cb;
-	crp->crp_sid = sav->tdb_cryptoid;
-	crp->crp_opaque = (caddr_t) tc;
+	crp->crp_sid = cryptoid;
+	crp->crp_opaque = (caddr_t) xd;
 
 	/* These are passed as-is to the callback */
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = sav->sah->saidx.dst;
-	tc->tc_proto = sav->sah->saidx.proto;
-	tc->tc_protoff = protoff;
-	tc->tc_skip = skip;
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
+	xd->sav = sav;
+	xd->protoff = protoff;
+	xd->skip = skip;
+	xd->cryptoid = cryptoid;
 
 	/* Decryption descriptor */
-	if (espx) {
-		IPSEC_ASSERT(crde != NULL, ("null esp crypto descriptor"));
-		crde->crd_skip = skip + hlen;
-		crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
-		crde->crd_inject = skip + hlen - sav->ivlen;
-
-		crde->crd_alg = espx->type;
-		crde->crd_key = sav->key_enc->key_data;
-		crde->crd_klen = _KEYBITS(sav->key_enc);
-		/* XXX Rounds ? */
+	IPSEC_ASSERT(crde != NULL, ("null esp crypto descriptor"));
+	crde->crd_skip = skip + hlen;
+	crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
+	crde->crd_inject = skip + hlen - sav->ivlen;
+
+	if (SAV_ISCTRORGCM(sav)) {
+		ivp = &crde->crd_iv[0];
+
+		/* GCM IV Format: RFC4106 4 */
+		/* CTR IV Format: RFC3686 4 */
+		/* Salt is last four bytes of key, RFC4106 8.1 */
+		/* Nonce is last four bytes of key, RFC3686 5.1 */
+		memcpy(ivp, sav->key_enc->key_data +
+		    _KEYLEN(sav->key_enc) - 4, 4);
+
+		if (SAV_ISCTR(sav)) {
+			/* Initial block counter is 1, RFC3686 4 */
+			be32enc(&ivp[sav->ivlen + 4], 1);
+		}
+
+		m_copydata(m, skip + hlen - sav->ivlen, sav->ivlen, &ivp[4]);
+		crde->crd_flags |= CRD_F_IV_EXPLICIT;
 	}
 
-	if (mtag == NULL)
-		return crypto_dispatch(crp);
-	else
-		return esp_input_cb(crp);
+	crde->crd_alg = espx->type;
+
+	return (crypto_dispatch(crp));
 }
 
 /*
@@ -459,50 +435,41 @@ esp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 static int
 esp_input_cb(struct cryptop *crp)
 {
+	char buf[128];
 	u_int8_t lastthree[3], aalg[AH_HMAC_MAXHASHLEN];
-	int hlen, skip, protoff, error, alen;
+	const struct auth_hash *esph;
+	const struct enc_xform *espx;
 	struct mbuf *m;
 	struct cryptodesc *crd;
-	struct auth_hash *esph;
-	struct enc_xform *espx;
-	struct tdb_crypto *tc;
-	struct m_tag *mtag;
+	struct xform_data *xd;
 	struct secasvar *sav;
 	struct secasindex *saidx;
 	caddr_t ptr;
+	uint64_t cryptoid;
+	int hlen, skip, protoff, error, alen;
 
 	crd = crp->crp_desc;
 	IPSEC_ASSERT(crd != NULL, ("null crypto descriptor!"));
 
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque crypto data area!"));
-	skip = tc->tc_skip;
-	protoff = tc->tc_protoff;
-	mtag = (struct m_tag *) tc->tc_ptr;
 	m = (struct mbuf *) crp->crp_buf;
-
-	sav = tc->tc_sav;
-	IPSEC_ASSERT(sav != NULL, ("null SA!"));
-
+	xd = (struct xform_data *) crp->crp_opaque;
+	sav = xd->sav;
+	skip = xd->skip;
+	protoff = xd->protoff;
+	cryptoid = xd->cryptoid;
 	saidx = &sav->sah->saidx;
-	IPSEC_ASSERT(saidx->dst.sa.sa_family == AF_INET ||
-		saidx->dst.sa.sa_family == AF_INET6,
-		("unexpected protocol family %u", saidx->dst.sa.sa_family));
-
 	esph = sav->tdb_authalgxform;
 	espx = sav->tdb_encalgxform;
 
 	/* Check for crypto errors */
 	if (crp->crp_etype) {
-		/* Reset the session ID */
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			error = crypto_dispatch(crp);
-			return error;
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
-
 		ESPSTAT_INC(esps_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
 		error = crp->crp_etype;
@@ -520,48 +487,29 @@ esp_input_cb(struct cryptop *crp)
 
 	/* If authentication was performed, check now. */
 	if (esph != NULL) {
-		switch (esph->type) {
-		case CRYPTO_SHA2_256_HMAC:
-		case CRYPTO_SHA2_384_HMAC:
-		case CRYPTO_SHA2_512_HMAC:
-			alen = esph->hashsize/2;
-			break;
-		default:
-			alen = AH_HMAC_HASHLEN;
-			break;
-		}
-		/*
-		 * If we have a tag, it means an IPsec-aware NIC did
-		 * the verification for us.  Otherwise we need to
-		 * check the authentication calculation.
-		 */
+		alen = xform_ah_authsize(esph);
 		AHSTAT_INC(ahs_hist[sav->alg_auth]);
-		if (mtag == NULL) {
-			/* Copy the authenticator from the packet */
-			m_copydata(m, m->m_pkthdr.len - alen,
-				alen, aalg);
-
-			ptr = (caddr_t) (tc + 1);
-
-			/* Verify authenticator */
-			if (bcmp(ptr, aalg, alen) != 0) {
-				DPRINTF(("%s: "
-		    "authentication hash mismatch for packet in SA %s/%08lx\n",
-				    __func__,
-				    ipsec_address(&saidx->dst),
-				    (u_long) ntohl(sav->spi)));
-				ESPSTAT_INC(esps_badauth);
-				error = EACCES;
-				goto bad;
-			}
+		/* Copy the authenticator from the packet */
+		m_copydata(m, m->m_pkthdr.len - alen, alen, aalg);
+		ptr = (caddr_t) (xd + 1);
+
+		/* Verify authenticator */
+		if (timingsafe_bcmp(ptr, aalg, alen) != 0) {
+			DPRINTF(("%s: authentication hash mismatch for "
+			    "packet in SA %s/%08lx\n", __func__,
+			    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+			    (u_long) ntohl(sav->spi)));
+			ESPSTAT_INC(esps_badauth);
+			error = EACCES;
+			goto bad;
 		}
-
+		m->m_flags |= M_AUTHIPDGM;
 		/* Remove trailing authenticator */
 		m_adj(m, -alen);
 	}
 
 	/* Release the crypto descriptors */
-	free(tc, M_XDATA), tc = NULL;
+	free(xd, M_XDATA), xd = NULL;
 	crypto_freereq(crp), crp = NULL;
 
 	/*
@@ -577,13 +525,16 @@ esp_input_cb(struct cryptop *crp)
 
 		m_copydata(m, skip + offsetof(struct newesp, esp_seq),
 			   sizeof (seq), (caddr_t) &seq);
+		SECASVAR_LOCK(sav);
 		if (ipsec_updatereplay(ntohl(seq), sav)) {
+			SECASVAR_UNLOCK(sav);
 			DPRINTF(("%s: packet replay check for %s\n", __func__,
-			    ipsec_logsastr(sav)));
+			    ipsec_sa2str(sav, buf, sizeof(buf))));
 			ESPSTAT_INC(esps_replay);
-			error = ENOBUFS;
+			error = EACCES;
 			goto bad;
 		}
+		SECASVAR_UNLOCK(sav);
 	}
 
 	/* Determine the ESP header length */
@@ -597,7 +548,7 @@ esp_input_cb(struct cryptop *crp)
 	if (error) {
 		ESPSTAT_INC(esps_hdrops);
 		DPRINTF(("%s: bad mbuf chain, SA %s/%08lx\n", __func__,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi)));
 		goto bad;
 	}
@@ -609,10 +560,10 @@ esp_input_cb(struct cryptop *crp)
 	if (lastthree[1] + 2 > m->m_pkthdr.len - skip) {
 		ESPSTAT_INC(esps_badilen);
 		DPRINTF(("%s: invalid padding length %d for %u byte packet "
-			"in SA %s/%08lx\n", __func__,
-			 lastthree[1], m->m_pkthdr.len - skip,
-			 ipsec_address(&sav->sah->saidx.dst),
-			 (u_long) ntohl(sav->spi)));
+		    "in SA %s/%08lx\n", __func__, lastthree[1],
+		    m->m_pkthdr.len - skip,
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		error = EINVAL;
 		goto bad;
 	}
@@ -622,9 +573,9 @@ esp_input_cb(struct cryptop *crp)
 		if (lastthree[1] != lastthree[0] && lastthree[1] != 0) {
 			ESPSTAT_INC(esps_badenc);
 			DPRINTF(("%s: decryption failed for packet in "
-				"SA %s/%08lx\n", __func__,
-				ipsec_address(&sav->sah->saidx.dst),
-				(u_long) ntohl(sav->spi)));
+			    "SA %s/%08lx\n", __func__, ipsec_address(
+			    &sav->sah->saidx.dst, buf, sizeof(buf)),
+			    (u_long) ntohl(sav->spi)));
 			error = EINVAL;
 			goto bad;
 		}
@@ -639,60 +590,52 @@ esp_input_cb(struct cryptop *crp)
 	switch (saidx->dst.sa.sa_family) {
 #ifdef INET6
 	case AF_INET6:
-		error = ipsec6_common_input_cb(m, sav, skip, protoff, mtag);
+		error = ipsec6_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 #ifdef INET
 	case AF_INET:
-		error = ipsec4_common_input_cb(m, sav, skip, protoff, mtag);
+		error = ipsec4_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 	default:
 		panic("%s: Unexpected address family: %d saidx=%p", __func__,
 		    saidx->dst.sa.sa_family, saidx);
 	}
-
-	KEY_FREESAV(&sav);
 	return error;
 bad:
-	if (sav)
-		KEY_FREESAV(&sav);
+	if (sav != NULL)
+		key_freesav(&sav);
 	if (m != NULL)
 		m_freem(m);
-	if (tc != NULL)
-		free(tc, M_XDATA);
+	if (xd != NULL)
+		free(xd, M_XDATA);
 	if (crp != NULL)
 		crypto_freereq(crp);
 	return error;
 }
-
 /*
- * ESP output routine, called by ipsec[46]_process_packet().
+ * ESP output routine, called by ipsec[46]_perform_request().
  */
 static int
-esp_output(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	struct mbuf **mp,
-	int skip,
-	int protoff
-)
+esp_output(struct mbuf *m, struct secpolicy *sp, struct secasvar *sav,
+    u_int idx, int skip, int protoff)
 {
-	struct enc_xform *espx;
-	struct auth_hash *esph;
-	int hlen, rlen, plen, padding, blks, alen, i, roff;
-	struct mbuf *mo = (struct mbuf *) NULL;
-	struct tdb_crypto *tc;
-	struct secasvar *sav;
+	char buf[IPSEC_ADDRSTRLEN];
+	struct cryptodesc *crde = NULL, *crda = NULL;
+	struct cryptop *crp;
+	const struct auth_hash *esph;
+	const struct enc_xform *espx;
+	struct mbuf *mo = NULL;
+	struct xform_data *xd;
 	struct secasindex *saidx;
 	unsigned char *pad;
-	u_int8_t prot;
+	uint8_t *ivp;
+	uint64_t cntr, cryptoid;
+	int hlen, rlen, padding, blks, alen, i, roff;
 	int error, maxpacketsize;
+	uint8_t prot;
 
-	struct cryptodesc *crde = NULL, *crda = NULL;
-	struct cryptop *crp;
-
-	sav = isr->sav;
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	esph = sav->tdb_authalgxform;
 	espx = sav->tdb_encalgxform;
@@ -705,28 +648,14 @@ esp_output(
 
 	rlen = m->m_pkthdr.len - skip;	/* Raw payload length. */
 	/*
-	 * NB: The null encoding transform has a blocksize of 4
-	 *     so that headers are properly aligned.
+	 * RFC4303 2.4 Requires 4 byte alignment.
 	 */
-	blks = espx->blocksize;		/* IV blocksize */
+	blks = MAX(4, espx->blocksize);		/* Cipher blocksize */
 
 	/* XXX clamp padding length a la KAME??? */
 	padding = ((blks - ((rlen + 2) % blks)) % blks) + 2;
-	plen = rlen + padding;		/* Padded payload length. */
-
-	if (esph)
-		switch (esph->type) {
-		case CRYPTO_SHA2_256_HMAC:
-		case CRYPTO_SHA2_384_HMAC:
-		case CRYPTO_SHA2_512_HMAC:
-			alen = esph->hashsize/2;
-			break;
-		default:
-		alen = AH_HMAC_HASHLEN;
-			break;
-		}
-	else
-		alen = 0;
+
+	alen = xform_ah_authsize(esph);
 
 	ESPSTAT_INC(esps_output);
 
@@ -746,16 +675,20 @@ esp_output(
 	default:
 		DPRINTF(("%s: unknown/unsupported protocol "
 		    "family %d, SA %s/%08lx\n", __func__,
-		    saidx->dst.sa.sa_family, ipsec_address(&saidx->dst),
-		    (u_long) ntohl(sav->spi)));
+		    saidx->dst.sa.sa_family, ipsec_address(&saidx->dst,
+			buf, sizeof(buf)), (u_long) ntohl(sav->spi)));
 		ESPSTAT_INC(esps_nopf);
 		error = EPFNOSUPPORT;
 		goto bad;
 	}
+	/*
+	DPRINTF(("%s: skip %d hlen %d rlen %d padding %d alen %d blksd %d\n",
+		__func__, skip, hlen, rlen, padding, alen, blks)); */
 	if (skip + hlen + rlen + padding + alen > maxpacketsize) {
 		DPRINTF(("%s: packet in SA %s/%08lx got too big "
 		    "(len %u, max len %u)\n", __func__,
-		    ipsec_address(&saidx->dst), (u_long) ntohl(sav->spi),
+		    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi),
 		    skip + hlen + rlen + padding + alen, maxpacketsize));
 		ESPSTAT_INC(esps_toobig);
 		error = EMSGSIZE;
@@ -768,7 +701,8 @@ esp_output(
 	m = m_unshare(m, M_NOWAIT);
 	if (m == NULL) {
 		DPRINTF(("%s: cannot clone mbuf chain, SA %s/%08lx\n", __func__,
-		    ipsec_address(&saidx->dst), (u_long) ntohl(sav->spi)));
+		    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		ESPSTAT_INC(esps_hdrops);
 		error = ENOBUFS;
 		goto bad;
@@ -778,17 +712,19 @@ esp_output(
 	mo = m_makespace(m, skip, hlen, &roff);
 	if (mo == NULL) {
 		DPRINTF(("%s: %u byte ESP hdr inject failed for SA %s/%08lx\n",
-		    __func__, hlen, ipsec_address(&saidx->dst),
-		    (u_long) ntohl(sav->spi)));
-		ESPSTAT_INC(esps_hdrops);		/* XXX diffs from openbsd */
+		    __func__, hlen, ipsec_address(&saidx->dst, buf,
+		    sizeof(buf)), (u_long) ntohl(sav->spi)));
+		ESPSTAT_INC(esps_hdrops);	/* XXX diffs from openbsd */
 		error = ENOBUFS;
 		goto bad;
 	}
 
 	/* Initialize ESP header. */
-	bcopy((caddr_t) &sav->spi, mtod(mo, caddr_t) + roff, sizeof(u_int32_t));
+	bcopy((caddr_t) &sav->spi, mtod(mo, caddr_t) + roff,
+	    sizeof(uint32_t));
+	SECASVAR_LOCK(sav);
 	if (sav->replay) {
-		u_int32_t replay;
+		uint32_t replay;
 
 #ifdef REGRESSION
 		/* Emulate replay attack when ipsec_replay is TRUE. */
@@ -796,10 +732,14 @@ esp_output(
 #endif
 			sav->replay->count++;
 		replay = htonl(sav->replay->count);
-		bcopy((caddr_t) &replay,
-		    mtod(mo, caddr_t) + roff + sizeof(u_int32_t),
-		    sizeof(u_int32_t));
+
+		bcopy((caddr_t) &replay, mtod(mo, caddr_t) + roff +
+		    sizeof(uint32_t), sizeof(uint32_t));
 	}
+	cryptoid = sav->tdb_cryptoid;
+	if (SAV_ISCTRORGCM(sav))
+		cntr = sav->cntr++;
+	SECASVAR_UNLOCK(sav);
 
 	/*
 	 * Add padding -- better to do it ourselves than use the crypto engine,
@@ -808,7 +748,8 @@ esp_output(
 	pad = (u_char *) m_pad(m, padding + alen);
 	if (pad == NULL) {
 		DPRINTF(("%s: m_pad failed for SA %s/%08lx\n", __func__,
-		    ipsec_address(&saidx->dst), (u_long) ntohl(sav->spi)));
+		    ipsec_address(&saidx->dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		m = NULL;		/* NB: free'd by m_pad */
 		error = ENOBUFS;
 		goto bad;
@@ -840,7 +781,7 @@ esp_output(
 	m_copyback(m, protoff, sizeof(u_int8_t), (u_char *) &prot);
 
 	/* Get crypto descriptors. */
-	crp = crypto_getreq(esph && espx ? 2 : 1);
+	crp = crypto_getreq(esph != NULL ? 2 : 1);
 	if (crp == NULL) {
 		DPRINTF(("%s: failed to acquire crypto descriptors\n",
 			__func__));
@@ -849,61 +790,70 @@ esp_output(
 		goto bad;
 	}
 
-	if (espx) {
-		crde = crp->crp_desc;
-		crda = crde->crd_next;
-
-		/* Encryption descriptor. */
-		crde->crd_skip = skip + hlen;
-		crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
-		crde->crd_flags = CRD_F_ENCRYPT;
-		crde->crd_inject = skip + hlen - sav->ivlen;
-
-		/* Encryption operation. */
-		crde->crd_alg = espx->type;
-		crde->crd_key = sav->key_enc->key_data;
-		crde->crd_klen = _KEYBITS(sav->key_enc);
-		/* XXX Rounds ? */
-	} else
-		crda = crp->crp_desc;
-
 	/* IPsec-specific opaque crypto info. */
-	tc = (struct tdb_crypto *) malloc(sizeof(struct tdb_crypto),
-		M_XDATA, M_NOWAIT|M_ZERO);
-	if (tc == NULL) {
+	xd =  malloc(sizeof(struct xform_data), M_XDATA, M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
 		crypto_freereq(crp);
-		DPRINTF(("%s: failed to allocate tdb_crypto\n", __func__));
+		DPRINTF(("%s: failed to allocate xform_data\n", __func__));
 		ESPSTAT_INC(esps_crypto);
 		error = ENOBUFS;
 		goto bad;
 	}
 
+	crde = crp->crp_desc;
+	crda = crde->crd_next;
+
+	/* Encryption descriptor. */
+	crde->crd_skip = skip + hlen;
+	crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
+	crde->crd_flags = CRD_F_ENCRYPT;
+	crde->crd_inject = skip + hlen - sav->ivlen;
+
+	/* Encryption operation. */
+	crde->crd_alg = espx->type;
+	if (SAV_ISCTRORGCM(sav)) {
+		ivp = &crde->crd_iv[0];
+
+		/* GCM IV Format: RFC4106 4 */
+		/* CTR IV Format: RFC3686 4 */
+		/* Salt is last four bytes of key, RFC4106 8.1 */
+		/* Nonce is last four bytes of key, RFC3686 5.1 */
+		memcpy(ivp, sav->key_enc->key_data +
+		    _KEYLEN(sav->key_enc) - 4, 4);
+		be64enc(&ivp[4], cntr);
+		if (SAV_ISCTR(sav)) {
+			/* Initial block counter is 1, RFC3686 4 */
+			/* XXXAE: should we use this only for first packet? */
+			be32enc(&ivp[sav->ivlen + 4], 1);
+		}
+
+		m_copyback(m, skip + hlen - sav->ivlen, sav->ivlen, &ivp[4]);
+		crde->crd_flags |= CRD_F_IV_EXPLICIT|CRD_F_IV_PRESENT;
+	}
+
 	/* Callback parameters */
-	tc->tc_isr = isr;
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = saidx->dst;
-	tc->tc_proto = saidx->proto;
+	xd->sp = sp;
+	xd->sav = sav;
+	xd->idx = idx;
+	xd->cryptoid = cryptoid;
 
 	/* Crypto operation descriptor. */
 	crp->crp_ilen = m->m_pkthdr.len; /* Total input length. */
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = esp_output_cb;
-	crp->crp_opaque = (caddr_t) tc;
-	crp->crp_sid = sav->tdb_cryptoid;
+	crp->crp_opaque = (caddr_t) xd;
+	crp->crp_sid = cryptoid;
 
 	if (esph) {
 		/* Authentication descriptor. */
+		crda->crd_alg = esph->type;
 		crda->crd_skip = skip;
-		crda->crd_len = m->m_pkthdr.len - (skip + alen);
+		if (SAV_ISGCM(sav))
+			crda->crd_len = 8;	/* RFC4106 5, SPI + SN */
+		else
+			crda->crd_len = m->m_pkthdr.len - (skip + alen);
 		crda->crd_inject = m->m_pkthdr.len - alen;
-
-		/* Authentication operation. */
-		crda->crd_alg = esph->type;
-		crda->crd_key = sav->key_auth->key_data;
-		crda->crd_klen = _KEYBITS(sav->key_auth);
 	}
 
 	return crypto_dispatch(crp);
@@ -912,51 +862,40 @@ bad:
 		m_freem(m);
 	return (error);
 }
-
 /*
  * ESP output callback from the crypto driver.
  */
 static int
 esp_output_cb(struct cryptop *crp)
 {
-	struct tdb_crypto *tc;
-	struct ipsecrequest *isr;
+	struct xform_data *xd;
+	struct secpolicy *sp;
 	struct secasvar *sav;
 	struct mbuf *m;
-	int err, error;
+	uint64_t cryptoid;
+	u_int idx;
+	int error;
 
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque data area!"));
+	xd = (struct xform_data *) crp->crp_opaque;
 	m = (struct mbuf *) crp->crp_buf;
-
-	isr = tc->tc_isr;
-	IPSECREQUEST_LOCK(isr);
-	sav = tc->tc_sav;
-	/* With the isr lock released SA pointer can be updated. */
-	if (sav != isr->sav) {
-		ESPSTAT_INC(esps_notdb);
-		DPRINTF(("%s: SA gone during crypto (SA %s/%08lx proto %u)\n",
-		    __func__, ipsec_address(&tc->tc_dst),
-		    (u_long) ntohl(tc->tc_spi), tc->tc_proto));
-		error = ENOBUFS;		/*XXX*/
-		goto bad;
-	}
+	sp = xd->sp;
+	sav = xd->sav;
+	idx = xd->idx;
+	cryptoid = xd->cryptoid;
 
 	/* Check for crypto errors. */
 	if (crp->crp_etype) {
-		/* Reset session ID. */
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			IPSECREQUEST_UNLOCK(isr);
-			error = crypto_dispatch(crp);
-			return error;
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
-
 		ESPSTAT_INC(esps_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
 		error = crp->crp_etype;
+		m_freem(m);
 		goto bad;
 	}
 
@@ -967,19 +906,17 @@ esp_output_cb(struct cryptop *crp)
 		error = EINVAL;
 		goto bad;
 	}
+	free(xd, M_XDATA);
+	crypto_freereq(crp);
 	ESPSTAT_INC(esps_hist[sav->alg_enc]);
 	if (sav->tdb_authalgxform != NULL)
 		AHSTAT_INC(ahs_hist[sav->alg_auth]);
 
-	/* Release crypto descriptors. */
-	free(tc, M_XDATA);
-	crypto_freereq(crp);
-
 #ifdef REGRESSION
 	/* Emulate man-in-the-middle attack when ipsec_integrity is TRUE. */
 	if (V_ipsec_integrity) {
 		static unsigned char ipseczeroes[AH_HMAC_MAXHASHLEN];
-		struct auth_hash *esph;
+		const struct auth_hash *esph;
 
 		/*
 		 * Corrupt HMAC if we want to test integrity verification of
@@ -989,16 +926,7 @@ esp_output_cb(struct cryptop *crp)
 		if (esph !=  NULL) {
 			int alen;
 
-			switch (esph->type) {
-			case CRYPTO_SHA2_256_HMAC:
-			case CRYPTO_SHA2_384_HMAC:
-			case CRYPTO_SHA2_512_HMAC:
-				alen = esph->hashsize/2;
-				break;
-			default:
-				alen = AH_HMAC_HASHLEN;
-				break;
-			}
+			alen = xform_ah_authsize(esph);
 			m_copyback(m, m->m_pkthdr.len - alen,
 			    alen, ipseczeroes);
 		}
@@ -1006,44 +934,26 @@ esp_output_cb(struct cryptop *crp)
 #endif
 
 	/* NB: m is reclaimed by ipsec_process_done. */
-	err = ipsec_process_done(m, isr);
-	KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
-	return err;
+	error = ipsec_process_done(m, sp, sav, idx);
+	return (error);
 bad:
-	if (sav)
-		KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
-	if (m)
-		m_freem(m);
-	free(tc, M_XDATA);
+	free(xd, M_XDATA);
 	crypto_freereq(crp);
-	return error;
+	key_freesav(&sav);
+	key_freesp(&sp);
+	return (error);
 }
 
 static struct xformsw esp_xformsw = {
-	XF_ESP,		XFT_CONF|XFT_AUTH,	"IPsec ESP",
-	esp_init,	esp_zeroize,		esp_input,
-	esp_output
+	.xf_type =	XF_ESP,
+	.xf_name =	"IPsec ESP",
+	.xf_init =	esp_init,
+	.xf_zeroize =	esp_zeroize,
+	.xf_input =	esp_input,
+	.xf_output =	esp_output,
 };
 
-static void
-esp_attach(void)
-{
-#define	MAXIV(xform)					\
-	if (xform.blocksize > V_esp_max_ivlen)		\
-		V_esp_max_ivlen = xform.blocksize	\
-
-	MAXIV(enc_xform_des);		/* SADB_EALG_DESCBC */
-	MAXIV(enc_xform_3des);		/* SADB_EALG_3DESCBC */
-	MAXIV(enc_xform_rijndael128);	/* SADB_X_EALG_AES */
-	MAXIV(enc_xform_blf);		/* SADB_X_EALG_BLOWFISHCBC */
-	MAXIV(enc_xform_cast5);		/* SADB_X_EALG_CAST128CBC */
-	MAXIV(enc_xform_skipjack);	/* SADB_X_EALG_SKIPJACK */
-	MAXIV(enc_xform_null);		/* SADB_EALG_NULL */
-	MAXIV(enc_xform_camellia);	/* SADB_X_EALG_CAMELLIACBC */
-
-	xform_register(&esp_xformsw);
-#undef MAXIV
-}
-SYSINIT(esp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE, esp_attach, NULL);
+SYSINIT(esp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    xform_attach, &esp_xformsw);
+SYSUNINIT(esp_xform_uninit, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    xform_detach, &esp_xformsw);
diff --git a/freebsd/sys/netipsec/xform_ipcomp.c b/freebsd/sys/netipsec/xform_ipcomp.c
index 2478c948..f5a7aad4 100644
--- a/freebsd/sys/netipsec/xform_ipcomp.c
+++ b/freebsd/sys/netipsec/xform_ipcomp.c
@@ -39,7 +39,6 @@
 #include <sys/mbuf.h>
 #include <rtems/bsd/sys/lock.h>
 #include <sys/mutex.h>
-#include <sys/rwlock.h>
 #include <sys/socket.h>
 #include <sys/kernel.h>
 #include <sys/protosw.h>
@@ -49,8 +48,9 @@
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
 #include <netinet/ip_var.h>
+#include <netinet/ip_encap.h>
 
-#include <net/route.h>
+#include <net/netisr.h>
 #include <net/vnet.h>
 
 #include <netipsec/ipsec.h>
@@ -58,6 +58,7 @@
 
 #ifdef INET6
 #include <netinet/ip6.h>
+#include <netinet6/ip6_var.h>
 #include <netipsec/ipsec6.h>
 #endif
 
@@ -72,27 +73,76 @@
 #include <opencrypto/xform.h>
 
 VNET_DEFINE(int, ipcomp_enable) = 1;
-VNET_DEFINE(struct ipcompstat, ipcompstat);
+VNET_PCPUSTAT_DEFINE(struct ipcompstat, ipcompstat);
+VNET_PCPUSTAT_SYSINIT(ipcompstat);
+
+#ifdef VIMAGE
+VNET_PCPUSTAT_SYSUNINIT(ipcompstat);
+#endif /* VIMAGE */
 
 SYSCTL_DECL(_net_inet_ipcomp);
-SYSCTL_VNET_INT(_net_inet_ipcomp, OID_AUTO,
-	ipcomp_enable,	CTLFLAG_RW,	&VNET_NAME(ipcomp_enable),	0, "");
-SYSCTL_VNET_STRUCT(_net_inet_ipcomp, IPSECCTL_STATS,
-	stats,		CTLFLAG_RD,	&VNET_NAME(ipcompstat),	ipcompstat, "");
+SYSCTL_INT(_net_inet_ipcomp, OID_AUTO, ipcomp_enable,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ipcomp_enable), 0, "");
+SYSCTL_VNET_PCPUSTAT(_net_inet_ipcomp, IPSECCTL_STATS, stats,
+    struct ipcompstat, ipcompstat,
+    "IPCOMP statistics (struct ipcompstat, netipsec/ipcomp_var.h");
 
 static int ipcomp_input_cb(struct cryptop *crp);
 static int ipcomp_output_cb(struct cryptop *crp);
 
-struct comp_algo *
-ipcomp_algorithm_lookup(int alg)
+/*
+ * RFC 3173 p 2.2. Non-Expansion Policy:
+ * If the total size of a compressed payload and the IPComp header, as
+ * defined in section 3, is not smaller than the size of the original
+ * payload, the IP datagram MUST be sent in the original non-compressed
+ * form.
+ *
+ * When we use IPComp in tunnel mode, for small packets we will receive
+ * encapsulated IP-IP datagrams without any compression and without IPComp
+ * header.
+ */
+static int
+ipcomp_encapcheck(union sockaddr_union *src, union sockaddr_union *dst)
+{
+	struct secasvar *sav;
+
+	sav = key_allocsa_tunnel(src, dst, IPPROTO_IPCOMP);
+	if (sav == NULL)
+		return (0);
+	key_freesav(&sav);
+
+	if (src->sa.sa_family == AF_INET)
+		return (sizeof(struct in_addr) << 4);
+	else
+		return (sizeof(struct in6_addr) << 4);
+}
+
+static int
+ipcomp_nonexp_input(struct mbuf **mp, int *offp, int proto)
 {
-	if (alg >= IPCOMP_ALG_MAX)
-		return NULL;
-	switch (alg) {
-	case SADB_X_CALG_DEFLATE:
-		return &comp_algo_deflate;
+	int isr;
+
+	switch (proto) {
+#ifdef INET
+	case IPPROTO_IPV4:
+		isr = NETISR_IP;
+		break;
+#endif
+#ifdef INET6
+	case IPPROTO_IPV6:
+		isr = NETISR_IPV6;
+		break;
+#endif
+	default:
+		IPCOMPSTAT_INC(ipcomps_nopf);
+		m_freem(*mp);
+		return (IPPROTO_DONE);
 	}
-	return NULL;
+	m_adj(*mp, *offp);
+	IPCOMPSTAT_ADD(ipcomps_ibytes, (*mp)->m_pkthdr.len);
+	IPCOMPSTAT_INC(ipcomps_input);
+	netisr_dispatch(isr, *mp);
+	return (IPPROTO_DONE);
 }
 
 /*
@@ -101,11 +151,11 @@ ipcomp_algorithm_lookup(int alg)
 static int
 ipcomp_init(struct secasvar *sav, struct xformsw *xsp)
 {
-	struct comp_algo *tcomp;
+	const struct comp_algo *tcomp;
 	struct cryptoini cric;
 
 	/* NB: algorithm really comes in alg_enc and not alg_comp! */
-	tcomp = ipcomp_algorithm_lookup(sav->alg_enc);
+	tcomp = comp_algorithm_lookup(sav->alg_enc);
 	if (tcomp == NULL) {
 		DPRINTF(("%s: unsupported compression algorithm %d\n", __func__,
 			 sav->alg_comp));
@@ -141,7 +191,7 @@ ipcomp_zeroize(struct secasvar *sav)
 static int
 ipcomp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 {
-	struct tdb_crypto *tc;
+	struct xform_data *xd;
 	struct cryptodesc *crdc;
 	struct cryptop *crp;
 	struct ipcomp *ipcomp;
@@ -176,12 +226,12 @@ ipcomp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 		return ENOBUFS;
 	}
 	/* Get IPsec-specific opaque pointer */
-	tc = (struct tdb_crypto *) malloc(sizeof (*tc), M_XDATA, M_NOWAIT|M_ZERO);
-	if (tc == NULL) {
-		m_freem(m);
-		crypto_freereq(crp);
-		DPRINTF(("%s: cannot allocate tdb_crypto\n", __func__));
+	xd = malloc(sizeof(*xd), M_XDATA, M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
+		DPRINTF(("%s: cannot allocate xform_data\n", __func__));
 		IPCOMPSTAT_INC(ipcomps_crypto);
+		crypto_freereq(crp);
+		m_freem(m);
 		return ENOBUFS;
 	}
 	crdc = crp->crp_desc;
@@ -190,27 +240,25 @@ ipcomp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 	crdc->crd_len = m->m_pkthdr.len - (skip + hlen);
 	crdc->crd_inject = skip;
 
-	tc->tc_ptr = 0;
-
 	/* Decompression operation */
 	crdc->crd_alg = sav->tdb_compalgxform->type;
 
+
 	/* Crypto operation descriptor */
 	crp->crp_ilen = m->m_pkthdr.len - (skip + hlen);
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = ipcomp_input_cb;
-	crp->crp_sid = sav->tdb_cryptoid;
-	crp->crp_opaque = (caddr_t) tc;
+	crp->crp_opaque = (caddr_t) xd;
 
 	/* These are passed as-is to the callback */
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = sav->sah->saidx.dst;
-	tc->tc_proto = sav->sah->saidx.proto;
-	tc->tc_protoff = protoff;
-	tc->tc_skip = skip;
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
+	xd->sav = sav;
+	xd->protoff = protoff;
+	xd->skip = skip;
+
+	SECASVAR_LOCK(sav);
+	crp->crp_sid = xd->cryptoid = sav->tdb_cryptoid;
+	SECASVAR_UNLOCK(sav);
 
 	return crypto_dispatch(crp);
 }
@@ -221,29 +269,26 @@ ipcomp_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
 static int
 ipcomp_input_cb(struct cryptop *crp)
 {
+	char buf[IPSEC_ADDRSTRLEN];
 	struct cryptodesc *crd;
-	struct tdb_crypto *tc;
-	int skip, protoff;
-	struct mtag *mtag;
+	struct xform_data *xd;
 	struct mbuf *m;
 	struct secasvar *sav;
 	struct secasindex *saidx;
-	int hlen = IPCOMP_HLENGTH, error, clen;
-	u_int8_t nproto;
 	caddr_t addr;
+	uint64_t cryptoid;
+	int hlen = IPCOMP_HLENGTH, error, clen;
+	int skip, protoff;
+	uint8_t nproto;
 
 	crd = crp->crp_desc;
 
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque crypto data area!"));
-	skip = tc->tc_skip;
-	protoff = tc->tc_protoff;
-	mtag = (struct mtag *) tc->tc_ptr;
 	m = (struct mbuf *) crp->crp_buf;
-
-	sav = tc->tc_sav;
-	IPSEC_ASSERT(sav != NULL, ("null SA!"));
-
+	xd = (struct xform_data *) crp->crp_opaque;
+	sav = xd->sav;
+	skip = xd->skip;
+	protoff = xd->protoff;
+	cryptoid = xd->cryptoid;
 	saidx = &sav->sah->saidx;
 	IPSEC_ASSERT(saidx->dst.sa.sa_family == AF_INET ||
 		saidx->dst.sa.sa_family == AF_INET6,
@@ -251,12 +296,12 @@ ipcomp_input_cb(struct cryptop *crp)
 
 	/* Check for crypto errors */
 	if (crp->crp_etype) {
-		/* Reset the session ID */
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			return crypto_dispatch(crp);
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
 		IPCOMPSTAT_INC(ipcomps_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
@@ -275,13 +320,13 @@ ipcomp_input_cb(struct cryptop *crp)
 	clen = crp->crp_olen;		/* Length of data after processing */
 
 	/* Release the crypto descriptors */
-	free(tc, M_XDATA), tc = NULL;
+	free(xd, M_XDATA), xd = NULL;
 	crypto_freereq(crp), crp = NULL;
 
 	/* In case it's not done already, adjust the size of the mbuf chain */
 	m->m_pkthdr.len = clen + hlen + skip;
 
-	if (m->m_len < skip + hlen && (m = m_pullup(m, skip + hlen)) == 0) {
+	if (m->m_len < skip + hlen && (m = m_pullup(m, skip + hlen)) == NULL) {
 		IPCOMPSTAT_INC(ipcomps_hdrops);		/*XXX*/
 		DPRINTF(("%s: m_pullup failed\n", __func__));
 		error = EINVAL;				/*XXX*/
@@ -297,8 +342,8 @@ ipcomp_input_cb(struct cryptop *crp)
 	if (error) {
 		IPCOMPSTAT_INC(ipcomps_hdrops);
 		DPRINTF(("%s: bad mbuf chain, IPCA %s/%08lx\n", __func__,
-			 ipsec_address(&sav->sah->saidx.dst),
-			 (u_long) ntohl(sav->spi)));
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
+		    (u_long) ntohl(sav->spi)));
 		goto bad;
 	}
 
@@ -308,53 +353,45 @@ ipcomp_input_cb(struct cryptop *crp)
 	switch (saidx->dst.sa.sa_family) {
 #ifdef INET6
 	case AF_INET6:
-		error = ipsec6_common_input_cb(m, sav, skip, protoff, NULL);
+		error = ipsec6_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 #ifdef INET
 	case AF_INET:
-		error = ipsec4_common_input_cb(m, sav, skip, protoff, NULL);
+		error = ipsec4_common_input_cb(m, sav, skip, protoff);
 		break;
 #endif
 	default:
 		panic("%s: Unexpected address family: %d saidx=%p", __func__,
 		    saidx->dst.sa.sa_family, saidx);
 	}
-
-	KEY_FREESAV(&sav);
 	return error;
 bad:
-	if (sav)
-		KEY_FREESAV(&sav);
-	if (m)
+	if (sav != NULL)
+		key_freesav(&sav);
+	if (m != NULL)
 		m_freem(m);
-	if (tc != NULL)
-		free(tc, M_XDATA);
-	if (crp)
+	if (xd != NULL)
+		free(xd, M_XDATA);
+	if (crp != NULL)
 		crypto_freereq(crp);
 	return error;
 }
 
 /*
- * IPComp output routine, called by ipsec[46]_process_packet()
+ * IPComp output routine, called by ipsec[46]_perform_request()
  */
 static int
-ipcomp_output(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	struct mbuf **mp,
-	int skip,
-	int protoff
-)
+ipcomp_output(struct mbuf *m, struct secpolicy *sp, struct secasvar *sav,
+    u_int idx, int skip, int protoff)
 {
-	struct secasvar *sav;
-	struct comp_algo *ipcompx;
-	int error, ralen, maxpacketsize;
+	char buf[IPSEC_ADDRSTRLEN];
+	const struct comp_algo *ipcompx;
 	struct cryptodesc *crdc;
 	struct cryptop *crp;
-	struct tdb_crypto *tc;
+	struct xform_data *xd;
+	int error, ralen, maxpacketsize;
 
-	sav = isr->sav;
 	IPSEC_ASSERT(sav != NULL, ("null SA"));
 	ipcompx = sav->tdb_compalgxform;
 	IPSEC_ASSERT(ipcompx != NULL, ("null compression xform"));
@@ -367,7 +404,7 @@ ipcomp_output(
 	 */
 	if (m->m_pkthdr.len <= ipcompx->minlen) {
 		IPCOMPSTAT_INC(ipcomps_threshold);
-		return ipsec_process_done(m, isr);
+		return ipsec_process_done(m, sp, sav, idx);
 	}
 
 	ralen = m->m_pkthdr.len - skip;	/* Raw payload length before comp. */
@@ -390,7 +427,7 @@ ipcomp_output(
 		DPRINTF(("%s: unknown/unsupported protocol family %d, "
 		    "IPCA %s/%08lx\n", __func__,
 		    sav->sah->saidx.dst.sa.sa_family,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi)));
 		error = EPFNOSUPPORT;
 		goto bad;
@@ -399,7 +436,7 @@ ipcomp_output(
 		IPCOMPSTAT_INC(ipcomps_toobig);
 		DPRINTF(("%s: packet in IPCA %s/%08lx got too big "
 		    "(len %u, max len %u)\n", __func__,
-		    ipsec_address(&sav->sah->saidx.dst),
+		    ipsec_address(&sav->sah->saidx.dst, buf, sizeof(buf)),
 		    (u_long) ntohl(sav->spi),
 		    ralen + skip + IPCOMP_HLENGTH, maxpacketsize));
 		error = EMSGSIZE;
@@ -413,8 +450,8 @@ ipcomp_output(
 	if (m == NULL) {
 		IPCOMPSTAT_INC(ipcomps_hdrops);
 		DPRINTF(("%s: cannot clone mbuf chain, IPCA %s/%08lx\n",
-		    __func__, ipsec_address(&sav->sah->saidx.dst),
-		    (u_long) ntohl(sav->spi)));
+		    __func__, ipsec_address(&sav->sah->saidx.dst, buf,
+		    sizeof(buf)), (u_long) ntohl(sav->spi)));
 		error = ENOBUFS;
 		goto bad;
 	}
@@ -441,32 +478,31 @@ ipcomp_output(
 	crdc->crd_alg = ipcompx->type;
 
 	/* IPsec-specific opaque crypto info */
-	tc = (struct tdb_crypto *) malloc(sizeof(struct tdb_crypto),
-		M_XDATA, M_NOWAIT|M_ZERO);
-	if (tc == NULL) {
+	xd =  malloc(sizeof(struct xform_data), M_XDATA, M_NOWAIT | M_ZERO);
+	if (xd == NULL) {
 		IPCOMPSTAT_INC(ipcomps_crypto);
-		DPRINTF(("%s: failed to allocate tdb_crypto\n", __func__));
+		DPRINTF(("%s: failed to allocate xform_data\n", __func__));
 		crypto_freereq(crp);
 		error = ENOBUFS;
 		goto bad;
 	}
 
-	tc->tc_isr = isr;
-	KEY_ADDREFSA(sav);
-	tc->tc_sav = sav;
-	tc->tc_spi = sav->spi;
-	tc->tc_dst = sav->sah->saidx.dst;
-	tc->tc_proto = sav->sah->saidx.proto;
-	tc->tc_protoff = protoff;
-	tc->tc_skip = skip;
+	xd->sp = sp;
+	xd->sav = sav;
+	xd->idx = idx;
+	xd->skip = skip;
+	xd->protoff = protoff;
 
 	/* Crypto operation descriptor */
 	crp->crp_ilen = m->m_pkthdr.len;	/* Total input length */
 	crp->crp_flags = CRYPTO_F_IMBUF | CRYPTO_F_CBIFSYNC;
 	crp->crp_buf = (caddr_t) m;
 	crp->crp_callback = ipcomp_output_cb;
-	crp->crp_opaque = (caddr_t) tc;
-	crp->crp_sid = sav->tdb_cryptoid;
+	crp->crp_opaque = (caddr_t) xd;
+
+	SECASVAR_LOCK(sav);
+	crp->crp_sid = xd->cryptoid = sav->tdb_cryptoid;
+	SECASVAR_UNLOCK(sav);
 
 	return crypto_dispatch(crp);
 bad:
@@ -481,37 +517,32 @@ bad:
 static int
 ipcomp_output_cb(struct cryptop *crp)
 {
-	struct tdb_crypto *tc;
-	struct ipsecrequest *isr;
+	char buf[IPSEC_ADDRSTRLEN];
+	struct xform_data *xd;
+	struct secpolicy *sp;
 	struct secasvar *sav;
 	struct mbuf *m;
-	int error, skip;
+	uint64_t cryptoid;
+	u_int idx;
+	int error, skip, protoff;
 
-	tc = (struct tdb_crypto *) crp->crp_opaque;
-	IPSEC_ASSERT(tc != NULL, ("null opaque data area!"));
 	m = (struct mbuf *) crp->crp_buf;
-	skip = tc->tc_skip;
-
-	isr = tc->tc_isr;
-	IPSECREQUEST_LOCK(isr);
-	sav = tc->tc_sav;
-	/* With the isr lock released SA pointer can be updated. */
-	if (sav != isr->sav) {
-		IPCOMPSTAT_INC(ipcomps_notdb);
-		DPRINTF(("%s: SA expired while in crypto\n", __func__));
-		error = ENOBUFS;		/*XXX*/
-		goto bad;
-	}
+	xd = (struct xform_data *) crp->crp_opaque;
+	idx = xd->idx;
+	sp = xd->sp;
+	sav = xd->sav;
+	skip = xd->skip;
+	protoff = xd->protoff;
+	cryptoid = xd->cryptoid;
 
 	/* Check for crypto errors */
 	if (crp->crp_etype) {
-		/* Reset the session ID */
-		if (sav->tdb_cryptoid != 0)
-			sav->tdb_cryptoid = crp->crp_sid;
-
 		if (crp->crp_etype == EAGAIN) {
-			IPSECREQUEST_UNLOCK(isr);
-			return crypto_dispatch(crp);
+			/* Reset the session ID */
+			if (ipsec_updateid(sav, &crp->crp_sid, &cryptoid) != 0)
+				crypto_freesession(cryptoid);
+			xd->cryptoid = crp->crp_sid;
+			return (crypto_dispatch(crp));
 		}
 		IPCOMPSTAT_INC(ipcomps_noxform);
 		DPRINTF(("%s: crypto error %d\n", __func__, crp->crp_etype));
@@ -537,9 +568,10 @@ ipcomp_output_cb(struct cryptop *crp)
 		mo = m_makespace(m, skip, IPCOMP_HLENGTH, &roff);
 		if (mo == NULL) {
 			IPCOMPSTAT_INC(ipcomps_wrap);
-			DPRINTF(("%s: IPCOMP header inject failed for IPCA %s/%08lx\n",
-			    __func__, ipsec_address(&sav->sah->saidx.dst),
-			    (u_long) ntohl(sav->spi)));
+			DPRINTF(("%s: IPCOMP header inject failed "
+			    "for IPCA %s/%08lx\n",
+			    __func__, ipsec_address(&sav->sah->saidx.dst, buf,
+			    sizeof(buf)), (u_long) ntohl(sav->spi)));
 			error = ENOBUFS;
 			goto bad;
 		}
@@ -564,7 +596,7 @@ ipcomp_output_cb(struct cryptop *crp)
 
 		/* Fix Next Protocol in IPv4/IPv6 header */
 		prot = IPPROTO_IPCOMP;
-		m_copyback(m, tc->tc_protoff, sizeof(u_int8_t),
+		m_copyback(m, protoff, sizeof(u_int8_t),
 		    (u_char *)&prot);
 
 		/* Adjust the length in the IP header */
@@ -585,8 +617,8 @@ ipcomp_output_cb(struct cryptop *crp)
 			DPRINTF(("%s: unknown/unsupported protocol "
 			    "family %d, IPCA %s/%08lx\n", __func__,
 			    sav->sah->saidx.dst.sa.sa_family,
-			    ipsec_address(&sav->sah->saidx.dst),
-			    (u_long) ntohl(sav->spi)));
+			    ipsec_address(&sav->sah->saidx.dst, buf,
+				sizeof(buf)), (u_long) ntohl(sav->spi)));
 			error = EPFNOSUPPORT;
 			goto bad;
 		}
@@ -600,47 +632,143 @@ ipcomp_output_cb(struct cryptop *crp)
 	}
 
 	/* Release the crypto descriptor */
-	free(tc, M_XDATA);
+	free(xd, M_XDATA);
 	crypto_freereq(crp);
 
 	/* NB: m is reclaimed by ipsec_process_done. */
-	error = ipsec_process_done(m, isr);
-	KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
-	return error;
+	error = ipsec_process_done(m, sp, sav, idx);
+	return (error);
 bad:
-	if (sav)
-		KEY_FREESAV(&sav);
-	IPSECREQUEST_UNLOCK(isr);
 	if (m)
 		m_freem(m);
-	free(tc, M_XDATA);
+	free(xd, M_XDATA);
 	crypto_freereq(crp);
-	return error;
+	key_freesav(&sav);
+	key_freesp(&sp);
+	return (error);
+}
+
+#ifdef INET
+static const struct encaptab *ipe4_cookie = NULL;
+extern struct domain inetdomain;
+static struct protosw ipcomp4_protosw = {
+	.pr_type =	SOCK_RAW,
+	.pr_domain =	&inetdomain,
+	.pr_protocol =	0 /* IPPROTO_IPV[46] */,
+	.pr_flags =	PR_ATOMIC | PR_ADDR | PR_LASTHDR,
+	.pr_input =	ipcomp_nonexp_input,
+	.pr_output =	rip_output,
+	.pr_ctloutput =	rip_ctloutput,
+	.pr_usrreqs =	&rip_usrreqs
+};
+
+static int
+ipcomp4_nonexp_encapcheck(const struct mbuf *m, int off, int proto,
+    void *arg __unused)
+{
+	union sockaddr_union src, dst;
+	const struct ip *ip;
+
+	if (V_ipcomp_enable == 0)
+		return (0);
+	if (proto != IPPROTO_IPV4 && proto != IPPROTO_IPV6)
+		return (0);
+	bzero(&src, sizeof(src));
+	bzero(&dst, sizeof(dst));
+	src.sa.sa_family = dst.sa.sa_family = AF_INET;
+	src.sin.sin_len = dst.sin.sin_len = sizeof(struct sockaddr_in);
+	ip = mtod(m, const struct ip *);
+	src.sin.sin_addr = ip->ip_src;
+	dst.sin.sin_addr = ip->ip_dst;
+	return (ipcomp_encapcheck(&src, &dst));
 }
+#endif
+#ifdef INET6
+static const struct encaptab *ipe6_cookie = NULL;
+extern struct domain inet6domain;
+static struct protosw ipcomp6_protosw = {
+	.pr_type =	SOCK_RAW,
+	.pr_domain =	&inet6domain,
+	.pr_protocol =	0 /* IPPROTO_IPV[46] */,
+	.pr_flags =	PR_ATOMIC | PR_ADDR | PR_LASTHDR,
+	.pr_input =	ipcomp_nonexp_input,
+	.pr_output =	rip6_output,
+	.pr_ctloutput =	rip6_ctloutput,
+	.pr_usrreqs =	&rip6_usrreqs
+};
+
+static int
+ipcomp6_nonexp_encapcheck(const struct mbuf *m, int off, int proto,
+    void *arg __unused)
+{
+	union sockaddr_union src, dst;
+	const struct ip6_hdr *ip6;
+
+	if (V_ipcomp_enable == 0)
+		return (0);
+	if (proto != IPPROTO_IPV4 && proto != IPPROTO_IPV6)
+		return (0);
+	bzero(&src, sizeof(src));
+	bzero(&dst, sizeof(dst));
+	src.sa.sa_family = dst.sa.sa_family = AF_INET;
+	src.sin6.sin6_len = dst.sin6.sin6_len = sizeof(struct sockaddr_in6);
+	ip6 = mtod(m, const struct ip6_hdr *);
+	src.sin6.sin6_addr = ip6->ip6_src;
+	dst.sin6.sin6_addr = ip6->ip6_dst;
+	if (IN6_IS_SCOPE_LINKLOCAL(&src.sin6.sin6_addr)) {
+		/* XXX: sa6_recoverscope() */
+		src.sin6.sin6_scope_id =
+		    ntohs(src.sin6.sin6_addr.s6_addr16[1]);
+		src.sin6.sin6_addr.s6_addr16[1] = 0;
+	}
+	if (IN6_IS_SCOPE_LINKLOCAL(&dst.sin6.sin6_addr)) {
+		/* XXX: sa6_recoverscope() */
+		dst.sin6.sin6_scope_id =
+		    ntohs(dst.sin6.sin6_addr.s6_addr16[1]);
+		dst.sin6.sin6_addr.s6_addr16[1] = 0;
+	}
+	return (ipcomp_encapcheck(&src, &dst));
+}
+#endif
 
 static struct xformsw ipcomp_xformsw = {
-	XF_IPCOMP,		XFT_COMP,		"IPcomp",
-	ipcomp_init,		ipcomp_zeroize,		ipcomp_input,
-	ipcomp_output
+	.xf_type =	XF_IPCOMP,
+	.xf_name =	"IPcomp",
+	.xf_init =	ipcomp_init,
+	.xf_zeroize =	ipcomp_zeroize,
+	.xf_input =	ipcomp_input,
+	.xf_output =	ipcomp_output,
 };
 
 static void
 ipcomp_attach(void)
 {
 
-	xform_register(&ipcomp_xformsw);
+#ifdef INET
+	ipe4_cookie = encap_attach_func(AF_INET, -1,
+	    ipcomp4_nonexp_encapcheck, &ipcomp4_protosw, NULL);
+#endif
+#ifdef INET6
+	ipe6_cookie = encap_attach_func(AF_INET6, -1,
+	    ipcomp6_nonexp_encapcheck, &ipcomp6_protosw, NULL);
+#endif
+	xform_attach(&ipcomp_xformsw);
 }
 
-SYSINIT(ipcomp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE, ipcomp_attach, NULL);
-
 static void
-vnet_ipcomp_attach(const void *unused __unused)
+ipcomp_detach(void)
 {
 
-	/* XXX */
-	V_ipcompstat.version = IPCOMPSTAT_VERSION;
+#ifdef INET
+	encap_detach(ipe4_cookie);
+#endif
+#ifdef INET6
+	encap_detach(ipe6_cookie);
+#endif
+	xform_detach(&ipcomp_xformsw);
 }
 
-VNET_SYSINIT(vnet_ipcomp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
-    vnet_ipcomp_attach, NULL);
+SYSINIT(ipcomp_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    ipcomp_attach, NULL);
+SYSUNINIT(ipcomp_xform_uninit, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE,
+    ipcomp_detach, NULL);
diff --git a/freebsd/sys/netipsec/xform_ipip.c b/freebsd/sys/netipsec/xform_ipip.c
deleted file mode 100644
index b7234be9..00000000
--- a/freebsd/sys/netipsec/xform_ipip.c
+++ /dev/null
@@ -1,728 +0,0 @@
-#include <machine/rtems-bsd-kernel-space.h>
-
-/*	$FreeBSD$	*/
-/*	$OpenBSD: ip_ipip.c,v 1.25 2002/06/10 18:04:55 itojun Exp $ */
-/*-
- * The authors of this code are John Ioannidis (ji@tla.org),
- * Angelos D. Keromytis (kermit@csd.uch.gr) and
- * Niels Provos (provos@physnet.uni-hamburg.de).
- *
- * The original version of this code was written by John Ioannidis
- * for BSD/OS in Athens, Greece, in November 1995.
- *
- * Ported to OpenBSD and NetBSD, with additional transforms, in December 1996,
- * by Angelos D. Keromytis.
- *
- * Additional transforms and features in 1997 and 1998 by Angelos D. Keromytis
- * and Niels Provos.
- *
- * Additional features in 1999 by Angelos D. Keromytis.
- *
- * Copyright (C) 1995, 1996, 1997, 1998, 1999 by John Ioannidis,
- * Angelos D. Keromytis and Niels Provos.
- * Copyright (c) 2001, Angelos D. Keromytis.
- *
- * Permission to use, copy, and modify this software with or without fee
- * is hereby granted, provided that this entire notice is included in
- * all copies of any software which is or includes a copy or
- * modification of this software.
- * You may use this code under the GNU public license if you so wish. Please
- * contribute changes back to the authors under this freer than GPL license
- * so that we may further the use of strong encryption without limitations to
- * all.
- *
- * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR
- * IMPLIED WARRANTY. IN PARTICULAR, NONE OF THE AUTHORS MAKES ANY
- * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE
- * MERCHANTABILITY OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR
- * PURPOSE.
- */
-
-/*
- * IP-inside-IP processing
- */
-#include <rtems/bsd/local/opt_inet.h>
-#include <rtems/bsd/local/opt_inet6.h>
-#include <rtems/bsd/local/opt_enc.h>
-
-#include <rtems/bsd/sys/param.h>
-#include <sys/systm.h>
-#include <sys/mbuf.h>
-#include <sys/socket.h>
-#include <sys/kernel.h>
-#include <sys/protosw.h>
-#include <sys/sysctl.h>
-
-#include <net/if.h>
-#include <net/pfil.h>
-#include <net/route.h>
-#include <net/netisr.h>
-#include <net/vnet.h>
-
-#include <netinet/in.h>
-#include <netinet/in_systm.h>
-#include <netinet/in_var.h>
-#include <netinet/ip.h>
-#include <netinet/ip_ecn.h>
-#include <netinet/ip_var.h>
-#include <netinet/ip_encap.h>
-#ifdef MROUTING
-#include <netinet/ip_mroute.h>
-#endif
-
-#include <netipsec/ipsec.h>
-#include <netipsec/xform.h>
-
-#include <netipsec/ipip_var.h>
-
-#ifdef INET6
-#include <netinet/ip6.h>
-#include <netipsec/ipsec6.h>
-#include <netinet6/ip6_ecn.h>
-#include <netinet6/in6_var.h>
-#include <netinet6/ip6protosw.h>
-#endif
-
-#include <netipsec/key.h>
-#include <netipsec/key_debug.h>
-
-#include <machine/stdarg.h>
-
-/*
- * We can control the acceptance of IP4 packets by altering the sysctl
- * net.inet.ipip.allow value.  Zero means drop them, all else is acceptance.
- */
-VNET_DEFINE(int, ipip_allow) = 0;
-VNET_DEFINE(struct ipipstat, ipipstat);
-
-SYSCTL_DECL(_net_inet_ipip);
-SYSCTL_VNET_INT(_net_inet_ipip, OID_AUTO,
-	ipip_allow,	CTLFLAG_RW,	&VNET_NAME(ipip_allow),	0, "");
-SYSCTL_VNET_STRUCT(_net_inet_ipip, IPSECCTL_STATS,
-	stats,		CTLFLAG_RD,	&VNET_NAME(ipipstat),	ipipstat, "");
-
-/* XXX IPCOMP */
-#define	M_IPSEC	(M_AUTHIPHDR|M_AUTHIPDGM|M_DECRYPTED)
-
-static void _ipip_input(struct mbuf *m, int iphlen, struct ifnet *gifp);
-
-#ifdef INET6
-/*
- * Really only a wrapper for ipip_input(), for use with IPv6.
- */
-int
-ip4_input6(struct mbuf **m, int *offp, int proto)
-{
-#if 0
-	/* If we do not accept IP-in-IP explicitly, drop.  */
-	if (!V_ipip_allow && ((*m)->m_flags & M_IPSEC) == 0) {
-		DPRINTF(("%s: dropped due to policy\n", __func__));
-		IPIPSTAT_INC(ipips_pdrops);
-		m_freem(*m);
-		return IPPROTO_DONE;
-	}
-#endif
-	_ipip_input(*m, *offp, NULL);
-	return IPPROTO_DONE;
-}
-#endif /* INET6 */
-
-#ifdef INET
-/*
- * Really only a wrapper for ipip_input(), for use with IPv4.
- */
-void
-ip4_input(struct mbuf *m, int off)
-{
-#if 0
-	/* If we do not accept IP-in-IP explicitly, drop.  */
-	if (!V_ipip_allow && (m->m_flags & M_IPSEC) == 0) {
-		DPRINTF(("%s: dropped due to policy\n", __func__));
-		IPIPSTAT_INC(ipips_pdrops);
-		m_freem(m);
-		return;
-	}
-#endif
-	_ipip_input(m, off, NULL);
-}
-#endif /* INET */
-
-/*
- * ipip_input gets called when we receive an IP{46} encapsulated packet,
- * either because we got it at a real interface, or because AH or ESP
- * were being used in tunnel mode (in which case the rcvif element will
- * contain the address of the encX interface associated with the tunnel.
- */
-
-static void
-_ipip_input(struct mbuf *m, int iphlen, struct ifnet *gifp)
-{
-#ifdef INET
-	register struct sockaddr_in *sin;
-#endif
-	register struct ifnet *ifp;
-	register struct ifaddr *ifa;
-	struct ip *ipo;
-#ifdef INET6
-	register struct sockaddr_in6 *sin6;
-	struct ip6_hdr *ip6 = NULL;
-	u_int8_t itos;
-#endif
-	u_int8_t nxt;
-	int isr;
-	u_int8_t otos;
-	u_int8_t v;
-	int hlen;
-
-	IPIPSTAT_INC(ipips_ipackets);
-
-	m_copydata(m, 0, 1, &v);
-
-	switch (v >> 4) {
-#ifdef INET
-        case 4:
-		hlen = sizeof(struct ip);
-		break;
-#endif /* INET */
-#ifdef INET6
-        case 6:
-		hlen = sizeof(struct ip6_hdr);
-		break;
-#endif
-        default:
-		IPIPSTAT_INC(ipips_family);
-		m_freem(m);
-		return /* EAFNOSUPPORT */;
-	}
-
-	/* Bring the IP header in the first mbuf, if not there already */
-	if (m->m_len < hlen) {
-		if ((m = m_pullup(m, hlen)) == NULL) {
-			DPRINTF(("%s: m_pullup (1) failed\n", __func__));
-			IPIPSTAT_INC(ipips_hdrops);
-			return;
-		}
-	}
-
-	ipo = mtod(m, struct ip *);
-
-#ifdef MROUTING
-	if (ipo->ip_v == IPVERSION && ipo->ip_p == IPPROTO_IPV4) {
-		if (IN_MULTICAST(((struct ip *)((char *) ipo + iphlen))->ip_dst.s_addr)) {
-			ipip_mroute_input (m, iphlen);
-			return;
-		}
-	}
-#endif /* MROUTING */
-
-	/* Keep outer ecn field. */
-	switch (v >> 4) {
-#ifdef INET
-	case 4:
-		otos = ipo->ip_tos;
-		break;
-#endif /* INET */
-#ifdef INET6
-	case 6:
-		otos = (ntohl(mtod(m, struct ip6_hdr *)->ip6_flow) >> 20) & 0xff;
-		break;
-#endif
-	default:
-		panic("ipip_input: unknown ip version %u (outer)", v>>4);
-	}
-
-	/* Remove outer IP header */
-	m_adj(m, iphlen);
-
-	/* Sanity check */
-	if (m->m_pkthdr.len < sizeof(struct ip))  {
-		IPIPSTAT_INC(ipips_hdrops);
-		m_freem(m);
-		return;
-	}
-
-	m_copydata(m, 0, 1, &v);
-
-	switch (v >> 4) {
-#ifdef INET
-        case 4:
-		hlen = sizeof(struct ip);
-		break;
-#endif /* INET */
-
-#ifdef INET6
-        case 6:
-		hlen = sizeof(struct ip6_hdr);
-		break;
-#endif
-	default:
-		IPIPSTAT_INC(ipips_family);
-		m_freem(m);
-		return; /* EAFNOSUPPORT */
-	}
-
-	/*
-	 * Bring the inner IP header in the first mbuf, if not there already.
-	 */
-	if (m->m_len < hlen) {
-		if ((m = m_pullup(m, hlen)) == NULL) {
-			DPRINTF(("%s: m_pullup (2) failed\n", __func__));
-			IPIPSTAT_INC(ipips_hdrops);
-			return;
-		}
-	}
-
-	/*
-	 * RFC 1853 specifies that the inner TTL should not be touched on
-	 * decapsulation. There's no reason this comment should be here, but
-	 * this is as good as any a position.
-	 */
-
-	/* Some sanity checks in the inner IP header */
-	switch (v >> 4) {
-#ifdef INET
-    	case 4:
-                ipo = mtod(m, struct ip *);
-                nxt = ipo->ip_p;
-		ip_ecn_egress(V_ip4_ipsec_ecn, &otos, &ipo->ip_tos);
-                break;
-#endif /* INET */
-#ifdef INET6
-    	case 6:
-                ip6 = (struct ip6_hdr *) ipo;
-                nxt = ip6->ip6_nxt;
-		itos = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
-		ip_ecn_egress(V_ip6_ipsec_ecn, &otos, &itos);
-		ip6->ip6_flow &= ~htonl(0xff << 20);
-		ip6->ip6_flow |= htonl((u_int32_t) itos << 20);
-                break;
-#endif
-	default:
-		panic("ipip_input: unknown ip version %u (inner)", v>>4);
-	}
-
-	/* Check for local address spoofing. */
-	if ((m->m_pkthdr.rcvif == NULL ||
-	    !(m->m_pkthdr.rcvif->if_flags & IFF_LOOPBACK)) &&
-	    V_ipip_allow != 2) {
-	    	IFNET_RLOCK_NOSLEEP();
-		TAILQ_FOREACH(ifp, &V_ifnet, if_link) {
-			TAILQ_FOREACH(ifa, &ifp->if_addrhead, ifa_link) {
-#ifdef INET
-				if (ipo) {
-					if (ifa->ifa_addr->sa_family !=
-					    AF_INET)
-						continue;
-
-					sin = (struct sockaddr_in *) ifa->ifa_addr;
-
-					if (sin->sin_addr.s_addr ==
-					    ipo->ip_src.s_addr)	{
-						IPIPSTAT_INC(ipips_spoof);
-						m_freem(m);
-						IFNET_RUNLOCK_NOSLEEP();
-						return;
-					}
-				}
-#endif /* INET */
-
-#ifdef INET6
-				if (ip6) {
-					if (ifa->ifa_addr->sa_family !=
-					    AF_INET6)
-						continue;
-
-					sin6 = (struct sockaddr_in6 *) ifa->ifa_addr;
-
-					if (IN6_ARE_ADDR_EQUAL(&sin6->sin6_addr, &ip6->ip6_src)) {
-						IPIPSTAT_INC(ipips_spoof);
-						m_freem(m);
-						IFNET_RUNLOCK_NOSLEEP();
-						return;
-					}
-
-				}
-#endif /* INET6 */
-			}
-		}
-		IFNET_RUNLOCK_NOSLEEP();
-	}
-
-	/* 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
-	 * interface. Otherwise, it'll point to an enc interface, which
-	 * will allow a packet filter to distinguish between secure and
-	 * untrusted packets.
-	 */
-
-	switch (v >> 4) {
-#ifdef INET
-	case 4:
-		isr = NETISR_IP;
-		break;
-#endif
-#ifdef INET6
-	case 6:
-		isr = NETISR_IPV6;
-		break;
-#endif
-	default:
-		panic("%s: bogus ip version %u", __func__, v>>4);
-	}
-
-	m_addr_changed(m);
-
-	if (netisr_queue(isr, m)) {	/* (0) on success. */
-		IPIPSTAT_INC(ipips_qfull);
-		DPRINTF(("%s: packet dropped because of full queue\n",
-			__func__));
-	}
-}
-
-int
-ipip_output(
-	struct mbuf *m,
-	struct ipsecrequest *isr,
-	struct mbuf **mp,
-	int skip,
-	int protoff
-)
-{
-	struct secasvar *sav;
-	u_int8_t tp, otos;
-	struct secasindex *saidx;
-	int error;
-#if defined(INET) || defined(INET6)
-	u_int8_t itos;
-#endif
-#ifdef INET
-	struct ip *ipo;
-#endif /* INET */
-#ifdef INET6
-	struct ip6_hdr *ip6, *ip6o;
-#endif /* INET6 */
-
-	sav = isr->sav;
-	IPSEC_ASSERT(sav != NULL, ("null SA"));
-	IPSEC_ASSERT(sav->sah != NULL, ("null SAH"));
-
-	/* XXX Deal with empty TDB source/destination addresses. */
-
-	m_copydata(m, 0, 1, &tp);
-	tp = (tp >> 4) & 0xff;  /* Get the IP version number. */
-
-	saidx = &sav->sah->saidx;
-	switch (saidx->dst.sa.sa_family) {
-#ifdef INET
-	case AF_INET:
-		if (saidx->src.sa.sa_family != AF_INET ||
-		    saidx->src.sin.sin_addr.s_addr == INADDR_ANY ||
-		    saidx->dst.sin.sin_addr.s_addr == INADDR_ANY) {
-			DPRINTF(("%s: unspecified tunnel endpoint "
-			    "address in SA %s/%08lx\n", __func__,
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-			IPIPSTAT_INC(ipips_unspec);
-			error = EINVAL;
-			goto bad;
-		}
-
-		M_PREPEND(m, sizeof(struct ip), M_DONTWAIT);
-		if (m == 0) {
-			DPRINTF(("%s: M_PREPEND failed\n", __func__));
-			IPIPSTAT_INC(ipips_hdrops);
-			error = ENOBUFS;
-			goto bad;
-		}
-
-		ipo = mtod(m, struct ip *);
-
-		ipo->ip_v = IPVERSION;
-		ipo->ip_hl = 5;
-		ipo->ip_len = htons(m->m_pkthdr.len);
-		ipo->ip_ttl = V_ip_defttl;
-		ipo->ip_sum = 0;
-		ipo->ip_src = saidx->src.sin.sin_addr;
-		ipo->ip_dst = saidx->dst.sin.sin_addr;
-
-		ipo->ip_id = ip_newid();
-
-		/* If the inner protocol is IP... */
-		switch (tp) {
-		case IPVERSION:
-			/* Save ECN notification */
-			m_copydata(m, sizeof(struct ip) +
-			    offsetof(struct ip, ip_tos),
-			    sizeof(u_int8_t), (caddr_t) &itos);
-
-			ipo->ip_p = IPPROTO_IPIP;
-
-			/*
-			 * We should be keeping tunnel soft-state and
-			 * send back ICMPs if needed.
-			 */
-			m_copydata(m, sizeof(struct ip) +
-			    offsetof(struct ip, ip_off),
-			    sizeof(u_int16_t), (caddr_t) &ipo->ip_off);
-			ipo->ip_off = ntohs(ipo->ip_off);
-			ipo->ip_off &= ~(IP_DF | IP_MF | IP_OFFMASK);
-			ipo->ip_off = htons(ipo->ip_off);
-			break;
-#ifdef INET6
-		case (IPV6_VERSION >> 4):
-		{
-			u_int32_t itos32;
-
-			/* Save ECN notification. */
-			m_copydata(m, sizeof(struct ip) +
-			    offsetof(struct ip6_hdr, ip6_flow),
-			    sizeof(u_int32_t), (caddr_t) &itos32);
-			itos = ntohl(itos32) >> 20;
-			ipo->ip_p = IPPROTO_IPV6;
-			ipo->ip_off = 0;
-			break;
-		}
-#endif /* INET6 */
-		default:
-			goto nofamily;
-		}
-
-		otos = 0;
-		ip_ecn_ingress(ECN_ALLOWED, &otos, &itos);
-		ipo->ip_tos = otos;
-		break;
-#endif /* INET */
-
-#ifdef INET6
-	case AF_INET6:
-		if (IN6_IS_ADDR_UNSPECIFIED(&saidx->dst.sin6.sin6_addr) ||
-		    saidx->src.sa.sa_family != AF_INET6 ||
-		    IN6_IS_ADDR_UNSPECIFIED(&saidx->src.sin6.sin6_addr)) {
-			DPRINTF(("%s: unspecified tunnel endpoint "
-			    "address in SA %s/%08lx\n", __func__,
-			    ipsec_address(&saidx->dst),
-			    (u_long) ntohl(sav->spi)));
-			IPIPSTAT_INC(ipips_unspec);
-			error = ENOBUFS;
-			goto bad;
-		}
-
-		/* scoped address handling */
-		ip6 = mtod(m, struct ip6_hdr *);
-		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src))
-			ip6->ip6_src.s6_addr16[1] = 0;
-		if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst))
-			ip6->ip6_dst.s6_addr16[1] = 0;
-
-		M_PREPEND(m, sizeof(struct ip6_hdr), M_DONTWAIT);
-		if (m == 0) {
-			DPRINTF(("%s: M_PREPEND failed\n", __func__));
-			IPIPSTAT_INC(ipips_hdrops);
-			error = ENOBUFS;
-			goto bad;
-		}
-
-		/* Initialize IPv6 header */
-		ip6o = mtod(m, struct ip6_hdr *);
-		ip6o->ip6_flow = 0;
-		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_dst = saidx->dst.sin6.sin6_addr;
-		ip6o->ip6_src = saidx->src.sin6.sin6_addr;
-
-		switch (tp) {
-#ifdef INET
-		case IPVERSION:
-			/* Save ECN notification */
-			m_copydata(m, sizeof(struct ip6_hdr) +
-			    offsetof(struct ip, ip_tos), sizeof(u_int8_t),
-			    (caddr_t) &itos);
-
-			/* This is really IPVERSION. */
-			ip6o->ip6_nxt = IPPROTO_IPIP;
-			break;
-#endif /* INET */
-		case (IPV6_VERSION >> 4):
-		{
-			u_int32_t itos32;
-
-			/* Save ECN notification. */
-			m_copydata(m, sizeof(struct ip6_hdr) +
-			    offsetof(struct ip6_hdr, ip6_flow),
-			    sizeof(u_int32_t), (caddr_t) &itos32);
-			itos = ntohl(itos32) >> 20;
-
-			ip6o->ip6_nxt = IPPROTO_IPV6;
-			break;
-		}
-		default:
-			goto nofamily;
-		}
-
-		otos = 0;
-		ip_ecn_ingress(ECN_ALLOWED, &otos, &itos);
-		ip6o->ip6_flow |= htonl((u_int32_t) otos << 20);
-		break;
-#endif /* INET6 */
-
-	default:
-nofamily:
-		DPRINTF(("%s: unsupported protocol family %u\n", __func__,
-		    saidx->dst.sa.sa_family));
-		IPIPSTAT_INC(ipips_family);
-		error = EAFNOSUPPORT;		/* XXX diffs from openbsd */
-		goto bad;
-	}
-
-	IPIPSTAT_INC(ipips_opackets);
-	*mp = m;
-
-#ifdef INET
-	if (saidx->dst.sa.sa_family == AF_INET) {
-#if 0
-		if (sav->tdb_xform->xf_type == XF_IP4)
-			tdb->tdb_cur_bytes +=
-			    m->m_pkthdr.len - sizeof(struct ip);
-#endif
-		IPIPSTAT_ADD(ipips_obytes,
-		    m->m_pkthdr.len - sizeof(struct ip));
-	}
-#endif /* INET */
-
-#ifdef INET6
-	if (saidx->dst.sa.sa_family == AF_INET6) {
-#if 0
-		if (sav->tdb_xform->xf_type == XF_IP4)
-			tdb->tdb_cur_bytes +=
-			    m->m_pkthdr.len - sizeof(struct ip6_hdr);
-#endif
-		IPIPSTAT_ADD(ipips_obytes,
-		    m->m_pkthdr.len - sizeof(struct ip6_hdr));
-	}
-#endif /* INET6 */
-
-	return 0;
-bad:
-	if (m)
-		m_freem(m);
-	*mp = NULL;
-	return (error);
-}
-
-#ifdef IPSEC
-#if defined(INET) || defined(INET6)
-static int
-ipe4_init(struct secasvar *sav, struct xformsw *xsp)
-{
-	sav->tdb_xform = xsp;
-	return 0;
-}
-
-static int
-ipe4_zeroize(struct secasvar *sav)
-{
-	sav->tdb_xform = NULL;
-	return 0;
-}
-
-static int
-ipe4_input(struct mbuf *m, struct secasvar *sav, int skip, int protoff)
-{
-	/* This is a rather serious mistake, so no conditional printing. */
-	printf("%s: should never be called\n", __func__);
-	if (m)
-		m_freem(m);
-	return EOPNOTSUPP;
-}
-
-static struct xformsw ipe4_xformsw = {
-	XF_IP4,		0,		"IPv4 Simple Encapsulation",
-	ipe4_init,	ipe4_zeroize,	ipe4_input,	ipip_output,
-};
-
-extern struct domain inetdomain;
-#endif /* INET || INET6 */
-#ifdef INET
-static struct protosw ipe4_protosw = {
-	.pr_type =	SOCK_RAW,
-	.pr_domain =	&inetdomain,
-	.pr_protocol =	IPPROTO_IPV4,
-	.pr_flags =	PR_ATOMIC|PR_ADDR|PR_LASTHDR,
-	.pr_input =	ip4_input,
-	.pr_ctloutput =	rip_ctloutput,
-	.pr_usrreqs =	&rip_usrreqs
-};
-#endif /* INET */
-#if defined(INET6) && defined(INET)
-static struct ip6protosw ipe6_protosw = {
-	.pr_type =	SOCK_RAW,
-	.pr_domain =	&inetdomain,
-	.pr_protocol =	IPPROTO_IPV6,
-	.pr_flags =	PR_ATOMIC|PR_ADDR|PR_LASTHDR,
-	.pr_input =	ip4_input6,
-	.pr_ctloutput =	rip_ctloutput,
-	.pr_usrreqs =	&rip_usrreqs
-};
-#endif /* INET6 && INET */
-
-#if defined(INET)
-/*
- * Check the encapsulated packet to see if we want it
- */
-static int
-ipe4_encapcheck(const struct mbuf *m, int off, int proto, void *arg)
-{
-	/*
-	 * Only take packets coming from IPSEC tunnels; the rest
-	 * must be handled by the gif tunnel code.  Note that we
-	 * also return a minimum priority when we want the packet
-	 * so any explicit gif tunnels take precedence.
-	 */
-	return ((m->m_flags & M_IPSEC) != 0 ? 1 : 0);
-}
-#endif /* INET */
-
-static void
-ipe4_attach(void)
-{
-
-	xform_register(&ipe4_xformsw);
-	/* attach to encapsulation framework */
-	/* XXX save return cookie for detach on module remove */
-#ifdef INET
-	(void) encap_attach_func(AF_INET, -1,
-		ipe4_encapcheck, &ipe4_protosw, NULL);
-#endif
-#if defined(INET6) && defined(INET)
-	(void) encap_attach_func(AF_INET6, -1,
-		ipe4_encapcheck, (struct protosw *)&ipe6_protosw, NULL);
-#endif
-}
-SYSINIT(ipe4_xform_init, SI_SUB_PROTO_DOMAIN, SI_ORDER_MIDDLE, ipe4_attach, NULL);
-#endif	/* IPSEC */
diff --git a/freebsd/sys/netipsec/xform_tcp.c b/freebsd/sys/netipsec/xform_tcp.c
index 398dca13..81b4f1d0 100644
--- a/freebsd/sys/netipsec/xform_tcp.c
+++ b/freebsd/sys/netipsec/xform_tcp.c
@@ -1,9 +1,8 @@
 #include <machine/rtems-bsd-kernel-space.h>
 
-/*	$FreeBSD$ */
-
 /*-
  * Copyright (c) 2003 Bruce M. Simpson <bms@spc.org>
+ * Copyright (c) 2016 Andrey V. Elsukov <ae@FreeBSD.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -30,29 +29,37 @@
  */
 
 /* TCP MD5 Signature Option (RFC2385) */
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
 #include <rtems/bsd/local/opt_inet.h>
 #include <rtems/bsd/local/opt_inet6.h>
+#include <rtems/bsd/local/opt_ipsec.h>
 
 #include <rtems/bsd/sys/param.h>
 #include <sys/systm.h>
 #include <sys/mbuf.h>
 #include <rtems/bsd/sys/lock.h>
+#include <sys/md5.h>
+#include <sys/rmlock.h>
 #include <sys/socket.h>
+#include <sys/sockopt.h>
 #include <sys/kernel.h>
+#include <sys/module.h>
 #include <sys/protosw.h>
-#include <sys/sysctl.h>
 
 #include <netinet/in.h>
+#include <netinet/in_pcb.h>
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
 #include <netinet/ip_var.h>
 #include <netinet/tcp.h>
 #include <netinet/tcp_var.h>
 
-#include <net/route.h>
 #include <net/vnet.h>
 
 #include <netipsec/ipsec.h>
+#include <netipsec/ipsec_support.h>
 #include <netipsec/xform.h>
 
 #ifdef INET6
@@ -63,13 +70,256 @@
 #include <netipsec/key.h>
 #include <netipsec/key_debug.h>
 
+#define	TCP_SIGLEN	16	/* length of computed digest in bytes */
+#define	TCP_KEYLEN_MIN	1	/* minimum length of TCP-MD5 key */
+#define	TCP_KEYLEN_MAX	80	/* maximum length of TCP-MD5 key */
+
+static int
+tcp_ipsec_pcbctl(struct inpcb *inp, struct sockopt *sopt)
+{
+	struct tcpcb *tp;
+	int error, optval;
+
+	INP_WLOCK_ASSERT(inp);
+	if (sopt->sopt_name != TCP_MD5SIG) {
+		INP_WUNLOCK(inp);
+		return (ENOPROTOOPT);
+	}
+
+	tp = intotcpcb(inp);
+	if (sopt->sopt_dir == SOPT_GET) {
+		optval = (tp->t_flags & TF_SIGNATURE) ? 1 : 0;
+		INP_WUNLOCK(inp);
+
+		/* On success return with released INP_WLOCK */
+		return (sooptcopyout(sopt, &optval, sizeof(optval)));
+	}
+
+	INP_WUNLOCK(inp);
+
+	error = sooptcopyin(sopt, &optval, sizeof(optval), sizeof(optval));
+	if (error != 0)
+		return (error);
+
+	/* INP_WLOCK_RECHECK */
+	INP_WLOCK(inp);
+	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+		INP_WUNLOCK(inp);
+		return (ECONNRESET);
+	}
+	if (optval > 0)
+		tp->t_flags |= TF_SIGNATURE;
+	else
+		tp->t_flags &= ~TF_SIGNATURE;
+
+	/* On success return with acquired INP_WLOCK */
+	return (error);
+}
+
+/*
+ * Callback function invoked by m_apply() to digest TCP segment data
+ * contained within an mbuf chain.
+ */
+static int
+tcp_signature_apply(void *fstate, void *data, u_int len)
+{
+
+	MD5Update(fstate, (u_char *)data, len);
+	return (0);
+}
+
+#ifdef INET
+static int
+ip_pseudo_compute(struct mbuf *m, MD5_CTX *ctx)
+{
+	struct ippseudo ipp;
+	struct ip *ip;
+
+	ip = mtod(m, struct ip *);
+	ipp.ippseudo_src.s_addr = ip->ip_src.s_addr;
+	ipp.ippseudo_dst.s_addr = ip->ip_dst.s_addr;
+	ipp.ippseudo_p = IPPROTO_TCP;
+	ipp.ippseudo_pad = 0;
+	ipp.ippseudo_len = htons(m->m_pkthdr.len - (ip->ip_hl << 2));
+	MD5Update(ctx, (char *)&ipp, sizeof(ipp));
+	return (ip->ip_hl << 2);
+}
+#endif
+
+#ifdef INET6
+static int
+ip6_pseudo_compute(struct mbuf *m, MD5_CTX *ctx)
+{
+	struct ip6_pseudo {
+		struct in6_addr src, dst;
+		uint32_t len;
+		uint32_t nxt;
+	} ip6p __aligned(4);
+	struct ip6_hdr *ip6;
+
+	ip6 = mtod(m, struct ip6_hdr *);
+	ip6p.src = ip6->ip6_src;
+	ip6p.dst = ip6->ip6_dst;
+	ip6p.len = htonl(m->m_pkthdr.len - sizeof(*ip6)); /* XXX: ext headers */
+	ip6p.nxt = htonl(IPPROTO_TCP);
+	MD5Update(ctx, (char *)&ip6p, sizeof(ip6p));
+	return (sizeof(*ip6));
+}
+#endif
+
+static int
+tcp_signature_compute(struct mbuf *m, struct tcphdr *th,
+    struct secasvar *sav, u_char *buf)
+{
+	MD5_CTX ctx;
+	int len;
+	u_short csum;
+
+	MD5Init(&ctx);
+	 /* Step 1: Update MD5 hash with IP(v6) pseudo-header. */
+	switch (sav->sah->saidx.dst.sa.sa_family) {
+#ifdef INET
+	case AF_INET:
+		len = ip_pseudo_compute(m, &ctx);
+		break;
+#endif
+#ifdef INET6
+	case AF_INET6:
+		len = ip6_pseudo_compute(m, &ctx);
+		break;
+#endif
+	default:
+		return (EAFNOSUPPORT);
+	}
+	/*
+	 * Step 2: Update MD5 hash with TCP header, excluding options.
+	 * The TCP checksum must be set to zero.
+	 */
+	csum = th->th_sum;
+	th->th_sum = 0;
+	MD5Update(&ctx, (char *)th, sizeof(struct tcphdr));
+	th->th_sum = csum;
+	/*
+	 * Step 3: Update MD5 hash with TCP segment data.
+	 * Use m_apply() to avoid an early m_pullup().
+	 */
+	len += (th->th_off << 2);
+	if (m->m_pkthdr.len - len > 0)
+		m_apply(m, len, m->m_pkthdr.len - len,
+		    tcp_signature_apply, &ctx);
+	/*
+	 * Step 4: Update MD5 hash with shared secret.
+	 */
+	MD5Update(&ctx, sav->key_auth->key_data, _KEYLEN(sav->key_auth));
+	MD5Final(buf, &ctx);
+	key_sa_recordxfer(sav, m);
+	return (0);
+}
+
+static void
+setsockaddrs(const struct mbuf *m, union sockaddr_union *src,
+    union sockaddr_union *dst)
+{
+	struct ip *ip;
+
+	IPSEC_ASSERT(m->m_len >= sizeof(*ip), ("unexpected mbuf len"));
+
+	ip = mtod(m, struct ip *);
+	switch (ip->ip_v) {
+#ifdef INET
+	case IPVERSION:
+		ipsec4_setsockaddrs(m, src, dst);
+		break;
+#endif
+#ifdef INET6
+	case (IPV6_VERSION >> 4):
+		ipsec6_setsockaddrs(m, src, dst);
+		break;
+#endif
+	default:
+		bzero(src, sizeof(*src));
+		bzero(dst, sizeof(*dst));
+	}
+}
+
+/*
+ * Compute TCP-MD5 hash of an *INBOUND* TCP segment.
+ * Parameters:
+ * m		pointer to head of mbuf chain
+ * th		pointer to TCP header
+ * buf		pointer to storage for computed MD5 digest
+ *
+ * Return 0 if successful, otherwise return -1.
+ */
+static int
+tcp_ipsec_input(struct mbuf *m, struct tcphdr *th, u_char *buf)
+{
+	char tmpdigest[TCP_SIGLEN];
+	struct secasindex saidx;
+	struct secasvar *sav;
+
+	setsockaddrs(m, &saidx.src, &saidx.dst);
+	saidx.proto = IPPROTO_TCP;
+	saidx.mode = IPSEC_MODE_TCPMD5;
+	saidx.reqid = 0;
+	sav = key_allocsa_tcpmd5(&saidx);
+	if (sav == NULL) {
+		KMOD_TCPSTAT_INC(tcps_sig_err_buildsig);
+		return (EACCES);
+	}
+	/*
+	 * tcp_input() operates with TCP header fields in host
+	 * byte order. We expect them in network byte order.
+	 */
+	tcp_fields_to_net(th);
+	tcp_signature_compute(m, th, sav, tmpdigest);
+	tcp_fields_to_host(th);
+	key_freesav(&sav);
+	if (bcmp(buf, tmpdigest, TCP_SIGLEN) != 0) {
+		KMOD_TCPSTAT_INC(tcps_sig_rcvbadsig);
+		return (EACCES);
+	}
+	KMOD_TCPSTAT_INC(tcps_sig_rcvgoodsig);
+	return (0);
+}
+
+/*
+ * Compute TCP-MD5 hash of an *OUTBOUND* TCP segment.
+ * Parameters:
+ * m		pointer to head of mbuf chain
+ * th		pointer to TCP header
+ * buf		pointer to storage for computed MD5 digest
+ *
+ * Return 0 if successful, otherwise return error code.
+ */
+static int
+tcp_ipsec_output(struct mbuf *m, struct tcphdr *th, u_char *buf)
+{
+	struct secasindex saidx;
+	struct secasvar *sav;
+
+	setsockaddrs(m, &saidx.src, &saidx.dst);
+	saidx.proto = IPPROTO_TCP;
+	saidx.mode = IPSEC_MODE_TCPMD5;
+	saidx.reqid = 0;
+	sav = key_allocsa_tcpmd5(&saidx);
+	if (sav == NULL) {
+		KMOD_TCPSTAT_INC(tcps_sig_err_buildsig);
+		return (EACCES);
+	}
+	tcp_signature_compute(m, th, sav, buf);
+	key_freesav(&sav);
+	return (0);
+}
+
 /*
  * Initialize a TCP-MD5 SA. Called when the SA is being set up.
  *
  * We don't need to set up the tdb prefixed fields, as we don't use the
  * opencrypto code; we just perform a key length check.
  *
- * XXX: Currently we only allow a single 'magic' SPI to be used.
+ * XXX: Currently we have used single 'magic' SPI and need to still
+ * support this.
  *
  * This allows per-host granularity without affecting the userland
  * interface, which is a simple socket option toggle switch,
@@ -88,11 +338,6 @@ tcpsignature_init(struct secasvar *sav, struct xformsw *xsp)
 {
 	int keylen;
 
-	if (sav->spi != htonl(TCP_SIG_SPI)) {
-		DPRINTF(("%s: SPI must be TCP_SIG_SPI (0x1000)\n",
-		    __func__));
-		return (EINVAL);
-	}
 	if (sav->alg_auth != SADB_X_AALG_TCP_MD5) {
 		DPRINTF(("%s: unsupported authentication algorithm %u\n",
 		    __func__, sav->alg_auth));
@@ -107,67 +352,76 @@ tcpsignature_init(struct secasvar *sav, struct xformsw *xsp)
 		DPRINTF(("%s: invalid key length %u\n", __func__, keylen));
 		return (EINVAL);
 	}
-
+	sav->tdb_xform = xsp;
 	return (0);
 }
 
 /*
- * Paranoia.
- *
  * Called when the SA is deleted.
  */
 static int
 tcpsignature_zeroize(struct secasvar *sav)
 {
 
-	if (sav->key_auth)
+	if (sav->key_auth != NULL)
 		bzero(sav->key_auth->key_data, _KEYLEN(sav->key_auth));
-
-	sav->tdb_cryptoid = 0;
-	sav->tdb_authalgxform = NULL;
 	sav->tdb_xform = NULL;
-
 	return (0);
 }
 
-/*
- * Verify that an input packet passes authentication.
- * Called from the ipsec layer.
- * We do this from within tcp itself, so this routine is just a stub.
- */
-static int
-tcpsignature_input(struct mbuf *m, struct secasvar *sav, int skip,
-    int protoff)
-{
+static struct xformsw tcpsignature_xformsw = {
+	.xf_type =	XF_TCPSIGNATURE,
+	.xf_name =	"TCP-MD5",
+	.xf_init =	tcpsignature_init,
+	.xf_zeroize =	tcpsignature_zeroize,
+};
 
-	return (0);
-}
+static const struct tcpmd5_methods tcpmd5_methods = {
+	.input = tcp_ipsec_input,
+	.output = tcp_ipsec_output,
+	.pcbctl = tcp_ipsec_pcbctl,
+};
+
+#ifndef KLD_MODULE
+/* TCP-MD5 support is build in the kernel */
+static const struct tcpmd5_support tcpmd5_ipsec = {
+	.enabled = IPSEC_MODULE_ENABLED,
+	.methods = &tcpmd5_methods
+};
+const struct tcpmd5_support * const tcp_ipsec_support = &tcpmd5_ipsec;
+#endif /* !KLD_MODULE */
 
-/*
- * Prepend the authentication header.
- * Called from the ipsec layer.
- * We do this from within tcp itself, so this routine is just a stub.
- */
 static int
-tcpsignature_output(struct mbuf *m, struct ipsecrequest *isr,
-    struct mbuf **mp, int skip, int protoff)
+tcpmd5_modevent(module_t mod, int type, void *data)
 {
 
-	return (EINVAL);
+	switch (type) {
+	case MOD_LOAD:
+		xform_attach(&tcpsignature_xformsw);
+#ifdef KLD_MODULE
+		tcpmd5_support_enable(&tcpmd5_methods);
+#endif
+		break;
+	case MOD_UNLOAD:
+#ifdef KLD_MODULE
+		tcpmd5_support_disable();
+#endif
+		xform_detach(&tcpsignature_xformsw);
+		break;
+	default:
+		return (EOPNOTSUPP);
+	}
+	return (0);
 }
 
-static struct xformsw tcpsignature_xformsw = {
-	XF_TCPSIGNATURE,	XFT_AUTH,		"TCPMD5",
-	tcpsignature_init,	tcpsignature_zeroize,
-	tcpsignature_input,	tcpsignature_output
+static moduledata_t tcpmd5_mod = {
+	"tcpmd5",
+	tcpmd5_modevent,
+	0
 };
 
-static void
-tcpsignature_attach(void)
-{
-
-	xform_register(&tcpsignature_xformsw);
-}
-
-SYSINIT(tcpsignature_xform_init, SI_SUB_DRIVERS, SI_ORDER_FIRST,
-    tcpsignature_attach, NULL);
+DECLARE_MODULE(tcpmd5, tcpmd5_mod, SI_SUB_PROTO_DOMAIN, SI_ORDER_ANY);
+MODULE_VERSION(tcpmd5, 1);
+#ifdef KLD_MODULE
+MODULE_DEPEND(tcpmd5, ipsec_support, 1, 1, 1);
+#endif