sync with kame tree as of july00. tons of bug fixes/improvements.

API changes:
- additional IPv6 ioctls
- IPsec PF_KEY API was changed, it is mandatory to upgrade setkey(8).
  (also syntax change)

1 files changed, 3598 insertions, 1707 deletions

diff --git a/sys/netkey/key.c b/sys/netkey/key.c
index f00792d..7af4a26 100644
--- a/sys/netkey/key.c
+++ b/sys/netkey/key.c
@@ -1,3 +1,6 @@
+/*	$FreeBSD$	*/
+/*	$KAME: key.c,v 1.137 2000/06/24 00:47:07 itojun Exp $	*/
+
 /*
  * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project.
  * All rights reserved.
@@ -25,12 +28,8 @@
  * 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$
  */
 
-/* KAME $Id: key.c,v 1.1.6.5.2.19 1999/07/22 14:09:24 itojun Exp $ */
-
 /*
  * This code is referd to RFC 2367
  */
@@ -62,16 +61,21 @@
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
 #include <netinet/in_var.h>
-#include <netinet/in_pcb.h>
 
 #ifdef INET6
-#include <netinet6/ip6.h>
+#include <netinet/ip6.h>
 #include <netinet6/in6_var.h>
+#include <netinet6/ip6_var.h>
+#endif /* INET6 */
+
+#ifdef INET
+#include <netinet/in_pcb.h>
+#endif
+#ifdef INET6
 #include <netinet6/in6_pcb.h>
 #endif /* INET6 */
 
 #include <net/pfkeyv2.h>
-#include <netkey/key_var.h>
 #include <netkey/keydb.h>
 #include <netkey/key.h>
 #include <netkey/keysock.h>
@@ -82,9 +86,11 @@
 #endif
 
 #include <netinet6/ipsec.h>
-#include <netinet6/ah.h>
 #ifdef INET6
 #include <netinet6/ipsec6.h>
+#endif
+#include <netinet6/ah.h>
+#ifdef INET6
 #include <netinet6/ah6.h>
 #endif
 #ifdef IPSEC_ESP
@@ -93,15 +99,37 @@
 #include <netinet6/esp6.h>
 #endif
 #endif
+#include <netinet6/ipcomp.h>
 
-MALLOC_DEFINE(M_SECA, "key mgmt", "security associations, key management");
+#include <machine/stdarg.h>
 
-#if defined(IPSEC_DEBUG)
+#include <net/net_osdep.h>
+
+#ifndef offsetof
+#define offsetof(type, member)	((size_t)(&((type *)0)->member))
+#endif
+#ifndef satosin
+#define satosin(s) ((struct sockaddr_in *)s)
+#endif
+
+/*
+ * Note on SA reference counting:
+ * - SAs that are not in DEAD state will have (total external reference + 1)
+ *   following value in reference count field.  they cannot be freed and are
+ *   referenced from SA header.
+ * - SAs that are in DEAD state will have (total external reference)
+ *   in reference count field.  they are ready to be freed.  reference from
+ *   SA header will be removed in key_delsav(), when the reference count
+ *   field hits 0 (= no external reference other than from SA header.
+ */
+
+#ifdef IPSEC_DEBUG
 u_int32_t key_debug_level = 0;
-#endif /* defined(IPSEC_DEBUG) */
+#endif
 static u_int key_spi_trycnt = 1000;
 static u_int32_t key_spi_minval = 0x100;
 static u_int32_t key_spi_maxval = 0x0fffffff;	/* XXX */
+static u_int32_t policy_id = 0;
 static u_int key_int_random = 60;	/*interval to initialize randseed,1(m)*/
 static u_int key_larval_lifetime = 30;	/* interval to expire acquiring, 30(s)*/
 static int key_blockacq_count = 10;	/* counter for blocking SADB_ACQUIRE.*/
@@ -117,12 +145,17 @@ static LIST_HEAD(_regtree, secreg) regtree[SADB_SATYPE_MAX + 1];
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 static LIST_HEAD(_acqtree, secacq) acqtree;		/* acquiring list */
 #endif
+static LIST_HEAD(_spacqtree, secspacq) spacqtree;	/* SP acquiring list */
 
 struct key_cb key_cb;
 
 /* search order for SAs */
 static u_int saorder_state_valid[] = {
-	SADB_SASTATE_MATURE, SADB_SASTATE_DYING
+	SADB_SASTATE_DYING, SADB_SASTATE_MATURE,
+	/*
+	 * This order is important because we must select a oldest SA
+	 * for outbound processing.  For inbound, This is not important.
+	 */
 };
 static u_int saorder_state_alive[] = {
 	/* except DEAD */
@@ -133,10 +166,59 @@ static u_int saorder_state_any[] = {
 	SADB_SASTATE_LARVAL, SADB_SASTATE_DEAD
 };
 
-#if defined(IPSEC_DEBUG)
+static const int minsize[] = {
+	sizeof(struct sadb_msg),	/* SADB_EXT_RESERVED */
+	sizeof(struct sadb_sa),		/* SADB_EXT_SA */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_CURRENT */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_HARD */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_SOFT */
+	sizeof(struct sadb_address),	/* SADB_EXT_ADDRESS_SRC */
+	sizeof(struct sadb_address),	/* SADB_EXT_ADDRESS_DST */
+	sizeof(struct sadb_address),	/* SADB_EXT_ADDRESS_PROXY */
+	sizeof(struct sadb_key),	/* SADB_EXT_KEY_AUTH */
+	sizeof(struct sadb_key),	/* SADB_EXT_KEY_ENCRYPT */
+	sizeof(struct sadb_ident),	/* SADB_EXT_IDENTITY_SRC */
+	sizeof(struct sadb_ident),	/* SADB_EXT_IDENTITY_DST */
+	sizeof(struct sadb_sens),	/* SADB_EXT_SENSITIVITY */
+	sizeof(struct sadb_prop),	/* SADB_EXT_PROPOSAL */
+	sizeof(struct sadb_supported),	/* SADB_EXT_SUPPORTED_AUTH */
+	sizeof(struct sadb_supported),	/* SADB_EXT_SUPPORTED_ENCRYPT */
+	sizeof(struct sadb_spirange),	/* SADB_EXT_SPIRANGE */
+	0,				/* SADB_X_EXT_KMPRIVATE */
+	sizeof(struct sadb_x_policy),	/* SADB_X_EXT_POLICY */
+	sizeof(struct sadb_x_sa2),	/* SADB_X_SA2 */
+};
+static const int maxsize[] = {
+	sizeof(struct sadb_msg),	/* SADB_EXT_RESERVED */
+	sizeof(struct sadb_sa),		/* SADB_EXT_SA */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_CURRENT */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_HARD */
+	sizeof(struct sadb_lifetime),	/* SADB_EXT_LIFETIME_SOFT */
+	0,				/* SADB_EXT_ADDRESS_SRC */
+	0,				/* SADB_EXT_ADDRESS_DST */
+	0,				/* SADB_EXT_ADDRESS_PROXY */
+	0,				/* SADB_EXT_KEY_AUTH */
+	0,				/* SADB_EXT_KEY_ENCRYPT */
+	0,				/* SADB_EXT_IDENTITY_SRC */
+	0,				/* SADB_EXT_IDENTITY_DST */
+	0,				/* SADB_EXT_SENSITIVITY */
+	0,				/* SADB_EXT_PROPOSAL */
+	0,				/* SADB_EXT_SUPPORTED_AUTH */
+	0,				/* SADB_EXT_SUPPORTED_ENCRYPT */
+	sizeof(struct sadb_spirange),	/* SADB_EXT_SPIRANGE */
+	0,				/* SADB_X_EXT_KMPRIVATE */
+	0,				/* SADB_X_EXT_POLICY */
+	sizeof(struct sadb_x_sa2),	/* SADB_X_SA2 */
+};
+
+#ifdef SYSCTL_DECL
+SYSCTL_DECL(_net_key);
+#endif
+
+#ifdef IPSEC_DEBUG
 SYSCTL_INT(_net_key, KEYCTL_DEBUG_LEVEL,	debug,	CTLFLAG_RW, \
 	&key_debug_level,	0,	"");
-#endif /* defined(IPSEC_DEBUG) */
+#endif
 
 /* max count of trial for the decision of spi value */
 SYSCTL_INT(_net_key, KEYCTL_SPI_TRY,		spi_trycnt,	CTLFLAG_RW, \
@@ -155,7 +237,7 @@ SYSCTL_INT(_net_key, KEYCTL_RANDOM_INT,	int_random,	CTLFLAG_RW, \
 	&key_int_random,	0,	"");
 
 /* lifetime for larval SA */
-SYSCTL_INT(_net_key, KEYCTL_LARVAL_LIFETIME,	larval_lifetime,	CTLFLAG_RW, \
+SYSCTL_INT(_net_key, KEYCTL_LARVAL_LIFETIME,	larval_lifetime, CTLFLAG_RW, \
 	&key_larval_lifetime,	0,	"");
 
 /* counter for blocking to send SADB_ACQUIRE to IKEd */
@@ -163,43 +245,75 @@ SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_COUNT,	blockacq_count,	CTLFLAG_RW, \
 	&key_blockacq_count,	0,	"");
 
 /* lifetime for blocking to send SADB_ACQUIRE to IKEd */
-SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_LIFETIME,	blockacq_lifetime,	CTLFLAG_RW, \
+SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_LIFETIME,	blockacq_lifetime, CTLFLAG_RW, \
 	&key_blockacq_lifetime,	0,	"");
 
-#define	__LIST_FOREACH(elm, head, field)                                     \
+static const int ipsec_esp_keymin = 256;
+static const int ipsec_esp_auth = 0;
+static const int ipsec_ah_keymin = 128;
+
+#ifndef LIST_FOREACH
+#define LIST_FOREACH(elm, head, field)                                     \
 	for (elm = LIST_FIRST(head); elm; elm = LIST_NEXT(elm, field))
-#define	__LIST_CHAINED(elm) \
+#endif
+#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) {                                    \
+#define KEY_CHKSASTATE(head, sav, name) \
+do { \
 	if ((head) != (sav)) {                                               \
 		printf("%s: state mismatched (TREE=%d SA=%d)\n",             \
 			(name), (head), (sav));                              \
 		continue;                                                    \
 	}                                                                    \
-}
+} while (0)
 
-#define	KEY_CHKSPDIR(head, sp, name) {                                       \
+#define KEY_CHKSPDIR(head, sp, name) \
+do { \
 	if ((head) != (sp)) {                                                \
 		printf("%s: direction mismatched (TREE=%d SP=%d), "          \
 			"anyway continue.\n",                                \
 			(name), (head), (sp));                               \
 	}                                                                    \
-}
+} while (0)
 
-#define	KMALLOC(p, t, n)                                                     \
+#if 1
+#define KMALLOC(p, t, n)                                                     \
 	((p) = (t) malloc((unsigned long)(n), M_SECA, M_NOWAIT))
-#define	KFREE(p)                                                             \
+#define KFREE(p)                                                             \
 	free((caddr_t)(p), M_SECA);
+#else
+#define KMALLOC(p, t, n) \
+do { \
+	((p) = (t)malloc((unsigned long)(n), M_SECA, M_NOWAIT));             \
+	printf("%s %d: %p <- KMALLOC(%s, %d)\n",                             \
+		__FILE__, __LINE__, (p), #t, n);                             \
+} while (0)
 
-#define	KEY_NEWBUF(dst, t, src, len)                                         \
-	((dst) = (t)key_newbuf((src), (len)))
+#define KFREE(p)                                                             \
+	do {                                                                 \
+		printf("%s %d: %p -> KFREE()\n", __FILE__, __LINE__, (p));   \
+		free((caddr_t)(p), M_SECA);                                  \
+	} while (0)
+#endif
 
 /*
  * set parameters into secpolicyindex buffer.
  * Must allocate secpolicyindex buffer passed to this function.
  */
-#define	KEY_SETSECSPIDX(_dir, s, d, ps, pd, ulp, idx) do {                   \
+#define KEY_SETSECSPIDX(_dir, s, d, ps, pd, ulp, idx) \
+do { \
 	bzero((idx), sizeof(struct secpolicyindex));                             \
 	(idx)->dir = (_dir);                                                 \
 	(idx)->prefs = (ps);                                                 \
@@ -213,10 +327,12 @@ SYSCTL_INT(_net_key, KEYCTL_BLOCKACQ_LIFETIME,	blockacq_lifetime,	CTLFLAG_RW, \
  * set parameters into secasindex buffer.
  * Must allocate secasindex buffer before calling this function.
  */
-#define	KEY_SETSECASIDX(p, m, s, d, idx) do {                                \
+#define KEY_SETSECASIDX(p, m, r, s, d, idx) \
+do { \
 	bzero((idx), sizeof(struct secasindex));                             \
 	(idx)->proto = (p);                                                  \
 	(idx)->mode = (m);                                                   \
+	(idx)->reqid = (r);                                                  \
 	bcopy((s), &(idx)->src, ((struct sockaddr *)(s))->sa_len);           \
 	bcopy((d), &(idx)->dst, ((struct sockaddr *)(d))->sa_len);           \
 } while (0)
@@ -226,85 +342,139 @@ struct _keystat {
 	u_long getspi_count; /* the avarage of count to try to get new SPI */
 } keystat;
 
-static struct secasvar *key_allocsa_policy __P((struct ipsecrequest *isr));
-static void key_freesp_so __P((struct secpolicy **sp));
-static struct secasvar *key_do_allocsa_policy __P((struct secashead *sah,
-						u_int state));
-static void key_delsp __P((struct secpolicy *sp));
-static struct secpolicy *key_getsp __P((struct secpolicyindex *spidx));
-static struct sadb_msg *key_spdadd __P((caddr_t *mhp));
-static struct sadb_msg *key_spddelete __P((caddr_t *mhp));
-static struct sadb_msg *key_spdflush __P((caddr_t *mhp));
-static int key_spddump __P((caddr_t *mhp, struct socket *so, int target));
-static u_int key_setdumpsp __P((struct sadb_msg *newmsg, struct secpolicy *sp,
-				u_int8_t type, u_int32_t seq, u_int32_t pid));
-static u_int key_getspmsglen __P((struct secpolicy *sp));
-static u_int key_getspreqmsglen __P((struct secpolicy *sp));
-static struct secashead *key_newsah __P((struct secasindex *saidx));
-static void key_delsah __P((struct secashead *sah));
-static struct secasvar *key_newsav __P((caddr_t *mhp, struct secashead *sah));
-static void key_delsav __P((struct secasvar *sav));
-static struct secashead *key_getsah __P((struct secasindex *saidx));
-static struct secasvar *key_checkspidup __P((struct secasindex *saidx,
-						u_int32_t spi));
-static struct secasvar *key_getsavbyspi __P((struct secashead *sah,
-						u_int32_t spi));
-static int key_setsaval __P((struct secasvar *sav, caddr_t *mhp));
-static u_int key_getmsglen __P((struct secasvar *sav));
-static int key_mature __P((struct secasvar *sav));
-static u_int key_setdumpsa __P((struct sadb_msg *newmsg, struct secasvar *sav,
-				u_int8_t type, u_int8_t satype,
-				u_int32_t seq, u_int32_t pid));
-static caddr_t key_setsadbmsg __P((caddr_t buf, u_int8_t type, int tlen,
-				u_int8_t satype, u_int32_t seq, pid_t pid,
-				u_int8_t reserved1, u_int8_t reserved2));
-static caddr_t key_setsadbsa __P((caddr_t buf, struct secasvar *sav));
-static caddr_t key_setsadbaddr __P((caddr_t buf, u_int16_t exttype,
-	struct sockaddr *saddr, u_int8_t prefixlen, u_int16_t ul_proto));
-static caddr_t key_setsadbident
-	__P((caddr_t buf, u_int16_t exttype, u_int16_t idtype,
-		caddr_t string, int stringlen, u_int64_t id));
-static caddr_t key_setsadbext __P((caddr_t p, caddr_t ext));
-static void *key_newbuf __P((void *src, u_int len));
+struct sadb_msghdr {
+	struct sadb_msg *msg;
+	struct sadb_ext *ext[SADB_EXT_MAX + 1];
+	int extoff[SADB_EXT_MAX + 1];
+	int extlen[SADB_EXT_MAX + 1];
+};
+
+static struct secasvar *key_allocsa_policy __P((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 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 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 *));
+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,
+	struct sockaddr *, u_int8_t, u_int16_t));
+#if 0
+static struct mbuf *key_setsadbident __P((u_int16_t, u_int16_t, caddr_t,
+	int, u_int64_t));
+#endif
+static struct mbuf *key_setsadbxsa2(u_int8_t, u_int32_t);
+static struct mbuf *key_setsadbxpolicy __P((u_int16_t, u_int8_t,
+	u_int32_t));
+static void *key_newbuf __P((const void *, u_int));
 #ifdef INET6
-static int key_ismyaddr6 __P((caddr_t addr));
+static int key_ismyaddr6 __P((struct sockaddr_in6 *));
 #endif
 static int key_cmpsaidx_exactly
-	__P((struct secasindex *saidx0, struct secasindex *saidx1));
+	__P((struct secasindex *, struct secasindex *));
 static int key_cmpsaidx_withmode
-	__P((struct secasindex *saidx0, struct secasindex *saidx1));
+	__P((struct secasindex *, struct secasindex *));
+static int key_cmpsaidx_withoutmode
+	__P((struct secasindex *, struct secasindex *));
 static int key_cmpspidx_exactly
-	__P((struct secpolicyindex *spidx0, struct secpolicyindex *spidx1));
+	__P((struct secpolicyindex *, struct secpolicyindex *));
 static int key_cmpspidx_withmask
-	__P((struct secpolicyindex *spidx0, struct secpolicyindex *spidx1));
-static int key_bbcmp __P((caddr_t p1, caddr_t p2, u_int bits));
-static u_int16_t key_satype2proto __P((u_int8_t satype));
-static u_int8_t key_proto2satype __P((u_int16_t proto));
-
-static struct sadb_msg *key_getspi __P((caddr_t *mhp));
-static u_int32_t key_do_getnewspi __P((struct sadb_spirange *spirange,
-					struct secasindex *saidx));
-static struct sadb_msg *key_update __P((caddr_t *mhp));
-static struct secasvar *key_getsavbyseq __P((struct secashead *sah,
-						u_int32_t seq));
-static struct sadb_msg *key_add __P((caddr_t *mhp));
-static struct sadb_msg *key_getmsgbuf_x1 __P((caddr_t *mhp));
-static struct sadb_msg *key_delete __P((caddr_t *mhp));
-static struct sadb_msg *key_get __P((caddr_t *mhp));
-static int key_acquire __P((struct secasindex *saidx,
-				struct secpolicyindex *spidx));
-static struct secacq *key_newacq __P((struct secasindex *saidx));
-static struct secacq *key_getacq __P((struct secasindex *saidx));
-static struct secacq *key_getacqbyseq __P((u_int32_t seq));
-static struct sadb_msg *key_acquire2 __P((caddr_t *mhp));
-static struct sadb_msg *key_register __P((caddr_t *mhp, struct socket *so));
-static int key_expire __P((struct secasvar *sav));
-static struct sadb_msg *key_flush __P((caddr_t *mhp));
-static int key_dump __P((caddr_t *mhp, struct socket *so, int target));
-static void key_promisc __P((caddr_t *mhp, struct socket *so));
-static int key_sendall __P((struct sadb_msg *msg, u_int len));
-static int key_align __P((struct sadb_msg *msg, caddr_t *mhp));
-static void key_sa_chgstate __P((struct secasvar *sav, u_int8_t state));
+	__P((struct secpolicyindex *, struct secpolicyindex *));
+static int key_sockaddrcmp __P((struct sockaddr *, struct sockaddr *, int));
+static int key_bbcmp __P((caddr_t, caddr_t, u_int));
+static void key_srandom __P((void));
+static u_long key_random __P((void));
+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 *));
+
+#ifdef IPSEC_ESP
+static struct mbuf *key_getcomb_esp __P((void));
+#endif
+static struct mbuf *key_getcomb_ah __P((void));
+static struct mbuf *key_getprop __P((const struct secasindex *));
+
+static int key_acquire __P((struct secasindex *, struct secpolicy *));
+#ifndef IPSEC_NONBLOCK_ACQUIRE
+static struct secacq *key_newacq __P((struct secasindex *));
+static struct secacq *key_getacq __P((struct secasindex *));
+static struct secacq *key_getacqbyseq __P((u_int32_t));
+#endif
+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 *));
+#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));
+
 /* %%% IPsec policy management */
 /*
  * allocating a SP for OUTBOUND or INBOUND packet.
@@ -339,7 +509,7 @@ key_allocsp(spidx, dir)
 		printf("*** objects\n");
 		kdebug_secpolicyindex(spidx));
 
-	__LIST_FOREACH(sp, &sptree[dir], chain) {
+	LIST_FOREACH(sp, &sptree[dir], chain) {
 		KEYDEBUG(KEYDEBUG_IPSEC_DATA,
 			printf("*** in SPD\n");
 			kdebug_secpolicyindex(&sp->spidx));
@@ -368,23 +538,25 @@ found:
 }
 
 /*
+ * allocating a SA entry for a *OUTBOUND* packet.
  * checking each request entries in SP, and acquire SA if need.
  * OUT:	0: there are valid requests.
  *	ENOENT: policy may be valid, but SA with REQUIRE is on acquiring.
  */
 int
-key_checkrequest(isr)
+key_checkrequest(isr, saidx)
 	struct ipsecrequest *isr;
+	struct secasindex *saidx;
 {
 	u_int level;
 	int error;
 
 	/* sanity check */
-	if (isr == NULL)
+	if (isr == NULL || saidx == NULL)
 		panic("key_checkrequest: NULL pointer is passed.\n");
 
 	/* check mode */
-	switch (isr->saidx.mode) {
+	switch (saidx->mode) {
 	case IPSEC_MODE_TRANSPORT:
 	case IPSEC_MODE_TUNNEL:
 		break;
@@ -396,43 +568,60 @@ key_checkrequest(isr)
 	/* get current level */
 	level = ipsec_get_reqlevel(isr);
 
+#if 0
 	/*
-	 * We don't allocate new SA if the state of SA in the holder is
-	 * SADB_SASTATE_MATURE, and if this is newer one.
+	 * We do allocate new SA only if the state of SA in the holder is
+	 * SADB_SASTATE_DEAD.  The SA for outbound must be the oldest.
 	 */
 	if (isr->sav != NULL) {
-		/*
-		 * XXX While SA is hanging on policy request(isr), its refcnt
-		 * can not be zero.  So isr->sav->sah is valid pointer if
-		 * isr->sav != NULL.  But that may not be true in fact.
-		 * There may be missunderstanding by myself.  Anyway I set
-		 * zero to isr->sav->sah when isr->sav is flushed.
-		 * I must check to have conviction this issue.
-		 */
-		if (isr->sav->sah != NULL
-		 && isr->sav != (struct secasvar *)LIST_FIRST(
-			    &isr->sav->sah->savtree[SADB_SASTATE_MATURE])) {
+		if (isr->sav->sah == NULL)
+			panic("key_checkrequest: sah is null.\n");
+		if (isr->sav == (struct secasvar *)LIST_FIRST(
+			    &isr->sav->sah->savtree[SADB_SASTATE_DEAD])) {
 			KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 				printf("DP checkrequest calls free SA:%p\n",
 					isr->sav));
 			key_freesav(isr->sav);
+			isr->sav = NULL;
 		}
+	}
+#else
+	/*
+	 * we free any SA stashed 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.
+	 *
+	 * The operation may have negative impact to performance.  We may
+	 * want to check cached SA carefully, rather than picking new SA
+	 * every time.
+	 */
+	if (isr->sav != NULL) {
+		key_freesav(isr->sav);
 		isr->sav = NULL;
 	}
+#endif
 
-	/* new SA allocation if no SA found. */
+	/*
+	 * new SA allocation if no SA found.
+	 * key_allocsa_policy should allocate the oldest SA available.
+	 * See key_do_allocsa_policy(), and draft-jenkins-ipsec-rekeying-03.txt.
+	 */
 	if (isr->sav == NULL)
-		isr->sav = key_allocsa_policy(isr);
+		isr->sav = key_allocsa_policy(saidx);
 
 	/* When there is SA. */
 	if (isr->sav != NULL)
 		return 0;
 
 	/* there is no SA */
-	if ((error = key_acquire(&isr->saidx, &isr->sp->spidx)) != 0) {
+	if ((error = key_acquire(saidx, isr->sp)) != 0) {
 		/* XXX What I do ? */
+#ifdef IPSEC_DEBUG
 		printf("key_checkrequest: error %d returned "
 			"from key_acquire.\n", error);
+#endif
 		return error;
 	}
 
@@ -446,17 +635,17 @@ key_checkrequest(isr)
  *	others:	found and return the pointer.
  */
 static struct secasvar *
-key_allocsa_policy(isr)
-	struct ipsecrequest *isr;
+key_allocsa_policy(saidx)
+	struct secasindex *saidx;
 {
 	struct secashead *sah;
 	struct secasvar *sav;
 	u_int stateidx, state;
 
-	__LIST_FOREACH(sah, &sahtree, chain) {
+	LIST_FOREACH(sah, &sahtree, chain) {
 		if (sah->state == SADB_SASTATE_DEAD)
 			continue;
-		if (key_cmpsaidx_withmode(&sah->saidx, &isr->saidx))
+		if (key_cmpsaidx_withmode(&sah->saidx, saidx))
 			goto found;
 	}
 
@@ -496,7 +685,7 @@ key_do_allocsa_policy(sah, state)
 	/* initilize */
 	candidate = NULL;
 
-	__LIST_FOREACH(sav, &sah->savtree[state], chain) {
+	LIST_FOREACH(sav, &sah->savtree[state], chain) {
 
 		/* sanity check */
 		KEY_CHKSASTATE(sav->state, state, "key_do_allocsa_policy");
@@ -510,15 +699,12 @@ key_do_allocsa_policy(sah, state)
 		/* Which SA is the better ? */
 
 		/* sanity check 2 */
-		if (candidate->lft_c == NULL || sav->lft_c == NULL) {
-			/*XXX do panic ? */
-			printf("key_do_allocsa_policy: "
+		if (candidate->lft_c == NULL || sav->lft_c == NULL)
+			panic("key_do_allocsa_policy: "
 				"lifetime_current is NULL.\n");
-			continue;
-		}
 
 		/* XXX What the best method is to compare ? */
-		if (candidate->lft_c->sadb_lifetime_addtime <
+		if (candidate->lft_c->sadb_lifetime_addtime >
 				sav->lft_c->sadb_lifetime_addtime) {
 			candidate = sav;
 			continue;
@@ -540,6 +726,15 @@ key_do_allocsa_policy(sah, state)
  * Must call key_freesav() later.
  * OUT: positive:	pointer to a sav.
  *	NULL:		not found, or error occured.
+ *
+ * In the comparison, source address will be ignored 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(family, src, dst, proto, spi)
@@ -550,6 +745,8 @@ key_allocsa(family, src, dst, proto, spi)
 	struct secashead *sah;
 	struct secasvar *sav;
 	u_int stateidx, state;
+	struct sockaddr_in sin;
+	struct sockaddr_in6 sin6;
 	int s;
 
 	/* sanity check */
@@ -563,30 +760,96 @@ key_allocsa(family, src, dst, proto, spi)
 	 * encrypted so we can't check internal IP header.
 	 */
 	s = splnet();	/*called from softclock()*/
-	__LIST_FOREACH(sah, &sahtree, chain) {
-
+	LIST_FOREACH(sah, &sahtree, chain) {
 		/* search valid state */
 		for (stateidx = 0;
 		     stateidx < _ARRAYLEN(saorder_state_valid);
 		     stateidx++) {
-
 			state = saorder_state_valid[stateidx];
-			__LIST_FOREACH(sav, &sah->savtree[state], chain) {
-
+			LIST_FOREACH(sav, &sah->savtree[state], chain) {
 				/* sanity check */
 				KEY_CHKSASTATE(sav->state, state, "key_allocsav");
 				if (proto != sav->sah->saidx.proto)
 					continue;
 				if (spi != sav->spi)
 					continue;
+				if (family != sav->sah->saidx.src.ss_family ||
+				    family != sav->sah->saidx.dst.ss_family)
+					continue;
 
-				if (key_bbcmp(src,
-				     _INADDRBYSA(&sav->sah->saidx.src),
-				     _INALENBYAF(sav->sah->saidx.src.ss_family) << 3)
-				 && key_bbcmp(dst,
-				     _INADDRBYSA(&sav->sah->saidx.dst),
-				     _INALENBYAF(sav->sah->saidx.dst.ss_family) << 3))
-					goto found;
+#if 0	/* don't check src */
+				/* check src address */
+				switch (family) {
+				case AF_INET:
+					bzero(&sin, sizeof(sin));
+					sin.sin_family = AF_INET;
+					sin.sin_len = sizeof(sin);
+					bcopy(src, &sin.sin_addr,
+					    sizeof(sin.sin_addr));
+					if (key_sockaddrcmp((struct sockaddr*)&sin,
+					    (struct sockaddr *)&sav->sah->saidx.src, 0) != 0)
+						continue;
+
+					break;
+				case AF_INET6:
+					bzero(&sin6, sizeof(sin6));
+					sin6.sin6_family = AF_INET6;
+					sin6.sin6_len = sizeof(sin6);
+					bcopy(src, &sin6.sin6_addr,
+					    sizeof(sin6.sin6_addr));
+					if (IN6_IS_SCOPE_LINKLOCAL(&sin6.sin6_addr)) {
+						/* kame fake scopeid */
+						sin6.sin6_scope_id =
+						    ntohs(sin6.sin6_addr.s6_addr16[1]);
+						sin6.sin6_addr.s6_addr16[1] = 0;
+					}
+					if (key_sockaddrcmp((struct sockaddr*)&sin6,
+					    (struct sockaddr *)&sav->sah->saidx.src, 0) != 0)
+						continue;
+					break;
+				default:
+					printf("key_allocsa: unknown address family=%d.\n",
+						family);
+					continue;
+				}
+
+#endif
+				/* check dst address */
+				switch (family) {
+				case AF_INET:
+					bzero(&sin, sizeof(sin));
+					sin.sin_family = AF_INET;
+					sin.sin_len = sizeof(sin);
+					bcopy(dst, &sin.sin_addr,
+					    sizeof(sin.sin_addr));
+					if (key_sockaddrcmp((struct sockaddr*)&sin,
+					    (struct sockaddr *)&sav->sah->saidx.dst, 0) != 0)
+						continue;
+
+					break;
+				case AF_INET6:
+					bzero(&sin6, sizeof(sin6));
+					sin6.sin6_family = AF_INET6;
+					sin6.sin6_len = sizeof(sin6);
+					bcopy(dst, &sin6.sin6_addr,
+					    sizeof(sin6.sin6_addr));
+					if (IN6_IS_SCOPE_LINKLOCAL(&sin6.sin6_addr)) {
+						/* kame fake scopeid */
+						sin6.sin6_scope_id =
+						    ntohs(sin6.sin6_addr.s6_addr16[1]);
+						sin6.sin6_addr.s6_addr16[1] = 0;
+					}
+					if (key_sockaddrcmp((struct sockaddr*)&sin6,
+					    (struct sockaddr *)&sav->sah->saidx.dst, 0) != 0)
+						continue;
+					break;
+				default:
+					printf("key_allocsa: unknown address family=%d.\n",
+						family);
+					continue;
+				}
+
+				goto found;
 			}
 		}
 	}
@@ -656,6 +919,15 @@ key_freeso(so)
 #ifdef INET6
 	case PF_INET6:
 	    {
+#ifdef HAVE_NRL_INPCB
+		struct inpcb *pcb  = sotoinpcb(so);
+
+		/* 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);
+#else
 		struct in6pcb *pcb  = sotoin6pcb(so);
 
 		/* Does it have a PCB ? */
@@ -663,12 +935,15 @@ key_freeso(so)
 			return;
 		key_freesp_so(&pcb->in6p_sp->sp_in);
 		key_freesp_so(&pcb->in6p_sp->sp_out);
+#endif
 	    }
 		break;
 #endif /* INET6 */
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_freeso: unknown address family=%d.\n",
 			so->so_proto->pr_domain->dom_family);
+#endif
 		return;
 	}
 
@@ -766,7 +1041,7 @@ key_delsp(sp)
 	}
     }
 
-	KFREE(sp);
+	keydb_delsecpolicy(sp);
 
 	splx(s);
 
@@ -788,7 +1063,7 @@ key_getsp(spidx)
 	if (spidx == NULL)
 		panic("key_getsp: NULL pointer is passed.\n");
 
-	__LIST_FOREACH(sp, &sptree[spidx->dir], chain) {
+	LIST_FOREACH(sp, &sptree[spidx->dir], chain) {
 		if (sp->state == IPSEC_SPSTATE_DEAD)
 			continue;
 		if (key_cmpspidx_exactly(spidx, &sp->spidx)) {
@@ -800,17 +1075,46 @@ key_getsp(spidx)
 	return NULL;
 }
 
+/*
+ * get SP by index.
+ * OUT:	NULL	: not found
+ *	others	: found, pointer to a SP.
+ */
+static struct secpolicy *
+key_getspbyid(id)
+	u_int32_t id;
+{
+	struct secpolicy *sp;
+
+	LIST_FOREACH(sp, &sptree[IPSEC_DIR_INBOUND], chain) {
+		if (sp->state == IPSEC_SPSTATE_DEAD)
+			continue;
+		if (sp->id == id) {
+			sp->refcnt++;
+			return sp;
+		}
+	}
+
+	LIST_FOREACH(sp, &sptree[IPSEC_DIR_OUTBOUND], chain) {
+		if (sp->state == IPSEC_SPSTATE_DEAD)
+			continue;
+		if (sp->id == id) {
+			sp->refcnt++;
+			return sp;
+		}
+	}
+
+	return NULL;
+}
+
 struct secpolicy *
 key_newsp()
 {
 	struct secpolicy *newsp = NULL;
 
-	KMALLOC(newsp, struct secpolicy *, sizeof(*newsp));
-	if (newsp == NULL) {
-		printf("key_newsp: No more memory.\n");
-		return NULL;
-	}
-	bzero(newsp, sizeof(*newsp));
+	newsp = keydb_newsecpolicy();
+	if (!newsp)
+		return newsp;
 
 	newsp->refcnt = 1;
 	newsp->req = NULL;
@@ -824,17 +1128,30 @@ key_newsp()
  * so must be set properly later.
  */
 struct secpolicy *
-key_msg2sp(xpl0)
+key_msg2sp(xpl0, len, error)
 	struct sadb_x_policy *xpl0;
+	size_t len;
+	int *error;
 {
 	struct secpolicy *newsp;
 
 	/* sanity check */
 	if (xpl0 == NULL)
 		panic("key_msg2sp: NULL pointer was passed.\n");
+	if (len < sizeof(*xpl0))
+		panic("key_msg2sp: invalid length.\n");
+	if (len != PFKEY_EXTLEN(xpl0)) {
+#ifdef IPSEC_DEBUG
+		printf("key_msg2sp: Invalid msg length.\n");
+#endif
+		*error = EINVAL;
+		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;
@@ -855,9 +1172,12 @@ key_msg2sp(xpl0)
 		struct ipsecrequest **p_isr = &newsp->req;
 
 		/* validity check */
-		if (PFKEY_EXTLEN(xpl0) <= sizeof(*xpl0)) {
+		if (PFKEY_EXTLEN(xpl0) < sizeof(*xpl0)) {
+#ifdef IPSEC_DEBUG
 			printf("key_msg2sp: Invalid msg length.\n");
+#endif
 			key_freesp(newsp);
+			*error = EINVAL;
 			return NULL;
 		}
 
@@ -868,17 +1188,23 @@ key_msg2sp(xpl0)
 
 			/* length check */
 			if (xisr->sadb_x_ipsecrequest_len < sizeof(*xisr)) {
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: "
 					"invalid ipsecrequest length.\n");
+#endif
 				key_freesp(newsp);
+				*error = EINVAL;
 				return NULL;
 			}
 
 			/* allocate request buffer */
 			KMALLOC(*p_isr, struct ipsecrequest *, sizeof(**p_isr));
 			if ((*p_isr) == NULL) {
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: No more memory.\n");
+#endif
 				key_freesp(newsp);
+				*error = ENOBUFS;
 				return NULL;
 			}
 			bzero(*p_isr, sizeof(**p_isr));
@@ -889,11 +1215,17 @@ key_msg2sp(xpl0)
 			switch (xisr->sadb_x_ipsecrequest_proto) {
 			case IPPROTO_ESP:
 			case IPPROTO_AH:
+#if 1	/*nonstandard*/
+			case IPPROTO_IPCOMP:
+#endif
 				break;
 			default:
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: invalid proto type=%u\n",
 					xisr->sadb_x_ipsecrequest_proto);
+#endif
 				key_freesp(newsp);
+				*error = EPROTONOSUPPORT;
 				return NULL;
 			}
 			(*p_isr)->saidx.proto = xisr->sadb_x_ipsecrequest_proto;
@@ -904,9 +1236,12 @@ key_msg2sp(xpl0)
 				break;
 			case IPSEC_MODE_ANY:
 			default:
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: invalid mode=%u\n",
 					xisr->sadb_x_ipsecrequest_mode);
+#endif
 				key_freesp(newsp);
+				*error = EINVAL;
 				return NULL;
 			}
 			(*p_isr)->saidx.mode = xisr->sadb_x_ipsecrequest_mode;
@@ -916,10 +1251,47 @@ key_msg2sp(xpl0)
 			case IPSEC_LEVEL_USE:
 			case IPSEC_LEVEL_REQUIRE:
 				break;
+			case IPSEC_LEVEL_UNIQUE:
+				/* validity check */
+				/*
+				 * If range violation of reqid, kernel will
+				 * update it, don't refuse it.
+				 */
+				if (xisr->sadb_x_ipsecrequest_reqid
+						> IPSEC_MANUAL_REQID_MAX) {
+#ifdef IPSEC_DEBUG
+					printf("key_msg2sp: reqid=%d "
+						"range violation, "
+						"updated by kernel.\n",
+						xisr->sadb_x_ipsecrequest_reqid);
+#endif
+					xisr->sadb_x_ipsecrequest_reqid = 0;
+				}
+
+				/* allocate new reqid id if reqid is zero. */
+				if (xisr->sadb_x_ipsecrequest_reqid == 0) {
+					u_int32_t reqid;
+					if ((reqid = key_newreqid()) == 0) {
+						key_freesp(newsp);
+						*error = ENOBUFS;
+						return NULL;
+					}
+					(*p_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;
+				}
+				break;
+
 			default:
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: invalid level=%u\n",
 					xisr->sadb_x_ipsecrequest_level);
+#endif
 				key_freesp(newsp);
+				*error = EINVAL;
 				return NULL;
 			}
 			(*p_isr)->level = xisr->sadb_x_ipsecrequest_level;
@@ -933,9 +1305,12 @@ key_msg2sp(xpl0)
 				/* validity check */
 				if (paddr->sa_len
 				    > sizeof((*p_isr)->saidx.src)) {
+#ifdef IPSEC_DEBUG
 					printf("key_msg2sp: invalid request "
 						"address length.\n");
+#endif
 					key_freesp(newsp);
+					*error = EINVAL;
 					return NULL;
 				}
 				bcopy(paddr, &(*p_isr)->saidx.src,
@@ -947,9 +1322,12 @@ key_msg2sp(xpl0)
 				/* validity check */
 				if (paddr->sa_len
 				    > sizeof((*p_isr)->saidx.dst)) {
+#ifdef IPSEC_DEBUG
 					printf("key_msg2sp: invalid request "
 						"address length.\n");
+#endif
 					key_freesp(newsp);
+					*error = EINVAL;
 					return NULL;
 				}
 				bcopy(paddr, &(*p_isr)->saidx.dst,
@@ -965,8 +1343,11 @@ key_msg2sp(xpl0)
 
 			/* validity check */
 			if (tlen < 0) {
+#ifdef IPSEC_DEBUG
 				printf("key_msg2sp: becoming tlen < 0.\n");
+#endif
 				key_freesp(newsp);
+				*error = EINVAL;
 				return NULL;
 			}
 
@@ -976,24 +1357,42 @@ key_msg2sp(xpl0)
 	    }
 		break;
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_msg2sp: invalid policy type.\n");
+#endif
 		key_freesp(newsp);
+		*error = EINVAL;
 		return NULL;
 	}
 
+	*error = 0;
 	return newsp;
 }
 
+static u_int32_t
+key_newreqid()
+{
+	static u_int32_t auto_reqid = IPSEC_MANUAL_REQID_MAX + 1;
+
+	auto_reqid = (auto_reqid == ~0
+			? IPSEC_MANUAL_REQID_MAX + 1 : auto_reqid + 1);
+
+	/* XXX should be unique check */
+
+	return auto_reqid;
+}
+
 /*
  * copy secpolicy struct to sadb_x_policy structure indicated.
  */
-struct sadb_x_policy *
+struct mbuf *
 key_sp2msg(sp)
 	struct secpolicy *sp;
 {
 	struct sadb_x_policy *xpl;
 	int tlen;
 	caddr_t p;
+	struct mbuf *m;
 
 	/* sanity check. */
 	if (sp == NULL)
@@ -1001,17 +1400,23 @@ key_sp2msg(sp)
 
 	tlen = key_getspreqmsglen(sp);
 
-	KMALLOC(xpl, struct sadb_x_policy *, tlen);
-	if (xpl == NULL) {
-		printf("key_sp2msg: No more memory.\n");
+	m = key_alloc_mbuf(tlen);
+	if (!m || m->m_next) {	/*XXX*/
+		if (m)
+			m_freem(m);
 		return NULL;
 	}
+
+	m->m_len = tlen;
+	m->m_next = NULL;
+	xpl = mtod(m, struct sadb_x_policy *);
 	bzero(xpl, tlen);
 
 	xpl->sadb_x_policy_len = PFKEY_UNIT64(tlen);
 	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);
 
 	/* if is the policy for ipsec ? */
@@ -1026,6 +1431,7 @@ key_sp2msg(sp)
 			xisr->sadb_x_ipsecrequest_proto = isr->saidx.proto;
 			xisr->sadb_x_ipsecrequest_mode = isr->saidx.mode;
 			xisr->sadb_x_ipsecrequest_level = isr->level;
+			xisr->sadb_x_ipsecrequest_reqid = isr->saidx.reqid;
 
 			p += sizeof(*xisr);
 			bcopy(&isr->saidx.src, p, isr->saidx.src.ss_len);
@@ -1040,11 +1446,96 @@ key_sp2msg(sp)
 		}
 	}
 
-	return xpl;
+	return m;
+}
+
+/* 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
+{
+	va_list ap;
+	int idx;
+	int i;
+	struct mbuf *result = NULL, *n;
+	int len;
+
+	if (m == NULL || mhp == NULL)
+		panic("null pointer passed to key_gather");
+
+	va_start(ap, nitem);
+	for (i = 0; i < nitem; i++) {
+		idx = va_arg(ap, int);
+		if (idx < 0 || idx > SADB_EXT_MAX)
+			goto fail;
+		/* don't attempt to pull empty extension */
+		if (idx == SADB_EXT_RESERVED && mhp->msg == NULL)
+			continue;
+		if (idx != SADB_EXT_RESERVED  &&
+		    (mhp->ext[idx] == NULL || mhp->extlen[idx] == 0))
+			continue;
+
+		if (idx == SADB_EXT_RESERVED) {
+			len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
+#ifdef DIAGNOSTIC
+			if (len > MHLEN)
+				panic("assumption failed");
+#endif
+			MGETHDR(n, M_DONTWAIT, MT_DATA);
+			if (!n)
+				goto fail;
+			n->m_len = len;
+			n->m_next = NULL;
+			m_copydata(m, 0, sizeof(struct sadb_msg),
+			    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);
+				goto fail;
+			}
+			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);
+		}
+		if (n == NULL)
+			goto fail;
+
+		if (result)
+			m_cat(result, n);
+		else
+			result = n;
+	}
+	va_end(ap);
+
+	if ((result->m_flags & M_PKTHDR) != 0) {
+		result->m_pkthdr.len = 0;
+		for (n = result; n; n = n->m_next)
+			result->m_pkthdr.len += n->m_len;
+	}
+
+	return result;
+
+fail:
+	m_freem(result);
+	return NULL;
 }
 
 /*
- * SADB_SPDADD processing
+ * SADB_X_SPDADD, SADB_X_SPDSETIDX or SADB_X_SPDUPDATE processing
  * add a entry to SP database, when received
  *   <base, address(SD), policy>
  * from the user(?).
@@ -1053,40 +1544,51 @@ key_sp2msg(sp)
  *   <base, address(SD), policy>
  * to the socket which was send.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * SPDADD set a unique policy entry.
+ * SPDSETIDX like SPDADD without a part of policy requests.
+ * SPDUPDATE replace a unique policy entry.
  *
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_spdadd(mhp)
-	caddr_t *mhp;
+static int
+key_spdadd(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_address *src0, *dst0;
-	struct sadb_x_policy *xpl0;
+	struct sadb_x_policy *xpl0, *xpl;
 	struct secpolicyindex spidx;
 	struct secpolicy *newsp;
+	int error;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_spdadd: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
-	if (mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL
-	 || mhp[SADB_X_EXT_POLICY] == NULL) {
+	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
+	    mhp->ext[SADB_X_EXT_POLICY] == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_spdadd: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_spdadd: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	src0 = (struct sadb_address *)mhp[SADB_EXT_ADDRESS_SRC];
-	dst0 = (struct sadb_address *)mhp[SADB_EXT_ADDRESS_DST];
-	xpl0 = (struct sadb_x_policy *)mhp[SADB_X_EXT_POLICY];
+	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
+	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
+	xpl0 = (struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY];
 
 	/* make secindex */
+	/* XXX boundary check against sa_len */
 	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
 	                src0 + 1,
 	                dst0 + 1,
@@ -1101,35 +1603,70 @@ key_spdadd(mhp)
 	case IPSEC_DIR_OUTBOUND:
 		break;
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_spdadd: Invalid SP direction.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
-	}
-
-	/* Is there SP in SPD ? */
-	newsp = key_getsp(&spidx);
-	if (newsp != NULL) {
-		key_freesp(newsp);
-		printf("key_spdadd: a SP entry exists already.\n");
-		msg0->sadb_msg_errno = EEXIST;
-		return NULL;
+#endif
+		mhp->msg->sadb_msg_errno = EINVAL;
+		return 0;
 	}
 
 	/* 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) {
+#ifdef IPSEC_DEBUG
 		printf("key_spdadd: Invalid policy type.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_spdadd: some policy requests part required.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
+	}
+
+	/*
+	 * checking there is SP already or not.
+	 * If type is SPDUPDATE and no SP found, then error.
+	 * If type is either SPDADD or SPDSETIDX and SP found, then error.
+	 */
+	newsp = key_getsp(&spidx);
+	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
+		if (newsp == NULL) {
+#ifdef IPSEC_DEBUG
+			printf("key_spdadd: no SP found.\n");
+#endif
+			return key_senderror(so, m, ENOENT);
+		}
+
+		newsp->state = IPSEC_SPSTATE_DEAD;
+		key_freesp(newsp);
+	} else {
+		if (newsp != NULL) {
+			key_freesp(newsp);
+#ifdef IPSEC_DEBUG
+			printf("key_spdadd: a SP entry exists already.\n");
+#endif
+			return key_senderror(so, m, EEXIST);
+		}
 	}
 
 	/* allocation new SP entry */
-	if ((newsp = key_msg2sp(xpl0)) == NULL) {
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+	if ((newsp = key_msg2sp(xpl0, PFKEY_EXTLEN(xpl0), &error)) == NULL) {
+		return key_senderror(so, m, error);
 	}
 
+	if ((newsp->id = key_getnewspid()) == 0) {
+		keydb_delsecpolicy(newsp);
+		return key_senderror(so, m, ENOBUFS);
+	}
+
+	/* XXX boundary check against sa_len */
 	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
 	                src0 + 1,
 	                dst0 + 1,
@@ -1138,43 +1675,123 @@ key_spdadd(mhp)
 	                src0->sadb_address_proto,
 	                &newsp->spidx);
 
+	/* sanity check on addr pair */
+	if (((struct sockaddr *)(src0 + 1))->sa_family !=
+			((struct sockaddr *)(dst0+ 1))->sa_family) {
+		keydb_delsecpolicy(newsp);
+		return key_senderror(so, m, EINVAL);
+	}
+	if (((struct sockaddr *)(src0 + 1))->sa_len !=
+			((struct sockaddr *)(dst0+ 1))->sa_len) {
+		keydb_delsecpolicy(newsp);
+		return key_senderror(so, m, EINVAL);
+	}
+#if 1
+	if (newsp->req && newsp->req->saidx.src.ss_family) {
+		struct sockaddr *sa;
+		sa = (struct sockaddr *)(src0 + 1);
+		if (sa->sa_family != newsp->req->saidx.src.ss_family) {
+			keydb_delsecpolicy(newsp);
+			return key_senderror(so, m, EINVAL);
+		}
+	}
+	if (newsp->req && newsp->req->saidx.dst.ss_family) {
+		struct sockaddr *sa;
+		sa = (struct sockaddr *)(dst0 + 1);
+		if (sa->sa_family != newsp->req->saidx.dst.ss_family) {
+			keydb_delsecpolicy(newsp);
+			return key_senderror(so, m, EINVAL);
+		}
+	}
+#endif
+
 	newsp->refcnt = 1;	/* do not reclaim until I say I do */
 	newsp->state = IPSEC_SPSTATE_ALIVE;
-	LIST_INSERT_HEAD(&sptree[newsp->spidx.dir], newsp, chain);
+	LIST_INSERT_TAIL(&sptree[newsp->spidx.dir], newsp, secpolicy, chain);
+
+	/* delete the entry in spacqtree */
+	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
+		struct secspacq *spacq;
+		if ((spacq = key_getspacq(&spidx)) != NULL) {
+			/* reset counter in order to deletion by timehander. */
+			spacq->tick = key_blockacq_lifetime;
+			spacq->count = 0;
+		}
+    	}
 
     {
+	struct mbuf *n, *mpolicy;
 	struct sadb_msg *newmsg;
-	u_int len;
-	caddr_t p;
+	int off;
 
 	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-	    + PFKEY_EXTLEN(mhp[SADB_X_EXT_POLICY])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_SRC])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_DST]);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_spdadd: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
-	}
-	bzero((caddr_t)newmsg, len);
+	n = key_gather_mbuf(m, mhp, 2, 4, SADB_EXT_RESERVED,
+	    SADB_X_EXT_POLICY, SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
 
-	bcopy((caddr_t)msg0, (caddr_t)newmsg, sizeof(*msg0));
+	if (n->m_len < sizeof(*newmsg)) {
+		n = m_pullup(n, sizeof(*newmsg));
+		if (!n)
+			return key_senderror(so, m, ENOBUFS);
+	}
+	newmsg = mtod(n, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
+	newmsg->sadb_msg_len = PFKEY_UNIT64(n->m_pkthdr.len);
 
-	p = key_setsadbext(p, mhp[SADB_X_EXT_POLICY]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_SRC]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_DST]);
+	off = 0;
+	mpolicy = m_pulldown(n, PFKEY_ALIGN8(sizeof(struct sadb_msg)),
+	    sizeof(*xpl), &off);
+	if (mpolicy == NULL) {
+		/* n is already freed */
+		return key_senderror(so, m, ENOBUFS);
+	}
+	xpl = (struct sadb_x_policy *)(mtod(mpolicy, caddr_t) + off);
+	if (xpl->sadb_x_policy_exttype != SADB_X_EXT_POLICY) {
+		m_freem(n);
+		return key_senderror(so, m, EINVAL);
+	}
+	xpl->sadb_x_policy_id = newsp->id;
 
-	return newmsg;
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
     }
 }
 
 /*
+ * get new policy id.
+ * OUT:
+ *	0:	failure.
+ *	others: success.
+ */
+static u_int32_t
+key_getnewspid()
+{
+	u_int32_t newid = 0;
+	int count = key_spi_trycnt;	/* XXX */
+	struct secpolicy *sp;
+
+	/* when requesting to allocate spi ranged */
+	while (count--) {
+		newid = (policy_id = (policy_id == ~0 ? 1 : ++policy_id));
+
+		if ((sp = key_getspbyid(newid)) == NULL)
+			break;
+
+		key_freesp(sp);
+	}
+
+	if (count == 0 || newid == 0) {
+#ifdef IPSEC_DEBUG
+		printf("key_getnewspid: to allocate policy id is failed.\n");
+#endif
+		return 0;
+	}
+
+	return newid;
+}
+
+/*
  * SADB_SPDDELETE processing
  * receive
  *   <base, address(SD), policy(*)>
@@ -1184,39 +1801,46 @@ key_spdadd(mhp)
  * to the ikmpd.
  * policy(*) including direction of policy.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	other if success, return pointer to the message to send.
- *	0 if fail.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_spddelete(mhp)
-	caddr_t *mhp;
+static int
+key_spddelete(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_address *src0, *dst0;
 	struct sadb_x_policy *xpl0;
 	struct secpolicyindex spidx;
 	struct secpolicy *sp;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_spddelete: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
-	if (mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL
-	 || mhp[SADB_X_EXT_POLICY] == NULL) {
+	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
+	    mhp->ext[SADB_X_EXT_POLICY] == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_spddelete: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_spddelete: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
-	xpl0 = (struct sadb_x_policy *)mhp[SADB_X_EXT_POLICY];
+	src0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_SRC];
+	dst0 = (struct sadb_address *)mhp->ext[SADB_EXT_ADDRESS_DST];
+	xpl0 = (struct sadb_x_policy *)mhp->ext[SADB_X_EXT_POLICY];
 
 	/* make secindex */
+	/* XXX boundary check against sa_len */
 	KEY_SETSECSPIDX(xpl0->sadb_x_policy_dir,
 	                src0 + 1,
 	                dst0 + 1,
@@ -1231,54 +1855,271 @@ key_spddelete(mhp)
 	case IPSEC_DIR_OUTBOUND:
 		break;
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_spddelete: Invalid SP direction.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
 	/* Is there SP in SPD ? */
 	if ((sp = key_getsp(&spidx)) == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_spddelete: no SP found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
+	/* save policy id to buffer to be returned. */
+	xpl0->sadb_x_policy_id = sp->id;
+
 	sp->state = IPSEC_SPSTATE_DEAD;
 	key_freesp(sp);
 
     {
+	struct mbuf *n;
 	struct sadb_msg *newmsg;
-	u_int len;
-	caddr_t p;
 
 	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-	    + PFKEY_EXTLEN(mhp[SADB_X_EXT_POLICY])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_SRC])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_DST]);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_spddelete: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
-	}
-	bzero((caddr_t)newmsg, len);
+	n = key_gather_mbuf(m, mhp, 1, 4, SADB_EXT_RESERVED,
+	    SADB_X_EXT_POLICY, SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
+	newmsg = mtod(n, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
+	newmsg->sadb_msg_len = PFKEY_UNIT64(n->m_pkthdr.len);
+
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
+    }
+}
+
+/*
+ * SADB_SPDDELETE2 processing
+ * receive
+ *   <base, policy(*)>
+ * from the user(?), and set SADB_SASTATE_DEAD,
+ * and send,
+ *   <base, policy(*)>
+ * to the ikmpd.
+ * policy(*) including direction of policy.
+ *
+ * m will always be freed.
+ */
+static int
+key_spddelete2(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
+{
+	u_int32_t id;
+	struct secpolicy *sp;
+
+	/* sanity check */
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
+		panic("key_spddelete2: NULL pointer is passed.\n");
+
+	if (mhp->ext[SADB_X_EXT_POLICY] == NULL ||
+	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
+#ifdef IPSEC_DEBUG
+		printf("key_spddelete2: invalid message is passed.\n");
+#endif
+		key_senderror(so, m, EINVAL);
+		return 0;
+	}
+
+	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) {
+#ifdef IPSEC_DEBUG
+		printf("key_spddelete2: no SP found id:%u.\n", id);
+#endif
+		key_senderror(so, m, EINVAL);
+	}
 
-	p = key_setsadbext(p, mhp[SADB_X_EXT_POLICY]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_SRC]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_DST]);
+	sp->state = IPSEC_SPSTATE_DEAD;
+	key_freesp(sp);
+
+    {
+	struct mbuf *n, *nn;
+	struct sadb_msg *newmsg;
+	int off, len;
+
+	/* create new sadb_msg to reply. */
+	len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
+
+	if (len > MCLBYTES)
+		return key_senderror(so, m, ENOBUFS);
+	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	if (n && len > MHLEN) {
+		MCLGET(n, M_DONTWAIT);
+		if ((n->m_flags & M_EXT) == 0) {
+			m_freem(n);
+			n = NULL;
+		}
+	}
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
+
+	n->m_len = len;
+	n->m_next = NULL;
+	off = 0;
+
+	m_copydata(m, 0, sizeof(struct sadb_msg), mtod(n, caddr_t) + off);
+	off += PFKEY_ALIGN8(sizeof(struct sadb_msg));
 
-	return newmsg;
+#ifdef DIAGNOSTIC
+	if (off != len)
+		panic("length inconsistency in key_spddelete2");
+#endif
+
+	n->m_next = m_copym(m, mhp->extoff[SADB_X_EXT_POLICY],
+	    mhp->extlen[SADB_X_EXT_POLICY], M_DONTWAIT);
+	if (!n->m_next) {
+		m_freem(n);
+		return key_senderror(so, m, ENOBUFS);
+	}
+
+	n->m_pkthdr.len = 0;
+	for (nn = n; nn; nn = nn->m_next)
+		n->m_pkthdr.len += nn->m_len;
+
+	newmsg = mtod(n, struct sadb_msg *);
+	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_ALL);
     }
 }
 
 /*
+ * SADB_X_GET processing
+ * receive
+ *   <base, policy(*)>
+ * from the user(?),
+ * and send,
+ *   <base, address(SD), policy>
+ * to the ikmpd.
+ * policy(*) including direction of policy.
+ *
+ * m will always be freed.
+ */
+static int
+key_spdget(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
+{
+	u_int32_t id;
+	struct secpolicy *sp;
+	struct mbuf *n;
+
+	/* sanity check */
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
+		panic("key_spdget: NULL pointer is passed.\n");
+
+	if (mhp->ext[SADB_X_EXT_POLICY] == NULL ||
+	    mhp->extlen[SADB_X_EXT_POLICY] < sizeof(struct sadb_x_policy)) {
+#ifdef IPSEC_DEBUG
+		printf("key_spdget: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
+	}
+
+	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) {
+#ifdef IPSEC_DEBUG
+		printf("key_spdget: no SP found id:%u.\n", id);
+#endif
+		return key_senderror(so, m, ENOENT);
+	}
+
+	n = key_setdumpsp(sp, SADB_X_SPDGET, 0, mhp->msg->sadb_msg_pid);
+	if (n != NULL) {
+		m_freem(m);
+		return key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+	} else
+		return key_senderror(so, m, ENOBUFS);
+}
+
+/*
+ * SADB_X_SPDACQUIRE processing.
+ * Acquire policy and SA(s) for a *OUTBOUND* packet.
+ * send
+ *   <base, policy(*)>
+ * to KMD, and expect to receive
+ *   <base> with SADB_X_SPDACQUIRE if error occured,
+ * or
+ *   <base, policy>
+ * with SADB_X_SPDUPDATE from KMD by PF_KEY.
+ * policy(*) is without policy requests.
+ *
+ *    0     : succeed
+ *    others: error number
+ */
+int
+key_spdacquire(sp)
+	struct secpolicy *sp;
+{
+	struct mbuf *result = NULL, *m;
+	struct secspacq *newspacq;
+	int error;
+
+	/* sanity check */
+	if (sp == NULL)
+		panic("key_spdacquire: NULL pointer is passed.\n");
+	if (sp->req != NULL)
+		panic("key_spdacquire: called but there is request.\n");
+	if (sp->policy != IPSEC_POLICY_IPSEC)
+		panic("key_spdacquire: policy mismathed. IPsec is expected.\n");
+
+	/* get a entry to check whether sent message or not. */
+	if ((newspacq = key_getspacq(&sp->spidx)) != NULL) {
+		if (key_blockacq_count < newspacq->count) {
+			/* reset counter and do send message. */
+			newspacq->count = 0;
+		} else {
+			/* increment counter and do nothing. */
+			newspacq->count++;
+			return 0;
+		}
+	} else {
+		/* make new entry for blocking to send SADB_ACQUIRE. */
+		if ((newspacq = key_newspacq(&sp->spidx)) == NULL)
+			return ENOBUFS;
+
+		/* add to acqtree */
+		LIST_INSERT_HEAD(&spacqtree, newspacq, chain);
+	}
+
+	/* create new sadb_msg to reply. */
+	m = key_setsadbmsg(SADB_X_SPDACQUIRE, 0, 0, 0, 0, 0);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	result = m;
+
+	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);
+
+	return key_sendup_mbuf(NULL, m, KEY_SENDUP_REGISTERED);
+
+fail:
+	if (result)
+		m_freem(result);
+	return error;
+}
+
+/*
  * SADB_SPDFLUSH processing
  * receive
  *   <base>
@@ -1288,51 +2129,47 @@ key_spddelete(mhp)
  * to the user.
  * NOTE: what to do is only marking SADB_SASTATE_DEAD.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	other if success, return pointer to the message to send.
- *	0 if fail.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_spdflush(mhp)
-	caddr_t *mhp;
+static int
+key_spdflush(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
+	struct sadb_msg *newmsg;
 	struct secpolicy *sp;
 	u_int dir;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_spdflush: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
+	if (m->m_len != PFKEY_ALIGN8(sizeof(struct sadb_msg)))
+		return key_senderror(so, m, EINVAL);
 
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		__LIST_FOREACH(sp, &sptree[dir], chain) {
+		LIST_FOREACH(sp, &sptree[dir], chain) {
 			sp->state = IPSEC_SPSTATE_DEAD;
 		}
 	}
 
-    {
-	struct sadb_msg *newmsg;
-	u_int len;
-
-	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
+	if (sizeof(struct sadb_msg) > m->m_len + M_TRAILINGSPACE(m)) {
+#ifdef IPSEC_DEBUG
 		printf("key_spdflush: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+#endif
+		return key_senderror(so, m, ENOBUFS);
 	}
-	bzero((caddr_t)newmsg, len);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
+	if (m->m_next)
+		m_freem(m->m_next);
+	m->m_next = NULL;
+	m->m_pkthdr.len = m->m_len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
+	newmsg = mtod(m, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
+	newmsg->sadb_msg_len = PFKEY_UNIT64(m->m_pkthdr.len);
 
-	return(newmsg);
-    }
+	return key_sendup_mbuf(so, m, KEY_SENDUP_ALL);
 }
 
 /*
@@ -1344,134 +2181,102 @@ key_spdflush(mhp)
  *   <base> .....
  * to the ikmpd.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	other if success, return pointer to the message to send.
- *	0 if fail.
+ * m will always be freed.
  */
 static int
-key_spddump(mhp, so, target)
-	caddr_t *mhp;
+key_spddump(so, m, mhp)
 	struct socket *so;
-	int target;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct secpolicy *sp;
-	int len, cnt, cnt_sanity;
-	struct sadb_msg *newmsg;
+	int cnt;
 	u_int dir;
+	struct mbuf *n;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_spddump: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* search SPD entry and get buffer size. */
-	cnt = cnt_sanity = 0;
+	cnt = 0;
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		__LIST_FOREACH(sp, &sptree[dir], chain) {
+		LIST_FOREACH(sp, &sptree[dir], chain) {
 			cnt++;
 		}
 	}
 
 	if (cnt == 0)
-		return ENOENT;
+		return key_senderror(so, m, ENOENT);
 
-	newmsg = NULL;
 	for (dir = 0; dir < IPSEC_DIR_MAX; dir++) {
-		__LIST_FOREACH(sp, &sptree[dir], chain) {
-			len = key_getspmsglen(sp);
-
-			/* making buffer */
-			KMALLOC(newmsg, struct sadb_msg *, len);
-			if (newmsg == NULL) {
-				printf("key_spddump: No more memory.\n");
-				return ENOBUFS;
-			}
-			bzero((caddr_t)newmsg, len);
-
+		LIST_FOREACH(sp, &sptree[dir], chain) {
 			--cnt;
-			(void)key_setdumpsp(newmsg, sp, SADB_X_SPDDUMP,
-			                    cnt, msg0->sadb_msg_pid);
+			n = key_setdumpsp(sp, SADB_X_SPDDUMP, cnt,
+			    mhp->msg->sadb_msg_pid);
 
-			key_sendup(so, newmsg, len, target);
-			KFREE(newmsg);
-			newmsg = NULL;
+			if (n)
+				key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
 		}
 	}
 
+	m_freem(m);
 	return 0;
 }
 
-static u_int
-key_setdumpsp(newmsg, sp, type, seq, pid)
-	struct sadb_msg *newmsg;
+static struct mbuf *
+key_setdumpsp(sp, type, seq, pid)
 	struct secpolicy *sp;
 	u_int8_t type;
 	u_int32_t seq, pid;
 {
-	u_int tlen;
-	caddr_t p;
-
-	tlen = key_getspmsglen(sp);
-
-	p = key_setsadbmsg((caddr_t)newmsg, type, tlen,
-	                   SADB_SATYPE_UNSPEC, seq, pid,
-	                   IPSEC_MODE_ANY, sp->refcnt);
+	struct mbuf *result = NULL, *m;
 
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_SRC,
-	                    (struct sockaddr *)&sp->spidx.src,
-	                    sp->spidx.prefs,
-	                    sp->spidx.ul_proto);
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_DST,
-	                    (struct sockaddr *)&sp->spidx.dst,
-	                    sp->spidx.prefd,
-	                    sp->spidx.ul_proto);
+	m = key_setsadbmsg(type, 0, SADB_SATYPE_UNSPEC, seq, pid, sp->refcnt);
+	if (!m)
+		goto fail;
+	result = m;
 
-    {
-	struct sadb_x_policy *tmp;
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
+	    (struct sockaddr *)&sp->spidx.src, sp->spidx.prefs,
+	    sp->spidx.ul_proto);
+	if (!m)
+		goto fail;
+	m_cat(result, m);
 
-	if ((tmp = key_sp2msg(sp)) == NULL) {
-		printf("key_setdumpsp: No more memory.\n");
-		return ENOBUFS;
-	}
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
+	    (struct sockaddr *)&sp->spidx.dst, sp->spidx.prefd,
+	    sp->spidx.ul_proto);
+	if (!m)
+		goto fail;
+	m_cat(result, m);
 
-	/* validity check */
-	if (key_getspreqmsglen(sp) != PFKEY_UNUNIT64(tmp->sadb_x_policy_len))
-		panic("key_setdumpsp: length mismatch."
-		      "sp:%d msg:%d\n",
-			key_getspreqmsglen(sp),
-			PFKEY_UNUNIT64(tmp->sadb_x_policy_len));
-
-	bcopy(tmp, p, PFKEY_UNUNIT64(tmp->sadb_x_policy_len));
-	KFREE(tmp);
-    }
+	m = key_sp2msg(sp);
+	if (!m)
+		goto fail;
+	m_cat(result, m);
 
-	return tlen;
-}
+	if ((result->m_flags & M_PKTHDR) == 0)
+		goto fail;
 
-/* get sadb message length for a SP. */
-static u_int
-key_getspmsglen(sp)
-	struct secpolicy *sp;
-{
-	u_int tlen;
+	if (result->m_len < sizeof(struct sadb_msg)) {
+		result = m_pullup(result, sizeof(struct sadb_msg));
+		if (result == NULL)
+			goto fail;
+	}
 
-	/* sanity check */
-	if (sp == NULL)
-		panic("key_getspmsglen: NULL pointer is passed.\n");
+	result->m_pkthdr.len = 0;
+	for (m = result; m; m = m->m_next)
+		result->m_pkthdr.len += m->m_len;
 
-	tlen = (sizeof(struct sadb_msg)
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(_SALENBYAF(sp->spidx.src.ss_family))
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(_SALENBYAF(sp->spidx.dst.ss_family)));
+	mtod(result, struct sadb_msg *)->sadb_msg_len =
+	    PFKEY_UNIT64(result->m_pkthdr.len);
 
-	tlen += key_getspreqmsglen(sp);
+	return result;
 
-	return tlen;
+fail:
+	m_freem(result);
+	return NULL;
 }
 
 /*
@@ -1517,26 +2322,17 @@ key_newsah(saidx)
 	struct secasindex *saidx;
 {
 	struct secashead *newsah;
-	u_int stateidx;
 
 	/* sanity check */
 	if (saidx == NULL)
 		panic("key_newsaidx: NULL pointer is passed.\n");
 
-	KMALLOC(newsah, struct secashead *, sizeof(struct secashead));
-	if (newsah == NULL) {
+	newsah = keydb_newsecashead();
+	if (newsah == NULL)
 		return NULL;
-	}
-	bzero((caddr_t)newsah, sizeof(struct secashead));
 
 	bcopy(saidx, &newsah->saidx, sizeof(newsah->saidx));
 
-	for (stateidx = 0;
-	     stateidx < _ARRAYLEN(saorder_state_any);
-	     stateidx++) {
-		LIST_INIT(&newsah->savtree[saorder_state_any[stateidx]]);
-	}
-
 	/* add to saidxtree */
 	newsah->state = SADB_SASTATE_MATURE;
 	LIST_INSERT_HEAD(&sahtree, newsah, chain);
@@ -1554,6 +2350,7 @@ key_delsah(sah)
 	struct secasvar *sav, *nextsav;
 	u_int stateidx, state;
 	int s;
+	int zombie = 0;
 
 	/* sanity check */
 	if (sah == NULL)
@@ -1561,10 +2358,6 @@ key_delsah(sah)
 
 	s = splnet();	/*called from softclock()*/
 
-	/* remove from tree of SA index */
-	if (__LIST_CHAINED(sah))
-		LIST_REMOVE(sah, chain);
-
 	/* searching all SA registerd in the secindex. */
 	for (stateidx = 0;
 	     stateidx < _ARRAYLEN(saorder_state_any);
@@ -1577,27 +2370,38 @@ key_delsah(sah)
 
 			nextsav = LIST_NEXT(sav, chain);
 
+			if (sav->refcnt > 0) {
+				/* give up to delete this sa */
+				zombie++;
+				continue;
+			}
+
 			/* sanity check */
 			KEY_CHKSASTATE(state, sav->state, "key_delsah");
 
+			key_freesav(sav);
+
 			/* remove back pointer */
 			sav->sah = NULL;
-
-			if (sav->refcnt < 0) {
-				printf("key_delsah: why refcnt < 0 ?, "
-					"sav->refcnt=%d\n",
-					sav->refcnt);
-			}
-			key_freesav(sav);
 			sav = NULL;
 		}
 	}
 
+	/* don't delete sah only if there are savs. */
+	if (zombie) {
+		splx(s);
+		return;
+	}
+
 	if (sah->sa_route.ro_rt) {
 		RTFREE(sah->sa_route.ro_rt);
 		sah->sa_route.ro_rt = (struct rtentry *)NULL;
 	}
 
+	/* remove from tree of SA index */
+	if (__LIST_CHAINED(sah))
+		LIST_REMOVE(sah, chain);
+
 	KFREE(sah);
 
 	splx(s);
@@ -1613,75 +2417,87 @@ key_delsah(sah)
  *	not to call key_setsava().
  * OUT:	NULL	: fail
  *	others	: pointer to new secasvar.
+ *
+ * does not modify mbuf.  does not free mbuf on error.
  */
 static struct secasvar *
-key_newsav(mhp, sah)
-	caddr_t *mhp;
+key_newsav(m, mhp, sah, errp)
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 	struct secashead *sah;
+	int *errp;
 {
 	struct secasvar *newsav;
-	struct sadb_msg *msg0;
+	const struct sadb_sa *xsa;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL || sah == NULL)
+	if (m == NULL || mhp == NULL || mhp->msg == NULL || sah == NULL)
 		panic("key_newsa: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	KMALLOC(newsav, struct secasvar *, sizeof(struct secasvar));
 	if (newsav == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_newsa: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
+#endif
+		*errp = ENOBUFS;
 		return NULL;
 	}
 	bzero((caddr_t)newsav, sizeof(struct secasvar));
 
-	switch (msg0->sadb_msg_type) {
+	switch (mhp->msg->sadb_msg_type) {
 	case SADB_GETSPI:
 		newsav->spi = 0;
 
+#ifdef IPSEC_DOSEQCHECK
 		/* sync sequence number */
-		if (msg0->sadb_msg_seq == 0)
+		if (mhp->msg->sadb_msg_seq == 0)
 			newsav->seq =
 				(acq_seq = (acq_seq == ~0 ? 1 : ++acq_seq));
 		else
-			newsav->seq = msg0->sadb_msg_seq;
+#endif
+			newsav->seq = mhp->msg->sadb_msg_seq;
 		break;
 
 	case SADB_ADD:
 		/* sanity check */
-		if (mhp[SADB_EXT_SA] == NULL) {
+		if (mhp->ext[SADB_EXT_SA] == NULL) {
 			KFREE(newsav);
+#ifdef IPSEC_DEBUG
 			printf("key_newsa: invalid message is passed.\n");
-			msg0->sadb_msg_errno = EINVAL;
+#endif
+			*errp = EINVAL;
 			return NULL;
 		}
-		newsav->spi = ((struct sadb_sa *)mhp[SADB_EXT_SA])->sadb_sa_spi;
-		newsav->seq = msg0->sadb_msg_seq;
+		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:
 		KFREE(newsav);
-		msg0->sadb_msg_errno = EINVAL;
+		*errp = EINVAL;
 		return NULL;
 	}
 
 	/* copy sav values */
-	if (msg0->sadb_msg_type != SADB_GETSPI && key_setsaval(newsav, mhp)) {
-		KFREE(newsav);
-		/* msg0->sadb_msg_errno is set at key_setsaval. */
-		return NULL;
+	if (mhp->msg->sadb_msg_type != SADB_GETSPI) {
+		*errp = key_setsaval(newsav, m, mhp);
+		if (*errp) {
+			KFREE(newsav);
+			return NULL;
+		}
 	}
 
 	/* reset tick */
 	newsav->tick = 0;
 
-	newsav->pid = msg0->sadb_msg_pid;
+	newsav->pid = mhp->msg->sadb_msg_pid;
 
 	/* add to satree */
 	newsav->sah = sah;
 	newsav->refcnt = 1;
 	newsav->state = SADB_SASTATE_LARVAL;
-	LIST_INSERT_HEAD(&sah->savtree[SADB_SASTATE_LARVAL], newsav, chain);
+	LIST_INSERT_TAIL(&sah->savtree[SADB_SASTATE_LARVAL], newsav,
+			secasvar, chain);
 
 	return newsav;
 }
@@ -1697,7 +2513,8 @@ key_delsav(sav)
 	if (sav == NULL)
 		panic("key_delsav: NULL pointer is passed.\n");
 
-	if (sav->refcnt > 0) return; /* can't free */
+	if (sav->refcnt > 0)
+		return;		/* can't free */
 
 	/* remove from SA header */
 	if (__LIST_CHAINED(sav))
@@ -1707,11 +2524,8 @@ key_delsav(sav)
 		KFREE(sav->key_auth);
 	if (sav->key_enc != NULL)
 		KFREE(sav->key_enc);
-	if (sav->replay != NULL) {
-		if (sav->replay->bitmap != NULL)
-			KFREE(sav->replay->bitmap);
-		KFREE(sav->replay);
-	}
+	if (sav->replay != NULL)
+		keydb_delsecreplay(sav->replay);
 	if (sav->lft_c != NULL)
 		KFREE(sav->lft_c);
 	if (sav->lft_h != NULL)
@@ -1729,10 +2543,6 @@ key_delsav(sav)
 		KFREE(sav->misc3);
 #endif
 
-	sav->sah = NULL;
-		/* XXX for making sure.  See key_checkrequest(),
-		 * Refcnt may be suspicious. */
-
 	KFREE(sav);
 
 	return;
@@ -1750,7 +2560,7 @@ key_getsah(saidx)
 {
 	struct secashead *sah;
 
-	__LIST_FOREACH(sah, &sahtree, chain) {
+	LIST_FOREACH(sah, &sahtree, chain) {
 		if (sah->state == SADB_SASTATE_DEAD)
 			continue;
 		if (key_cmpsaidx_exactly(&sah->saidx, saidx))
@@ -1776,15 +2586,16 @@ key_checkspidup(saidx, spi)
 	struct secasvar *sav;
 
 	/* check address family */
-	if (saidx->src.ss_family != saidx->src.ss_family) {
+	if (saidx->src.ss_family != saidx->dst.ss_family) {
+#ifdef IPSEC_DEBUG
 		printf("key_checkspidup: address family mismatched.\n");
+#endif
 		return NULL;
 	}
 
 	/* check all SAD */
-	__LIST_FOREACH(sah, &sahtree, chain) {
-		if (!key_ismyaddr(sah->saidx.dst.ss_family,
-		                  _INADDRBYSA(&sah->saidx.dst)))
+	LIST_FOREACH(sah, &sahtree, chain) {
+		if (!key_ismyaddr((struct sockaddr *)&sah->saidx.dst))
 			continue;
 		sav = key_getsavbyspi(sah, spi);
 		if (sav != NULL)
@@ -1814,14 +2625,16 @@ key_getsavbyspi(sah, spi)
 	     stateidx++) {
 
 		state = saorder_state_alive[stateidx];
-		__LIST_FOREACH(sav, &sah->savtree[state], chain) {
+		LIST_FOREACH(sav, &sah->savtree[state], chain) {
 
 			/* sanity check */
 			if (sav->state != state) {
+#ifdef IPSEC_DEBUG
 				printf("key_getsavbyspi: "
 					"invalid sav->state "
 					"(queue: %d SA: %d)\n",
 					state, sav->state);
+#endif
 				continue;
 			}
 
@@ -1837,22 +2650,25 @@ key_getsavbyspi(sah, spi)
  * copy SA values from PF_KEY message except *SPI, SEQ, PID, STATE and TYPE*.
  * You must update these if need.
  * OUT:	0:	success.
- *	1:	failure. set errno to (mhp[0])->sadb_msg_errno.
+ *	!0:	failure.
+ *
+ * does not modify mbuf.  does not free mbuf on error.
  */
 static int
-key_setsaval(sav, mhp)
+key_setsaval(sav, m, mhp)
 	struct secasvar *sav;
-	caddr_t *mhp;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
+#ifdef IPSEC_ESP
+	const struct esp_algorithm *algo;
+#endif
 	int error = 0;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_setsaval: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* initialization */
 	sav->replay = NULL;
 	sav->key_auth = NULL;
@@ -1861,10 +2677,21 @@ key_setsaval(sav, mhp)
 	sav->lft_c = NULL;
 	sav->lft_h = NULL;
 	sav->lft_s = NULL;
+#if notyet
+	sav->misc1 = NULL;
+	sav->misc2 = NULL;
+	sav->misc3 = NULL;
+#endif
 
 	/* SA */
-	if (mhp[SADB_EXT_SA] != NULL) {
-		struct sadb_sa *sa0 = (struct sadb_sa *)mhp[SADB_EXT_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)) {
+			error = EINVAL;
+			goto fail;
+		}
 
 		sav->alg_auth = sa0->sadb_sa_auth;
 		sav->alg_enc = sa0->sadb_sa_encrypt;
@@ -1872,144 +2699,135 @@ key_setsaval(sav, mhp)
 
 		/* replay window */
 		if ((sa0->sadb_sa_flags & SADB_X_EXT_OLD) == 0) {
-			KMALLOC(sav->replay, struct secreplay *,
-				sizeof(struct secreplay));
+			sav->replay = keydb_newsecreplay(sa0->sadb_sa_replay);
 			if (sav->replay == NULL) {
+#ifdef IPSEC_DEBUG
 				printf("key_setsaval: No more memory.\n");
+#endif
 				error = ENOBUFS;
-				goto err;
-			}
-			bzero(sav->replay, sizeof(struct secreplay));
-
-			if ((sav->replay->wsize = sa0->sadb_sa_replay) != 0) {
-				KMALLOC(sav->replay->bitmap, caddr_t,
-					sav->replay->wsize);
-				if (sav->replay->bitmap == NULL) {
-					printf("key_setsaval: "
-					       "No more memory.\n");
-					error = ENOBUFS;
-					goto err;
-				}
-				bzero(sav->replay->bitmap, sa0->sadb_sa_replay);
+				goto fail;
 			}
 		}
 	}
 
 	/* Authentication keys */
-	if (mhp[SADB_EXT_KEY_AUTH] != NULL) {
-		struct sadb_key *key0;
-		u_int len;
+	if (mhp->ext[SADB_EXT_KEY_AUTH] != NULL) {
+		const struct sadb_key *key0;
+		int len;
 
-		key0 = (struct sadb_key *)mhp[SADB_EXT_KEY_AUTH];
-		len = PFKEY_UNUNIT64(key0->sadb_key_len);
+		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_AUTH];
+		len = mhp->extlen[SADB_EXT_KEY_AUTH];
 
 		error = 0;
-		if (len < sizeof(struct sadb_key))
+		if (len < sizeof(*key0)) {
 			error = EINVAL;
-		switch (msg0->sadb_msg_satype) {
+			goto fail;
+		}
+		switch (mhp->msg->sadb_msg_satype) {
 		case SADB_SATYPE_AH:
 		case SADB_SATYPE_ESP:
-			if (len == sizeof(struct sadb_key)
-			 && sav->alg_auth != SADB_AALG_NULL) {
+			if (len == PFKEY_ALIGN8(sizeof(struct sadb_key)) &&
+			    sav->alg_auth != SADB_AALG_NULL)
 				error = EINVAL;
-			}
 			break;
 		case SADB_X_SATYPE_IPCOMP:
-			error = EINVAL;
-			break;
 		default:
 			error = EINVAL;
 			break;
 		}
 		if (error) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: invalid key_auth values.\n");
-			goto err;
+#endif
+			goto fail;
 		}
 
-		KEY_NEWBUF(sav->key_auth, struct sadb_key *, key0, len);
+		sav->key_auth = (struct sadb_key *)key_newbuf(key0, len);
 		if (sav->key_auth == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: No more memory.\n");
+#endif
 			error = ENOBUFS;
-			goto err;
+			goto fail;
 		}
-
-		/* make length shift up for kernel*/
-		sav->key_auth->sadb_key_len = len;
 	}
 
 	/* Encryption key */
-	if (mhp[SADB_EXT_KEY_ENCRYPT] != NULL) {
-		struct sadb_key *key0;
-		u_int len;
+	if (mhp->ext[SADB_EXT_KEY_ENCRYPT] != NULL) {
+		const struct sadb_key *key0;
+		int len;
 
-		key0 = (struct sadb_key *)mhp[SADB_EXT_KEY_ENCRYPT];
-		len = PFKEY_UNUNIT64(key0->sadb_key_len);
+		key0 = (const struct sadb_key *)mhp->ext[SADB_EXT_KEY_ENCRYPT];
+		len = mhp->extlen[SADB_EXT_KEY_ENCRYPT];
 
 		error = 0;
-		if (len < sizeof(struct sadb_key))
+		if (len < sizeof(*key0)) {
 			error = EINVAL;
-		switch (msg0->sadb_msg_satype) {
+			goto fail;
+		}
+		switch (mhp->msg->sadb_msg_satype) {
 		case SADB_SATYPE_ESP:
-			if (len == sizeof(struct sadb_key)
-			 && sav->alg_enc != SADB_EALG_NULL) {
+			if (len == PFKEY_ALIGN8(sizeof(struct sadb_key)) &&
+			    sav->alg_enc != SADB_EALG_NULL)
 				error = EINVAL;
-			}
 			break;
 		case SADB_SATYPE_AH:
-			error = EINVAL;
-			break;
 		case SADB_X_SATYPE_IPCOMP:
-			break;
-		default:
 			error = EINVAL;
 			break;
 		}
 		if (error) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsatval: invalid key_enc value.\n");
-			goto err;
+#endif
+			goto fail;
 		}
 
-		KEY_NEWBUF(sav->key_enc, struct sadb_key *, key0, len);
+		sav->key_enc = (struct sadb_key *)key_newbuf(key0, len);
 		if (sav->key_enc == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: No more memory.\n");
+#endif
 			error = ENOBUFS;
-			goto err;
+			goto fail;
 		}
-
-		/* make length shift up for kernel*/
-		sav->key_enc->sadb_key_len = len;
 	}
 
 	/* set iv */
 	sav->ivlen = 0;
 
-	switch (msg0->sadb_msg_satype) {
+	switch (mhp->msg->sadb_msg_satype) {
 	case SADB_SATYPE_ESP:
 #ifdef IPSEC_ESP
-	    {
-		struct esp_algorithm *algo;
-
 		algo = &esp_algorithms[sav->alg_enc];
 		if (algo && algo->ivlen)
 			sav->ivlen = (*algo->ivlen)(sav);
+		if (sav->ivlen == 0)
+			break;
 		KMALLOC(sav->iv, caddr_t, sav->ivlen);
 		if (sav->iv == 0) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: No more memory.\n");
+#endif
 			error = ENOBUFS;
-			goto err;
+			goto fail;
 		}
 		/* initialize ? */
 		break;
-	    }
 #else
 		break;
 #endif
 	case SADB_SATYPE_AH:
+#if 1	/*nonstandard*/
+	case SADB_X_SATYPE_IPCOMP:
+#endif
 		break;
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_setsaval: invalid SA type.\n");
+#endif
 		error = EINVAL;
-		goto err;
+		goto fail;
 	}
 
 	/* reset tick */
@@ -2020,17 +2838,19 @@ key_setsaval(sav, mhp)
 	struct timeval tv;
 
 	KMALLOC(sav->lft_c, struct sadb_lifetime *,
-		sizeof(struct sadb_lifetime));
+	    sizeof(struct sadb_lifetime));
 	if (sav->lft_c == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_setsaval: No more memory.\n");
+#endif
 		error = ENOBUFS;
-		goto err;
+		goto fail;
 	}
 
 	microtime(&tv);
 
 	sav->lft_c->sadb_lifetime_len =
-		PFKEY_UNIT64(sizeof(struct sadb_lifetime));
+	    PFKEY_UNIT64(sizeof(struct sadb_lifetime));
 	sav->lft_c->sadb_lifetime_exttype = SADB_EXT_LIFETIME_CURRENT;
 	sav->lft_c->sadb_lifetime_allocations = 0;
 	sav->lft_c->sadb_lifetime_bytes = 0;
@@ -2040,43 +2860,82 @@ key_setsaval(sav, mhp)
 
 	/* lifetimes for HARD and SOFT */
     {
-	struct sadb_lifetime *lft0;
+	const struct sadb_lifetime *lft0;
 
-	lft0 = (struct sadb_lifetime *)mhp[SADB_EXT_LIFETIME_HARD];
+	lft0 = (struct sadb_lifetime *)mhp->ext[SADB_EXT_LIFETIME_HARD];
 	if (lft0 != NULL) {
-		KEY_NEWBUF(sav->lft_h, struct sadb_lifetime *,
-		           lft0, sizeof(*lft0));
+		if (mhp->extlen[SADB_EXT_LIFETIME_HARD] < sizeof(*lft0)) {
+			error = EINVAL;
+			goto fail;
+		}
+		sav->lft_h = (struct sadb_lifetime *)key_newbuf(lft0,
+		    sizeof(*lft0));
 		if (sav->lft_h == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: No more memory.\n");
+#endif
 			error = ENOBUFS;
-			goto err;
+			goto fail;
 		}
 		/* to be initialize ? */
 	}
 
-	lft0 = (struct sadb_lifetime *)mhp[SADB_EXT_LIFETIME_SOFT];
+	lft0 = (struct sadb_lifetime *)mhp->ext[SADB_EXT_LIFETIME_SOFT];
 	if (lft0 != NULL) {
-		KEY_NEWBUF(sav->lft_s, struct sadb_lifetime *,
-		           lft0, sizeof(*lft0));
+		if (mhp->extlen[SADB_EXT_LIFETIME_SOFT] < sizeof(*lft0)) {
+			error = EINVAL;
+			goto fail;
+		}
+		sav->lft_s = (struct sadb_lifetime *)key_newbuf(lft0,
+		    sizeof(*lft0));
 		if (sav->lft_s == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_setsaval: No more memory.\n");
+#endif
 			error = ENOBUFS;
-			goto err;
+			goto fail;
 		}
 		/* to be initialize ? */
 	}
     }
 
-	msg0->sadb_msg_errno = 0;
+#if notyet
+	/* pre-processing for DES */
+	switch (sav->alg_enc) {
+	case SADB_EALG_DESCBC:
+        	if (des_key_sched((C_Block *)_KEYBUF(sav->key_enc),
+		                  (des_key_schedule)sav->misc1) != 0) {
+#ifdef IPSEC_DEBUG
+			printf("key_setsaval: error des_key_sched.\n");
+#endif
+			sav->misc1 = NULL;
+			/* THROUGH */
+		}
+		break;
+	case SADB_EALG_3DESCBC:
+        	if (des_key_sched((C_Block *)_KEYBUF(sav->key_enc),
+		                  (des_key_schedule)sav->misc1) != 0
+        	 || des_key_sched((C_Block *)(_KEYBUF(sav->key_enc) + 8),
+		                  (des_key_schedule)sav->misc2) != 0
+        	 || des_key_sched((C_Block *)(_KEYBUF(sav->key_enc) + 16),
+		                  (des_key_schedule)sav->misc3) != 0) {
+#ifdef IPSEC_DEBUG
+			printf("key_setsaval: error des_key_sched.\n");
+#endif
+			sav->misc1 = NULL;
+			sav->misc2 = NULL;
+			sav->misc3 = NULL;
+			/* THROUGH */
+		}
+	}
+#endif
+
 	return 0;
 
-    err:
+ fail:
 	/* initialization */
-	if (sav->replay != NULL) {
-		if (sav->replay->bitmap != NULL)
-			KFREE(sav->replay->bitmap);
-		KFREE(sav->replay);
-	}
+	if (sav->replay != NULL)
+		keydb_delsecreplay(sav->replay);
 	if (sav->key_auth != NULL)
 		KFREE(sav->key_auth);
 	if (sav->key_enc != NULL)
@@ -2089,39 +2948,16 @@ key_setsaval(sav, mhp)
 		KFREE(sav->lft_h);
 	if (sav->lft_s != NULL)
 		KFREE(sav->lft_s);
+#if notyet
+	if (sav->misc1 != NULL)
+		KFREE(sav->misc1);
+	if (sav->misc2 != NULL)
+		KFREE(sav->misc2);
+	if (sav->misc3 != NULL)
+		KFREE(sav->misc3);
+#endif
 
-	msg0->sadb_msg_errno = error;
-	return 1;
-}
-
-/*
- * get message buffer length.
- */
-static u_int
-key_getmsglen(sav)
-	struct secasvar *sav;
-{
-	int len = sizeof(struct sadb_msg);
-
-	len += sizeof(struct sadb_sa);
-	len += (sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(_SALENBYAF(sav->sah->saidx.src.ss_family)));
-	len += (sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(_SALENBYAF(sav->sah->saidx.dst.ss_family)));
-
-	if (sav->key_auth != NULL)
-		len += sav->key_auth->sadb_key_len;
-	if (sav->key_enc != NULL)
-		len += sav->key_enc->sadb_key_len;
-
-	if (sav->lft_c != NULL)
-		len += sizeof(struct sadb_lifetime);
-	if (sav->lft_h != NULL)
-		len += sizeof(struct sadb_lifetime);
-	if (sav->lft_s != NULL)
-		len += sizeof(struct sadb_lifetime);
-
-	return len;
+	return error;
 }
 
 /*
@@ -2141,7 +2977,9 @@ key_mature(sav)
 
 	/* check SPI value */
 	if (ntohl(sav->spi) >= 0 && ntohl(sav->spi) <= 255) {
+#ifdef IPSEC_DEBUG
 		printf("key_mature: illegal range of SPI %d.\n", sav->spi);
+#endif
 		return EINVAL;
 	}
 
@@ -2151,8 +2989,10 @@ key_mature(sav)
 		/* check flags */
 		if ((sav->flags & SADB_X_EXT_OLD)
 		 && (sav->flags & SADB_X_EXT_DERIV)) {
+#ifdef IPSEC_DEBUG
 			printf("key_mature: "
 				"invalid flag (derived) given to old-esp.\n");
+#endif
 			return EINVAL;
 		}
 		checkmask = 3;
@@ -2161,20 +3001,46 @@ key_mature(sav)
 	case IPPROTO_AH:
 		/* check flags */
 		if (sav->flags & SADB_X_EXT_DERIV) {
+#ifdef IPSEC_DEBUG
 			printf("key_mature: "
 				"invalid flag (derived) given to AH SA.\n");
+#endif
 			return EINVAL;
 		}
 		if (sav->alg_enc != SADB_EALG_NONE) {
+#ifdef IPSEC_DEBUG
 			printf("key_mature: "
 				"protocol and algorithm mismated.\n");
+#endif
 			return(EINVAL);
 		}
 		checkmask = 2;
 		mustmask = 2;
 		break;
+#if 1	/*nonstandard*/
+	case IPPROTO_IPCOMP:
+		if (sav->alg_auth != SADB_AALG_NONE) {
+#ifdef IPSEC_DEBUG
+			printf("key_mature: "
+				"protocol and algorithm mismated.\n");
+#endif
+			return(EINVAL);
+		}
+		if ((sav->flags & SADB_X_EXT_RAWCPI) == 0
+		 && ntohl(sav->spi) >= 0x10000) {
+#ifdef IPSEC_DEBUG
+			printf("key_mature: invalid cpi for IPComp.\n");
+#endif
+			return(EINVAL);
+		}
+		checkmask = 4;
+		mustmask = 4;
+		break;
+#endif
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_mature: Invalid satype.\n");
+#endif
 		return EPROTONOSUPPORT;
 	}
 
@@ -2193,8 +3059,10 @@ key_mature(sav)
 		case SADB_AALG_NULL:
 			break;
 		default:
+#ifdef IPSEC_DEBUG
 			printf("key_mature: "
 				"unknown authentication algorithm.\n");
+#endif
 			return EINVAL;
 		}
 
@@ -2206,9 +3074,11 @@ key_mature(sav)
 		else
 			keylen = 0;
 		if (keylen < algo->keymin || algo->keymax < keylen) {
+#ifdef IPSEC_DEBUG
 			printf("key_mature: invalid AH key length %d "
 				"(%d-%d allowed)\n", keylen,
 				algo->keymin, algo->keymax);
+#endif
 			return EINVAL;
 		}
 
@@ -2220,8 +3090,12 @@ key_mature(sav)
 				mature = SADB_SATYPE_AH;
 		}
 
-		if ((mustmask & 2) != 0 &&  mature != SADB_SATYPE_AH)
+		if ((mustmask & 2) != 0 &&  mature != SADB_SATYPE_AH) {
+#ifdef IPSEC_DEBUG
+			printf("key_mature: no satisfy algorithm for AH\n");
+#endif
 			return EINVAL;
+		}
 	}
 
 	/* check encryption algorithm */
@@ -2240,8 +3114,10 @@ key_mature(sav)
 		case SADB_EALG_RC5CBC:
 			break;
 		default:
+#ifdef IPSEC_DEBUG
 			printf("key_mature: unknown encryption algorithm.\n");
-			return(EINVAL);
+#endif
+			return EINVAL;
 		}
 
 		/* algorithm-dependent check */
@@ -2252,9 +3128,11 @@ key_mature(sav)
 		else
 			keylen = 0;
 		if (keylen < algo->keymin || algo->keymax < keylen) {
+#ifdef IPSEC_DEBUG
 			printf("key_mature: invalid ESP key length %d "
 				"(%d-%d allowed)\n", keylen,
 				algo->keymin, algo->keymax);
+#endif
 			return EINVAL;
 		}
 
@@ -2266,14 +3144,49 @@ key_mature(sav)
 				mature = SADB_SATYPE_ESP;
 		}
 
-		if ((mustmask & 1) != 0 &&  mature != SADB_SATYPE_ESP)
+		if ((mustmask & 1) != 0 &&  mature != SADB_SATYPE_ESP) {
+#ifdef IPSEC_DEBUG
+			printf("key_mature: no satisfy algorithm for ESP\n");
+#endif
 			return EINVAL;
-#else
+		}
+#else /*IPSEC_ESP*/
+#ifdef IPSEC_DEBUG
 		printf("key_mature: ESP not supported in this configuration\n");
+#endif
 		return EINVAL;
 #endif
 	}
 
+	/* check compression algorithm */
+	if ((checkmask & 4) != 0) {
+		struct ipcomp_algorithm *algo;
+
+		switch (sav->alg_enc) {
+		case SADB_X_CALG_NONE:
+		case SADB_X_CALG_OUI:
+		case SADB_X_CALG_DEFLATE:
+		case SADB_X_CALG_LZS:
+			break;
+		default:
+#ifdef IPSEC_DEBUG
+			printf("key_mature: unknown compression algorithm.\n");
+#endif
+			return EINVAL;
+		}
+
+		/* algorithm-dependent check */
+		algo = &ipcomp_algorithms[sav->alg_enc];
+
+		if (!(algo->compress && algo->decompress)) {
+#ifdef IPSEC_DEBUG
+			printf("key_mature: "
+				"unsupported compression algorithm.\n");
+#endif
+			return EINVAL;
+		}
+	}
+
 	key_sa_chgstate(sav, SADB_SASTATE_MATURE);
 
 	return 0;
@@ -2281,115 +3194,184 @@ key_mature(sav)
 
 /*
  * subroutine for SADB_GET and SADB_DUMP.
- * the buf must be allocated sufficent space.
  */
-static u_int
-key_setdumpsa(newmsg, sav, type, satype, seq, pid)
-	struct sadb_msg *newmsg;
+static struct mbuf *
+key_setdumpsa(sav, type, satype, seq, pid)
 	struct secasvar *sav;
 	u_int8_t type, satype;
 	u_int32_t seq, pid;
 {
-	u_int tlen;
-	caddr_t p;
+	struct mbuf *result = NULL, *tres = NULL, *m;
+	int l = 0;
 	int i;
-
-	tlen = key_getmsglen(sav);
-
-	p = key_setsadbmsg((caddr_t)newmsg, type, tlen,
-	                   satype, seq, pid,
-	                   sav->sah->saidx.mode, sav->refcnt);
-
-	for (i = 1; i <= SADB_EXT_MAX; i++) {
-		switch (i) {
+	void *p;
+	int dumporder[] = {
+		SADB_EXT_SA, SADB_X_EXT_SA2,
+		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,
+	};
+
+	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--) {
+		m = NULL;
+		p = NULL;
+		switch (dumporder[i]) {
 		case SADB_EXT_SA:
-			p = key_setsadbsa(p, sav);
+			m = key_setsadbsa(sav);
+			if (!m)
+				goto fail;
+			break;
+
+		case SADB_X_EXT_SA2:
+			m = key_setsadbxsa2(sav->sah->saidx.mode,
+					sav->sah->saidx.reqid);
+			if (!m)
+				goto fail;
 			break;
 
 		case SADB_EXT_ADDRESS_SRC:
-			p = key_setsadbaddr(p,
-			      SADB_EXT_ADDRESS_SRC,
-			      (struct sockaddr *)&sav->sah->saidx.src,
-			      _INALENBYAF(sav->sah->saidx.src.ss_family) << 3,
-			      IPSEC_ULPROTO_ANY);
+			m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
+			    (struct sockaddr *)&sav->sah->saidx.src,
+			    sav->sah->saidx.src.ss_len << 3, IPSEC_ULPROTO_ANY);
+			if (!m)
+				goto fail;
 			break;
 
 		case SADB_EXT_ADDRESS_DST:
-			p = key_setsadbaddr(p,
-			      SADB_EXT_ADDRESS_DST,
-			      (struct sockaddr *)&sav->sah->saidx.dst,
-			      _INALENBYAF(sav->sah->saidx.dst.ss_family) << 3,
-			      IPSEC_ULPROTO_ANY);
+			m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
+			    (struct sockaddr *)&sav->sah->saidx.dst,
+			    sav->sah->saidx.dst.ss_len << 3, IPSEC_ULPROTO_ANY);
+			if (!m)
+				goto fail;
 			break;
 
 		case SADB_EXT_KEY_AUTH:
-		    {
-			u_int len;
-			if (sav->key_auth == NULL) break;
-			len = sav->key_auth->sadb_key_len; /* real length */
-			bcopy((caddr_t)sav->key_auth, p, len);
-			((struct sadb_ext *)p)->sadb_ext_len = PFKEY_UNIT64(len);
-			p += len;
-		    }
+			if (!sav->key_auth)
+				continue;
+			l = PFKEY_UNUNIT64(sav->key_auth->sadb_key_len);
+			p = sav->key_auth;
 			break;
 
 		case SADB_EXT_KEY_ENCRYPT:
-		    {
-			u_int len;
-			if (sav->key_enc == NULL) break;
-			len = sav->key_enc->sadb_key_len; /* real length */
-			bcopy((caddr_t)sav->key_enc, p, len);
-			((struct sadb_ext *)p)->sadb_ext_len = PFKEY_UNIT64(len);
-			p += len;
-		    }
-			break;;
+			if (!sav->key_enc)
+				continue;
+			l = PFKEY_UNUNIT64(sav->key_enc->sadb_key_len);
+			p = sav->key_enc;
+			break;
 
 		case SADB_EXT_LIFETIME_CURRENT:
-			if (sav->lft_c == NULL) break;
-			p = key_setsadbext(p, (caddr_t)sav->lft_c);
+			if (!sav->lft_c)
+				continue;
+			l = PFKEY_UNUNIT64(((struct sadb_ext *)sav->lft_c)->sadb_ext_len);
+			p = sav->lft_c;
 			break;
 
 		case SADB_EXT_LIFETIME_HARD:
-			if (sav->lft_h == NULL) break;
-			p = key_setsadbext(p, (caddr_t)sav->lft_h);
+			if (!sav->lft_h)
+				continue;
+			l = PFKEY_UNUNIT64(((struct sadb_ext *)sav->lft_h)->sadb_ext_len);
+			p = sav->lft_h;
 			break;
 
 		case SADB_EXT_LIFETIME_SOFT:
-			if (sav->lft_s == NULL) break;
-			p = key_setsadbext(p, (caddr_t)sav->lft_s);
+			if (!sav->lft_s)
+				continue;
+			l = PFKEY_UNUNIT64(((struct sadb_ext *)sav->lft_s)->sadb_ext_len);
+			p = sav->lft_s;
 			break;
 
+		case SADB_EXT_ADDRESS_PROXY:
 		case SADB_EXT_IDENTITY_SRC:
 		case SADB_EXT_IDENTITY_DST:
 			/* XXX: should we brought from SPD ? */
 		case SADB_EXT_SENSITIVITY:
 		default:
-			break;
+			continue;
 		}
+
+		if ((!m && !p) || (m && p))
+			goto fail;
+		if (p && tres) {
+			M_PREPEND(tres, l, M_DONTWAIT);
+			if (!tres)
+				goto fail;
+			bcopy(p, mtod(tres, caddr_t), l);
+			continue;
+		}
+		if (p) {
+			m = key_alloc_mbuf(l);
+			if (!m)
+				goto fail;
+			m_copyback(m, 0, l, p);
+		}
+
+		if (tres)
+			m_cat(m, tres);
+		tres = m;
 	}
 
-	return tlen;
+	m_cat(result, tres);
+
+	if (result->m_len < sizeof(struct sadb_msg)) {
+		result = m_pullup(result, sizeof(struct sadb_msg));
+		if (result == NULL)
+			goto fail;
+	}
+
+	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);
+
+	return result;
+
+fail:
+	m_freem(result);
+	m_freem(tres);
+	return NULL;
 }
 
 /*
  * set data into sadb_msg.
- * `buf' must has been allocated sufficiently.
  */
-static caddr_t
-key_setsadbmsg(buf, type, tlen, satype, seq, pid, reserved1, reserved2)
-	caddr_t buf;
+static struct mbuf *
+key_setsadbmsg(type, tlen, satype, seq, pid, reserved)
 	u_int8_t type, satype;
 	u_int16_t tlen;
 	u_int32_t seq;
 	pid_t pid;
-	u_int8_t reserved1;
-	u_int8_t reserved2;
+	u_int16_t reserved;
 {
+	struct mbuf *m;
 	struct sadb_msg *p;
-	u_int len;
+	int len;
+
+	len = PFKEY_ALIGN8(sizeof(struct sadb_msg));
+	if (len > MCLBYTES)
+		return NULL;
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m && len > MHLEN) {
+		MCLGET(m, M_DONTWAIT);
+		if ((m->m_flags & M_EXT) == 0) {
+			m_freem(m);
+			m = NULL;
+		}
+	}
+	if (!m)
+		return NULL;
+	m->m_pkthdr.len = m->m_len = len;
+	m->m_next = NULL;
 
-	p = (struct sadb_msg *)buf;
-	len = sizeof(struct sadb_msg);
+	p = mtod(m, struct sadb_msg *);
 
 	bzero(p, len);
 	p->sadb_msg_version = PF_KEY_V2;
@@ -2397,28 +3379,33 @@ key_setsadbmsg(buf, type, tlen, satype, seq, pid, reserved1, reserved2)
 	p->sadb_msg_errno = 0;
 	p->sadb_msg_satype = satype;
 	p->sadb_msg_len = PFKEY_UNIT64(tlen);
-	p->sadb_msg_mode = reserved1;
-	p->sadb_msg_reserved = reserved2;
+	p->sadb_msg_reserved = reserved;
 	p->sadb_msg_seq = seq;
 	p->sadb_msg_pid = (u_int32_t)pid;
 
-	return(buf + len);
+	return m;
 }
 
 /*
  * copy secasvar data into sadb_address.
- * `buf' must has been allocated sufficiently.
  */
-static caddr_t
-key_setsadbsa(buf, sav)
-	caddr_t buf;
+static struct mbuf *
+key_setsadbsa(sav)
 	struct secasvar *sav;
 {
+	struct mbuf *m;
 	struct sadb_sa *p;
-	u_int len;
+	int len;
 
-	p = (struct sadb_sa *)buf;
-	len = sizeof(struct sadb_sa);
+	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;
+	}
+
+	p = mtod(m, struct sadb_sa *);
 
 	bzero(p, len);
 	p->sadb_sa_len = PFKEY_UNIT64(len);
@@ -2430,26 +3417,33 @@ key_setsadbsa(buf, sav)
 	p->sadb_sa_encrypt = sav->alg_enc;
 	p->sadb_sa_flags = sav->flags;
 
-	return(buf + len);
+	return m;
 }
 
 /*
  * set data into sadb_address.
- * `buf' must has been allocated sufficiently.
  */
-static caddr_t
-key_setsadbaddr(buf, exttype, saddr, prefixlen, ul_proto)
-	caddr_t buf;
+static struct mbuf *
+key_setsadbaddr(exttype, saddr, prefixlen, ul_proto)
 	u_int16_t exttype;
 	struct sockaddr *saddr;
 	u_int8_t prefixlen;
 	u_int16_t ul_proto;
 {
+	struct mbuf *m;
 	struct sadb_address *p;
-	u_int len;
+	size_t len;
+
+	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;
+	}
 
-	p = (struct sadb_address *)buf;
-	len = sizeof(struct sadb_address) + PFKEY_ALIGN8(saddr->sa_len);
+	p = mtod(m, struct sadb_address *);
 
 	bzero(p, len);
 	p->sadb_address_len = PFKEY_UNIT64(len);
@@ -2458,28 +3452,37 @@ key_setsadbaddr(buf, exttype, saddr, prefixlen, ul_proto)
 	p->sadb_address_prefixlen = prefixlen;
 	p->sadb_address_reserved = 0;
 
-	bcopy(saddr, p + 1, saddr->sa_len);
+	bcopy(saddr,
+	    mtod(m, caddr_t) + PFKEY_ALIGN8(sizeof(struct sadb_address)),
+	    saddr->sa_len);
 
-	return(buf + len);
+	return m;
 }
 
+#if 0
 /*
  * set data into sadb_ident.
- * `buf' must has been allocated sufficiently.
  */
-static caddr_t
-key_setsadbident(buf, exttype, idtype, string, stringlen, id)
-	caddr_t buf;
+static struct mbuf *
+key_setsadbident(exttype, idtype, string, stringlen, id)
 	u_int16_t exttype, idtype;
 	caddr_t string;
 	int stringlen;
 	u_int64_t id;
 {
+	struct mbuf *m;
 	struct sadb_ident *p;
-	u_int len;
+	size_t len;
 
-	p = (struct sadb_ident *)buf;
-	len = sizeof(struct sadb_ident) + PFKEY_ALIGN8(stringlen);
+	len = PFKEY_ALIGN8(sizeof(struct sadb_ident)) + PFKEY_ALIGN8(stringlen);
+	m = key_alloc_mbuf(len);
+	if (!m || m->m_next) {	/*XXX*/
+		if (m)
+			m_freem(m);
+		return NULL;
+	}
+
+	p = mtod(m, struct sadb_ident *);
 
 	bzero(p, len);
 	p->sadb_ident_len = PFKEY_UNIT64(len);
@@ -2488,27 +3491,79 @@ key_setsadbident(buf, exttype, idtype, string, stringlen, id)
 	p->sadb_ident_reserved = 0;
 	p->sadb_ident_id = id;
 
-	bcopy(string, p + 1, stringlen);
+	bcopy(string,
+	    mtod(m, caddr_t) + PFKEY_ALIGN8(sizeof(struct sadb_ident)),
+	    stringlen);
 
-	return(buf + len);
+	return m;
 }
+#endif
 
 /*
- * copy buffer of any sadb extension type into sadb_ext.
- * assume that sadb_ext_len shifted down >> 3.
- * i.e. shift length up when setting length of extension.
+ * set data into sadb_x_sa2.
  */
-static caddr_t
-key_setsadbext(p, ext)
-	caddr_t p, ext;
+static struct mbuf *
+key_setsadbxsa2(mode, reqid)
+	u_int8_t mode;
+	u_int32_t reqid;
 {
-	u_int len;
+	struct mbuf *m;
+	struct sadb_x_sa2 *p;
+	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;
+	}
+
+	p = mtod(m, struct sadb_x_sa2 *);
+
+	bzero(p, len);
+	p->sadb_x_sa2_len = PFKEY_UNIT64(len);
+	p->sadb_x_sa2_exttype = SADB_X_EXT_SA2;
+	p->sadb_x_sa2_mode = mode;
+	p->sadb_x_sa2_reserved1 = 0;
+	p->sadb_x_sa2_reserved2 = 0;
+	p->sadb_x_sa2_reserved3 = 0;
+	p->sadb_x_sa2_reqid = reqid;
+
+	return m;
+}
+
+/*
+ * set data into sadb_x_policy
+ */
+static struct mbuf *
+key_setsadbxpolicy(type, dir, id)
+	u_int16_t type;
+	u_int8_t dir;
+	u_int32_t id;
+{
+	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;
+	}
 
-	len = PFKEY_UNUNIT64(((struct sadb_ext *)ext)->sadb_ext_len);
+	p = mtod(m, struct sadb_x_policy *);
 
-	bcopy(ext, p, len);
+	bzero(p, len);
+	p->sadb_x_policy_len = PFKEY_UNIT64(len);
+	p->sadb_x_policy_exttype = SADB_X_EXT_POLICY;
+	p->sadb_x_policy_type = type;
+	p->sadb_x_policy_dir = dir;
+	p->sadb_x_policy_id = id;
 
-	return(p + len);
+	return m;
 }
 
 /* %%% utilities */
@@ -2517,17 +3572,19 @@ key_setsadbext(p, ext)
  */
 static void *
 key_newbuf(src, len)
-	void *src;
+	const void *src;
 	u_int len;
 {
 	caddr_t new;
 
 	KMALLOC(new, caddr_t, len);
 	if (new == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_newbuf: No more memory.\n");
+#endif
 		return NULL;
 	}
-	bcopy((caddr_t)src, new, len);
+	bcopy(src, new, len);
 
 	return new;
 }
@@ -2537,30 +3594,37 @@ key_newbuf(src, len)
  *	0: false
  */
 int
-key_ismyaddr(family, addr)
-	u_int family;
-	caddr_t addr;
+key_ismyaddr(sa)
+	struct sockaddr *sa;
 {
+#ifdef INET
+	struct sockaddr_in *sin;
+	struct in_ifaddr *ia;
+#endif
+
 	/* sanity check */
-	if (addr == NULL)
+	if (sa == NULL)
 		panic("key_ismyaddr: NULL pointer is passed.\n");
 
-	switch (family) {
+	switch (sa->sa_family) {
+#ifdef INET
 	case AF_INET:
-	{
-		struct in_ifaddr *ia;
-
+		sin = (struct sockaddr_in *)sa;
 		for (ia = in_ifaddrhead.tqh_first; ia;
 		     ia = ia->ia_link.tqe_next)
-			if (bcmp(addr,
-			        (caddr_t)&ia->ia_addr.sin_addr,
-			        _INALENBYAF(family)) == 0)
+		{
+			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)
+			{
 				return 1;
-	}
+			}
+		}
 		break;
+#endif
 #ifdef INET6
 	case AF_INET6:
-		return key_ismyaddr6(addr);
+		return key_ismyaddr6((struct sockaddr_in6 *)sa);
 #endif
 	}
 
@@ -2574,38 +3638,36 @@ key_ismyaddr(family, addr)
  * 0: other
  * NOTE: derived ip6_input() in KAME. This is necessary to modify more.
  */
-#include <netinet6/in6.h>
 #include <netinet6/in6_var.h>
 
 static int
-key_ismyaddr6(addr)
-	caddr_t addr;
+key_ismyaddr6(sin6)
+	struct sockaddr_in6 *sin6;
 {
-	struct in6_addr *a = (struct in6_addr *)addr;
 	struct in6_ifaddr *ia;
+	struct in6_multi *in6m;
 
 	for (ia = in6_ifaddr; ia; ia = ia->ia_next) {
-		if (bcmp(addr, (caddr_t)&ia->ia_addr.sin6_addr,
-				_INALENBYAF(AF_INET6)) == 0) {
+		if (key_sockaddrcmp((struct sockaddr *)&sin6,
+		    (struct sockaddr *)&ia->ia_addr, 0) == 0)
 			return 1;
-		}
 
-		/* XXX Multicast */
-	    {
-	  	struct	in6_multi *in6m = 0;
-
-		IN6_LOOKUP_MULTI(*(struct in6_addr *)addr, ia->ia_ifp, in6m);
+		/*
+		 * 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)
 			return 1;
-	    }
 	}
 
 	/* loopback, just for safety */
-	if (IN6_IS_ADDR_LOOPBACK(a))
+	if (IN6_IS_ADDR_LOOPBACK(&sin6->sin6_addr))
 		return 1;
 
-	/* XXX anycast */
-
 	return 0;
 }
 #endif /*INET6*/
@@ -2614,7 +3676,7 @@ key_ismyaddr6(addr)
  * compare two secasindex structure exactly.
  * IN:
  *	saidx0: source, it can be in SAD.
- *	saidx1: object, it can be from SPD.
+ *	saidx1: object.
  * OUT:
  *	1 : equal
  *	0 : not equal
@@ -2631,11 +3693,12 @@ key_cmpsaidx_exactly(saidx0, saidx1)
 		return 0;
 
 	if (saidx0->proto != saidx1->proto
-	 || saidx0->mode != saidx1->mode)
+	 || saidx0->mode != saidx1->mode
+	 || saidx0->reqid != saidx1->reqid)
 		return 0;
 
-	if (bcmp(&saidx0->src, &saidx1->src, saidx0->src.ss_len) != 0
-	 || bcmp(&saidx0->dst, &saidx1->dst, saidx0->dst.ss_len) != 0)
+	if (bcmp(&saidx0->src, &saidx1->src, saidx0->src.ss_len) != 0 ||
+	    bcmp(&saidx0->dst, &saidx1->dst, saidx0->dst.ss_len) != 0)
 		return 0;
 
 	return 1;
@@ -2662,22 +3725,63 @@ key_cmpsaidx_withmode(saidx0, saidx1)
 	if (saidx0 == NULL || saidx1 == NULL)
 		return 0;
 
-	if (saidx0->proto != saidx1->proto
-	 || saidx0->src.ss_family != saidx1->src.ss_family
-	 || saidx0->dst.ss_family != saidx1->dst.ss_family)
+	if (saidx0->proto != saidx1->proto)
 		return 0;
 
-	if (saidx0->mode != IPSEC_MODE_ANY
-	 && saidx0->mode != saidx1->mode)
+	/*
+	 * 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)
 		return 0;
 
-    {
-	int sa_len = _INALENBYAF(saidx0->src.ss_family);
+	if (saidx0->mode != IPSEC_MODE_ANY && saidx0->mode != saidx1->mode)
+		return 0;
 
-	if (bcmp(_INADDRBYSA(&saidx0->src), _INADDRBYSA(&saidx1->src), sa_len)
-	 || bcmp(_INADDRBYSA(&saidx0->dst), _INADDRBYSA(&saidx1->dst), sa_len))
+	if (key_sockaddrcmp((struct sockaddr *)&saidx0->src,
+	    (struct sockaddr *)&saidx1->src, 0) != 0) {
+		return 0;
+	}
+	if (key_sockaddrcmp((struct sockaddr *)&saidx0->dst,
+	    (struct sockaddr *)&saidx1->dst, 0) != 0) {
+		return 0;
+	}
+
+	return 1;
+}
+
+/*
+ * compare two secasindex structure without mode.
+ * don't compare port.
+ * IN:
+ *	saidx0: source, it is often in SAD.
+ *	saidx1: object, it is often from user.
+ * OUT:
+ *	1 : equal
+ *	0 : not equal
+ */
+static int
+key_cmpsaidx_withoutmode(saidx0, saidx1)
+	struct secasindex *saidx0, *saidx1;
+{
+	/* sanity */
+	if (saidx0 == NULL && saidx1 == NULL)
+		return 1;
+
+	if (saidx0 == NULL || saidx1 == NULL)
+		return 0;
+
+	if (saidx0->proto != saidx1->proto)
 		return 0;
-    }
+
+	if (key_sockaddrcmp((struct sockaddr *)&saidx0->src,
+	    (struct sockaddr *)&saidx1->src, 0) != 0) {
+		return 0;
+	}
+	if (key_sockaddrcmp((struct sockaddr *)&saidx0->dst,
+	    (struct sockaddr *)&saidx1->dst, 0) != 0) {
+		return 0;
+	}
 
 	return 1;
 }
@@ -2707,9 +3811,14 @@ key_cmpspidx_exactly(spidx0, spidx1)
 	 || spidx0->ul_proto != spidx1->ul_proto)
 		return 0;
 
-	if (bcmp(&spidx0->src, &spidx1->src, spidx0->src.ss_len) != 0
-	 || bcmp(&spidx0->dst, &spidx1->dst, spidx0->dst.ss_len) != 0)
+	if (key_sockaddrcmp((struct sockaddr *)&spidx0->src,
+	    (struct sockaddr *)&spidx1->src, 1) != 0) {
+		return 0;
+	}
+	if (key_sockaddrcmp((struct sockaddr *)&spidx0->dst,
+	    (struct sockaddr *)&spidx1->dst, 1) != 0) {
 		return 0;
+	}
 
 	return 1;
 }
@@ -2734,8 +3843,10 @@ key_cmpspidx_withmask(spidx0, spidx1)
 	if (spidx0 == NULL || spidx1 == NULL)
 		return 0;
 
-	if (spidx0->src.ss_family != spidx1->src.ss_family
-	 || spidx0->dst.ss_family != spidx1->dst.ss_family)
+	if (spidx0->src.ss_family != spidx1->src.ss_family ||
+	    spidx0->dst.ss_family != spidx1->dst.ss_family ||
+	    spidx0->src.ss_len != spidx1->src.ss_len ||
+	    spidx0->dst.ss_len != spidx1->dst.ss_len)
 		return 0;
 
 	/* if spidx.ul_proto == IPSEC_ULPROTO_ANY, ignore. */
@@ -2743,27 +3854,112 @@ key_cmpspidx_withmask(spidx0, spidx1)
 	 && spidx0->ul_proto != spidx1->ul_proto)
 		return 0;
 
-	if (_INPORTBYSA(&spidx0->src) != IPSEC_PORT_ANY
-	 && _INPORTBYSA(&spidx0->src) != _INPORTBYSA(&spidx1->src))
-		return 0;
+	switch (spidx0->src.ss_family) {
+	case AF_INET:
+		if (satosin(&spidx0->src)->sin_port != IPSEC_PORT_ANY
+		 && satosin(&spidx0->src)->sin_port !=
+		    satosin(&spidx1->src)->sin_port)
+			return 0;
+		if (!key_bbcmp((caddr_t)&satosin(&spidx0->src)->sin_addr,
+		    (caddr_t)&satosin(&spidx1->src)->sin_addr, spidx0->prefs))
+			return 0;
+		break;
+	case AF_INET6:
+		if (satosin6(&spidx0->src)->sin6_port != IPSEC_PORT_ANY
+		 && satosin6(&spidx0->src)->sin6_port !=
+		    satosin6(&spidx1->src)->sin6_port)
+			return 0;
+		if (satosin6(&spidx0->src)->sin6_scope_id !=
+		    satosin6(&spidx1->src)->sin6_scope_id)
+			return 0;
+		if (!key_bbcmp((caddr_t)&satosin6(&spidx0->src)->sin6_addr,
+		    (caddr_t)&satosin6(&spidx1->src)->sin6_addr, spidx0->prefs))
+			return 0;
+		break;
+	default:
+		/* XXX */
+		if (bcmp(&spidx0->src, &spidx1->src, spidx0->src.ss_len) != 0)
+			return 0;
+		break;
+	}
 
-	if (_INPORTBYSA(&spidx0->dst) != IPSEC_PORT_ANY
-	 && _INPORTBYSA(&spidx0->dst) != _INPORTBYSA(&spidx1->dst))
-		return 0;
+	switch (spidx0->dst.ss_family) {
+	case AF_INET:
+		if (satosin(&spidx0->dst)->sin_port != IPSEC_PORT_ANY
+		 && satosin(&spidx0->dst)->sin_port !=
+		    satosin(&spidx1->dst)->sin_port)
+			return 0;
+		if (!key_bbcmp((caddr_t)&satosin(&spidx0->dst)->sin_addr,
+		    (caddr_t)&satosin(&spidx1->dst)->sin_addr, spidx0->prefd))
+			return 0;
+		break;
+	case AF_INET6:
+		if (satosin6(&spidx0->dst)->sin6_port != IPSEC_PORT_ANY
+		 && satosin6(&spidx0->dst)->sin6_port !=
+		    satosin6(&spidx1->dst)->sin6_port)
+			return 0;
+		if (satosin6(&spidx0->dst)->sin6_scope_id !=
+		    satosin6(&spidx1->dst)->sin6_scope_id)
+			return 0;
+		if (!key_bbcmp((caddr_t)&satosin6(&spidx0->dst)->sin6_addr,
+		    (caddr_t)&satosin6(&spidx1->dst)->sin6_addr, spidx0->prefd))
+			return 0;
+		break;
+	default:
+		/* XXX */
+		if (bcmp(&spidx0->dst, &spidx1->dst, spidx0->dst.ss_len) != 0)
+			return 0;
+		break;
+	}
 
-	if (!key_bbcmp(_INADDRBYSA(&spidx0->src),
-	               _INADDRBYSA(&spidx1->src),
-	               spidx0->prefs))
-		return 0;
+	/* XXX Do we check other field ?  e.g. flowinfo */
 
-	if (!key_bbcmp(_INADDRBYSA(&spidx0->dst),
-	               _INADDRBYSA(&spidx1->dst),
-	               spidx0->prefd))
-		return 0;
+	return 1;
+}
 
-	/* XXX Do we check other field ?  e.g. flowinfo, scope_id. */
+/* returns 0 on match */
+static int
+key_sockaddrcmp(sa1, sa2, port)
+	struct sockaddr *sa1;
+	struct sockaddr *sa2;
+	int port;
+{
+	if (sa1->sa_family != sa2->sa_family || sa1->sa_len != sa2->sa_len)
+		return 1;
 
-	return 1;
+	switch (sa1->sa_family) {
+	case AF_INET:
+		if (sa1->sa_len != sizeof(struct sockaddr_in))
+			return 1;
+		if (satosin(sa1)->sin_addr.s_addr !=
+		    satosin(sa2)->sin_addr.s_addr) {
+			return 1;
+		}
+		if (port && satosin(sa1)->sin_port != satosin(sa2)->sin_port)
+			return 1;
+		break;
+	case AF_INET6:
+		if (sa1->sa_len != sizeof(struct sockaddr_in6))
+			return 1;	/*EINVAL*/
+		if (satosin6(sa1)->sin6_scope_id !=
+		    satosin6(sa2)->sin6_scope_id) {
+			return 1;
+		}
+		if (!IN6_ARE_ADDR_EQUAL(&satosin6(sa1)->sin6_addr,
+		    &satosin6(sa2)->sin6_addr)) {
+			return 1;
+		}
+		if (port &&
+		    satosin6(sa1)->sin6_port != satosin6(sa2)->sin6_port) {
+			return 1;
+		}
+	default:
+		if (bcmp(sa1, sa2, sa1->sa_len) != 0)
+			return 1;
+		break;
+	}
+
+	return 0;
 }
 
 /*
@@ -2778,8 +3974,8 @@ key_cmpspidx_withmask(spidx0, spidx1)
  */
 static int
 key_bbcmp(p1, p2, bits)
-	register caddr_t p1, p2;
-	register u_int bits;
+	caddr_t p1, p2;
+	u_int bits;
 {
 	u_int8_t mask;
 
@@ -2883,8 +4079,10 @@ key_timehandler(void)
 
 			/* sanity check */
 			if (sav->lft_c == NULL) {
+#ifdef IPSEC_DEBUG
 				printf("key_timehandler: "
 					"There is no CURRENT time, why?\n");
+#endif
 				continue;
 			}
 
@@ -2943,8 +4141,10 @@ key_timehandler(void)
 
 			/* sanity check */
 			if (sav->lft_c == NULL) {
+#ifdef IPSEC_DEBUG
 				printf("key_timehandler: "
 					"There is no CURRENT time, why?\n");
+#endif
 				continue;
 			}
 
@@ -2955,6 +4155,22 @@ key_timehandler(void)
 				key_freesav(sav);
 				sav = NULL;
 			}
+#if 0	/* XXX Should we keep to send expire message until HARD lifetime ? */
+			else if (sav->lft_s != NULL
+			      && sav->lft_s->sadb_lifetime_addtime != 0
+			      && sav->lft_s->sadb_lifetime_addtime < sav->tick) {
+				/*
+				 * XXX: should be checked to be
+				 * installed the valid SA.
+				 */
+
+				/*
+				 * If there is no SA then sending
+				 * expire message.
+				 */
+				key_expire(sav);
+			}
+#endif
 			/* check HARD lifetime by bytes */
 			else if (sav->lft_h->sadb_lifetime_bytes != 0
 			      && sav->lft_h->sadb_lifetime_bytes < sav->lft_c->sadb_lifetime_bytes) {
@@ -2973,11 +4189,13 @@ key_timehandler(void)
 
 			/* sanity check */
 			if (sav->state != SADB_SASTATE_DEAD) {
+#ifdef IPSEC_DEBUG
 				printf("key_timehandler: "
 					"invalid sav->state "
 					"(queue: %d SA: %d): "
 					"kill it anyway\n",
 					SADB_SASTATE_DEAD, sav->state);
+#endif
 			}
 
 			/*
@@ -3011,6 +4229,25 @@ key_timehandler(void)
     }
 #endif
 
+	/* SP ACQ tree */
+    {
+	struct secspacq *acq, *nextacq;
+
+	for (acq = LIST_FIRST(&spacqtree);
+	     acq != NULL;
+	     acq = nextacq) {
+
+		nextacq = LIST_NEXT(acq, chain);
+
+		acq->tick++;
+
+		if (key_blockacq_lifetime < acq->tick && __LIST_CHAINED(acq)) {
+			LIST_REMOVE(acq, chain);
+			KFREE(acq);
+		}
+	}
+    }
+
 	/* initialize random seed */
 	if (key_tick_init_random++ > key_int_random) {
 		key_tick_init_random = 0;
@@ -3029,18 +4266,32 @@ key_timehandler(void)
 /*
  * to initialize a seed for random()
  */
-void
+static void
 key_srandom()
 {
 	struct timeval tv;
 
 	microtime(&tv);
+
 	srandom(tv.tv_usec);
 
 	return;
 }
 
 /*
+ * to initialize a seed for random()
+ */
+static u_long
+key_random()
+{
+	u_long value;
+
+	value = random();
+
+	return value;
+}
+
+/*
  * map SADB_SATYPE_* to IPPROTO_*.
  * if satype == SADB_SATYPE then satype is mapped to ~0.
  * OUT:
@@ -3057,6 +4308,11 @@ key_satype2proto(satype)
 		return IPPROTO_AH;
 	case SADB_SATYPE_ESP:
 		return IPPROTO_ESP;
+#if 1	/*nonstandard*/
+	case SADB_X_SATYPE_IPCOMP:
+		return IPPROTO_IPCOMP;
+		break;
+#endif
 	default:
 		return 0;
 	}
@@ -3077,6 +4333,11 @@ key_proto2satype(proto)
 		return SADB_SATYPE_AH;
 	case IPPROTO_ESP:
 		return SADB_SATYPE_ESP;
+#if 1	/*nonstandard*/
+	case IPPROTO_IPCOMP:
+		return SADB_X_SATYPE_IPCOMP;
+		break;
+#endif
 	default:
 		return 0;
 	}
@@ -3086,7 +4347,7 @@ key_proto2satype(proto)
 /* %%% PF_KEY */
 /*
  * SADB_GETSPI processing is to receive
- *	<base, src address, dst address, (SPI range)>
+ *	<base, (SA2), src address, dst address, (SPI range)>
  * from the IKMPd, to assign a unique spi value, to hang on the INBOUND
  * tree with the status of LARVAL, and send
  *	<base, SA(*), address(SD)>
@@ -3096,67 +4357,119 @@ key_proto2satype(proto)
  * OUT:	NULL if fail.
  *	other if success, return pointer to the message to send.
  */
-static struct sadb_msg *
-key_getspi(mhp)
-	caddr_t *mhp;
+static int
+key_getspi(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	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;
+	int error;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_getspi: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
-	if (mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL) {
+	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_getspi: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_getspi: invalid message is passed.\n");
+#endif
+		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 {
+		mode = IPSEC_MODE_ANY;
+		reqid = 0;
 	}
 
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+	src0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_SRC]);
+	dst0 = (struct sadb_address *)(mhp->ext[SADB_EXT_ADDRESS_DST]);
 
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_getspi: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	/* make sure if port number is zero. */
+	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:
+		; /*???*/
+	}
+	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:
+		; /*???*/
+	}
+
+	/* XXX boundary check against sa_len */
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
 
 	/* SPI allocation */
-	spi = key_do_getnewspi((struct sadb_spirange *)mhp[SADB_EXT_SPIRANGE],
+	spi = key_do_getnewspi((struct sadb_spirange *)mhp->ext[SADB_EXT_SPIRANGE],
 	                       &saidx);
-	if (spi == 0) {
-		msg0->sadb_msg_errno = EEXIST;
-		return NULL;
-	}
+	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) {
+#ifdef IPSEC_DEBUG
 			printf("key_getspi: No more memory.\n");
-			msg0->sadb_msg_errno = ENOBUFS;
-			return NULL;
+#endif
+			return key_senderror(so, m, ENOBUFS);
 		}
 	}
 
 	/* get a new SA */
-	if ((newsav = key_newsav(mhp, newsah)) == NULL) {
-		msg0->sadb_msg_errno = ENOBUFS;
+	/* XXX rewrite */
+	newsav = key_newsav(m, mhp, newsah, &error);
+	if (newsav == NULL) {
 		/* XXX don't free new SA index allocated in above. */
-		return NULL;
+		return key_senderror(so, m, error);
 	}
 
 	/* set spi */
@@ -3164,9 +4477,9 @@ key_getspi(mhp)
 
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 	/* delete the entry in acqtree */
-	if (msg0->sadb_msg_seq != 0) {
+	if (mhp->msg->sadb_msg_seq != 0) {
 		struct secacq *acq;
-		if ((acq = key_getacqbyseq(msg0->sadb_msg_seq)) != NULL) {
+		if ((acq = key_getacqbyseq(mhp->msg->sadb_msg_seq)) != NULL) {
 			/* reset counter in order to deletion by timehander. */
 			acq->tick = key_blockacq_lifetime;
 			acq->count = 0;
@@ -3175,43 +4488,70 @@ key_getspi(mhp)
 #endif
 
     {
+	struct mbuf *n, *nn;
+	struct sadb_sa *m_sa;
 	struct sadb_msg *newmsg;
-	u_int len;
-	caddr_t p;
+	int off, len;
 
 	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-	    + sizeof(struct sadb_sa)
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_SRC])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_DST]);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_getspi: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+	len = PFKEY_ALIGN8(sizeof(struct sadb_msg)) +
+	    PFKEY_ALIGN8(sizeof(struct sadb_sa));
+	if (len > MCLBYTES)
+		return key_senderror(so, m, ENOBUFS);
+
+	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	if (len > MHLEN) {
+		MCLGET(n, M_DONTWAIT);
+		if ((n->m_flags & M_EXT) == 0) {
+			m_freem(n);
+			n = NULL;
+		}
 	}
-	bzero((caddr_t)newmsg, len);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
-	newmsg->sadb_msg_seq = newsav->seq;
-	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
+	n->m_len = len;
+	n->m_next = NULL;
+	off = 0;
 
-      {
-	struct sadb_sa *m_sa;
-	m_sa = (struct sadb_sa *)p;
+	m_copydata(m, 0, sizeof(struct sadb_msg), mtod(n, caddr_t) + off);
+	off += PFKEY_ALIGN8(sizeof(struct sadb_msg));
+
+	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);
-	p += sizeof(struct sadb_sa);
-      }
+	off += PFKEY_ALIGN8(sizeof(struct sadb_sa));
+
+#ifdef DIAGNOSTIC
+	if (off != len)
+		panic("length inconsistency in key_getspi");
+#endif
+
+	n->m_next = key_gather_mbuf(m, mhp, 0, 2, SADB_EXT_ADDRESS_SRC,
+	    SADB_EXT_ADDRESS_DST);
+	if (!n->m_next) {
+		m_freem(n);
+		return key_senderror(so, m, ENOBUFS);
+	}
 
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_SRC]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_DST]);
+	if (n->m_len < sizeof(struct sadb_msg)) {
+		n = m_pullup(n, sizeof(struct sadb_msg));
+		if (n == NULL)
+			return key_sendup_mbuf(so, m, KEY_SENDUP_ONE);
+	}
+
+	n->m_pkthdr.len = 0;
+	for (nn = n; nn; nn = nn->m_next)
+		n->m_pkthdr.len += nn->m_len;
 
-	return newmsg;
+	newmsg = mtod(n, struct sadb_msg *);
+	newmsg->sadb_msg_seq = newsav->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);
     }
 }
 
@@ -3239,10 +4579,23 @@ key_do_getnewspi(spirange, saidx)
 		min = key_spi_minval;
 		max = key_spi_maxval;
 	}
+	/* IPCOMP needs 2-byte SPI */
+	if (saidx->proto == IPPROTO_IPCOMP) {
+		u_int32_t t;
+		if (min >= 0x10000)
+			min = 0xffff;
+		if (max >= 0x10000)
+			max = 0xffff;
+		if (min > max) {
+			t = min; min = max; max = t;
+		}
+	}
 
 	if (min == max) {
 		if (key_checkspidup(saidx, min) != NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_do_getnewspi: SPI %u exists already.\n", min);
+#endif
 			return 0;
 		}
 
@@ -3257,14 +4610,16 @@ key_do_getnewspi(spirange, saidx)
 		/* when requesting to allocate spi ranged */
 		while (count--) {
 			/* generate pseudo-random SPI value ranged. */
-			newspi = min + (random() % ( max - min + 1 ));
+			newspi = min + (key_random() % (max - min + 1));
 
 			if (key_checkspidup(saidx, newspi) == NULL)
 				break;
 		}
 
 		if (count == 0 || newspi == 0) {
+#ifdef IPSEC_DEBUG
 			printf("key_do_getnewspi: to allocate spi is failed.\n");
+#endif
 			return 0;
 		}
 	}
@@ -3279,130 +4634,171 @@ key_do_getnewspi(spirange, saidx)
 /*
  * SADB_UPDATE processing
  * receive
- *   <base, SA, (lifetime(HSC),) address(SD), (address(P),)
+ *   <base, SA, (SA2), (lifetime(HSC),) address(SD), (address(P),)
  *       key(AE), (identity(SD),) (sensitivity)>
  * from the ikmpd, and update a secasvar entry whose status is SADB_SASTATE_LARVAL.
  * and send
- *   <base, SA, (lifetime(HSC),) address(SD), (address(P),)
+ *   <base, SA, (SA2), (lifetime(HSC),) address(SD), (address(P),)
  *       (identity(SD),) (sensitivity)>
  * to the ikmpd.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_update(mhp)
-	caddr_t *mhp;
+static int
+key_update(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_sa *sa0;
 	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *sah;
 	struct secasvar *sav;
 	u_int16_t proto;
+	u_int8_t mode;
+	u_int32_t reqid;
+	int error;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_update: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_update: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-
-	if (mhp[SADB_EXT_SA] == NULL
-	 || mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL
-	 || (msg0->sadb_msg_satype == SADB_SATYPE_ESP
-	  && mhp[SADB_EXT_KEY_ENCRYPT] == NULL)
-	 || (msg0->sadb_msg_satype == SADB_SATYPE_AH
-	  && mhp[SADB_EXT_KEY_AUTH] == NULL)
-	 || (mhp[SADB_EXT_LIFETIME_HARD] != NULL
-	  && mhp[SADB_EXT_LIFETIME_SOFT] == NULL)
-	 || (mhp[SADB_EXT_LIFETIME_HARD] == NULL
-	  && mhp[SADB_EXT_LIFETIME_SOFT] != NULL)) {
+	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)) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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 {
+		mode = IPSEC_MODE_ANY;
+		reqid = 0;
 	}
+	/* XXX boundary checking for other extensions */
 
-	sa0 = (struct sadb_sa *)mhp[SADB_EXT_SA];
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+	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]);
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	/* XXX boundary check against sa_len */
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
 
 	/* get a SA header */
 	if ((sah = key_getsah(&saidx)) == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: no SA index found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
+#endif
+		return key_senderror(so, m, ENOENT);
 	}
 
+	/* set spidx if there */
+	/* XXX rewrite */
+	error = key_setident(sah, m, mhp);
+	if (error)
+		return key_senderror(so, m, error);
+
 	/* find a SA with sequence number. */
-	if ((sav = key_getsavbyseq(sah, msg0->sadb_msg_seq)) == NULL) {
+#ifdef IPSEC_DOSEQCHECK
+	if (mhp->msg->sadb_msg_seq != 0
+	 && (sav = key_getsavbyseq(sah, mhp->msg->sadb_msg_seq)) == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: no larval SA with sequence %u exists.\n",
-			msg0->sadb_msg_seq);
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
+			mhp->msg->sadb_msg_seq);
+#endif
+		return key_senderror(so, m, ENOENT);
+	}
+#else
+	if ((sav = key_getsavbyspi(sah, sa0->sadb_sa_spi)) == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_update: no such a SA found (spi:%u)\n",
+			(u_int32_t)ntohl(sa0->sadb_sa_spi));
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
+#endif
 
 	/* validity check */
 	if (sav->sah->saidx.proto != proto) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: protocol mismatched (DB=%u param=%u)\n",
 			sav->sah->saidx.proto, proto);
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
+#ifdef IPSEC_DOSEQCHECK
 	if (sav->spi != sa0->sadb_sa_spi) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: SPI mismatched (DB:%u param:%u)\n",
 			(u_int32_t)ntohl(sav->spi),
 			(u_int32_t)ntohl(sa0->sadb_sa_spi));
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-	if (sav->pid != msg0->sadb_msg_pid) {
+#endif
+	if (sav->pid != mhp->msg->sadb_msg_pid) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: pid mismatched (DB:%u param:%u)\n",
-			sav->pid, msg0->sadb_msg_pid);
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+			sav->pid, mhp->msg->sadb_msg_pid);
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
 	/* copy sav values */
-	if (key_setsaval(sav, mhp)) {
+	error = key_setsaval(sav, m, mhp);
+	if (error) {
 		key_freesav(sav);
-		return NULL;
+		return key_senderror(so, m, error);
 	}
 
 	/* check SA values to be mature. */
-	if ((msg0->sadb_msg_errno = key_mature(sav)) != 0) {
+	if ((mhp->msg->sadb_msg_errno = key_mature(sav)) != 0) {
 		key_freesav(sav);
-		return NULL;
+		return key_senderror(so, m, 0);
 	}
 
-	/*
-	 * we must call key_freesav() whenever we leave a function context,
-	 * as we did not allocated a new sav (we updated existing sav).
-	 */
-	key_freesav(sav);
-	sav = NULL;
-
     {
-	struct sadb_msg *newmsg;
+	struct mbuf *n;
 
 	/* set msg buf from mhp */
-	if ((newmsg = key_getmsgbuf_x1(mhp)) == NULL) {
+	n = key_getmsgbuf_x1(m, mhp);
+	if (n == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_update: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+#endif
+		return key_senderror(so, m, ENOBUFS);
 	}
-	return newmsg;
+
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
     }
 }
 
@@ -3413,6 +4809,7 @@ key_update(mhp)
  *	NULL	: not found
  *	others	: found, pointer to a SA.
  */
+#ifdef IPSEC_DOSEQCHECK
 static struct secasvar *
 key_getsavbyseq(sah, seq)
 	struct secashead *sah;
@@ -3424,7 +4821,7 @@ key_getsavbyseq(sah, seq)
 	state = SADB_SASTATE_LARVAL;
 
 	/* search SAD with sequence number ? */
-	__LIST_FOREACH(sav, &sah->savtree[state], chain) {
+	LIST_FOREACH(sav, &sah->savtree[state], chain) {
 
 		KEY_CHKSASTATE(state, sav->state, "key_getsabyseq");
 
@@ -3440,96 +4837,126 @@ key_getsavbyseq(sah, seq)
 
 	return NULL;
 }
+#endif
 
 /*
  * SADB_ADD processing
  * add a entry to SA database, when received
- *   <base, SA, (lifetime(HSC),) address(SD), (address(P),)
+ *   <base, SA, (SA2), (lifetime(HSC),) address(SD), (address(P),)
  *       key(AE), (identity(SD),) (sensitivity)>
  * from the ikmpd,
  * and send
- *   <base, SA, (lifetime(HSC),) address(SD), (address(P),)
+ *   <base, SA, (SA2), (lifetime(HSC),) address(SD), (address(P),)
  *       (identity(SD),) (sensitivity)>
  * to the ikmpd.
  *
  * IGNORE identity and sensitivity messages.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_add(mhp)
-	caddr_t *mhp;
+static int
+key_add(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_sa *sa0;
 	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *newsah;
 	struct secasvar *newsav;
 	u_int16_t proto;
+	u_int8_t mode;
+	u_int32_t reqid;
+	int error;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_add: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_add: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_add: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-
-	if (mhp[SADB_EXT_SA] == NULL
-	 || mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL
-	 || (msg0->sadb_msg_satype == SADB_SATYPE_ESP
-	  && mhp[SADB_EXT_KEY_ENCRYPT] == NULL)
-	 || (msg0->sadb_msg_satype == SADB_SATYPE_AH
-	  && mhp[SADB_EXT_KEY_AUTH] == NULL)
-	 || (mhp[SADB_EXT_LIFETIME_HARD] != NULL
-	  && mhp[SADB_EXT_LIFETIME_SOFT] == NULL)
-	 || (mhp[SADB_EXT_LIFETIME_HARD] == NULL
-	  && mhp[SADB_EXT_LIFETIME_SOFT] != NULL)) {
+	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 */
+#ifdef IPSEC_DEBUG
 		printf("key_add: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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 {
+		mode = IPSEC_MODE_ANY;
+		reqid = 0;
 	}
 
-	sa0 = (struct sadb_sa *)mhp[SADB_EXT_SA];
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+	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];
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	/* XXX boundary check against sa_len */
+	KEY_SETSECASIDX(proto, mode, reqid, src0 + 1, dst0 + 1, &saidx);
 
 	/* get a SA header */
 	if ((newsah = key_getsah(&saidx)) == NULL) {
-
 		/* create a new SA header */
 		if ((newsah = key_newsah(&saidx)) == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_add: No more memory.\n");
-			msg0->sadb_msg_errno = ENOBUFS;
-			return NULL;
+#endif
+			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. */
 	if (key_getsavbyspi(newsah, sa0->sadb_sa_spi)) {
+#ifdef IPSEC_DEBUG
 		printf("key_add: SA already exists.\n");
-		msg0->sadb_msg_errno = EEXIST;
-		return NULL;
+#endif
+		return key_senderror(so, m, EEXIST);
+	}
+	newsav = key_newsav(m, mhp, newsah, &error);
+	if (newsav == NULL) {
+		return key_senderror(so, m, error);
 	}
-	if ((newsav = key_newsav(mhp, newsah)) == NULL)
-		return NULL;
 
 	/* check SA values to be mature. */
-	if ((msg0->sadb_msg_errno = key_mature(newsav)) != NULL) {
+	if ((error = key_mature(newsav)) != 0) {
 		key_freesav(newsav);
-		return NULL;
+		return key_senderror(so, m, error);
 	}
 
 	/*
@@ -3538,65 +4965,132 @@ key_add(mhp)
 	 */
 
     {
-	struct sadb_msg *newmsg;
+	struct mbuf *n;
 
 	/* set msg buf from mhp */
-	if ((newmsg = key_getmsgbuf_x1(mhp)) == NULL) {
-		printf("key_add: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+	n = key_getmsgbuf_x1(m, mhp);
+	if (n == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_update: No more memory.\n");
+#endif
+		return key_senderror(so, m, ENOBUFS);
 	}
 
-	return newmsg;
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
     }
 }
 
-static struct sadb_msg *
-key_getmsgbuf_x1(mhp)
-	caddr_t *mhp;
+/* m is retained */
+static int
+key_setident(sah, m, mhp)
+	struct secashead *sah;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
-	struct sadb_msg *newmsg;
-	u_int len;
-	caddr_t p;
+	const struct sadb_ident *idsrc, *iddst;
+	int idsrclen, iddstlen;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
-		panic("key_getmsgbuf_x1: NULL pointer is passed.\n");
+	if (sah == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
+		panic("key_setident: NULL pointer is passed.\n");
+
+	/* don't make buffer if not there */
+	if (mhp->ext[SADB_EXT_IDENTITY_SRC] == NULL &&
+	    mhp->ext[SADB_EXT_IDENTITY_DST] == NULL) {
+		sah->idents = NULL;
+		sah->identd = NULL;
+		return 0;
+	}
+	
+	if (mhp->ext[SADB_EXT_IDENTITY_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_IDENTITY_DST] == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_setident: invalid identity.\n");
+#endif
+		return EINVAL;
+	}
 
-	msg0 = (struct sadb_msg *)mhp[0];
+	idsrc = (const struct sadb_ident *)mhp->ext[SADB_EXT_IDENTITY_SRC];
+	iddst = (const struct sadb_ident *)mhp->ext[SADB_EXT_IDENTITY_DST];
+	idsrclen = mhp->extlen[SADB_EXT_IDENTITY_SRC];
+	iddstlen = mhp->extlen[SADB_EXT_IDENTITY_DST];
 
-	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-	    + sizeof(struct sadb_sa)
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_SRC])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_DST])
-	    + (mhp[SADB_EXT_LIFETIME_HARD] == NULL
-		? 0 : sizeof(struct sadb_lifetime))
-	    + (mhp[SADB_EXT_LIFETIME_SOFT] == NULL
-		? 0 : sizeof(struct sadb_lifetime));
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL)
-		return NULL;
-	bzero((caddr_t)newmsg, len);
+	/* validity check */
+	if (idsrc->sadb_ident_type != iddst->sadb_ident_type) {
+#ifdef IPSEC_DEBUG
+		printf("key_setident: ident type mismatch.\n");
+#endif
+		return EINVAL;
+	}
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
-	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
+	switch (idsrc->sadb_ident_type) {
+	case SADB_IDENTTYPE_PREFIX:
+	case SADB_IDENTTYPE_FQDN:
+	case SADB_IDENTTYPE_USERFQDN:
+	default:
+		/* XXX do nothing */
+		sah->idents = NULL;
+		sah->identd = NULL;
+	 	return 0;
+	}
 
-	p = key_setsadbext(p, mhp[SADB_EXT_SA]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_SRC]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_DST]);
+	/* make structure */
+	KMALLOC(sah->idents, struct sadb_ident *, idsrclen);
+	if (sah->idents == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_setident: No more memory.\n");
+#endif
+		return ENOBUFS;
+	}
+	KMALLOC(sah->identd, struct sadb_ident *, iddstlen);
+	if (sah->identd == NULL) {
+		KFREE(sah->idents);
+#ifdef IPSEC_DEBUG
+		printf("key_setident: No more memory.\n");
+#endif
+		return ENOBUFS;
+	}
+	bcopy(idsrc, sah->idents, idsrclen);
+	bcopy(iddst, sah->identd, iddstlen);
+
+	return 0;
+}
+
+/*
+ * m will not be freed on return.
+ * it is caller's responsibility to free the result. 
+ */
+static struct mbuf *
+key_getmsgbuf_x1(m, mhp)
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
+{
+	struct mbuf *n;
 
-	if (mhp[SADB_EXT_LIFETIME_HARD] != NULL)
-		p = key_setsadbext(p, mhp[SADB_EXT_LIFETIME_HARD]);
+	/* sanity check */
+	if (m == NULL || mhp == NULL || mhp->msg == NULL)
+		panic("key_getmsgbuf_x1: NULL pointer is passed.\n");
+
+	/* create new sadb_msg to reply. */
+	n = key_gather_mbuf(m, mhp, 1, 9, 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);
+	if (!n)
+		return NULL;
 
-	if (mhp[SADB_EXT_LIFETIME_SOFT] != NULL)
-		p = key_setsadbext(p, mhp[SADB_EXT_LIFETIME_SOFT]);
+	if (n->m_len < sizeof(struct sadb_msg)) {
+		n = m_pullup(n, sizeof(struct sadb_msg));
+		if (n == NULL)
+			return NULL;
+	}
+	mtod(n, struct sadb_msg *)->sadb_msg_errno = 0;
+	mtod(n, struct sadb_msg *)->sadb_msg_len =
+	    PFKEY_UNIT64(n->m_pkthdr.len);
 
-	return newmsg;
+	return n;
 }
 
 /*
@@ -3608,61 +5102,74 @@ key_getmsgbuf_x1(mhp)
  *   <base, SA(*), address(SD)>
  * to the ikmpd.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_delete(mhp)
-	caddr_t *mhp;
+static int
+key_delete(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_sa *sa0;
 	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *sah;
-	struct secasvar *sav;
+	struct secasvar *sav = NULL;
 	u_int16_t proto;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_delete: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_delete: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp[SADB_EXT_SA] == NULL
-	 || mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL) {
+	if (mhp->ext[SADB_EXT_SA] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_delete: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-	sa0 = (struct sadb_sa *)mhp[SADB_EXT_SA];
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+	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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_delete: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
+	}
+
+	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]);
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	/* XXX boundary check against sa_len */
+	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
 
 	/* get a SA header */
-	if ((sah = key_getsah(&saidx)) == NULL) {
-		printf("key_delete: no SA found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
-	}
+	LIST_FOREACH(sah, &sahtree, chain) {
+		if (sah->state == SADB_SASTATE_DEAD)
+			continue;
+		if (key_cmpsaidx_withoutmode(&sah->saidx, &saidx) == 0)
+			continue;
 
-	/* get a SA with SPI. */
-	sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
-	if (sav == NULL) {
-		printf("key_delete: no alive SA found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
+		/* get a SA with SPI. */
+		sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
+		if (sav)
+			break;
+	}
+	if (sah == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_delete: no SA found.\n");
+#endif
+		return key_senderror(so, m, ENOENT);
 	}
 
 	key_sa_chgstate(sav, SADB_SASTATE_DEAD);
@@ -3670,34 +5177,26 @@ key_delete(mhp)
 	sav = NULL;
 
     {
+	struct mbuf *n;
 	struct sadb_msg *newmsg;
-	u_int len;
-	caddr_t p;
 
 	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-	    + sizeof(struct sadb_sa)
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_SRC])
-	    + PFKEY_EXTLEN(mhp[SADB_EXT_ADDRESS_DST]);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_delete: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
-	}
-	bzero((caddr_t)newmsg, len);
+	n = key_gather_mbuf(m, mhp, 1, 4, SADB_EXT_RESERVED,
+	    SADB_EXT_SA, SADB_EXT_ADDRESS_SRC, SADB_EXT_ADDRESS_DST);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
+	if (n->m_len < sizeof(struct sadb_msg)) {
+		n = m_pullup(n, sizeof(struct sadb_msg));
+		if (n == NULL)
+			return key_senderror(so, m, ENOBUFS);
+	}
+	newmsg = mtod(n, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
-
-	p = key_setsadbext(p, mhp[SADB_EXT_SA]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_SRC]);
-	p = key_setsadbext(p, mhp[SADB_EXT_ADDRESS_DST]);
+	newmsg->sadb_msg_len = PFKEY_UNIT64(n->m_pkthdr.len);
 
-	return newmsg;
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ALL);
     }
 }
 
@@ -3711,91 +5210,280 @@ key_delete(mhp)
  *       (identity(SD),) (sensitivity)>
  * to the ikmpd.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_get(mhp)
-	caddr_t *mhp;
+static int
+key_get(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct sadb_sa *sa0;
 	struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *sah;
-	struct secasvar *sav;
+	struct secasvar *sav = NULL;
 	u_int16_t proto;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_get: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_get: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp[SADB_EXT_SA] == NULL
-	 || mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL) {
+	if (mhp->ext[SADB_EXT_SA] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_get: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-	sa0 = (struct sadb_sa *)mhp[SADB_EXT_SA];
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+	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)) {
+#ifdef IPSEC_DEBUG
+		printf("key_get: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
+	}
+
+	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];
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	/* XXX boundary check against sa_len */
+	KEY_SETSECASIDX(proto, IPSEC_MODE_ANY, 0, src0 + 1, dst0 + 1, &saidx);
 
 	/* get a SA header */
-	if ((sah = key_getsah(&saidx)) == NULL) {
-		printf("key_get: no SA found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
-	}
+	LIST_FOREACH(sah, &sahtree, chain) {
+		if (sah->state == SADB_SASTATE_DEAD)
+			continue;
+		if (key_cmpsaidx_withoutmode(&sah->saidx, &saidx) == 0)
+			continue;
 
-	/* get a SA with SPI. */
-	sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
-	if (sav == NULL) {
-		printf("key_get: no SA with state of mature found.\n");
-		msg0->sadb_msg_errno = ENOENT;
-		return NULL;
+		/* get a SA with SPI. */
+		sav = key_getsavbyspi(sah, sa0->sadb_sa_spi);
+		if (sav)
+			break;
+	}
+	if (sah == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_get: no SA found.\n");
+#endif
+		return key_senderror(so, m, ENOENT);
 	}
 
     {
-	struct sadb_msg *newmsg;
-	u_int len;
+	struct mbuf *n;
 	u_int8_t satype;
 
 	/* map proto to satype */
 	if ((satype = key_proto2satype(sah->saidx.proto)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_get: there was invalid proto in SAD.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
+
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
+    }
+}
+
+#ifdef IPSEC_ESP
+/*
+ * XXX reorder combinations by preference
+ * XXX no idea if the user wants ESP authentication or not
+ * XXX lifetime - should be in policy?
+ */
+static struct mbuf *
+key_getcomb_esp()
+{
+	struct sadb_comb *comb;
+	struct esp_algorithm *algo;
+	struct mbuf *result = NULL, *m, *n;
+	int encmin;
+	int i, off, o;
+	int totlen;
+	const int l = PFKEY_ALIGN8(sizeof(struct sadb_comb));
+
+	m = NULL;
+	for (i = 1; i < SADB_EALG_MAX; i++) {
+		algo = &esp_algorithms[i];
+
+		if (algo->keymax < ipsec_esp_keymin)
+			continue;
+		if (algo->keymin < ipsec_esp_keymin)
+			encmin = ipsec_esp_keymin;
+		else
+			encmin = algo->keymin;
+
+		if (ipsec_esp_auth)
+			m = key_getcomb_ah();
+		else {
+#ifdef DIAGNOSTIC
+			if (l > MLEN)
+				panic("assumption failed in key_getcomb_esp");
+#endif
+			MGET(m, M_DONTWAIT, MT_DATA);
+			if (m) {
+				M_ALIGN(m, l);
+				m->m_len = l;
+				m->m_next = NULL;
+				bzero(mtod(m, caddr_t), m->m_len);
+			}
+		}
+		if (!m)
+			goto fail;
+
+		totlen = 0;
+		for (n = m; n; n = n->m_next)
+			totlen += n->m_len;
+#ifdef DIAGNOSTIC
+		if (totlen % l)
+			panic("assumption failed in key_getcomb_esp");
+#endif
+
+		for (off = 0; off < totlen; off += l) {
+			n = m_pulldown(m, off, l, &o);
+			if (!n) {
+				/* m is already freed */
+				goto fail;
+			}
+			comb = (struct sadb_comb *)(mtod(n, caddr_t) + o);
+			comb->sadb_comb_encrypt = i;
+			comb->sadb_comb_encrypt_minbits = encmin;
+			comb->sadb_comb_encrypt_maxbits = algo->keymax;
+		}
+
+		if (!result)
+			result = m;
+		else
+			m_cat(result, m);
 	}
 
-	/* calculate a length of message buffer */
-	len = key_getmsglen(sav);
+	return result;
+
+ fail:
+	if (result)
+		m_freem(result);
+	return NULL;
+}
+#endif
 
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_get: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
+/*
+ * XXX reorder combinations by preference
+ * XXX lifetime - should be in policy?
+ */
+static struct mbuf *
+key_getcomb_ah()
+{
+	struct sadb_comb *comb;
+	struct ah_algorithm *algo;
+	struct mbuf *m;
+	int min;
+	int i;
+	const int l = PFKEY_ALIGN8(sizeof(struct sadb_comb));
+
+	m = NULL;
+	for (i = 1; i < SADB_AALG_MAX; i++) {
+#if 1
+		/* we prefer HMAC algorithms, not old algorithms */
+		if (i != SADB_AALG_SHA1HMAC && i != SADB_AALG_MD5HMAC)
+			continue;
+#endif
+		algo = &ah_algorithms[i];
+
+		if (algo->keymax < ipsec_ah_keymin)
+			continue;
+		if (algo->keymin < ipsec_ah_keymin)
+			min = ipsec_ah_keymin;
+		else
+			min = algo->keymin;
+
+		if (!m) {
+#ifdef DIAGNOSTIC
+			if (l > MLEN)
+				panic("assumption failed in key_getcomb_ah");
+#endif
+			MGET(m, M_DONTWAIT, MT_DATA);
+			if (m) {
+				M_ALIGN(m, l);
+				m->m_len = l;
+				m->m_next = NULL;
+			}
+		} else
+			M_PREPEND(m, l, M_DONTWAIT);
+		if (!m)
+			return NULL;
+
+		comb = mtod(m, struct sadb_comb *);
+		bzero(comb, sizeof(*comb));
+		comb->sadb_comb_auth = i;
+		comb->sadb_comb_auth_minbits = min;
+		comb->sadb_comb_auth_maxbits = algo->keymax;
+	}
+
+	return m;
+}
+
+/*
+ * XXX no way to pass mode (transport/tunnel) to userland
+ * XXX replay checking?
+ * XXX sysctl interface to ipsec_{ah,esp}_keymin
+ */
+static struct mbuf *
+key_getprop(saidx)
+	const struct secasindex *saidx;
+{
+	struct sadb_prop *prop;
+	struct mbuf *m, *n;
+	const int l = PFKEY_ALIGN8(sizeof(struct sadb_prop));
+	int totlen;
+
+	switch (saidx->proto)  {
+#ifdef IPSEC_ESP
+	case IPPROTO_ESP:
+		m = key_getcomb_esp();
+		break;
+#endif
+	case IPPROTO_AH:
+		m = key_getcomb_ah();
+		break;
+	default:
 		return NULL;
 	}
 
-	/* create new sadb_msg to reply. */
-	(void)key_setdumpsa(newmsg, sav, SADB_GET,
-	                    satype, msg0->sadb_msg_seq, msg0->sadb_msg_pid);
+	if (!m)
+		return NULL;
+	M_PREPEND(m, l, M_DONTWAIT);
+	if (!m)
+		return NULL;
 
-	return newmsg;
-    }
+	totlen = 0;
+	for (n = m; n; n = n->m_next)
+		totlen += n->m_len;
+
+	prop = mtod(m, struct sadb_prop *);
+	bzero(prop, sizeof(*prop));
+	prop->sadb_prop_len = PFKEY_UNIT64(totlen);
+	prop->sadb_prop_exttype = SADB_EXT_PROPOSAL;
+	prop->sadb_prop_replay = 32;	/* XXX */
+
+	return m;
 }
 
 /*
@@ -3816,22 +5504,27 @@ key_get(mhp)
  *    others: error number
  */
 static int
-key_acquire(saidx, spidx)
+key_acquire(saidx, sp)
 	struct secasindex *saidx;
-	struct secpolicyindex *spidx;
+	struct secpolicy *sp;
 {
+	struct mbuf *result = NULL, *m;
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 	struct secacq *newacq;
 #endif
+	struct secpolicyindex *spidx = NULL;
 	u_int8_t satype;
-	int error;
+	int error = -1;
+	u_int32_t seq;
 
 	/* sanity check */
-	if (saidx == NULL || spidx == NULL)
+	if (saidx == NULL || sp == NULL)
 		panic("key_acquire: NULL pointer is passed.\n");
 	if ((satype = key_proto2satype(saidx->proto)) == 0)
 		panic("key_acquire: invalid proto is passed.\n");
 
+	spidx = &sp->spidx;
+
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 	/*
 	 * We never do anything about acquirng SA.  There is anather
@@ -3859,119 +5552,125 @@ key_acquire(saidx, spidx)
 	}
 #endif
 
-    {
-	struct sadb_msg *newmsg = NULL;
-	union sadb_x_ident_id id;
-	u_int len;
-	caddr_t p;
 
-	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(saidx->src.ss_len)
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(saidx->dst.ss_len)
-		+ sizeof(struct sadb_ident)
-		+ PFKEY_ALIGN8(spidx->src.ss_len)
-		+ sizeof(struct sadb_ident)
-		+ PFKEY_ALIGN8(spidx->dst.ss_len)
-		+ sizeof(struct sadb_prop)
-		+ sizeof(struct sadb_comb); /* XXX to be multiple */
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == 0) {
-		printf("key_acquire: No more memory.\n");
-		return ENOBUFS;
+#ifndef IPSEC_NONBLOCK_ACQUIRE
+	seq = newacq->seq;
+#else
+	seq = (acq_seq = (acq_seq == ~0 ? 1 : ++acq_seq));
+#endif
+	m = key_setsadbmsg(SADB_ACQUIRE, 0, satype, seq, 0, 0);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
 	}
-	bzero((caddr_t)newmsg, len);
+	result = m;
 
-	newmsg->sadb_msg_version = PF_KEY_V2;
-	newmsg->sadb_msg_type = SADB_ACQUIRE;
-	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_satype = satype;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
+	/* set sadb_address for saidx's. */
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
+	    (struct sockaddr *)&saidx->src, saidx->src.ss_len << 3,
+	    IPSEC_ULPROTO_ANY);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
-#ifndef IPSEC_NONBLOCK_ACQUIRE
-	newmsg->sadb_msg_seq = newacq->seq;
-#else
-	newmsg->sadb_msg_seq = (acq_seq = (acq_seq == ~0 ? 1 : ++acq_seq));
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
+	    (struct sockaddr *)&saidx->dst, saidx->dst.ss_len << 3,
+	    IPSEC_ULPROTO_ANY);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
+
+	/* XXX proxy address (optional) */
+
+	/* set sadb_x_policy */
+	m = key_setsadbxpolicy(sp->policy, sp->spidx.dir, sp->id);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
+
+	/* XXX identity (optional) */
+#if 0
+	if (idexttype && fqdn) {
+		/* create identity extension (FQDN) */
+		struct sadb_ident *id;
+		int fqdnlen;
+
+		fqdnlen = strlen(fqdn) + 1;	/* +1 for terminating-NUL */
+		id = (struct sadb_ident *)p;
+		bzero(id, sizeof(*id) + PFKEY_ALIGN8(fqdnlen));
+		id->sadb_ident_len = PFKEY_UNIT64(sizeof(*id) + PFKEY_ALIGN8(fqdnlen));
+		id->sadb_ident_exttype = idexttype;
+		id->sadb_ident_type = SADB_IDENTTYPE_FQDN;
+		bcopy(fqdn, id + 1, fqdnlen);
+		p += sizeof(struct sadb_ident) + PFKEY_ALIGN8(fqdnlen);
+	}
+
+	if (idexttype) {
+		/* create identity extension (USERFQDN) */
+		struct sadb_ident *id;
+		int userfqdnlen;
+
+		if (userfqdn) {
+			/* +1 for terminating-NUL */
+			userfqdnlen = strlen(userfqdn) + 1;
+		} else
+			userfqdnlen = 0;
+		id = (struct sadb_ident *)p;
+		bzero(id, sizeof(*id) + PFKEY_ALIGN8(userfqdnlen));
+		id->sadb_ident_len = PFKEY_UNIT64(sizeof(*id) + PFKEY_ALIGN8(userfqdnlen));
+		id->sadb_ident_exttype = idexttype;
+		id->sadb_ident_type = SADB_IDENTTYPE_USERFQDN;
+		/* XXX is it correct? */
+		if (curproc && curproc->p_cred)
+			id->sadb_ident_id = curproc->p_cred->p_ruid;
+		if (userfqdn && userfqdnlen)
+			bcopy(userfqdn, id + 1, userfqdnlen);
+		p += sizeof(struct sadb_ident) + PFKEY_ALIGN8(userfqdnlen);
+	}
 #endif
 
-	newmsg->sadb_msg_pid = 0;
-	p = (caddr_t)newmsg + sizeof(struct sadb_msg);
+	/* XXX sensitivity (optional) */
 
-	/* set sadb_address for saidx's. */
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_SRC,
-	                    (struct sockaddr *)&saidx->src,
-	                    _INALENBYAF(saidx->src.ss_family) << 3,
-	                    IPSEC_ULPROTO_ANY);
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_DST,
-	                    (struct sockaddr *)&saidx->dst,
-	                    _INALENBYAF(saidx->dst.ss_family) << 3,
-	                    IPSEC_ULPROTO_ANY);
-
-	/* set sadb_address for spidx's. */
-	id.sadb_x_ident_id_addr.prefix = spidx->prefs;
-	id.sadb_x_ident_id_addr.ul_proto = spidx->ul_proto;
-	p = key_setsadbident(p,
-	                    SADB_EXT_IDENTITY_SRC,
-			    SADB_X_IDENTTYPE_ADDR,
-	                    (caddr_t)&spidx->src,
-			    spidx->src.ss_len,
-			    *(u_int64_t *)&id);
-
-	id.sadb_x_ident_id_addr.prefix = spidx->prefd;
-	id.sadb_x_ident_id_addr.ul_proto = spidx->ul_proto;
-	p = key_setsadbident(p,
-	                    SADB_EXT_IDENTITY_DST,
-			    SADB_X_IDENTTYPE_ADDR,
-	                    (caddr_t)&spidx->dst,
-			    spidx->dst.ss_len,
-			    *(u_int64_t *)&id);
-
-	/* create proposal extension */
-	/* set combination extension */
-	/* XXX: to be defined by proposal database */
-    {
-	struct sadb_prop *prop;
-	struct sadb_comb *comb;
+	/* create proposal/combination extension */
+	m = key_getprop(saidx);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
-	prop = (struct sadb_prop *)p;
-	prop->sadb_prop_len = PFKEY_UNIT64(sizeof(*prop) + sizeof(*comb));
-		/* XXX to be multiple */
-	prop->sadb_prop_exttype = SADB_EXT_PROPOSAL;
-	prop->sadb_prop_replay = 32;	/* XXX be variable ? */
-	p += sizeof(struct sadb_prop);
-
-	comb = (struct sadb_comb *)p;
-	comb->sadb_comb_auth = SADB_AALG_SHA1HMAC; /* XXX ??? */
-	comb->sadb_comb_encrypt = SADB_EALG_DESCBC; /* XXX ??? */
-	comb->sadb_comb_flags = 0;
-	comb->sadb_comb_auth_minbits = 8; /* XXX */
-	comb->sadb_comb_auth_maxbits = 1024; /* XXX */
-	comb->sadb_comb_encrypt_minbits = 64; /* XXX */
-	comb->sadb_comb_encrypt_maxbits = 64; /* XXX */
-	comb->sadb_comb_soft_allocations = 0;
-	comb->sadb_comb_hard_allocations = 0;
-	comb->sadb_comb_soft_bytes = 0;
-	comb->sadb_comb_hard_bytes = 0;
-	comb->sadb_comb_soft_addtime = 0;
-	comb->sadb_comb_hard_addtime = 0;
-	comb->sadb_comb_soft_usetime = 0;
-	comb->sadb_comb_hard_usetime = 0;
-
-	p += sizeof(*comb);
-    }
+	if ((result->m_flags & M_PKTHDR) == 0) {
+		error = EINVAL;
+		goto fail;
+	}
 
-	error = key_sendall(newmsg, len);
-	if (error != 0)
-		printf("key_acquire: key_sendall returned %d\n", error);
-	return error;
-    }
+	if (result->m_len < sizeof(struct sadb_msg)) {
+		result = m_pullup(result, sizeof(struct sadb_msg));
+		if (result == NULL) {
+			error = ENOBUFS;
+			goto fail;
+		}
+	}
 
-	return 0;
+	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);
+
+	return key_sendup_mbuf(NULL, result, KEY_SENDUP_REGISTERED);
+
+ fail:
+	if (result)
+		m_freem(result);
+	return error;
 }
 
 #ifndef IPSEC_NONBLOCK_ACQUIRE
@@ -3984,7 +5683,9 @@ key_newacq(saidx)
 	/* get new entry */
 	KMALLOC(newacq, struct secacq *, sizeof(struct secacq));
 	if (newacq == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_newacq: No more memory.\n");
+#endif
 		return NULL;
 	}
 	bzero(newacq, sizeof(*newacq));
@@ -4004,7 +5705,7 @@ key_getacq(saidx)
 {
 	struct secacq *acq;
 
-	__LIST_FOREACH(acq, &acqtree, chain) {
+	LIST_FOREACH(acq, &acqtree, chain) {
 		if (key_cmpsaidx_exactly(saidx, &acq->saidx))
 			return acq;
 	}
@@ -4018,7 +5719,7 @@ key_getacqbyseq(seq)
 {
 	struct secacq *acq;
 
-	__LIST_FOREACH(acq, &acqtree, chain) {
+	LIST_FOREACH(acq, &acqtree, chain) {
 		if (acq->seq == seq)
 			return acq;
 	}
@@ -4027,6 +5728,44 @@ key_getacqbyseq(seq)
 }
 #endif
 
+static struct secspacq *
+key_newspacq(spidx)
+	struct secpolicyindex *spidx;
+{
+	struct secspacq *acq;
+
+	/* get new entry */
+	KMALLOC(acq, struct secspacq *, sizeof(struct secspacq));
+	if (acq == NULL) {
+#ifdef IPSEC_DEBUG
+		printf("key_newspacq: No more memory.\n");
+#endif
+		return NULL;
+	}
+	bzero(acq, sizeof(*acq));
+
+	/* copy secindex */
+	bcopy(spidx, &acq->spidx, sizeof(acq->spidx));
+	acq->tick = 0;
+	acq->count = 0;
+
+	return acq;
+}
+
+static struct secspacq *
+key_getspacq(spidx)
+	struct secpolicyindex *spidx;
+{
+	struct secspacq *acq;
+
+	LIST_FOREACH(acq, &spacqtree, chain) {
+		if (key_cmpspidx_exactly(spidx, &acq->spidx))
+			return acq;
+	}
+
+	return NULL;
+}
+
 /*
  * SADB_ACQUIRE processing,
  * in first situation, is receiving
@@ -4039,55 +5778,58 @@ key_getacqbyseq(seq)
  *   <base, address(SD), (address(P),) (identity(SD),) (sensitivity,) proposal>
  * to the socket.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always e freed.
  */
-static struct sadb_msg *
-key_acquire2(mhp)
-	caddr_t *mhp;
+static int
+key_acquire2(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
-	struct sadb_address *src0, *dst0;
+	const struct sadb_address *src0, *dst0;
 	struct secasindex saidx;
 	struct secashead *sah;
 	u_int16_t proto;
+	int error;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_acquire2: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/*
 	 * Error message from KMd.
 	 * We assume that if error was occured in IKEd, the length of PFKEY
 	 * message is equal to the size of sadb_msg structure.
-	 * We return ~0 even if error occured in this function.
+	 * We do not raise error even if error occured in this function.
 	 */
-	if (msg0->sadb_msg_len == PFKEY_UNIT64(sizeof(struct sadb_msg))) {
-
+	if (mhp->msg->sadb_msg_len == PFKEY_UNIT64(sizeof(struct sadb_msg))) {
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 		struct secacq *acq;
 
 		/* check sequence number */
-		if (msg0->sadb_msg_seq == 0) {
+		if (mhp->msg->sadb_msg_seq == 0) {
+#ifdef IPSEC_DEBUG
 			printf("key_acquire2: must specify sequence number.\n");
-			return (struct sadb_msg *)~0;
+#endif
+			m_freem(m);
+			return 0;
 		}
 
-		if ((acq = key_getacqbyseq(msg0->sadb_msg_seq)) == NULL) {
+		if ((acq = key_getacqbyseq(mhp->msg->sadb_msg_seq)) == NULL) {
+#ifdef IPSEC_DEBUG
 			printf("key_acquire2: "
 				"invalid sequence number is passed.\n");
-			return (struct sadb_msg *)~0;
+#endif
+			m_freem(m);
+			return 0;
 		}
 
 		/* reset acq counter in order to deletion by timehander. */
 		acq->tick = key_blockacq_lifetime;
 		acq->count = 0;
 #endif
-		return (struct sadb_msg *)~0;
-		/* NOTREACHED */
+		m_freem(m);
+		return 0;
 	}
 
 	/*
@@ -4095,59 +5837,62 @@ key_acquire2(mhp)
 	 */
 
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_acquire2: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
-	if (mhp[SADB_EXT_ADDRESS_SRC] == NULL
-	 || mhp[SADB_EXT_ADDRESS_DST] == NULL
-	 || mhp[SADB_EXT_PROPOSAL] == NULL) {
+	if (mhp->ext[SADB_EXT_ADDRESS_SRC] == NULL ||
+	    mhp->ext[SADB_EXT_ADDRESS_DST] == NULL ||
+	    mhp->ext[SADB_EXT_PROPOSAL] == NULL) {
 		/* error */
+#ifdef IPSEC_DEBUG
 		printf("key_acquire2: invalid message is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		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 */
+#ifdef IPSEC_DEBUG
+		printf("key_acquire2: invalid message is passed.\n");
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
-	src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-	dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
 
-	KEY_SETSECASIDX(proto, msg0->sadb_msg_mode, src0+1, dst0+1, &saidx);
+	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);
 
 	/* get a SA index */
-	if ((sah = key_getsah(&saidx)) != NULL) {
+	LIST_FOREACH(sah, &sahtree, chain) {
+		if (sah->state == SADB_SASTATE_DEAD)
+			continue;
+		if (key_cmpsaidx_withmode(&sah->saidx, &saidx))
+			break;
+	}
+	if (sah != NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_acquire2: a SA exists already.\n");
-		msg0->sadb_msg_errno = EEXIST;
-		return NULL;
+#endif
+		return key_senderror(so, m, EEXIST);
 	}
 
-	msg0->sadb_msg_errno = key_acquire(&saidx, NULL);
-	if (msg0->sadb_msg_errno != 0) {
-		/* XXX What I do ? */
+	error = key_acquire(&saidx, NULL);
+	if (error != 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_acquire2: error %d returned "
-			"from key_acquire.\n", msg0->sadb_msg_errno);
-		return NULL;
-	}
-
-    {
-	struct sadb_msg *newmsg;
-	u_int len;
-
-	/* create new sadb_msg to reply. */
-	len = PFKEY_UNUNIT64(msg0->sadb_msg_len);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_acquire2: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+			"from key_acquire.\n", mhp->msg->sadb_msg_errno);
+#endif
+		return key_senderror(so, m, error);
 	}
-	bzero((caddr_t)newmsg, len);
-
-	bcopy(mhp[0], (caddr_t)newmsg, len);
 
-	return newmsg;
-    }
+	return key_sendup_mbuf(so, m, KEY_SENDUP_REGISTERED);
 }
 
 /*
@@ -4160,63 +5905,68 @@ key_acquire2(mhp)
  *   <base, supported>
  * to KMD by PF_KEY.
  * If socket is detached, must free from regnode.
- * OUT:
- *    0     : succeed
- *    others: error number
+ *
+ * m will always e freed.
  */
-static struct sadb_msg *
-key_register(mhp, so)
-	caddr_t *mhp;
+static int
+key_register(so, m, mhp)
 	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct secreg *reg, *newreg = 0;
 
 	/* sanity check */
-	if (mhp == NULL || so == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_register: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
+	/* check for invalid register message */
+	if (mhp->msg->sadb_msg_satype >= sizeof(regtree)/sizeof(regtree[0]))
+		return key_senderror(so, m, EINVAL);
 
 	/* When SATYPE_UNSPEC is specified, only return sabd_supported. */
-	if (msg0->sadb_msg_satype == SADB_SATYPE_UNSPEC)
+	if (mhp->msg->sadb_msg_satype == SADB_SATYPE_UNSPEC)
 		goto setmsg;
 
 	/* check whether existing or not */
-	__LIST_FOREACH(reg, &regtree[msg0->sadb_msg_satype], chain) {
+	LIST_FOREACH(reg, &regtree[mhp->msg->sadb_msg_satype], chain) {
 		if (reg->so == so) {
+#ifdef IPSEC_DEBUG
 			printf("key_register: socket exists already.\n");
-			msg0->sadb_msg_errno = EEXIST;
-			return NULL;
+#endif
+			return key_senderror(so, m, EEXIST);
 		}
 	}
 
 	/* create regnode */
-	KMALLOC(newreg, struct secreg *, sizeof(struct secreg));
+	KMALLOC(newreg, struct secreg *, sizeof(*newreg));
 	if (newreg == NULL) {
+#ifdef IPSEC_DEBUG
 		printf("key_register: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+#endif
+		return key_senderror(so, m, ENOBUFS);
 	}
-	bzero((caddr_t)newreg, sizeof(struct secreg));
+	bzero((caddr_t)newreg, sizeof(*newreg));
 
 	newreg->so = so;
 	((struct keycb *)sotorawcb(so))->kp_registered++;
 
 	/* add regnode to regtree. */
-	LIST_INSERT_HEAD(&regtree[msg0->sadb_msg_satype], newreg, chain);
+	LIST_INSERT_HEAD(&regtree[mhp->msg->sadb_msg_satype], newreg, chain);
 
   setmsg:
-  {
+    {
+	struct mbuf *n;
 	struct sadb_msg *newmsg;
 	struct sadb_supported *sup;
 	u_int len, alen, elen;
-	caddr_t p;
+	int off;
+	int i;
+	struct sadb_alg *alg;
 
 	/* create new sadb_msg to reply. */
 	alen = sizeof(struct sadb_supported)
 		+ ((SADB_AALG_MAX - 1) * sizeof(struct sadb_alg));
-
 #ifdef IPSEC_ESP
 	elen = sizeof(struct sadb_supported)
 		+ ((SADB_EALG_MAX - 1) * sizeof(struct sadb_alg));
@@ -4224,79 +5974,89 @@ key_register(mhp, so)
 	elen = 0;
 #endif
 
-	len = sizeof(struct sadb_msg)
-		+ alen
-		+ elen;
+	len = sizeof(struct sadb_msg) + alen + elen;
 
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_register: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+	if (len > MCLBYTES)
+		return key_senderror(so, m, ENOBUFS);
+
+	MGETHDR(n, M_DONTWAIT, MT_DATA);
+	if (len > MHLEN) {
+		MCLGET(n, M_DONTWAIT);
+		if ((n->m_flags & M_EXT) == 0) {
+			m_freem(n);
+			n = NULL;
+		}
 	}
-	bzero((caddr_t)newmsg, len);
+	if (!n)
+		return key_senderror(so, m, ENOBUFS);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
+	n->m_pkthdr.len = n->m_len = len;
+	n->m_next = NULL;
+	off = 0;
+
+	m_copydata(m, 0, sizeof(struct sadb_msg), mtod(n, caddr_t) + off);
+	newmsg = mtod(n, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
 	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
-	p = (caddr_t)newmsg + sizeof(*msg0);
+	off += PFKEY_ALIGN8(sizeof(struct sadb_msg));
 
 	/* for authentication algorithm */
-	sup = (struct sadb_supported *)p;
-	sup->sadb_supported_len = PFKEY_UNIT64(alen);
-	sup->sadb_supported_exttype = SADB_EXT_SUPPORTED_AUTH;
-	p += sizeof(*sup);
-
-    {
-	int i;
-	struct sadb_alg *alg;
-	struct ah_algorithm *algo;
-
-	for (i = 1; i < SADB_AALG_MAX; i++) {
-		algo = &ah_algorithms[i];
-		alg = (struct sadb_alg *)p;
-		alg->sadb_alg_id = i;
-		alg->sadb_alg_ivlen = 0;
-		alg->sadb_alg_minbits = algo->keymin;
-		alg->sadb_alg_maxbits = algo->keymax;
-		p += sizeof(struct sadb_alg);
+	if (alen) {
+		sup = (struct sadb_supported *)(mtod(n, caddr_t) + off);
+		sup->sadb_supported_len = PFKEY_UNIT64(alen);
+		sup->sadb_supported_exttype = SADB_EXT_SUPPORTED_AUTH;
+		off += PFKEY_ALIGN8(sizeof(*sup));
+
+		for (i = 1; i < SADB_AALG_MAX; i++) {
+			struct ah_algorithm *aalgo;
+
+			aalgo = &ah_algorithms[i];
+			alg = (struct sadb_alg *)(mtod(n, caddr_t) + off);
+			alg->sadb_alg_id = i;
+			alg->sadb_alg_ivlen = 0;
+			alg->sadb_alg_minbits = aalgo->keymin;
+			alg->sadb_alg_maxbits = aalgo->keymax;
+			off += PFKEY_ALIGN8(sizeof(*alg));
+		}
 	}
-    }
 
 #ifdef IPSEC_ESP
 	/* for encryption algorithm */
-	sup = (struct sadb_supported *)p;
-	sup->sadb_supported_len = PFKEY_UNIT64(elen);
-	sup->sadb_supported_exttype = SADB_EXT_SUPPORTED_ENCRYPT;
-	p += sizeof(*sup);
-
-    {
-	int i;
-	struct sadb_alg *alg;
-	struct esp_algorithm *algo;
-
-	for (i = 1; i < SADB_EALG_MAX; i++) {
-		algo = &esp_algorithms[i];
-
-		alg = (struct sadb_alg *)p;
-		alg->sadb_alg_id = i;
-		if (algo && algo->ivlen) {
-			/*
-			 * give NULL to get the value preferred by algorithm
-			 * XXX SADB_X_EXT_DERIV ?
-			 */
-			alg->sadb_alg_ivlen = (*algo->ivlen)(NULL);
-		} else
-			alg->sadb_alg_ivlen = 0;
-		alg->sadb_alg_minbits = algo->keymin;
-		alg->sadb_alg_maxbits = algo->keymax;
-		p += sizeof(struct sadb_alg);
+	if (elen) {
+		sup = (struct sadb_supported *)(mtod(n, caddr_t) + off);
+		sup->sadb_supported_len = PFKEY_UNIT64(elen);
+		sup->sadb_supported_exttype = SADB_EXT_SUPPORTED_ENCRYPT;
+		off += PFKEY_ALIGN8(sizeof(*sup));
+
+		for (i = 1; i < SADB_EALG_MAX; i++) {
+			struct esp_algorithm *ealgo;
+
+			ealgo = &esp_algorithms[i];
+			alg = (struct sadb_alg *)(mtod(n, caddr_t) + off);
+			alg->sadb_alg_id = i;
+			if (ealgo && ealgo->ivlen) {
+				/*
+				 * give NULL to get the value preferred by
+				 * algorithm XXX SADB_X_EXT_DERIV ?
+				 */
+				alg->sadb_alg_ivlen = (*ealgo->ivlen)(NULL);
+			} else
+				alg->sadb_alg_ivlen = 0;
+			alg->sadb_alg_minbits = ealgo->keymin;
+			alg->sadb_alg_maxbits = ealgo->keymax;
+			off += PFKEY_ALIGN8(sizeof(struct sadb_alg));
+		}
 	}
-    }
 #endif
 
-	return newmsg;
-  }
+#ifdef DIGAGNOSTIC
+	if (off != len)
+		panic("length assumption failed in key_register");
+#endif
+
+	m_freem(m);
+	return key_sendup_mbuf(so, n, KEY_SENDUP_REGISTERED);
+    }
 }
 
 /*
@@ -4320,7 +6080,7 @@ key_freereg(so)
 	 * one socket is registered to multiple type of SA.
 	 */
 	for (i = 0; i <= SADB_SATYPE_MAX; i++) {
-		__LIST_FOREACH(reg, &regtree[i], chain) {
+		LIST_FOREACH(reg, &regtree[i], chain) {
 			if (reg->so == so
 			 && __LIST_CHAINED(reg)) {
 				LIST_REMOVE(reg, chain);
@@ -4329,14 +6089,14 @@ key_freereg(so)
 			}
 		}
 	}
-
+	
 	return;
 }
 
 /*
  * SADB_EXPIRE processing
  * send
- *   <base, SA, lifetime(C and one of HS), address(SD)>
+ *   <base, SA, SA2, lifetime(C and one of HS), address(SD)>
  * to KMD by PF_KEY.
  * NOTE: We send only soft lifetime extension.
  *
@@ -4349,6 +6109,10 @@ key_expire(sav)
 {
 	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()*/
@@ -4361,73 +6125,94 @@ key_expire(sav)
 	if ((satype = key_proto2satype(sav->sah->saidx.proto)) == 0)
 		panic("key_expire: invalid proto is passed.\n");
 
-    {
-	struct sadb_msg *newmsg = NULL;
-	u_int len;
-	caddr_t p;
-	int error;
-
-	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg)
-		+ sizeof(struct sadb_sa)
-		+ sizeof(struct sadb_lifetime)
-		+ sizeof(struct sadb_lifetime)
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(sav->sah->saidx.src.ss_len)
-		+ sizeof(struct sadb_address)
-		+ PFKEY_ALIGN8(sav->sah->saidx.dst.ss_len);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
-		printf("key_expire: No more memory.\n");
-		splx(s);
-		return ENOBUFS;
+	/* set msg header */
+	m = key_setsadbmsg(SADB_EXPIRE, 0, satype, sav->seq, 0, sav->refcnt);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
 	}
-	bzero((caddr_t)newmsg, len);
+	result = m;
 
-	/* set msg header */
-	p = key_setsadbmsg((caddr_t)newmsg, SADB_EXPIRE, len,
-	                   satype, sav->seq, 0,
-	                   sav->sah->saidx.mode, sav->refcnt);
+	/* create SA extension */
+	m = key_setsadbsa(sav);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
 	/* create SA extension */
-	p = key_setsadbsa(p, sav);
+	m = key_setsadbxsa2(sav->sah->saidx.mode, sav->sah->saidx.reqid);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
-	/* create lifetime extension */
-    {
-	struct sadb_lifetime *m_lt = (struct sadb_lifetime *)p;
-
-	m_lt->sadb_lifetime_len = PFKEY_UNIT64(sizeof(struct sadb_lifetime));
-	m_lt->sadb_lifetime_exttype = SADB_EXT_LIFETIME_CURRENT;
-	m_lt->sadb_lifetime_allocations = sav->lft_c->sadb_lifetime_allocations;
-	m_lt->sadb_lifetime_bytes = sav->lft_c->sadb_lifetime_bytes;
-	m_lt->sadb_lifetime_addtime = sav->lft_c->sadb_lifetime_addtime;
-	m_lt->sadb_lifetime_usetime = sav->lft_c->sadb_lifetime_usetime;
-	p += sizeof(struct sadb_lifetime);
-
-	/* copy SOFT lifetime extension. */
-	bcopy(sav->lft_s, p, sizeof(struct sadb_lifetime));
-	p += sizeof(struct sadb_lifetime);
-    }
+	/* 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);
+		error = ENOBUFS;
+		goto fail;
+	}
+	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->sadb_lifetime_allocations;
+	lt->sadb_lifetime_bytes = sav->lft_c->sadb_lifetime_bytes;
+	lt->sadb_lifetime_addtime = sav->lft_c->sadb_lifetime_addtime;
+	lt->sadb_lifetime_usetime = sav->lft_c->sadb_lifetime_usetime;
+	lt = (struct sadb_lifetime *)(mtod(m, caddr_t) + len / 2);
+	bcopy(sav->lft_s, lt, sizeof(*lt));
+	m_cat(result, m);
 
 	/* set sadb_address for source */
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_SRC,
-	                    (struct sockaddr *)&sav->sah->saidx.src,
-	                    _INALENBYAF(sav->sah->saidx.src.ss_family) << 3,
-	                    IPSEC_ULPROTO_ANY);
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_SRC,
+	    (struct sockaddr *)&sav->sah->saidx.src,
+	    sav->sah->saidx.src.ss_len << 3, IPSEC_ULPROTO_ANY);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
 	/* set sadb_address for destination */
-	p = key_setsadbaddr(p,
-	                    SADB_EXT_ADDRESS_DST,
-	                    (struct sockaddr *)&sav->sah->saidx.dst,
-	                    _INALENBYAF(sav->sah->saidx.dst.ss_family) << 3,
-	                    IPSEC_ULPROTO_ANY);
+	m = key_setsadbaddr(SADB_EXT_ADDRESS_DST,
+	    (struct sockaddr *)&sav->sah->saidx.dst,
+	    sav->sah->saidx.dst.ss_len << 3, IPSEC_ULPROTO_ANY);
+	if (!m) {
+		error = ENOBUFS;
+		goto fail;
+	}
+	m_cat(result, m);
 
-	error = key_sendall(newmsg, len);
+	if ((result->m_flags & M_PKTHDR) == 0)
+		goto fail;
+
+	if (result->m_len < sizeof(struct sadb_msg)) {
+		result = m_pullup(result, sizeof(struct sadb_msg));
+		if (result == NULL)
+			goto fail;
+	}
+
+	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);
+
+	return key_sendup_mbuf(NULL, result, KEY_SENDUP_REGISTERED);
+
+ fail:
+	if (result)
+		m_freem(result);
 	splx(s);
 	return error;
-    }
 }
 
 /*
@@ -4440,15 +6225,15 @@ key_expire(sav)
  * to the ikmpd.
  * NOTE: to do is only marking SADB_SASTATE_DEAD.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	NULL if fail.
- *	other if success, return pointer to the message to send.
+ * m will always be freed.
  */
-static struct sadb_msg *
-key_flush(mhp)
-	caddr_t *mhp;
+static int
+key_flush(so, m, mhp)
+	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
+	struct sadb_msg *newmsg;
 	struct secashead *sah, *nextsah;
 	struct secasvar *sav, *nextsav;
 	u_int16_t proto;
@@ -4456,33 +6241,30 @@ key_flush(mhp)
 	u_int stateidx;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_flush: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_flush: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
 	/* no SATYPE specified, i.e. flushing all SA. */
 	for (sah = LIST_FIRST(&sahtree);
 	     sah != NULL;
 	     sah = nextsah) {
-
 		nextsah = LIST_NEXT(sah, chain);
 
-		if (msg0->sadb_msg_satype != SADB_SATYPE_UNSPEC
+		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;
@@ -4491,33 +6273,30 @@ key_flush(mhp)
 				nextsav = LIST_NEXT(sav, chain);
 
 				key_sa_chgstate(sav, SADB_SASTATE_DEAD);
+				key_freesav(sav);
 			}
 		}
 
 		sah->state = SADB_SASTATE_DEAD;
 	}
 
-    {
-	struct sadb_msg *newmsg;
-	u_int len;
-
-	/* create new sadb_msg to reply. */
-	len = sizeof(struct sadb_msg);
-
-	KMALLOC(newmsg, struct sadb_msg *, len);
-	if (newmsg == NULL) {
+	if (m->m_len < sizeof(struct sadb_msg) ||
+	    sizeof(struct sadb_msg) > m->m_len + M_TRAILINGSPACE(m)) {
+#ifdef IPSEC_DEBUG
 		printf("key_flush: No more memory.\n");
-		msg0->sadb_msg_errno = ENOBUFS;
-		return NULL;
+#endif
+		return key_senderror(so, m, ENOBUFS);
 	}
-	bzero((caddr_t)newmsg, len);
 
-	bcopy((caddr_t)mhp[0], (caddr_t)newmsg, sizeof(*msg0));
+	if (m->m_next)
+		m_freem(m->m_next);
+	m->m_next = NULL;
+	m->m_pkthdr.len = m->m_len = sizeof(struct sadb_msg);
+	newmsg = mtod(m, struct sadb_msg *);
 	newmsg->sadb_msg_errno = 0;
-	newmsg->sadb_msg_len = PFKEY_UNIT64(len);
+	newmsg->sadb_msg_len = PFKEY_UNIT64(m->m_pkthdr.len);
 
-	return newmsg;
-    }
+	return key_sendup_mbuf(so, m, KEY_SENDUP_ALL);
 }
 
 /*
@@ -4530,206 +6309,171 @@ key_flush(mhp)
  *   <base> .....
  * to the ikmpd.
  *
- * IN:	mhp: pointer to the pointer to each header.
- * OUT:	error code.  0 on success.
+ * m will always be freed.
  */
 static int
-key_dump(mhp, so, target)
-	caddr_t *mhp;
+key_dump(so, m, mhp)
 	struct socket *so;
-	int target;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	struct secashead *sah;
 	struct secasvar *sav;
 	u_int16_t proto;
 	u_int stateidx;
 	u_int8_t satype;
 	u_int8_t state;
-	int len, cnt;
+	int cnt;
 	struct sadb_msg *newmsg;
+	struct mbuf *n;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_dump: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-
 	/* map satype to proto */
-	if ((proto = key_satype2proto(msg0->sadb_msg_satype)) == 0) {
+	if ((proto = key_satype2proto(mhp->msg->sadb_msg_satype)) == 0) {
+#ifdef IPSEC_DEBUG
 		printf("key_dump: invalid satype is passed.\n");
-		msg0->sadb_msg_errno = EINVAL;
-		return NULL;
+#endif
+		return key_senderror(so, m, EINVAL);
 	}
 
 	/* count sav entries to be sent to the userland. */
 	cnt = 0;
-	__LIST_FOREACH(sah, &sahtree, chain) {
-
-		if (msg0->sadb_msg_satype != SADB_SATYPE_UNSPEC
+	LIST_FOREACH(sah, &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) {
+			LIST_FOREACH(sav, &sah->savtree[state], chain) {
 				cnt++;
 			}
 		}
 	}
 
 	if (cnt == 0)
-		return ENOENT;
+		return key_senderror(so, m, ENOENT);
 
 	/* send this to the userland, one at a time. */
 	newmsg = NULL;
-	__LIST_FOREACH(sah, &sahtree, chain) {
-
-		if (msg0->sadb_msg_satype != SADB_SATYPE_UNSPEC
+	LIST_FOREACH(sah, &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) {
+#ifdef IPSEC_DEBUG
 			printf("key_dump: there was invalid proto in SAD.\n");
-			msg0->sadb_msg_errno = EINVAL;
-			return NULL;
+#endif
+			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) {
-
-				len = key_getmsglen(sav);
-				KMALLOC(newmsg, struct sadb_msg *, len);
-				if (newmsg == NULL) {
-					printf("key_dump: No more memory.\n");
-					return ENOBUFS;
-				}
-				bzero((caddr_t)newmsg, len);
+			LIST_FOREACH(sav, &sah->savtree[state], chain) {
+				n = key_setdumpsa(sav, SADB_DUMP, satype,
+				    --cnt, mhp->msg->sadb_msg_pid);
+				if (!n)
+					return key_senderror(so, m, ENOBUFS);
 
-				--cnt;
-				(void)key_setdumpsa(newmsg, sav, SADB_DUMP,
-				               satype, cnt, msg0->sadb_msg_pid);
-
-				key_sendup(so, newmsg, len, target);
-				KFREE(newmsg);
-				newmsg = NULL;
+				key_sendup_mbuf(so, n, KEY_SENDUP_ONE);
 			}
 		}
 	}
 
+	m_freem(m);
 	return 0;
 }
 
 /*
  * SADB_X_PROMISC processing
+ *
+ * m will always be freed.
  */
-static void
-key_promisc(mhp, so)
-	caddr_t *mhp;
+static int
+key_promisc(so, m, mhp)
 	struct socket *so;
+	struct mbuf *m;
+	const struct sadb_msghdr *mhp;
 {
-	struct sadb_msg *msg0;
 	int olen;
 
 	/* sanity check */
-	if (mhp == NULL || mhp[0] == NULL)
+	if (so == NULL || m == NULL || mhp == NULL || mhp->msg == NULL)
 		panic("key_promisc: NULL pointer is passed.\n");
 
-	msg0 = (struct sadb_msg *)mhp[0];
-	olen = PFKEY_UNUNIT64(msg0->sadb_msg_len);
+	olen = PFKEY_UNUNIT64(mhp->msg->sadb_msg_len);
 
 	if (olen < sizeof(struct sadb_msg)) {
-		return;
+#if 1
+		return key_senderror(so, m, EINVAL);
+#else
+		m_freem(m);
+		return 0;
+#endif
 	} else if (olen == sizeof(struct sadb_msg)) {
 		/* enable/disable promisc mode */
 		struct keycb *kp;
-		int target = 0;
-
-		target = KEY_SENDUP_ONE;
 
-		if (so == NULL) {
-			return;
-		}
-		if ((kp = (struct keycb *)sotorawcb(so)) == NULL) {
-			msg0->sadb_msg_errno = EINVAL;
-			goto sendorig;
-		}
-		msg0->sadb_msg_errno = 0;
-		if (msg0->sadb_msg_satype == 1 || msg0->sadb_msg_satype == 0) {
-			kp->kp_promisc = msg0->sadb_msg_satype;
-		} else {
-			msg0->sadb_msg_errno = EINVAL;
-			goto sendorig;
+		if ((kp = (struct keycb *)sotorawcb(so)) == NULL)
+			return key_senderror(so, m, EINVAL);
+		mhp->msg->sadb_msg_errno = 0;
+		switch (mhp->msg->sadb_msg_satype) {
+		case 0:
+		case 1:
+			kp->kp_promisc = mhp->msg->sadb_msg_satype;
+			break;
+		default:
+			return key_senderror(so, m, EINVAL);
 		}
 
 		/* send the original message back to everyone */
-		msg0->sadb_msg_errno = 0;
-		target = KEY_SENDUP_ALL;
-sendorig:
-		key_sendup(so, msg0, PFKEY_UNUNIT64(msg0->sadb_msg_len), target);
+		mhp->msg->sadb_msg_errno = 0;
+		return key_sendup_mbuf(so, m, KEY_SENDUP_ALL);
 	} else {
 		/* send packet as is */
-		struct sadb_msg *msg;
-		int len;
 
-		len = olen - sizeof(struct sadb_msg);
-		KMALLOC(msg, struct sadb_msg *, len);
-		if (msg == NULL) {
-			msg0->sadb_msg_errno = ENOBUFS;
-			key_sendup(so, msg0, PFKEY_UNUNIT64(msg0->sadb_msg_len),
-				KEY_SENDUP_ONE);	/*XXX*/
-		}
+		m_adj(m, PFKEY_ALIGN8(sizeof(struct sadb_msg)));
 
-		/* XXX if sadb_msg_seq is specified, send to specific pid */
-		key_sendup(so, msg, len, KEY_SENDUP_ALL);
-		KFREE(msg);
+		/* TODO: if sadb_msg_seq is specified, send to specific pid */
+		return key_sendup_mbuf(so, m, KEY_SENDUP_ALL);
 	}
 }
 
-/*
- * send message to the socket.
- * OUT:
- *	0	: success
- *	others	: fail
- */
-static int
-key_sendall(msg, len)
-	struct sadb_msg *msg;
-	u_int len;
-{
-	struct secreg *reg;
-	int error = 0;
-
-	/* sanity check */
-	if (msg == NULL)
-		panic("key_sendall: NULL pointer is passed.\n");
-
-	/* search table registerd socket to send a message. */
-	__LIST_FOREACH(reg, &regtree[msg->sadb_msg_satype], chain) {
-		error = key_sendup(reg->so, msg, len, KEY_SENDUP_ONE);
-		if (error != 0) {
-			if (error == ENOBUFS)
-				printf("key_sendall: No more memory.\n");
-			else {
-				printf("key_sendall: key_sendup returned %d\n",
-					error);
-			}
-			KFREE(msg);
-			return error;
-		}
-	}
-
-	KFREE(msg);
-	return 0;
-}
+static int (*key_typesw[]) __P((struct socket *, struct mbuf *,
+		const struct sadb_msghdr *)) = {
+	NULL,		/* SADB_RESERVED */
+	key_getspi,	/* SADB_GETSPI */
+	key_update,	/* SADB_UPDATE */
+	key_add,	/* SADB_ADD */
+	key_delete,	/* SADB_DELETE */
+	key_get,	/* SADB_GET */
+	key_acquire2,	/* SADB_ACQUIRE */
+	key_register,	/* SADB_REGISTER */
+	NULL,		/* SADB_EXPIRE */
+	key_flush,	/* SADB_FLUSH */
+	key_dump,	/* SADB_DUMP */
+	key_promisc,	/* SADB_X_PROMISC */
+	NULL,		/* SADB_X_PCHANGE */
+	key_spdadd,	/* SADB_X_SPDUPDATE */
+	key_spdadd,	/* SADB_X_SPDADD */
+	key_spddelete,	/* SADB_X_SPDDELETE */
+	key_spdget,	/* SADB_X_SPDGET */
+	NULL,		/* SADB_X_SPDACQUIRE */
+	key_spddump,	/* SADB_X_SPDDUMP */
+	key_spdflush,	/* SADB_X_SPDFLUSH */
+	key_spdadd,	/* SADB_X_SPDSETIDX */
+	NULL,		/* SADB_X_SPDEXPIRE */
+	key_spddelete2,	/* SADB_X_SPDDELETE2 */
+};
 
 /*
  * parse sadb_msg buffer to process PFKEYv2,
@@ -4743,49 +6487,103 @@ key_sendall(msg, len)
  *    length for buffer to send to user process.
  */
 int
-key_parse(msgp, so, targetp)
-	struct sadb_msg **msgp;
+key_parse(m, so)
+	struct mbuf *m;
 	struct socket *so;
-	int *targetp;
 {
-	struct sadb_msg *msg = *msgp, *newmsg = NULL;
-	caddr_t mhp[SADB_EXT_MAX + 1];
+	struct sadb_msg *msg;
+	struct sadb_msghdr mh;
 	u_int orglen;
 	int error;
+	int target;
 
 	/* sanity check */
-	if (msg == NULL || so == NULL)
+	if (m == NULL || so == NULL)
 		panic("key_parse: NULL pointer is passed.\n");
 
+#if 0	/*kdebug_sadb assumes msg in linear buffer*/
 	KEYDEBUG(KEYDEBUG_KEY_DUMP,
 		printf("key_parse: passed sadb_msg\n");
 		kdebug_sadb(msg));
+#endif
 
+	if (m->m_len < sizeof(struct sadb_msg)) {
+		m = m_pullup(m, sizeof(struct sadb_msg));
+		if (!m)
+			return ENOBUFS;
+	}
+	msg = mtod(m, struct sadb_msg *);
 	orglen = PFKEY_UNUNIT64(msg->sadb_msg_len);
+	target = KEY_SENDUP_ONE;
 
-	if (targetp)
-		*targetp = KEY_SENDUP_ONE;
+	if ((m->m_flags & M_PKTHDR) == 0 ||
+	    m->m_pkthdr.len != m->m_pkthdr.len) {
+#ifdef IPSEC_DEBUG
+		printf("key_parse: invalid message length.\n");
+#endif
+		pfkeystat.out_invlen++;
+		error = EINVAL;
+		goto senderror;
+	}
 
-	/* check version */
 	if (msg->sadb_msg_version != PF_KEY_V2) {
+#ifdef IPSEC_DEBUG
 		printf("key_parse: PF_KEY version %u is mismatched.\n",
 		    msg->sadb_msg_version);
-		return EINVAL;
+#endif
+		pfkeystat.out_invver++;
+		error = EINVAL;
+		goto senderror;
 	}
 
-	/* check type */
 	if (msg->sadb_msg_type > SADB_MAX) {
+#ifdef IPSEC_DEBUG
 		printf("key_parse: invalid type %u is passed.\n",
 		    msg->sadb_msg_type);
-		msg->sadb_msg_errno = EINVAL;
-		return orglen;
+#endif
+		pfkeystat.out_invmsgtype++;
+		error = EINVAL;
+		goto senderror;
 	}
 
-	/* align message. */
-	if (key_align(msg, mhp) != 0) {
-		msg->sadb_msg_errno = EINVAL;
-		return orglen;
+	/* for old-fashioned code - should be nuked */
+	if (m->m_pkthdr.len > MCLBYTES) {
+		m_freem(m);
+		return ENOBUFS;
 	}
+	if (m->m_next) {
+		struct mbuf *n;
+
+		MGETHDR(n, M_DONTWAIT, MT_DATA);
+		if (n && m->m_pkthdr.len > MHLEN) {
+			MCLGET(n, M_DONTWAIT);
+			if ((n->m_flags & M_EXT) == 0) {
+				m_free(n);
+				n = NULL;
+			}
+		}
+		if (!n) {
+			m_freem(m);
+			return ENOBUFS;
+		}
+		m_copydata(m, 0, m->m_pkthdr.len, mtod(n, caddr_t));
+		n->m_pkthdr.len = n->m_len = m->m_pkthdr.len;
+		n->m_next = NULL;
+		m_freem(m);
+		m = n;
+	}
+
+	/* align the mbuf chain so that extensions are in contiguous region. */
+	error = key_align(m, &mh);
+	if (error)
+		return error;
+
+	if (m->m_next) {	/*XXX*/
+		m_freem(m);
+		return ENOBUFS;
+	}
+
+	msg = mh.msg;
 
 	/* check SA type */
 	switch (msg->sadb_msg_satype) {
@@ -4798,87 +6596,150 @@ key_parse(msgp, so, targetp)
 		case SADB_GET:
 		case SADB_ACQUIRE:
 		case SADB_EXPIRE:
+#ifdef IPSEC_DEBUG
 			printf("key_parse: must specify satype "
 				"when msg type=%u.\n",
 				msg->sadb_msg_type);
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+#endif
+			pfkeystat.out_invsatype++;
+			error = EINVAL;
+			goto senderror;
 		}
 		break;
 	case SADB_SATYPE_AH:
 	case SADB_SATYPE_ESP:
+#if 1 /*nonstandard*/
+	case SADB_X_SATYPE_IPCOMP:
+#endif
 		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:
-			printf("key_parse: illegal satype=%u\n", msg->sadb_msg_type);
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+		case SADB_X_SPDSETIDX:
+		case SADB_X_SPDUPDATE:
+		case SADB_X_SPDDELETE2:
+#ifdef IPSEC_DEBUG
+			printf("key_parse: illegal satype=%u\n",
+			    msg->sadb_msg_type);
+#endif
+			pfkeystat.out_invsatype++;
+			error = EINVAL;
+			goto senderror;
 		}
 		break;
 	case SADB_SATYPE_RSVP:
 	case SADB_SATYPE_OSPFV2:
 	case SADB_SATYPE_RIPV2:
 	case SADB_SATYPE_MIP:
+#ifdef IPSEC_DEBUG
 		printf("key_parse: type %u isn't supported.\n",
 		    msg->sadb_msg_satype);
-		msg->sadb_msg_errno = EOPNOTSUPP;
-		return orglen;
-	case 1:	/* XXX: What does it do ? */
+#endif
+		pfkeystat.out_invsatype++;
+		error = EOPNOTSUPP;
+		goto senderror;
+	case 1:	/* XXX: What does it do? */
 		if (msg->sadb_msg_type == SADB_X_PROMISC)
 			break;
 		/*FALLTHROUGH*/
 	default:
+#ifdef IPSEC_DEBUG
 		printf("key_parse: invalid type %u is passed.\n",
 		    msg->sadb_msg_satype);
-		msg->sadb_msg_errno = EINVAL;
-		return orglen;
+#endif
+		pfkeystat.out_invsatype++;
+		error = EINVAL;
+		goto senderror;
 	}
 
 	/* check field of upper layer protocol and address family */
-	if (mhp[SADB_EXT_ADDRESS_SRC] != NULL
-	 && mhp[SADB_EXT_ADDRESS_DST] != NULL) {
+	if (mh.ext[SADB_EXT_ADDRESS_SRC] != NULL
+	 && mh.ext[SADB_EXT_ADDRESS_DST] != NULL) {
 		struct sadb_address *src0, *dst0;
-		u_int prefix;
+		u_int plen;
 
-		src0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_SRC]);
-		dst0 = (struct sadb_address *)(mhp[SADB_EXT_ADDRESS_DST]);
+		src0 = (struct sadb_address *)(mh.ext[SADB_EXT_ADDRESS_SRC]);
+		dst0 = (struct sadb_address *)(mh.ext[SADB_EXT_ADDRESS_DST]);
 
 		/* check upper layer protocol */
 		if (src0->sadb_address_proto != dst0->sadb_address_proto) {
+#ifdef IPSEC_DEBUG
 			printf("key_parse: upper layer protocol mismatched.\n");
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+#endif
+			pfkeystat.out_invaddr++;
+			error = EINVAL;
+			goto senderror;
 		}
 
 		/* check family */
-		if (PFKEY_ADDR_SADDR(src0)->sa_family
-		 != PFKEY_ADDR_SADDR(dst0)->sa_family) {
+		if (PFKEY_ADDR_SADDR(src0)->sa_family !=
+		    PFKEY_ADDR_SADDR(dst0)->sa_family) {
+#ifdef IPSEC_DEBUG
 			printf("key_parse: address family mismatched.\n");
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+#endif
+			pfkeystat.out_invaddr++;
+			error = EINVAL;
+			goto senderror;
+		}
+		if (PFKEY_ADDR_SADDR(src0)->sa_len !=
+		    PFKEY_ADDR_SADDR(dst0)->sa_len) {
+#ifdef IPSEC_DEBUG
+			printf("key_parse: address struct size mismatched.\n");
+#endif
+			pfkeystat.out_invaddr++;
+			error = EINVAL;
+			goto senderror;
 		}
 
-		prefix = _INALENBYAF(PFKEY_ADDR_SADDR(src0)->sa_family) << 3;
-
-		/* check max prefixlen */
-		if (prefix < src0->sadb_address_prefixlen
-		 || prefix < dst0->sadb_address_prefixlen) {
-			printf("key_parse: illegal prefixlen.\n");
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+		switch (PFKEY_ADDR_SADDR(src0)->sa_family) {
+		case AF_INET:
+			if (PFKEY_ADDR_SADDR(src0)->sa_len !=
+			    sizeof(struct sockaddr_in)) {
+				pfkeystat.out_invaddr++;
+				error = EINVAL;
+				goto senderror;
+			}
+			break;
+		case AF_INET6:
+			if (PFKEY_ADDR_SADDR(src0)->sa_len !=
+			    sizeof(struct sockaddr_in6)) {
+				pfkeystat.out_invaddr++;
+				error = EINVAL;
+				goto senderror;
+			}
+			break;
+		default:
+#ifdef IPSEC_DEBUG
+			printf("key_parse: unsupported address family.\n");
+#endif
+			pfkeystat.out_invaddr++;
+			error = EAFNOSUPPORT;
+			goto senderror;
 		}
 
 		switch (PFKEY_ADDR_SADDR(src0)->sa_family) {
 		case AF_INET:
+			plen = sizeof(struct in_addr) << 3;
+			break;
 		case AF_INET6:
+			plen = sizeof(struct in6_addr) << 3;
 			break;
 		default:
-			printf("key_parse: invalid address family.\n");
-			msg->sadb_msg_errno = EINVAL;
-			return orglen;
+			plen = 0;	/*fool gcc*/
+			break;
+		}
+
+		/* check max prefix length */
+		if (src0->sadb_address_prefixlen > plen ||
+		    dst0->sadb_address_prefixlen > plen) {
+#ifdef IPSEC_DEBUG
+			printf("key_parse: illegal prefixlen.\n");
+#endif
+			pfkeystat.out_invaddr++;
+			error = EINVAL;
+			goto senderror;
 		}
 
 		/*
@@ -4887,196 +6748,85 @@ key_parse(msgp, so, targetp)
 		 */
 	}
 
-	switch (msg->sadb_msg_type) {
-	case SADB_GETSPI:
-		if ((newmsg = key_getspi(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		break;
-
-	case SADB_UPDATE:
-		if ((newmsg = key_update(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		break;
-
-	case SADB_ADD:
-		if ((newmsg = key_add(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		break;
-
-	case SADB_DELETE:
-		if ((newmsg = key_delete(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		break;
-
-	case SADB_GET:
-		if ((newmsg = key_get(mhp)) == NULL)
-			return orglen;
-		break;
-
-	case SADB_ACQUIRE:
-		if ((newmsg = key_acquire2(mhp)) == NULL)
-			return orglen;
-
-		if (newmsg == (struct sadb_msg *)~0) {
-			/*
-			 * It's not need to reply because of the message
-			 * that was reporting an error occured from the KMd.
-			 */
-			KFREE(msg);
-			return 0;
-		}
-		break;
-
-	case SADB_REGISTER:
-		if ((newmsg = key_register(mhp, so)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_REGISTERED;
-		break;
-
-	case SADB_EXPIRE:
-		printf("key_parse: why is SADB_EXPIRE received ?\n");
-		msg->sadb_msg_errno = EINVAL;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		return orglen;
-
-	case SADB_FLUSH:
-		if ((newmsg = key_flush(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-		break;
-
-	case SADB_DUMP:
-		/* key_dump will call key_sendup() on her own */
-		error = key_dump(mhp, so, KEY_SENDUP_ONE);
-		if (error) {
-			msg->sadb_msg_errno = error;
-			return orglen;
-		} else {
-			KFREE(msg);
-			return 0;
-		}
-	        break;
-
-	case SADB_X_PROMISC:
-		/* everything is handled in key_promisc() */
-		key_promisc(mhp, so);
-		KFREE(msg);
-		return 0;	/*nothing to reply*/
-
-	case SADB_X_PCHANGE:
-		printf("key_parse: SADB_X_PCHANGE isn't supported.\n");
-		msg->sadb_msg_errno = EINVAL;
-		return orglen;
-
-	case SADB_X_SPDADD:
-		if ((newmsg = key_spdadd(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-	        break;
-
-	case SADB_X_SPDDELETE:
-		if ((newmsg = key_spddelete(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-	        break;
-
-	case SADB_X_SPDDUMP:
-		/* key_spddump will call key_sendup() on her own */
-		error = key_spddump(mhp, so, KEY_SENDUP_ONE);
-		if (error) {
-			msg->sadb_msg_errno = error;
-			return orglen;
-		} else {
-			KFREE(msg);
-			return 0;
-		}
-	        break;
+	if (msg->sadb_msg_type >= sizeof(key_typesw)/sizeof(key_typesw[0]) ||
+	    key_typesw[msg->sadb_msg_type] == NULL) {
+		pfkeystat.out_invmsgtype++;
+		error = EINVAL;
+		goto senderror;
+	}
 
+	return (*key_typesw[msg->sadb_msg_type])(so, m, &mh);
 
-	case SADB_X_SPDFLUSH:
-		if ((newmsg = key_spdflush(mhp)) == NULL)
-			return orglen;
-		if (targetp)
-			*targetp = KEY_SENDUP_ALL;
-	        break;
+senderror:
+	msg->sadb_msg_errno = error;
+	return key_sendup_mbuf(so, m, target);
+}
 
-	default:
-		msg->sadb_msg_errno = EOPNOTSUPP;
-		return orglen;
-	}
+static int
+key_senderror(so, m, code)
+	struct socket *so;
+	struct mbuf *m;
+	int code;
+{
+	struct sadb_msg *msg;
 
-	/* switch from old sadb_msg to new one if success. */
-	KFREE(msg);
-	*msgp = newmsg;
+	if (m->m_len < sizeof(struct sadb_msg))
+		panic("invalid mbuf passed to key_senderror");
 
-	return PFKEY_UNUNIT64((*msgp)->sadb_msg_len);
+	msg = mtod(m, struct sadb_msg *);
+	msg->sadb_msg_errno = code;
+	return key_sendup_mbuf(so, m, KEY_SENDUP_ONE);
 }
 
 /*
  * set the pointer to each header into message buffer.
- * IN:	msg: pointer to message buffer.
- *	mhp: pointer to the buffer allocated like below:
- *		caddr_t mhp[SADB_EXT_MAX + 1];
- * OUT: 0:
- *      EINVAL:
+ * m will be freed on error.
+ * XXX larger-than-MCLBYTES extension?
  */
 static int
-key_align(msg, mhp)
-	struct sadb_msg *msg;
-	caddr_t *mhp;
+key_align(m, mhp)
+	struct mbuf *m;
+	struct sadb_msghdr *mhp;
 {
+	struct mbuf *n;
 	struct sadb_ext *ext;
-	int tlen, extlen;
-	int i;
+	size_t off, end;
+	int extlen;
+	int toff;
 
 	/* sanity check */
-	if (msg == NULL || mhp == NULL)
+	if (m == NULL || mhp == NULL)
 		panic("key_align: NULL pointer is passed.\n");
+	if (m->m_len < sizeof(struct sadb_msg))
+		panic("invalid mbuf passed to key_align");
 
 	/* initialize */
-	for (i = 0; i < SADB_EXT_MAX + 1; i++)
-		mhp[i] = NULL;
-
-	mhp[0] = (caddr_t)msg;
+	bzero(mhp, sizeof(*mhp));
 
-	tlen = PFKEY_UNUNIT64(msg->sadb_msg_len) - sizeof(struct sadb_msg);
-	ext = (struct sadb_ext *)((caddr_t)msg + sizeof(struct sadb_msg));
+	mhp->msg = mtod(m, struct sadb_msg *);
+	mhp->ext[0] = (struct sadb_ext *)mhp->msg;	/*XXX backward compat */
 
-	while (tlen > 0) {
-		/* duplicate check */
-		/* XXX Are there duplication either KEY_AUTH or KEY_ENCRYPT ?*/
-		if (mhp[ext->sadb_ext_type] != NULL) {
-			printf("key_align: duplicate ext_type %u is passed.\n",
-				ext->sadb_ext_type);
-			return EINVAL;
+	end = PFKEY_UNUNIT64(mhp->msg->sadb_msg_len);
+	extlen = end;	/*just in case extlen is not updated*/
+	for (off = sizeof(struct sadb_msg); off < end; off += extlen) {
+		n = m_pulldown(m, off, sizeof(struct sadb_ext), &toff);
+		if (!n) {
+			/* m is already freed */
+			return ENOBUFS;
 		}
+		ext = (struct sadb_ext *)(mtod(n, caddr_t) + toff);
 
 		/* set pointer */
 		switch (ext->sadb_ext_type) {
 		case SADB_EXT_SA:
-		case SADB_EXT_LIFETIME_CURRENT:
-		case SADB_EXT_LIFETIME_HARD:
-		case SADB_EXT_LIFETIME_SOFT:
 		case SADB_EXT_ADDRESS_SRC:
 		case SADB_EXT_ADDRESS_DST:
 		case SADB_EXT_ADDRESS_PROXY:
+		case SADB_EXT_LIFETIME_CURRENT:
+		case SADB_EXT_LIFETIME_HARD:
+		case SADB_EXT_LIFETIME_SOFT:
 		case SADB_EXT_KEY_AUTH:
-			/* must to be chek weak keys. */
 		case SADB_EXT_KEY_ENCRYPT:
-			/* must to be chek weak keys. */
 		case SADB_EXT_IDENTITY_SRC:
 		case SADB_EXT_IDENTITY_DST:
 		case SADB_EXT_SENSITIVITY:
@@ -5085,17 +6835,116 @@ key_align(msg, mhp)
 		case SADB_EXT_SUPPORTED_ENCRYPT:
 		case SADB_EXT_SPIRANGE:
 		case SADB_X_EXT_POLICY:
-			mhp[ext->sadb_ext_type] = (caddr_t)ext;
+		case SADB_X_EXT_SA2:
+			/* duplicate check */
+			/*
+			 * XXX Are there duplication payloads of either
+			 * KEY_AUTH or KEY_ENCRYPT ?
+			 */
+			if (mhp->ext[ext->sadb_ext_type] != NULL) {
+#ifdef IPSEC_DEBUG
+				printf("key_align: duplicate ext_type %u "
+					"is passed.\n",
+					ext->sadb_ext_type);
+#endif
+				m_freem(m);
+				pfkeystat.out_dupext++;
+				return EINVAL;
+			}
 			break;
 		default:
+#ifdef IPSEC_DEBUG
 			printf("key_align: invalid ext_type %u is passed.\n",
 				ext->sadb_ext_type);
+#endif
+			m_freem(m);
+			pfkeystat.out_invexttype++;
 			return EINVAL;
 		}
 
 		extlen = PFKEY_UNUNIT64(ext->sadb_ext_len);
-		tlen -= extlen;
-		ext = (struct sadb_ext *)((caddr_t)ext + extlen);
+
+		if (key_validate_ext(ext, extlen)) {
+			m_freem(m);
+			pfkeystat.out_invlen++;
+			return EINVAL;
+		}
+
+		n = m_pulldown(m, off, extlen, &toff);
+		if (!n) {
+			/* m is already freed */
+			return ENOBUFS;
+		}
+		ext = (struct sadb_ext *)(mtod(n, caddr_t) + toff);
+
+		mhp->ext[ext->sadb_ext_type] = ext;
+		mhp->extoff[ext->sadb_ext_type] = off;
+		mhp->extlen[ext->sadb_ext_type] = extlen;
+	}
+
+	if (off != end) {
+		m_freem(m);
+		pfkeystat.out_invlen++;
+		return EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+key_validate_ext(ext, len)
+	const struct sadb_ext *ext;
+	int len;
+{
+	struct sockaddr *sa;
+	enum { NONE, ADDR } checktype = NONE;
+	int baselen;
+	const int sal = offsetof(struct sockaddr, sa_len) + sizeof(sa->sa_len);
+
+	if (len != PFKEY_UNUNIT64(ext->sadb_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]))
+		return EINVAL;
+	if (!minsize[ext->sadb_ext_type] || len < minsize[ext->sadb_ext_type])
+		return EINVAL;
+	if (maxsize[ext->sadb_ext_type] && len > maxsize[ext->sadb_ext_type])
+		return EINVAL;
+
+	/* more checks based on sadb_ext_type XXX need more */
+	switch (ext->sadb_ext_type) {
+	case SADB_EXT_ADDRESS_SRC:
+	case SADB_EXT_ADDRESS_DST:
+	case SADB_EXT_ADDRESS_PROXY:
+		baselen = PFKEY_ALIGN8(sizeof(struct sadb_address));
+		checktype = ADDR;
+		break;
+	case SADB_EXT_IDENTITY_SRC:
+	case SADB_EXT_IDENTITY_DST:
+		if (((struct sadb_ident *)ext)->sadb_ident_type ==
+		    SADB_X_IDENTTYPE_ADDR) {
+			baselen = PFKEY_ALIGN8(sizeof(struct sadb_ident));
+			checktype = ADDR;
+		} else
+			checktype = NONE;
+		break;
+	default:
+		checktype = NONE;
+		break;
+	}
+
+	switch (checktype) {
+	case NONE:
+		break;
+	case ADDR:
+		sa = (struct sockaddr *)((caddr_t)ext + baselen);
+		if (len < baselen + sal)
+			return EINVAL;
+		if (baselen + PFKEY_ALIGN8(sa->sa_len) != len)
+			return EINVAL;
+		break;
 	}
 
 	return 0;
@@ -5121,6 +6970,7 @@ key_init()
 #ifndef IPSEC_NONBLOCK_ACQUIRE
 	LIST_INIT(&acqtree);
 #endif
+	LIST_INIT(&spacqtree);
 
 	/* system default */
 	ip4_def_policy.policy = IPSEC_POLICY_NONE;
@@ -5131,7 +6981,7 @@ key_init()
 #endif
 
 #ifndef IPSEC_DEBUG2
-	timeout((void *)key_timehandler, (void *)0, 100);
+	timeout((void *)key_timehandler, (void *)0, hz);
 #endif /*IPSEC_DEBUG2*/
 
 	/* initialize key statistics */
@@ -5167,9 +7017,7 @@ key_checktunnelsanity(sav, family, src, dst)
 }
 
 #if 0
-#ifdef __FreeBSD__
-#define	hostnamelen	strlen(hostname)
-#endif
+#define hostnamelen	strlen(hostname)
 
 /*
  * Get FQDN for the host.
@@ -5245,6 +7093,10 @@ key_sa_recordxfer(sav, m)
 	if (!sav->lft_c)
 		return;
 
+	/*
+	 * XXX Currently, there is a difference of bytes size
+	 * between inbound and outbound processing.
+	 */
 	sav->lft_c->sadb_lifetime_bytes += m->m_pkthdr.len;
 	/* to check bytes lifetime is done in key_timehandler(). */
 
@@ -5285,7 +7137,7 @@ key_sa_routechange(dst)
 	struct secashead *sah;
 	struct route *ro;
 
-	__LIST_FOREACH(sah, &sahtree, chain) {
+	LIST_FOREACH(sah, &sahtree, chain) {
 		ro = &sah->sa_route;
 		if (ro->ro_rt && dst->sa_len == ro->ro_dst.sa_len
 		 && bcmp(dst, &ro->ro_dst, dst->sa_len) == 0) {
@@ -5314,3 +7166,42 @@ key_sa_chgstate(sav, state)
 	sav->state = state;
 	LIST_INSERT_HEAD(&sav->sah->savtree[state], sav, chain);
 }
+
+/* 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;
+}