i don't like splitting source code in too many source files but ikev2.c

has grown too large, so split it in 3 files and rename a few functions
to organize the code a bit better.

8 files changed, 2009 insertions, 1912 deletions

diff --git a/sbin/iked/Makefile b/sbin/iked/Makefile
index 90d2c4017ca..9ff2dcf1a2f 100644
--- a/sbin/iked/Makefile
+++ b/sbin/iked/Makefile
@@ -1,12 +1,12 @@
-# $OpenBSD: Makefile,v 1.2 2010/06/03 17:05:01 reyk Exp $
+# $OpenBSD: Makefile,v 1.3 2010/06/10 08:29:47 reyk Exp $
 # $vantronix: Makefile,v 1.22 2010/06/02 12:22:58 reyk Exp $
 
 PROG=		iked
-SRCS=		dh.c iked.c ikev1.c ikev2.c log.c \
-		proc.c util.c config.c policy.c crypto.c ca.c pfkey.c \
-		control.c chap_ms.c eap.c
+SRCS=		ca.c chap_ms.c config.c control.c crypto.c dh.c \
+		eap.c iked.c ikev1.c ikev2.c ikev2_msg.c ikev2_pld.c \
+		log.c pfkey.c policy.c proc.c util.c
+SRCS+=		${.OBJDIR}/eap_map.c ${.OBJDIR}/ikev2_map.c
 SRCS+=		parse.y
-SRCS+=		${.OBJDIR}/ikev2_map.c ${.OBJDIR}/eap_map.c
 MAN=		iked.conf.5 iked.8
 #NOMAN=		yes
 
diff --git a/sbin/iked/ca.c b/sbin/iked/ca.c
index 8d34e8d0b3e..7dd572fb3e5 100644
--- a/sbin/iked/ca.c
+++ b/sbin/iked/ca.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ca.c,v 1.1 2010/06/03 16:41:12 reyk Exp $	*/
+/*	$OpenBSD: ca.c,v 1.2 2010/06/10 08:29:47 reyk Exp $	*/
 /*	$vantronix: ca.c,v 1.29 2010/06/02 12:22:58 reyk Exp $	*/
 
 /*
@@ -252,7 +252,7 @@ ca_setauth(struct iked *env, struct iked_sa *sa,
 
 	if (type == IKEV2_AUTH_SHARED_KEY_MIC) {
 		sa->sa_stateflags |= IKED_REQ_AUTH;
-		return (ikev2_message_authsign(env, sa,
+		return (ikev2_msg_authsign(env, sa,
 		    &policy->pol_auth, authmsg));
 	}
 
@@ -423,7 +423,7 @@ ca_getauth(struct iked *env, struct imsg *imsg)
 		id = &sa.sa_rcert;
 	memcpy(id, &store->ca_privkey, sizeof(*id));
 
-	if (ikev2_message_authsign(env, &sa, &policy.pol_auth, authmsg) != 0) {
+	if (ikev2_msg_authsign(env, &sa, &policy.pol_auth, authmsg) != 0) {
 		log_debug("%s: AUTH sign failed", __func__);
 		policy.pol_auth.auth_method = IKEV2_AUTH_NONE;
 	}
diff --git a/sbin/iked/iked.h b/sbin/iked/iked.h
index 7ef52708ea0..e22a24247a3 100644
--- a/sbin/iked/iked.h
+++ b/sbin/iked/iked.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: iked.h,v 1.1 2010/06/03 16:41:12 reyk Exp $	*/
+/*	$OpenBSD: iked.h,v 1.2 2010/06/10 08:29:47 reyk Exp $	*/
 /*	$vantronix: iked.h,v 1.61 2010/06/03 07:57:33 reyk Exp $	*/
 
 /*
@@ -29,6 +29,21 @@
 #define _IKED_H
 
 /*
+ * Common IKEv1/IKEv2 header
+ */
+
+struct ike_header {
+	u_int64_t	 ike_ispi;		/* Initiator cookie */
+	u_int64_t	 ike_rspi;		/* Responder cookie */
+	u_int8_t	 ike_nextpayload;	/* Next payload type */
+	u_int8_t	 ike_version;		/* Major/Minor version number */
+	u_int8_t	 ike_exchange;		/* Exchange type */
+	u_int8_t	 ike_flags;		/* Message options */
+	u_int32_t	 ike_msgid;		/* Message identifier */
+	u_int32_t	 ike_length;		/* Total message length */
+} __packed;
+
+/*
  * Common daemon infrastructure, local imsg etc.
  */
 
@@ -538,14 +553,58 @@ pid_t	 ikev1(struct iked *, struct iked_proc *);
 
 /* ikev2.c */
 pid_t	 ikev2(struct iked *, struct iked_proc *);
-int	 ikev2_send_ike_e(struct iked *, struct iked_sa *, struct ibuf *,
-	    u_int8_t, u_int8_t, int);
-int	 ikev2_message_authsign(struct iked *, struct iked_sa *,
-	    struct iked_auth *, struct ibuf *);
+void	 ikev2_recv(struct iked *, struct iked_message *);
+int	 ikev2_sa_negotiate(struct iked_sa *, struct iked_proposals *,
+	    struct iked_proposals *, u_int8_t);
+int	 ikev2_childsa_delete(struct iked *, struct iked_sa *,
+	    u_int8_t, u_int64_t, u_int64_t *, int);
 struct ibuf *
 	 ikev2_prfplus(struct iked_hash *, struct ibuf *, struct ibuf *,
 	    size_t);
 ssize_t	 ikev2_psk(struct iked_sa *, u_int8_t *, size_t, u_int8_t **);
+ssize_t	 ikev2_nat_detection(struct iked_message *, void *, size_t,
+	    u_int, int);
+int	 ikev2_send_informational(struct iked *, struct iked_message *);
+int	 ikev2_send_ike_e(struct iked *, struct iked_sa *, struct ibuf *,
+	    u_int8_t, u_int8_t, int);
+struct ike_header *
+	 ikev2_add_header(struct ibuf *, struct iked_sa *,
+	    u_int32_t, u_int8_t, u_int8_t, u_int8_t);
+int	 ikev2_set_header(struct ike_header *, size_t);
+struct ikev2_payload *
+	 ikev2_add_payload(struct ibuf *);
+int	 ikev2_next_payload(struct ikev2_payload *, size_t,
+	    u_int8_t);
+
+/* ikev2_msg.c */
+void	 ikev2_msg_cb(int, short, void *);
+struct ibuf *
+	 ikev2_msg_init(struct iked *, struct iked_message *,
+	    struct sockaddr_storage *, socklen_t,
+	    struct sockaddr_storage *, socklen_t, int);
+u_int32_t
+	 ikev2_msg_id(struct iked *, struct iked_sa *, int);
+struct ibuf
+	*ikev2_msg_auth(struct iked *, struct iked_sa *, int);
+int	 ikev2_msg_authsign(struct iked *, struct iked_sa *,
+	    struct iked_auth *, struct ibuf *);
+int	 ikev2_msg_authverify(struct iked *, struct iked_sa *,
+	    struct iked_auth *, u_int8_t *, size_t, struct ibuf *);
+int	 ikev2_msg_valid_ike_sa(struct iked *, struct ike_header *,
+	    struct iked_message *);
+int	 ikev2_msg_send(struct iked *, int, struct iked_message *);
+int	 ikev2_msg_send_encrypt(struct iked *, struct iked_sa *,
+	    struct ibuf **, u_int8_t, u_int8_t, int);
+struct ibuf
+	*ikev2_msg_encrypt(struct iked *, struct iked_sa *, struct ibuf *);
+struct ibuf *
+	 ikev2_msg_decrypt(struct iked *, struct iked_sa *,
+	    struct ibuf *, struct ibuf *);
+int	 ikev2_msg_integr(struct iked *, struct iked_sa *, struct ibuf *);
+
+/* ikev2_pld.c */
+int	 ikev2_pld_parse(struct iked *, struct ike_header *,
+	    struct iked_message *, off_t);
 
 /* eap.c */
 ssize_t	 eap_identity_request(struct ibuf *);
diff --git a/sbin/iked/ikev1.c b/sbin/iked/ikev1.c
index ba4c4523a58..612d84cac10 100644
--- a/sbin/iked/ikev1.c
+++ b/sbin/iked/ikev1.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ikev1.c,v 1.1 2010/06/03 16:41:12 reyk Exp $	*/
+/*	$OpenBSD: ikev1.c,v 1.2 2010/06/10 08:29:47 reyk Exp $	*/
 /*	$vantronix: ikev1.c,v 1.13 2010/05/28 15:34:35 reyk Exp $	*/
 
 /*
@@ -47,8 +47,8 @@
 int	 ikev1_dispatch_parent(int, struct iked_proc *, struct imsg *);
 int	 ikev1_dispatch_ikev2(int, struct iked_proc *, struct imsg *);
 
-void	 ikev1_message_cb(int, short, void *);
-void	 ikev1_message_recv(struct iked *, struct iked_message *);
+void	 ikev1_msg_cb(int, short, void *);
+void	 ikev1_recv(struct iked *, struct iked_message *);
 
 static struct iked_proc procs[] = {
 	{ "parent",	PROC_PARENT,	ikev1_dispatch_parent },
@@ -71,7 +71,7 @@ ikev1_dispatch_parent(int fd, struct iked_proc *p, struct imsg *imsg)
 		log_debug("%s: config reload", __func__);
 		return (0);
 	case IMSG_UDP_SOCKET:
-		return (config_getsocket(env, imsg, ikev1_message_cb));
+		return (config_getsocket(env, imsg, ikev1_msg_cb));
 	default:
 		break;
 	}
@@ -102,7 +102,7 @@ ikev1_dispatch_ikev2(int fd, struct iked_proc *p, struct imsg *imsg)
 
 		log_debug("%s: message length %d", __func__, len);
 
-		ikev1_message_recv(env, &msg);
+		ikev1_recv(env, &msg);
 		message_cleanup(env, &msg);
 		return (0);
 	default:
@@ -113,7 +113,7 @@ ikev1_dispatch_ikev2(int fd, struct iked_proc *p, struct imsg *imsg)
 }
 
 void
-ikev1_message_cb(int fd, short event, void *arg)
+ikev1_msg_cb(int fd, short event, void *arg)
 {
 	struct iked_socket	*sock = arg;
 	struct iked		*env = sock->sock_env;
@@ -149,14 +149,14 @@ ikev1_message_cb(int fd, short event, void *arg)
 		goto done;
 	}
 
-	ikev1_message_recv(env, &msg);
+	ikev1_recv(env, &msg);
 
  done:
 	message_cleanup(env, &msg);
 }
 
 void
-ikev1_message_recv(struct iked *env, struct iked_message *msg)
+ikev1_recv(struct iked *env, struct iked_message *msg)
 {
 	struct ike_header	*hdr;
 
diff --git a/sbin/iked/ikev2.c b/sbin/iked/ikev2.c
index 7f967022040..00297a93537 100644
--- a/sbin/iked/ikev2.c
+++ b/sbin/iked/ikev2.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ikev2.c,v 1.2 2010/06/04 09:51:45 reyk Exp $	*/
+/*	$OpenBSD: ikev2.c,v 1.3 2010/06/10 08:29:47 reyk Exp $	*/
 /*	$vantronix: ikev2.c,v 1.101 2010/06/03 07:57:33 reyk Exp $	*/
 
 /*
@@ -55,96 +55,24 @@ struct iked_sa *
 	 ikev2_getimsgdata(struct iked *, struct imsg *, struct iked_sahdr *,
 	    u_int8_t *, u_int8_t **, size_t *);
 
-void	 ikev2_message_cb(int, short, void *);
-void	 ikev2_message_recv(struct iked *, struct iked_message *);
-int	 ikev2_message_valid_ike_sa(struct iked *, struct ike_header *,
-	    struct iked_message *);
-int	 ikev2_message_send(struct iked *, int, struct iked_message *);
-int	 ikev2_message_send_encrypt(struct iked *, struct iked_sa *,
-	    struct ibuf **, u_int8_t, u_int8_t, int);
-u_int32_t
-	 ikev2_message_id(struct iked *, struct iked_sa *, int);
-struct ibuf *
-	 ikev2_message_init(struct iked *, struct iked_message *,
-	    struct sockaddr_storage *, socklen_t,
-	    struct sockaddr_storage *, socklen_t, int);
-struct ibuf
-	*ikev2_message_auth(struct iked *, struct iked_sa *, int);
-int	 ikev2_message_authverify(struct iked *, struct iked_sa *,
-	    struct iked_auth *, u_int8_t *, size_t, struct ibuf *);
-
-struct ibuf
-	*ikev2_message_encrypt(struct iked *, struct iked_sa *, struct ibuf *);
-struct ibuf *
-	 ikev2_message_decrypt(struct iked *, struct iked_sa *,
-	    struct ibuf *, struct ibuf *);
-int	 ikev2_message_integr(struct iked *, struct iked_sa *, struct ibuf *);
+void	 ikev2_cb(int, short, void *);
+void	 ikev2_recv(struct iked *, struct iked_message *);
 
 int	 ikev2_resp_ike_sa_init(struct iked *, struct iked_message *);
 int	 ikev2_resp_ike_auth(struct iked *, struct iked_sa *);
 int	 ikev2_resp_ike_eap(struct iked *, struct iked_sa *, struct ibuf *);
 int	 ikev2_resp_create_child_sa(struct iked *, struct iked_message *);
-int	 ikev2_send_informational(struct iked *, struct iked_message *);
-
-int	 ikev2_parse_message(struct iked *, struct ike_header *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_payloads(struct iked *, struct iked_message *,
-	    off_t, size_t, u_int, int);
-int	 ikev2_parse_sa(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_xform(struct iked *, struct ikev2_sa_proposal *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_attr(struct iked *, struct ikev2_transform *,
-	    struct iked_message *, off_t, int);
-int	 ikev2_parse_ke(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_id(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t, u_int);
-int	 ikev2_parse_cert(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_certreq(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_nonce(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_notify(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_delete(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_ts(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t, u_int);
-int	 ikev2_parse_auth(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_e(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_cp(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
-int	 ikev2_parse_eap(struct iked *, struct ikev2_payload *,
-	    struct iked_message *, off_t);
 
 int	 ikev2_policy2id(struct iked_static_id *, struct iked_id *, int);
-int	 ikev2_sa_negotiate(struct iked_sa *, struct iked_proposals *,
-	    struct iked_proposals *, u_int8_t);
 int	 ikev2_sa_keys(struct iked_sa *);
 int	 ikev2_sa_tag(struct iked_sa *, struct iked_id *);
 int	 ikev2_childsa_negotiate(struct iked *, struct iked_sa *,
 	    struct iked_spi *);
 int	 ikev2_childsa_enable(struct iked *, struct iked_sa *);
-int	 ikev2_childsa_delete(struct iked *, struct iked_sa *,
-	    u_int8_t, u_int64_t, u_int64_t *, int);
 int	 ikev2_valid_proposal(struct iked_proposal *,
 	    struct iked_transform **, struct iked_transform **);
 
-struct ike_header *
-	 ikev2_add_header(struct ibuf *, struct iked_sa *,
-	    u_int32_t, u_int8_t, u_int8_t, u_int8_t);
-int	 ikev2_set_header(struct ike_header *, size_t);
-struct ikev2_payload *
-	 ikev2_add_payload(struct ibuf *);
-int	 ikev2_next_payload(struct ikev2_payload *, size_t,
-	    u_int8_t);
 ssize_t	 ikev2_add_proposals(struct ibuf *, struct iked_proposals *, u_int8_t);
-ssize_t	 ikev2_nat_detection(struct iked_message *, void *, size_t,
-	    u_int, int);
 ssize_t	 ikev2_add_cp(struct iked *, struct iked_sa *, struct ibuf *);
 ssize_t	 ikev2_add_transform(struct ibuf *,
 	    u_int8_t, u_int8_t, u_int16_t, u_int16_t);
@@ -173,7 +101,7 @@ ikev2_dispatch_parent(int fd, struct iked_proc *p, struct imsg *imsg)
 	case IMSG_CTL_RESET:
 		return (config_getreset(env, imsg));
 	case IMSG_UDP_SOCKET:
-		return (config_getsocket(env, imsg, ikev2_message_cb));
+		return (config_getsocket(env, imsg, ikev2_msg_cb));
 	case IMSG_PFKEY_SOCKET:
 		return (config_getpfkey(env, imsg));
 	case IMSG_CFG_POLICY:
@@ -210,7 +138,7 @@ ikev2_dispatch_ikev1(int fd, struct iked_proc *p, struct imsg *imsg)
 
 		log_debug("%s: message length %d", __func__, len);
 
-		ikev2_message_recv(env, &msg);
+		ikev2_recv(env, &msg);
 		message_cleanup(env, &msg);
 		return (0);
 	default:
@@ -373,67 +301,7 @@ ikev2_getimsgdata(struct iked *env, struct imsg *imsg, struct iked_sahdr *sh,
 }
 
 void
-ikev2_message_cb(int fd, short event, void *arg)
-{
-	struct iked_socket	*sock = arg;
-	struct iked		*env = sock->sock_env;
-	struct iked_message	 msg;
-	struct ike_header	 hdr;
-	u_int32_t		 natt = 0x00000000;
-	u_int8_t		 buf[IKED_MSGBUF_MAX];
-	ssize_t			 len;
-	off_t			 off;
-	struct iovec		 iov[2];
-
-	bzero(&msg, sizeof(msg));
-	bzero(buf, sizeof(buf));
-
-	msg.msg_peerlen = sizeof(msg.msg_peer);
-	msg.msg_locallen = sizeof(msg.msg_local);
-	memcpy(&msg.msg_local, &sock->sock_addr, sizeof(sock->sock_addr));
-
-	if ((len = recvfromto(fd, buf, sizeof(buf), 0,
-	    (struct sockaddr*)&msg.msg_peer, &msg.msg_peerlen,
-	    (struct sockaddr*)&msg.msg_local, &msg.msg_locallen)) <
-	    (ssize_t)sizeof(natt))
-		return;
-
-	if (socket_getport(&msg.msg_local) == IKED_NATT_PORT) {
-		if (bcmp(&natt, buf, sizeof(natt)) != 0)
-			return;
-		msg.msg_natt = 1;
-		off = sizeof(natt);
-	} else
-		off = 0;
-
-	if ((size_t)(len - off) <= sizeof(hdr))
-		return;
-	memcpy(&hdr, buf + off, sizeof(hdr));
-
-	if ((msg.msg_data = ibuf_new(buf + off, len - off)) == NULL)
-		return;
-
-	if (hdr.ike_version == IKEV1_VERSION) {
-		iov[0].iov_base = &msg;
-		iov[0].iov_len = sizeof(msg);
-		iov[1].iov_base = buf;
-		iov[1].iov_len = len;
-
-		imsg_composev_proc(env, PROC_IKEV1, IMSG_IKE_MESSAGE, -1,
-		    iov, 2);
-		goto done;
-	}
-	TAILQ_INIT(&msg.msg_proposals);
-
-	msg.msg_fd = fd;
-	ikev2_message_recv(env, &msg);
-
- done:
-	message_cleanup(env, &msg);
-}
-
-void
-ikev2_message_recv(struct iked *env, struct iked_message *msg)
+ikev2_recv(struct iked *env, struct iked_message *msg)
 {
 	u_int			 initiator;
 	struct iked_sa		*sa;
@@ -473,7 +341,7 @@ ikev2_message_recv(struct iked *env, struct iked_message *msg)
 		}
 		break;
 	case IKEV2_EXCHANGE_IKE_AUTH:
-		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+		if (ikev2_msg_valid_ike_sa(env, hdr, msg) == -1)
 			return;
 		if (sa_stateok(msg->msg_sa, IKEV2_STATE_VALID)) {
 			log_debug("%s: already authenticated", __func__);
@@ -481,11 +349,11 @@ ikev2_message_recv(struct iked *env, struct iked_message *msg)
 		}
 		break;
 	case IKEV2_EXCHANGE_CREATE_CHILD_SA:
-		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+		if (ikev2_msg_valid_ike_sa(env, hdr, msg) == -1)
 			return;
 		break;
 	case IKEV2_EXCHANGE_INFORMATIONAL:
-		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+		if (ikev2_msg_valid_ike_sa(env, hdr, msg) == -1)
 			return;
 		break;
 	default:
@@ -542,7 +410,7 @@ ikev2_message_recv(struct iked *env, struct iked_message *msg)
 		    print_host(&sa->sa_local.addr, NULL, 0));
 	}
 
-	if (ikev2_parse_message(env, hdr, msg, msg->msg_offset) != 0) {
+	if (ikev2_pld_parse(env, hdr, msg, msg->msg_offset) != 0) {
 		log_debug("%s: failed to parse message", __func__);
 		return;
 	}
@@ -614,79 +482,6 @@ ikev2_message_recv(struct iked *env, struct iked_message *msg)
 }
 
 int
-ikev2_message_valid_ike_sa(struct iked *env, struct ike_header *oldhdr,
-    struct iked_message *msg)
-{
-#if 0
-	/* XXX Disabled, see comment below */
-	struct iked_message		 resp;
-	struct ike_header		*hdr;
-	struct ikev2_payload		*pld;
-	struct ikev2_notify		*n;
-	struct ibuf			*buf;
-	struct iked_sa			 sa;
-#endif
-
-	if (msg->msg_sa != NULL && msg->msg_policy != NULL)
-		return (0);
-
-#if 0
-	/*
-	 * XXX Sending INVALID_IKE_SPIs notifications is disabled
-	 * XXX because it is not mandatory and ignored by most
-	 * XXX implementations.  We might want to enable it in
-	 * XXX combination with a rate-limitation to avoid DoS situations.
-	 */
-
-	/* Fail without error message */
-	if (msg->msg_response || msg->msg_policy == NULL)
-		return (-1);
-
-	/* Invalid IKE SA, return notification */
-	if ((buf = ikev2_message_init(env, &resp,
-	    &msg->msg_peer, msg->msg_peerlen,
-	    &msg->msg_local, msg->msg_locallen, 1)) == NULL)
-		goto done;
-
-	bzero(&sa, sizeof(sa));
-	if ((oldhdr->ike_flags & IKEV2_FLAG_INITIATOR) == 0)
-		sa.sa_hdr.sh_initiator = 1;
-	sa.sa_hdr.sh_ispi = betoh64(oldhdr->ike_ispi);
-	sa.sa_hdr.sh_rspi = betoh64(oldhdr->ike_rspi);
-
-	/* IKE header */
-	if ((hdr = ikev2_add_header(buf, &sa, betoh32(oldhdr->ike_msgid),
-	    IKEV2_PAYLOAD_NOTIFY, IKEV2_EXCHANGE_INFORMATIONAL,
-	    IKEV2_FLAG_RESPONSE)) == NULL)
-		goto done;
-
-	/* SA payload */
-	if ((pld = ikev2_add_payload(buf)) == NULL)
-		goto done;
-	if ((n = ibuf_advance(buf, sizeof(*n))) == NULL)
-		goto done;
-	n->n_protoid = IKEV2_SAPROTO_IKE;
-	n->n_spisize = 0;
-	n->n_type = htobe16(IKEV2_N_INVALID_IKE_SPI);
-
-	if (ikev2_next_payload(pld, sizeof(*n), IKEV2_PAYLOAD_NONE) == -1)
-		goto done;
-
-	if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
-		goto done;
-
-	(void)ikev2_parse_message(env, hdr, &resp, 0);
-	(void)ikev2_message_send(env, msg->msg_fd, &resp);
-
- done:
-	message_cleanup(env, &resp);
-#endif
-
-	/* Always fail */
-	return (-1);
-}
-
-int
 ikev2_policy2id(struct iked_static_id *polid, struct iked_id *id, int srcid)
 {
 	struct ikev2_id		 hdr;
@@ -1232,332 +1027,6 @@ ikev2_add_buf(struct ibuf *buf, struct ibuf *data)
 	return (0);
 }
 
-struct ibuf *
-ikev2_message_init(struct iked *env, struct iked_message *msg,
-    struct sockaddr_storage *peer, socklen_t peerlen,
-    struct sockaddr_storage *local, socklen_t locallen, int response)
-{
-	bzero(msg, sizeof(*msg));
-	memcpy(&msg->msg_peer, peer, peerlen);
-	msg->msg_peerlen = peerlen;
-	memcpy(&msg->msg_local, local, locallen);
-	msg->msg_locallen = locallen;
-	msg->msg_response = response ? 1 : 0;
-	msg->msg_fd = -1;
-	msg->msg_data = ibuf_static();
-
-	return (msg->msg_data);
-}
-
-int
-ikev2_message_send(struct iked *env, int fd, struct iked_message *msg)
-{
-	struct ibuf	*buf = msg->msg_data;
-	u_int32_t	 natt = 0x00000000;
-	struct ike_header	*hdr;
-
-	if (buf == NULL || (hdr = ibuf_seek(msg->msg_data,
-	    msg->msg_offset, sizeof(*hdr))) == NULL)
-		return (-1);
-
-	log_info("%s: %s to %s from %s", __func__,
-	    print_map(hdr->ike_exchange, ikev2_exchange_map),
-	    print_host(&msg->msg_peer, NULL, 0),
-	    print_host(&msg->msg_local, NULL, 0));
-
-	if (msg->msg_natt || (msg->msg_sa && msg->msg_sa->sa_natt)) {
-		if (ibuf_prepend(buf, &natt, sizeof(natt)) == -1) {
-			log_debug("%s: failed to set NAT-T", __func__);
-			return (-1);
-		}
-	}
-	if ((sendto(fd, ibuf_data(buf), ibuf_size(buf), 0,
-	    (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen)) == -1) {
-		log_warn("%s: sendto", __func__);
-		return (-1);
-	}
-
-	return (0);
-}
-
-u_int32_t
-ikev2_message_id(struct iked *env, struct iked_sa *sa, int response)
-{
-	if (response)
-		return (sa->sa_msgid);
-
-	if (++sa->sa_msgid == UINT32_MAX) {
-		/* XXX we should close and renegotiate the connection now */
-		log_debug("%s: IKEv2 message sequence overflow", __func__);
-	}
-
-	return (sa->sa_msgid);
-}
-
-struct ibuf *
-ikev2_message_encrypt(struct iked *env, struct iked_sa *sa, struct ibuf *src)
-{
-	size_t			 len, ivlen, encrlen, integrlen, blocklen,
-				    outlen;
-	u_int8_t		*buf, pad = 0, *ptr;
-	struct ibuf		*integr, *encr, *dst = NULL, *out = NULL;
-
-	buf = ibuf_data(src);
-	len = ibuf_size(src);
-
-	log_debug("%s: decrypted length %d", __func__, len);
-	print_hex(buf, 0, len);
-
-	if (sa == NULL ||
-	    sa->sa_encr == NULL ||
-	    sa->sa_integr == NULL) {
-		log_debug("%s: invalid SA", __func__);
-		goto done;
-	}
-
-	if (sa->sa_hdr.sh_initiator) {
-		encr = sa->sa_key_iencr;
-		integr = sa->sa_key_iauth;
-	} else {
-		encr = sa->sa_key_rencr;
-		integr = sa->sa_key_rauth;
-	}
-
-	blocklen = cipher_length(sa->sa_encr);
-	ivlen = cipher_ivlength(sa->sa_encr);
-	integrlen = hash_length(sa->sa_integr);
-	encrlen = roundup(len + sizeof(pad), blocklen);
-	pad = encrlen - (len + sizeof(pad));
-
-	/*
-	 * Pad the payload and encrypt it
-	 */
-	if (pad) {
-		if ((ptr = ibuf_advance(src, pad)) == NULL)
-			goto done;
-		arc4random_buf(ptr, pad);
-	}
-	if (ibuf_add(src, &pad, sizeof(pad)) != 0)
-		goto done;
-
-	log_debug("%s: padded length %d", __func__, ibuf_size(src));
-	print_hex(ibuf_data(src), 0, ibuf_size(src));
-
-	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
-	cipher_setiv(sa->sa_encr, NULL, 0);	/* new IV */
-	cipher_init_encrypt(sa->sa_encr);
-
-	if ((dst = ibuf_dup(sa->sa_encr->encr_iv)) == NULL)
-		goto done;
-
-	if ((out = ibuf_new(NULL,
-	    cipher_outlength(sa->sa_encr, encrlen))) == NULL)
-		goto done;
-
-	outlen = ibuf_size(out);
-	cipher_update(sa->sa_encr,
-	    ibuf_data(src), encrlen, ibuf_data(out), &outlen);
-
-	if (outlen && ibuf_add(dst, ibuf_data(out), outlen) != 0)
-		goto done;
-
-	outlen = cipher_outlength(sa->sa_encr, 0);
-	cipher_final(sa->sa_encr, out->buf, &outlen);
-	if (outlen)
-		ibuf_add(dst, out->buf, outlen);
-
-	if ((ptr = ibuf_advance(dst, integrlen)) == NULL)
-		goto done;
-	bzero(ptr, integrlen);
-
-	log_debug("%s: length %d, padding %d, output length %d",
-	    __func__, len + sizeof(pad), pad, ibuf_size(dst));
-	print_hex(ibuf_data(dst), 0, ibuf_size(dst));
-
-	ibuf_release(src);
-	ibuf_release(out);
-	return (dst);
- done:
-	ibuf_release(src);
-	ibuf_release(out);
-	ibuf_release(dst);
-	return (NULL);
-}
-
-int
-ikev2_message_integr(struct iked *env, struct iked_sa *sa, struct ibuf *src)
-{
-	int			 ret = -1;
-	size_t			 integrlen, tmplen;
-	struct ibuf		*integr, *prf, *tmp = NULL;
-	u_int8_t		*ptr;
-
-	log_debug("%s: message length %d", __func__, ibuf_size(src));
-	print_hex(ibuf_data(src), 0, ibuf_size(src));
-
-	if (sa == NULL ||
-	    sa->sa_integr == NULL) {
-		log_debug("%s: invalid SA", __func__);
-		return (-1);
-	}
-
-	if (sa->sa_hdr.sh_initiator) {
-		integr = sa->sa_key_iauth;
-		prf = sa->sa_key_iprf;
-	} else {
-		integr = sa->sa_key_rauth;
-		prf = sa->sa_key_rprf;
-	}
-
-	integrlen = hash_length(sa->sa_integr);
-
-	log_debug("%s: integrity checksum length %d", __func__,
-	    integrlen);
-
-	/*
-	 * Validate packet checksum
-	 */
-	if ((tmp = ibuf_new(NULL, hash_keylength(sa->sa_integr))) == NULL)
-		goto done;
-
-	hash_setkey(sa->sa_integr, ibuf_data(integr), ibuf_size(integr));
-	hash_init(sa->sa_integr);
-	hash_update(sa->sa_integr, ibuf_data(src),
-	    ibuf_size(src) - integrlen);
-	hash_final(sa->sa_integr, ibuf_data(tmp), &tmplen);
-
-	if (tmplen != integrlen) {
-		log_debug("%s: hash failure", __func__);
-		goto done;
-	}
-
-	if ((ptr = ibuf_seek(src,
-	    ibuf_size(src) - integrlen, integrlen)) == NULL)
-		goto done;
-	memcpy(ptr, ibuf_data(tmp), tmplen);
-
-	print_hex(ibuf_data(tmp), 0, ibuf_size(tmp));
-
-	ret = 0;
- done:
-	ibuf_release(tmp);
-
-	return (ret);
-}
-
-struct ibuf *
-ikev2_message_decrypt(struct iked *env, struct iked_sa *sa,
-    struct ibuf *msg, struct ibuf *src)
-{
-	size_t			 ivlen, encrlen, integrlen, blocklen,
-				    outlen, tmplen;
-	u_int8_t		 pad, *ptr;
-	struct ibuf		*integr, *encr, *tmp = NULL, *out = NULL;
-	off_t			 ivoff, encroff, integroff;
-
-	if (sa == NULL ||
-	    sa->sa_encr == NULL ||
-	    sa->sa_integr == NULL) {
-		log_debug("%s: invalid SA", __func__);
-		print_hex(ibuf_data(src), 0, ibuf_size(src));
-		goto done;
-	}
-
-	if (!sa->sa_hdr.sh_initiator) {
-		encr = sa->sa_key_iencr;
-		integr = sa->sa_key_iauth;
-	} else {
-		encr = sa->sa_key_rencr;
-		integr = sa->sa_key_rauth;
-	}
-
-	blocklen = cipher_length(sa->sa_encr);
-	ivlen = cipher_ivlength(sa->sa_encr);
-	ivoff = 0;
-	integrlen = hash_length(sa->sa_integr);
-	integroff = ibuf_size(src) - integrlen;
-	encroff = ivlen;
-	encrlen = ibuf_size(src) - integrlen - ivlen;
-
-	log_debug("%s: IV length %d", __func__, ivlen);
-	print_hex(ibuf_data(src), 0, ivlen);
-	log_debug("%s: encrypted payload length %d", __func__, encrlen);
-	print_hex(ibuf_data(src), encroff, encrlen);
-	log_debug("%s: integrity checksum length %d", __func__, integrlen);
-	print_hex(ibuf_data(src), integroff, integrlen);
-
-	/*
-	 * Validate packet checksum
-	 */
-	if ((tmp = ibuf_new(NULL, ibuf_length(integr))) == NULL)
-		goto done;
-
-	hash_setkey(sa->sa_integr, integr->buf, ibuf_length(integr));
-	hash_init(sa->sa_integr);
-	hash_update(sa->sa_integr, ibuf_data(msg),
-	    ibuf_size(msg) - integrlen);
-	hash_final(sa->sa_integr, tmp->buf, &tmplen);
-
-	if (memcmp(tmp->buf, ibuf_data(src) + integroff, integrlen) != 0) {
-		log_debug("%s: integrity check failed", __func__);
-		goto done;
-	}
-
-	log_debug("%s: integrity check succeeded", __func__, tmplen);
-	print_hex(tmp->buf, 0, tmplen);
-
-	ibuf_release(tmp);
-	tmp = NULL;
-
-	/*
-	 * Decrypt the payload and strip any padding
-	 */
-	if ((encrlen % blocklen) != 0) {
-		log_debug("%s: unaligned encrypted payload", __func__);
-		goto done;
-	}
-
-	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
-	cipher_setiv(sa->sa_encr, ibuf_data(src) + ivoff, ivlen);
-	cipher_init_decrypt(sa->sa_encr);
-
-	if ((out = ibuf_new(NULL, cipher_outlength(sa->sa_encr,
-	    encrlen))) == NULL)
-		goto done;
-
-	outlen = ibuf_length(out);
-	/* XXX why does it need encrlen + blocklen to work correctly? */
-	cipher_update(sa->sa_encr,
-	    ibuf_data(src) + encroff, encrlen + blocklen,
-	    ibuf_data(out), &outlen);
-	cipher_final(sa->sa_encr, ibuf_seek(out, outlen, blocklen), &tmplen);
-	if (tmplen)
-		outlen += tmplen;
-
-	/*
-	 * XXX 
-	 * XXX the padding is wrong
-	 * XXX
-	 */
-	ptr = ibuf_seek(out, outlen - 1, 1);
-	pad = *ptr;
-
-	log_debug("%s: decrypted payload length %d/%d padding %d",
-	    __func__, outlen, encrlen, pad);
-	print_hex(ibuf_data(out), 0, ibuf_size(out));
-
-	if (ibuf_setsize(out, outlen) != 0)
-		goto done;
-
-	ibuf_release(src);
-	return (out);
- done:
-	ibuf_release(tmp);
-	ibuf_release(out);
-	ibuf_release(src);
-	return (NULL);
-}
-
 int
 ikev2_resp_ike_sa_init(struct iked *env, struct iked_message *msg)
 {
@@ -1574,7 +1043,7 @@ ikev2_resp_ike_sa_init(struct iked *env, struct iked_message *msg)
 	ssize_t				 len;
 	int				 ret = -1;
 
-	if ((buf = ikev2_message_init(env, &resp,
+	if ((buf = ikev2_msg_init(env, &resp,
 	    &msg->msg_peer, msg->msg_peerlen,
 	    &msg->msg_local, msg->msg_locallen, 1)) == NULL)
 		goto done;
@@ -1676,7 +1145,7 @@ ikev2_resp_ike_sa_init(struct iked *env, struct iked_message *msg)
 	if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
 		goto done;
 
-	(void)ikev2_parse_message(env, hdr, &resp, 0);
+	(void)ikev2_pld_parse(env, hdr, &resp, 0);
 
 	ibuf_release(sa->sa_2ndmsg);
 	if ((sa->sa_2ndmsg = ibuf_dup(buf)) == NULL) {
@@ -1684,7 +1153,7 @@ ikev2_resp_ike_sa_init(struct iked *env, struct iked_message *msg)
 		goto done;
 	}
 
-	ret = ikev2_message_send(env, msg->msg_fd, &resp);
+	ret = ikev2_msg_send(env, msg->msg_fd, &resp);
 
  done:
 	message_cleanup(env, &resp);
@@ -1833,7 +1302,7 @@ ikev2_resp_ike_auth(struct iked *env, struct iked_sa *sa)
 	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
 		goto done;
 
-	ret = ikev2_message_send_encrypt(env, sa, &e,
+	ret = ikev2_msg_send_encrypt(env, sa, &e,
 	    IKEV2_EXCHANGE_IKE_AUTH, firstpayload, 1);
 	if (ret == 0)
 		ret = ikev2_childsa_enable(env, sa);
@@ -1921,7 +1390,7 @@ ikev2_resp_ike_eap(struct iked *env, struct iked_sa *sa, struct ibuf *eapmsg)
 	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
 		goto done;
 
-	ret = ikev2_message_send_encrypt(env, sa, &e,
+	ret = ikev2_msg_send_encrypt(env, sa, &e,
 	    IKEV2_EXCHANGE_IKE_AUTH, firstpayload, 1);
 
  done:
@@ -1950,7 +1419,7 @@ ikev2_send_ike_e(struct iked *env, struct iked_sa *sa, struct ibuf *buf,
 	if (ikev2_next_payload(pld, ibuf_size(buf), IKEV2_PAYLOAD_NONE) == -1)
 		goto done;
 
-	ret = ikev2_message_send_encrypt(env, sa, &e,
+	ret = ikev2_msg_send_encrypt(env, sa, &e,
 	    exchange, firstpayload, response);
 
  done:
@@ -2037,7 +1506,7 @@ ikev2_resp_create_child_sa(struct iked *env, struct iked_message *msg)
 	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
 		goto done;
 
-	ret = ikev2_message_send_encrypt(env, sa, &e,
+	ret = ikev2_msg_send_encrypt(env, sa, &e,
 	    IKEV2_EXCHANGE_CREATE_CHILD_SA, firstpayload, 1);
 	if (ret == 0)
 		ret = ikev2_childsa_enable(env, sa);
@@ -2061,7 +1530,7 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 	if (msg->msg_error == 0)
 		return (0);
 
-	if ((buf = ikev2_message_init(env, &resp,
+	if ((buf = ikev2_msg_init(env, &resp,
 	    &msg->msg_peer, msg->msg_peerlen,
 	    &msg->msg_local, msg->msg_locallen, 0)) == NULL)
 		goto done;
@@ -2096,7 +1565,7 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 	if (sa != NULL && msg->msg_decrypted) {
 		/* IKE header */
 		if ((hdr = ikev2_add_header(buf, sa,
-		    ikev2_message_id(env, sa, 0),
+		    ikev2_msg_id(env, sa, 0),
 		    IKEV2_PAYLOAD_E, IKEV2_EXCHANGE_INFORMATIONAL,
 		    0)) == NULL)
 			goto done;
@@ -2105,7 +1574,7 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 			goto done;
 
 		/* Encrypt message and add as an E payload */
-		if ((e = ikev2_message_encrypt(env, sa, e)) == NULL) {
+		if ((e = ikev2_msg_encrypt(env, sa, e)) == NULL) {
 			log_debug("%s: encryption failed", __func__);
 			goto done;
 		}
@@ -2119,7 +1588,7 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 			goto done;
 
 		/* Add integrity checksum (HMAC) */
-		if (ikev2_message_integr(env, sa, buf) != 0) {
+		if (ikev2_msg_integr(env, sa, buf) != 0) {
 			log_debug("%s: integrity checksum failed", __func__);
 			goto done;
 		}
@@ -2136,7 +1605,7 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 
 		/* IKE header */
 		if ((hdr = ikev2_add_header(buf, &sah,
-		    ikev2_message_id(env, &sah, 0),
+		    ikev2_msg_id(env, &sah, 0),
 		    IKEV2_PAYLOAD_NOTIFY, IKEV2_EXCHANGE_INFORMATIONAL,
 		    0)) == NULL)
 			goto done;
@@ -2151,10 +1620,10 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 	TAILQ_INIT(&resp.msg_proposals);
 
 	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
-	(void)ikev2_parse_message(env, hdr, &resp, 0);
+	(void)ikev2_pld_parse(env, hdr, &resp, 0);
 	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
 
-	ret = ikev2_message_send(env, msg->msg_fd, &resp);
+	ret = ikev2_msg_send(env, msg->msg_fd, &resp);
 
  done:
 	ibuf_release(e);
@@ -2163,712 +1632,6 @@ ikev2_send_informational(struct iked *env, struct iked_message *msg)
 	return (ret);
 }
 
-int
-ikev2_message_send_encrypt(struct iked *env, struct iked_sa *sa,
-    struct ibuf **ep, u_int8_t exchange, u_int8_t firstpayload, int response)
-{
-	struct iked_message		 resp;
-	struct ike_header		*hdr;
-	struct ikev2_payload		*pld;
-	struct ibuf			*buf, *e = *ep;
-	int				 ret = -1;
-
-	if ((buf = ikev2_message_init(env, &resp,
-	    &sa->sa_peer.addr, sa->sa_peer.addr.ss_len,
-	    &sa->sa_local.addr, sa->sa_local.addr.ss_len, 1)) == NULL)
-		goto done;
-
-	/* IKE header */
-	if ((hdr = ikev2_add_header(buf, sa,
-	    ikev2_message_id(env, sa, response),
-	    IKEV2_PAYLOAD_E, exchange,
-	    response ? IKEV2_FLAG_RESPONSE : 0)) == NULL)
-		goto done;
-
-	if ((pld = ikev2_add_payload(buf)) == NULL)
-		goto done;
-
-	/* Encrypt message and add as an E payload */
-	if ((e = ikev2_message_encrypt(env, sa, e)) == NULL) {
-		log_debug("%s: encryption failed", __func__);
-		goto done;
-	}
-	if (ibuf_cat(buf, e) != 0)
-		goto done;
-	if (ikev2_next_payload(pld, ibuf_size(e), firstpayload) == -1)
-		goto done;
-
-	if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
-		goto done;
-
-	/* Add integrity checksum (HMAC) */
-	if (ikev2_message_integr(env, sa, buf) != 0) {
-		log_debug("%s: integrity checksum failed", __func__);
-		goto done;
-	}
-
-	resp.msg_data = buf;
-	resp.msg_sa = sa;
-	TAILQ_INIT(&resp.msg_proposals);
-
-	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
-	(void)ikev2_parse_message(env, hdr, &resp, 0);
-	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
-
-	ret = ikev2_message_send(env, sa->sa_fd, &resp);
-
- done:
-	/* e is cleaned up by the calling function */
-	*ep = e;
-	message_cleanup(env, &resp);
-
-	return (ret);
-}
-
-struct ibuf *
-ikev2_message_auth(struct iked *env, struct iked_sa *sa, int response)
-{
-	struct ibuf		*authmsg = NULL, *nonce, *prfkey, *buf;
-	u_int8_t		*ptr;
-	struct iked_id		*id;
-	size_t			 tmplen;
-
-	/*
-	 * Create the payload to be signed/MAC'ed for AUTH
-	 */
-
-	if (!response) {
-		if ((nonce = sa->sa_rnonce) == NULL ||
-		    (sa->sa_iid.id_type == 0) ||
-		    (prfkey = sa->sa_key_iprf) == NULL ||
-		    (buf = sa->sa_1stmsg) == NULL)
-			return (NULL);
-		id = &sa->sa_iid;
-	} else {
-		if ((nonce = sa->sa_inonce) == NULL ||
-		    (sa->sa_rid.id_type == 0) ||
-		    (prfkey = sa->sa_key_rprf) == NULL ||
-		    (buf = sa->sa_2ndmsg) == NULL)
-			return (NULL);
-		id = &sa->sa_rid;
-	}
-
-	if ((authmsg = ibuf_dup(buf)) == NULL)
-		return (NULL);
-	if (ibuf_cat(authmsg, nonce) != 0)
-		goto fail;
-
-	if ((hash_setkey(sa->sa_prf, ibuf_data(prfkey),
-	    ibuf_size(prfkey))) == NULL)
-		goto fail;
-
-	if ((ptr = ibuf_advance(authmsg,
-	    hash_length(sa->sa_prf))) == NULL)
-		goto fail;
-
-	hash_init(sa->sa_prf);
-	hash_update(sa->sa_prf, ibuf_data(id->id_buf), ibuf_size(id->id_buf));
-	hash_final(sa->sa_prf, ptr, &tmplen);
-
-	if (tmplen != hash_length(sa->sa_prf))
-		goto fail;
-
-	log_debug("%s: %s auth data length %d",
-	    __func__, response ? "responder" : "initiator",
-	    ibuf_size(authmsg));
-	print_hex(ibuf_data(authmsg), 0, ibuf_size(authmsg));
-
-	return (authmsg);
-
- fail:
-	ibuf_release(authmsg);
-	return (NULL);
-}
-
-int
-ikev2_parse_message(struct iked *env, struct ike_header *hdr,
-    struct iked_message *msg, off_t offset)
-{
-	log_debug("%s: header ispi %s rspi %s"
-	    " nextpayload %s version 0x%02x exchange %s flags 0x%02x"
-	    " msgid %d length %d response %d", __func__,
-	    print_spi(betoh64(hdr->ike_ispi), 8),
-	    print_spi(betoh64(hdr->ike_rspi), 8),
-	    print_map(hdr->ike_nextpayload, ikev2_payload_map),
-	    hdr->ike_version,
-	    print_map(hdr->ike_exchange, ikev2_exchange_map),
-	    hdr->ike_flags,
-	    betoh32(hdr->ike_msgid),
-	    betoh32(hdr->ike_length),
-	    msg->msg_response);
-
-	if (ibuf_size(msg->msg_data) < betoh32(hdr->ike_length)) {
-		log_debug("%s: short message", __func__);
-		return (-1);
-	}
-
-	offset += sizeof(*hdr);
-
-	return (ikev2_parse_payloads(env, msg, offset,
-	    betoh32(hdr->ike_length), hdr->ike_nextpayload, 0));
-}
-
-int
-ikev2_parse_payloads(struct iked *env, struct iked_message *msg,
-    off_t offset, size_t length, u_int payload, int quick)
-{
-	struct ikev2_payload	 pld;
-	u_int			 e;
-	int			 ret;
-	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
-
-	/* Check if message was decrypted in an E payload */
-	e = msg->msg_decrypted ? IKED_E : 0;
-
-	if (quick)
-		print_debug("%s: %spayloads", __func__,
-		    e ? "decrypted " : "");
-	else
-		ikev2_parse_payloads(env, msg, offset, length, payload, 1);
-
-	while (payload != 0 && offset < (off_t)length) {
-		memcpy(&pld, msgbuf + offset, sizeof(pld));
-
-		if (quick)
-			print_debug(" %s",
-			    print_map(payload, ikev2_payload_map));
-		else
-			log_debug("%s: %spayload %s"
-			    " nextpayload %s critical 0x%02x length %d",
-			    __func__, e ? "decrypted " : "",
-			    print_map(payload, ikev2_payload_map),
-			    print_map(pld.pld_nextpayload, ikev2_payload_map),
-			    pld.pld_reserved & IKEV2_CRITICAL_PAYLOAD,
-			    betoh16(pld.pld_length));
-
-		offset += sizeof(pld);
-		ret = 0;
-
-		if (quick)
-			goto next;
-
-		switch (payload | e) {
-		case IKEV2_PAYLOAD_SA:
-		case IKEV2_PAYLOAD_SA | IKED_E:
-			ret = ikev2_parse_sa(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_KE:
-			ret = ikev2_parse_ke(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_IDi | IKED_E:
-		case IKEV2_PAYLOAD_IDr | IKED_E:
-			ret = ikev2_parse_id(env, &pld, msg, offset, payload);
-			break;
-		case IKEV2_PAYLOAD_CERT | IKED_E:
-			ret = ikev2_parse_cert(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_CERTREQ:
-		case IKEV2_PAYLOAD_CERTREQ | IKED_E:
-			ret = ikev2_parse_certreq(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_AUTH | IKED_E:
-			ret = ikev2_parse_auth(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_NONCE:
-		case IKEV2_PAYLOAD_NONCE | IKED_E:
-			ret = ikev2_parse_nonce(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_NOTIFY:
-		case IKEV2_PAYLOAD_NOTIFY | IKED_E:
-			ret = ikev2_parse_notify(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_DELETE | IKED_E:
-			ret = ikev2_parse_delete(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_TSi | IKED_E:
-		case IKEV2_PAYLOAD_TSr | IKED_E:
-			ret = ikev2_parse_ts(env, &pld, msg, offset, payload);
-			break;
-		case IKEV2_PAYLOAD_E:
-			ret = ikev2_parse_e(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_CP | IKED_E:
-			ret = ikev2_parse_cp(env, &pld, msg, offset);
-			break;
-		case IKEV2_PAYLOAD_EAP | IKED_E:
-			ret = ikev2_parse_eap(env, &pld, msg, offset);
-			break;
-		default:
-			print_hex(msgbuf, offset,
-			    betoh16(pld.pld_length) - sizeof(pld));
-			break;
-		}
-
-		if (ret != 0 && !msg->msg_response) {
-			(void)ikev2_send_informational(env, msg);
-			return (-1);
-		}
-
-		/* Encrypted payload must appear last */
-		if (payload == IKEV2_PAYLOAD_E)
-			return (0);
-
- next:
-		payload = pld.pld_nextpayload;
-		offset += betoh16(pld.pld_length) - sizeof(pld);
-	}
-
-	if (quick)
-		print_debug("\n");
-
-	return (0);
-}
-
-int
-ikev2_parse_sa(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_sa_proposal	 sap;
-	struct iked_proposal		*prop = NULL;
-	u_int32_t			 spi32;
-	u_int64_t			 spi = 0, spi64;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-	struct iked_sa			*sa = msg->msg_sa;
-
-	memcpy(&sap, msgbuf + offset, sizeof(sap));
-	offset += sizeof(sap);
-
-	if (sap.sap_spisize) {
-		switch (sap.sap_spisize) {
-		case 4:
-			memcpy(&spi32, msgbuf + offset, 4);
-			spi = betoh32(spi32);
-			break;
-		case 8:
-			memcpy(&spi64, msgbuf + offset, 8);
-			spi = betoh64(spi64);
-			break;
-		default:
-			log_debug("%s: unsupported SPI size %d",
-			    __func__, sap.sap_spisize);
-			return (-1);
-		}
-
-		offset += sap.sap_spisize;
-	}
-
-	log_debug("%s: more %d reserved %d length %d"
-	    " proposal #%d protoid %s spisize %d xforms %d spi %s",
-	    __func__, sap.sap_more, sap.sap_reserved,
-	    betoh16(sap.sap_length), sap.sap_proposalnr,
-	    print_map(sap.sap_protoid, ikev2_saproto_map), sap.sap_spisize,
-	    sap.sap_transforms, print_spi(spi, sap.sap_spisize));
-
-	if (!msg->msg_response) {
-		if ((msg->msg_prop = config_add_proposal(&msg->msg_proposals,
-		    sap.sap_proposalnr, sap.sap_protoid)) == NULL) {
-			log_debug("%s: invalid proposal", __func__);
-			return (-1);
-		}
-		prop = msg->msg_prop;
-		prop->prop_localspi.spi_size = sap.sap_spisize;
-		prop->prop_peerspi.spi = spi;
-	}
-
-	/*
-	 * Parse the attached transforms
-	 */
-	if (ikev2_parse_xform(env, &sap, msg, offset) != 0) {
-		log_debug("%s: invalid proposal transforms", __func__);
-		return (-1);
-	}
-
-	if (msg->msg_response)
-		return (0);
-
-	/* XXX we need a better way to get this */
-	if (ikev2_sa_negotiate(sa,
-	    &msg->msg_policy->pol_proposals,
-	    &msg->msg_proposals, msg->msg_decrypted ?
-	    IKEV2_SAPROTO_ESP : IKEV2_SAPROTO_IKE) != 0) {
-		log_debug("%s: no proposal chosen", __func__);
-		msg->msg_error = IKEV2_N_NO_PROPOSAL_CHOSEN;
-		return (-1);
-	} else if (sa_stateok(sa, IKEV2_STATE_SA_INIT))
-		sa_stateflags(sa, IKED_REQ_SA);
-
-	return (0);
-}
-
-int
-ikev2_parse_xform(struct iked *env, struct ikev2_sa_proposal *sap,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_transform		 xfrm;
-	char				 id[BUFSIZ];
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&xfrm, msgbuf + offset, sizeof(xfrm));
-
-	switch (xfrm.xfrm_type) {
-	case IKEV2_XFORMTYPE_ENCR:
-		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
-		    ikev2_xformencr_map), sizeof(id));
-		break;
-	case IKEV2_XFORMTYPE_PRF:
-		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
-		    ikev2_xformprf_map), sizeof(id));
-		break;
-	case IKEV2_XFORMTYPE_INTEGR:
-		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
-		    ikev2_xformauth_map), sizeof(id));
-		break;
-	case IKEV2_XFORMTYPE_DH:
-		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
-		    ikev2_xformdh_map), sizeof(id));
-		break;
-	case IKEV2_XFORMTYPE_ESN:
-		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
-		    ikev2_xformesn_map), sizeof(id));
-		break;
-	default:
-		snprintf(id, sizeof(id), "<%d>", betoh16(xfrm.xfrm_id));
-		break;
-	}
-
-	log_debug("%s: more %d reserved %d length %d"
-	    " type %s id %s",
-	    __func__, xfrm.xfrm_more, xfrm.xfrm_reserved,
-	    betoh16(xfrm.xfrm_length),
-	    print_map(xfrm.xfrm_type, ikev2_xformtype_map), id);
-
-	/*
-	 * Parse transform attributes, if available
-	 */
-	msg->msg_attrlength = 0;
-	if ((u_int)betoh16(xfrm.xfrm_length) > sizeof(xfrm))
-		ikev2_parse_attr(env, &xfrm, msg, offset + sizeof(xfrm),
-		    betoh16(xfrm.xfrm_length) - sizeof(xfrm));
-
-	if (!msg->msg_response) {
-		if (config_add_transform(msg->msg_prop, xfrm.xfrm_type,
-		    betoh16(xfrm.xfrm_id), msg->msg_attrlength,
-		    msg->msg_attrlength) == NULL) {
-			log_debug("%s: failed to add transform", __func__);
-			return (-1);
-		}
-	}
-
-	/* Next transform */
-	offset += betoh16(xfrm.xfrm_length);
-	if (xfrm.xfrm_more == IKEV2_XFORM_MORE)
-		ikev2_parse_xform(env, sap, msg, offset);
-
-	return (0);
-}
-
-int
-ikev2_parse_attr(struct iked *env, struct ikev2_transform *xfrm,
-    struct iked_message *msg, off_t offset, int total)
-{
-	struct ikev2_attribute		 attr;
-	u_int				 type;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&attr, msgbuf + offset, sizeof(attr));
-
-	type = betoh16(attr.attr_type) & ~IKEV2_ATTRAF_TV;
-
-	log_debug("%s: attribute type %s length %d total %d",
-	    __func__, print_map(type, ikev2_attrtype_map),
-	    betoh16(attr.attr_length), total);
-
-	if (betoh16(attr.attr_type) & IKEV2_ATTRAF_TV) {
-		/* Type-Value attribute */
-		offset += sizeof(attr);
-		total -= sizeof(attr);
-
-		if (type == IKEV2_ATTRTYPE_KEY_LENGTH)
-			msg->msg_attrlength = betoh16(attr.attr_length);
-	} else {
-		/* Type-Length-Value attribute */
-		print_hex(msgbuf, offset + sizeof(attr),
-		    betoh16(attr.attr_length) - sizeof(attr));
-		offset += betoh16(attr.attr_length);
-		total -= betoh16(attr.attr_length);
-	}
-
-	if (total > 0) {
-		/* Next attribute */
-		ikev2_parse_attr(env, xfrm, msg, offset, total);
-	}
-
-	return (0);
-}
-
-int
-ikev2_parse_ke(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_keyexchange	 kex;
-	u_int8_t			*buf;
-	size_t				 len;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&kex, msgbuf + offset, sizeof(kex));
-
-	log_debug("%s: dh group %s reserved %d",
-	    __func__,
-	    print_map(betoh16(kex.kex_dhgroup), ikev2_xformdh_map),
-	    betoh16(kex.kex_reserved));
-
-	buf = msgbuf + offset + sizeof(kex);
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(kex);
-
-	print_hex(buf, 0, len);
-
-	if (!msg->msg_response) {
-		if ((msg->msg_sa->sa_dhiexchange =
-		    ibuf_new(buf, len)) == NULL) {
-			log_debug("%s: failed to get exchange", __func__);
-			return (-1);
-		}
-	}
-
-	return (0);
-}
-
-int
-ikev2_parse_id(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset, u_int payload)
-{
-	u_int8_t			*ptr;
-	struct ikev2_id			 id;
-	size_t				 len;
-	struct iked_id			*idp, idb;
-	struct iked_sa			*sa = msg->msg_sa;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-	struct ibuf			*authmsg;
-	char				 idstr[IKED_ID_SIZE];
-
-	memcpy(&id, msgbuf + offset, sizeof(id));
-	bzero(&idb, sizeof(idb));
-
-	/* Don't strip the Id payload header */
-	ptr = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld);
-
-	idb.id_type = id.id_type;
-	if ((idb.id_buf = ibuf_new(ptr, len)) == NULL)
-		return (-1);
-
-	if (print_id(&idb, sizeof(id), idstr, sizeof(idstr)) == -1) {
-		log_debug("%s: malformed id", __func__);
-		return (-1);
-	}
-
-	log_debug("%s: id %s/%s length %d",
-	    __func__, print_map(id.id_type, ikev2_id_map), idstr, len);
-
-	if (msg->msg_response) {
-		ibuf_release(idb.id_buf);
-		return (0);
-	}
-
-	if (sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDr) {
-		idp = &sa->sa_rid;
-	} else if (!sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDi) {
-		idp = &sa->sa_iid;
-	} else {
-		log_debug("%s: unexpected id payload", __func__);
-		return (0);
-	}
-
-	ibuf_release(idp->id_buf);
-	idp->id_buf = idb.id_buf;
-	idp->id_type = idb.id_type;
-
-	if ((authmsg = ikev2_message_auth(env, sa,
-	    !sa->sa_hdr.sh_initiator)) == NULL) {
-		log_debug("%s: failed to get response auth data", __func__);
-		return (-1);
-	}
-
-	ca_setauth(env, sa, authmsg, PROC_CERT);
-
-	return (0);
-}
-
-int
-ikev2_parse_cert(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_cert		 cert;
-	u_int8_t			*buf;
-	size_t				 len;
-	struct iked_sa			*sa = msg->msg_sa;
-	struct iked_id			*certid, *id;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&cert, msgbuf + offset, sizeof(cert));
-	offset += sizeof(cert);
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
-
-	log_debug("%s: type %s length %d",
-	    __func__, print_map(cert.cert_type, ikev2_cert_map), len);
-
-	print_hex(buf, 0, len);
-
-	if (msg->msg_response)
-		return (0);
-
-	if (!sa->sa_hdr.sh_initiator && !msg->msg_response) {
-		certid = &sa->sa_icert;
-		id = &sa->sa_iid;
-	} else if (sa->sa_hdr.sh_initiator && msg->msg_response) {
-		certid = &sa->sa_rcert;
-		id = &sa->sa_rid;
-	} else
-		return (0);	/* ignore */
-
-	if ((certid->id_buf = ibuf_new(buf, len)) == NULL) {
-		log_debug("%s: failed to save cert", __func__);
-		return (-1);
-	}
-	certid->id_type = cert.cert_type;
-
-	ca_setcert(env, &msg->msg_sa->sa_hdr, id, cert.cert_type,
-	    buf, len, PROC_CERT);
-
-	return (0);
-}
-
-int
-ikev2_parse_certreq(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_cert		 cert;
-	u_int8_t			*buf;
-	size_t				 len;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&cert, msgbuf + offset, sizeof(cert));
-	offset += sizeof(cert);
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
-
-	log_debug("%s: type %s signatures length %d",
-	    __func__, print_map(cert.cert_type, ikev2_cert_map), len);
-	print_hex(buf, 0, len);
-
-	if (msg->msg_response)
-		return (0);
-
-	if (!len || (len % SHA_DIGEST_LENGTH) != 0) {
-		log_debug("%s: invalid certificate request", __func__);
-		return (-1);
-	}
-
-	if (msg->msg_sa == NULL)
-		return (-1);
-
-	/* Optional certreq for PSK */
-	msg->msg_sa->sa_staterequire |= IKED_REQ_CERT;
-
-	ca_setreq(env, &msg->msg_sa->sa_hdr, cert.cert_type,
-	    buf, len, PROC_CERT);
-
-	return (0);
-}
-
-int
-ikev2_parse_auth(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_auth		 auth;
-	struct iked_auth		 ikeauth;
-	u_int8_t			*buf;
-	size_t				 len;
-	struct ibuf			*authmsg;
-	struct iked_sa			*sa = msg->msg_sa;
-	struct iked_policy		*policy = sa->sa_policy;
-	int				 ret = -1;
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&auth, msgbuf + offset, sizeof(auth));
-	offset += sizeof(auth);
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(auth);
-
-	log_debug("%s: method %s length %d",
-	    __func__, print_map(auth.auth_method, ikev2_auth_map), len);
-
-	print_hex(buf, 0, len);
-
-	if (msg->msg_response)
-		return (0);
-
-	memcpy(&ikeauth, &policy->pol_auth, sizeof(ikeauth));
-
-	if (policy->pol_auth.auth_eap && sa->sa_eapmsk != NULL) {
-		/* The initiator EAP auth is a PSK derived from the MSK */
-		ikeauth.auth_method = IKEV2_AUTH_SHARED_KEY_MIC;
-
-		/* Copy session key as PSK */
-		memcpy(ikeauth.auth_data, ibuf_data(sa->sa_eapmsk),
-		    ibuf_size(sa->sa_eapmsk));
-		ikeauth.auth_length = ibuf_size(sa->sa_eapmsk);
-	}
-	if (auth.auth_method != ikeauth.auth_method) {
-		log_debug("%s: method %s required", __func__,
-		    print_map(ikeauth.auth_method, ikev2_auth_map));
-		return (-1);
-	}
-
-	/* The AUTH payload indicates if the responder wants EAP or not */
-	if (!sa_stateok(sa, IKEV2_STATE_EAP))
-		sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
-
-	if ((authmsg = ikev2_message_auth(env, sa,
-	    sa->sa_hdr.sh_initiator)) == NULL) {
-		log_debug("%s: failed to get auth data", __func__);
-		return (-1);
-	}
-
-	ret = ikev2_message_authverify(env, sa, &ikeauth, buf, len,
-	    authmsg);
-
-	ibuf_release(authmsg);
-	authmsg = NULL;
-
-	if (ret != 0)
-		goto done;
-
-	if (sa->sa_eapmsk != NULL) {
-		if ((authmsg = ikev2_message_auth(env, sa,
-		    !sa->sa_hdr.sh_initiator)) == NULL) {
-			log_debug("%s: failed to get auth data", __func__);
-			return (-1);
-		}
-
-		/* 2nd AUTH for EAP messages */
-		if ((ret = ikev2_message_authsign(env, sa,
-		    &ikeauth, authmsg)) != 0)
-			goto done;
-
-		sa_state(env, sa, IKEV2_STATE_EAP_VALID);
-	}
-
- done:
-	ibuf_release(authmsg);
-
-	return (ret);
-}
-
 ssize_t
 ikev2_psk(struct iked_sa *sa, u_int8_t *data, size_t length,
     u_int8_t **pskptr)
@@ -2891,615 +1654,6 @@ ikev2_psk(struct iked_sa *sa, u_int8_t *data, size_t length,
 }
 
 int
-ikev2_message_authverify(struct iked *env, struct iked_sa *sa,
-    struct iked_auth *auth, u_int8_t *buf, size_t len, struct ibuf *authmsg)
-{
-	u_int8_t			*key, *psk = NULL;
-	ssize_t				 keylen;
-	struct iked_id			*id;
-	struct iked_dsa			*dsa = NULL;
-	int				 ret = -1;
-	u_int8_t			 keytype;
-
-	if (sa->sa_hdr.sh_initiator)
-		id = &sa->sa_rcert;
-	else
-		id = &sa->sa_icert;
-
-	if ((dsa = dsa_verify_new(auth->auth_method, sa->sa_prf)) == NULL) {
-		log_debug("%s: invalid auth method", __func__);
-		return (-1);
-	}
-
-	switch (auth->auth_method) {
-	case IKEV2_AUTH_SHARED_KEY_MIC:
-		if (!auth->auth_length) {
-			log_debug("%s: no pre-shared key found", __func__);
-			goto done;
-		}
-		if ((keylen = ikev2_psk(sa, auth->auth_data,
-		    auth->auth_length, &psk)) == -1) {
-			log_debug("%s: failed to get PSK", __func__);
-			goto done;
-		}
-		key = psk;
-		keytype = 0;
-		break;
-	default:
-		if (id == NULL) {
-			log_debug("%s: no cert found", __func__);
-			goto done;
-		}
-		key = ibuf_data(id->id_buf);
-		keylen = ibuf_size(id->id_buf);
-		keytype = id->id_type;
-		break;
-	}
-
-	log_debug("%s: method %s keylen %d type %s", __func__,
-	    print_map(auth->auth_method, ikev2_auth_map), keylen,
-	    print_map(id->id_type, ikev2_cert_map));
-
-	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
-	    dsa_init(dsa) != 0 ||
-	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
-		log_debug("%s: failed to compute digital signature", __func__);
-		goto done;
-	}
-
-	if ((ret = dsa_verify_final(dsa, buf, len)) == 0) {
-		log_debug("%s: authentication successful", __func__);
-		sa_state(env, sa, IKEV2_STATE_AUTH_SUCCESS);
-
-		if (!sa->sa_policy->pol_auth.auth_eap &&
-		    auth->auth_method == IKEV2_AUTH_SHARED_KEY_MIC)
-			sa_state(env, sa, IKEV2_STATE_VALID);
-	} else {
-		log_debug("%s: authentication failed", __func__);
-		sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
-	}
-
- done:
-	if (psk != NULL)
-		free(psk);
-	dsa_free(dsa);
-
-	return (ret);
-}
-
-int
-ikev2_message_authsign(struct iked *env, struct iked_sa *sa,
-    struct iked_auth *auth, struct ibuf *authmsg)
-{
-	u_int8_t			*key, *psk = NULL;
-	ssize_t				 keylen;
-	struct iked_hash		*prf = sa->sa_prf;
-	struct iked_id			*id;
-	struct iked_dsa			*dsa = NULL;
-	struct ibuf			*buf;
-	int				 ret = -1;
-	u_int8_t			 keytype;
-
-	if (sa->sa_hdr.sh_initiator)
-		id = &sa->sa_icert;
-	else
-		id = &sa->sa_rcert;
-
-	if ((dsa = dsa_sign_new(auth->auth_method, prf)) == NULL) {
-		log_debug("%s: invalid auth method", __func__);
-		return (-1);
-	}
-
-	switch (auth->auth_method) {
-	case IKEV2_AUTH_SHARED_KEY_MIC:
-		if (!auth->auth_length) {
-			log_debug("%s: no pre-shared key found", __func__);
-			goto done;
-		}
-		if ((keylen = ikev2_psk(sa, auth->auth_data,
-		    auth->auth_length, &psk)) == -1) {
-			log_debug("%s: failed to get PSK", __func__);
-			goto done;
-		}
-		key = psk;
-		keytype = 0;
-		break;
-	default:
-		if (id == NULL) {
-			log_debug("%s: no cert found", __func__);
-			goto done;
-		}
-		key = ibuf_data(id->id_buf);
-		keylen = ibuf_size(id->id_buf);
-		keytype = id->id_type;
-		break;
-	}
-
-	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
-	    dsa_init(dsa) != 0 ||
-	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
-		log_debug("%s: failed to compute digital signature", __func__);
-		goto done;
-	}
-
-	ibuf_release(sa->sa_localauth.id_buf);
-	sa->sa_localauth.id_buf = NULL;
-
-	if ((buf = ibuf_new(NULL, dsa_length(dsa))) == NULL) {
-		log_debug("%s: failed to get auth buffer", __func__);
-		goto done;
-	}
-
-	if ((ret = dsa_sign_final(dsa,
-	    ibuf_data(buf), ibuf_size(buf))) == -1) {
-		log_debug("%s: failed to create auth signature", __func__);
-		ibuf_release(buf);
-		goto done;
-	}
-
-	sa->sa_localauth.id_type = auth->auth_method;
-	sa->sa_localauth.id_buf = buf;
-
-	ret = 0;
- done:
-	if (psk != NULL)
-		free(psk);
-	dsa_free(dsa);
-
-	return (ret);
-}
-
-int
-ikev2_parse_nonce(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	size_t		 len;
-	u_int8_t	*buf;
-	u_int8_t	*msgbuf = ibuf_data(msg->msg_data);
-	struct iked_sa	*sa = msg->msg_sa;
-	struct ibuf	*localnonce, *peernonce;
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld);
-	print_hex(buf, 0, len);
-
-	if (!msg->msg_response) {
-		if ((peernonce = ibuf_new(buf, len)) == NULL) {
-			log_debug("%s: failed to get peer nonce", __func__);
-			return (-1);
-		}
-		if ((localnonce =
-		    ibuf_random(IKED_NONCE_SIZE)) == NULL) {
-			log_debug("%s: failed to get local nonce", __func__);
-			ibuf_release(peernonce);
-			return (-1);
-		}
-
-		ibuf_release(sa->sa_inonce);
-		ibuf_release(sa->sa_rnonce);
-
-		log_debug("%s: updating nonces", __func__);
-
-		if (sa->sa_hdr.sh_initiator) {
-			sa->sa_inonce = localnonce;
-			sa->sa_rnonce = peernonce;
-		} else {
-			sa->sa_inonce = peernonce;
-			sa->sa_rnonce = localnonce;
-		}
-	}
-
-	return (0);
-}
-
-int
-ikev2_parse_notify(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_notify	*n;
-	u_int8_t		*buf, md[SHA_DIGEST_LENGTH];
-	size_t			 len;
-	u_int16_t		 type;
-	u_int32_t		 spi32;
-	u_int64_t		 spi64;
-	struct iked_spi		*rekey;
-	
-	if ((n = ibuf_seek(msg->msg_data, offset, sizeof(*n))) == NULL)
-		return (-1);
-	type = betoh16(n->n_type);
-
-	log_debug("%s: protoid %s spisize %d type %s",
-	    __func__,
-	    print_map(n->n_protoid, ikev2_saproto_map), n->n_spisize,
-	    print_map(type, ikev2_n_map));
-
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(*n);
-	if ((buf = ibuf_seek(msg->msg_data, offset + sizeof(*n), len)) == NULL)
-		return (-1);
-
-	print_hex(buf, 0, len);
-
-	switch (type) {
-	case IKEV2_N_NAT_DETECTION_SOURCE_IP:
-	case IKEV2_N_NAT_DETECTION_DESTINATION_IP:
-		if (ikev2_nat_detection(msg, md, sizeof(md), type,
-		    msg->msg_response) == -1)
-			return (-1);
-		if (len != sizeof(md) || memcmp(buf, md, len) != 0) {
-			log_debug("%s: %s detected NAT, enabling "
-			    "UDP encapsulation", __func__,
-			    print_map(type, ikev2_n_map));
-
-			/*
-			 * Enable UDP encapsulation of ESP packages if
-			 * the check detected NAT.
-			 */
-			if (msg->msg_sa != NULL)
-				msg->msg_sa->sa_udpencap = 1;
-		}
-		print_hex(md, 0, sizeof(md));
-		break;
-	case IKEV2_N_REKEY_SA:
-		if (len != n->n_spisize) {
-			log_debug("%s: malformed notification", __func__);
-			return (-1);
-		}
-		if (msg->msg_decrypted)
-			rekey = &msg->msg_decrypted->msg_rekey;
-		else
-			rekey = &msg->msg_rekey;
-		if (rekey->spi != 0) {
-			log_debug("%s: rekeying of multiple SAs not supported",
-			    __func__);
-			return (-1);
-		}
-		switch (n->n_spisize) {
-		case 4:
-			memcpy(&spi32, buf, len);
-			rekey->spi = betoh32(spi32);
-			break;
-		case 8:
-			memcpy(&spi64, buf, len);
-			rekey->spi = betoh64(spi64);
-			break;
-		default:
-			log_debug("%s: invalid spi size %d", __func__,
-			    n->n_spisize);
-			return (-1);
-		}
-		rekey->spi_size = n->n_spisize;
-		rekey->spi_protoid = n->n_protoid;
-
-		log_debug("%s: rekey %s spi %s", __func__,
-		    print_map(n->n_protoid, ikev2_saproto_map),
-		    print_spi(rekey->spi, n->n_spisize));
-		break;
-	}
-
-	return (0);
-}
-
-int
-ikev2_parse_delete(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_delete	*del, *localdel;
-	u_int64_t		 spi64, spi = 0, *localspi = NULL;
-	u_int32_t		 spi32;
-	size_t			 len, i, cnt, sz, found = 0, failed = 0;
-	u_int8_t		*buf, firstpayload = 0;
-	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
-	struct iked_sa		*sa = msg->msg_sa;
-	struct ibuf		*resp = NULL;
-	int			 ret = -1;
-
-	if ((del = ibuf_seek(msg->msg_data, offset, sizeof(*del))) == NULL)
-		return (-1);
-	cnt = betoh16(del->del_nspi);
-	sz = del->del_spisize;
-
-	log_debug("%s: protoid %s spisize %d nspi %d",
-	    __func__, print_map(del->del_protoid, ikev2_saproto_map),
-	    sz, cnt);
-
-	buf = msgbuf + offset + sizeof(*del);
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(*del);
-
-	print_hex(buf, 0, len);
-
-	switch (sz) {
-	case 4:
-	case 8:
-		break;
-	default:
-		if (!msg->msg_response &&
-		    del->del_protoid == IKEV2_SAPROTO_IKE) {
-			sa_state(env, sa, IKEV2_STATE_DELETE);
-			return (0);
-		}
-		log_debug("%s: invalid SPI size", __func__);
-		return (-1);
-	}
-
-	if ((len / sz) != cnt) {
-		log_debug("%s: invalid payload length %d/%d != %d",
-		    __func__, len, sz, cnt);
-		return (-1);
-	}
-
-	if (!msg->msg_response &&
-	    (localspi = calloc(cnt, sizeof(u_int64_t))) == NULL) {
-		log_warn("%s", __func__);
-		return (-1);
-	}
-
-	for (i = 0; i < cnt; i++) {
-		/* XXX delete SAs */
-		switch (sz) {
-		case 4:
-			memcpy(&spi32, buf + (i * sz), sizeof(spi32));
-			spi = betoh32(spi32);
-			break;
-		case 8:
-			memcpy(&spi64, buf + (i * sz), sizeof(spi64));
-			spi = betoh64(spi64);
-			break;
-		}
-		if (msg->msg_response) {
-			log_debug("%s: spi %s", __func__, print_spi(spi, sz));
-			continue;
-		}
-
-		if (ikev2_childsa_delete(env, sa,
-		    del->del_protoid, spi, &localspi[i], 0) == -1)
-			failed++;
-		else
-			found++;
-	}
-
-	if (msg->msg_response)
-		return (0);
-
-	if ((resp = ibuf_static()) == NULL)
-		goto done;
-
-	if (found) {
-		if ((localdel = ibuf_advance(resp, sizeof(*localdel))) == NULL)
-			goto done;
-
-		firstpayload = IKEV2_PAYLOAD_DELETE;
-		localdel->del_protoid = del->del_protoid;
-		localdel->del_spisize = del->del_spisize;
-		localdel->del_nspi = htobe16(found);
-
-		for (i = 0; i < cnt; i++) {
-			if (!localspi[i])
-				continue;
-
-			switch (sz) {
-			case 4:
-				spi32 = htobe32(localspi[i]);
-				if (ibuf_add(resp, &spi32, sizeof(spi32)) != 0)
-					goto done;
-				break;
-			case 8:
-				spi64 = htobe64(localspi[i]);
-				if (ibuf_add(resp, &spi64, sizeof(spi64)) != 0)
-					goto done;
-				break;
-			}
-		}
-
-	}
-
-	if (found) {
-		ret = ikev2_send_ike_e(env, sa, resp,
-		    firstpayload, IKEV2_EXCHANGE_INFORMATIONAL, 1);
-	} else {
-		/* XXX should we send an INVALID_SPI notification? */
-		ret = 0;
-	}
-
- done:
-	if (localspi != NULL)
-		free(localspi);
-	ibuf_release(resp);
-	return (ret);
-}
-
-int
-ikev2_parse_ts(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset, u_int payload)
-{
-	u_int8_t			*ptr;
-	struct ikev2_tsp		 tsp;
-	struct ikev2_ts			 ts;
-	size_t				 len, i;
-	struct sockaddr_in		 s4;
-	struct sockaddr_in6		 s6;
-	u_int8_t			 buf[2][128];
-	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
-
-	memcpy(&tsp, msgbuf + offset, sizeof(tsp));
-	offset += sizeof(tsp);
-
-	ptr = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(tsp);
-
-	log_debug("%s: count %d length %d", __func__,
-	    tsp.tsp_count, len);
-
-	for (i = 0; i < tsp.tsp_count; i++) {
-		memcpy(&ts, msgbuf + offset, sizeof(ts));
-
-		log_debug("%s: type %s protoid %u length %d "
-		    "startport %u endport %u", __func__,
-		    print_map(ts.ts_type, ikev2_ts_map),
-		    ts.ts_protoid, betoh16(ts.ts_length),
-		    betoh16(ts.ts_startport),
-		    betoh16(ts.ts_endport));
-
-		switch (ts.ts_type) {
-		case IKEV2_TS_IPV4_ADDR_RANGE:
-			bzero(&s4, sizeof(s4));
-			s4.sin_family = AF_INET;
-			s4.sin_len = sizeof(s4);
-			memcpy(&s4.sin_addr.s_addr,
-			    msgbuf + offset + sizeof(ts), 4);
-			print_host((struct sockaddr_storage *)&s4,
-			    (char *)buf[0], sizeof(buf[0]));
-			memcpy(&s4.sin_addr.s_addr,
-			    msgbuf + offset + sizeof(ts) + 4, 4);
-			print_host((struct sockaddr_storage *)&s4,
-			    (char *)buf[1], sizeof(buf[1]));
-			log_debug("%s: start %s end %s", __func__,
-			    buf[0], buf[1]);
-			break;
-		case IKEV2_TS_IPV6_ADDR_RANGE:
-			bzero(&s6, sizeof(s6));
-			s6.sin6_family = AF_INET6;
-			s6.sin6_len = sizeof(s6);
-			memcpy(&s6.sin6_addr,
-			    msgbuf + offset + sizeof(ts), 16);
-			print_host((struct sockaddr_storage *)&s6,
-			    (char *)buf[0], sizeof(buf[0]));
-			memcpy(&s6.sin6_addr,
-			    msgbuf + offset + sizeof(ts) + 16, 16);
-			print_host((struct sockaddr_storage *)&s6,
-			    (char *)buf[1], sizeof(buf[1]));
-			log_debug("%s: start %s end %s", __func__,
-			    buf[0], buf[1]);
-			break;
-		default:
-			break;
-		}
-
-		offset += betoh16(ts.ts_length);
-	}
-
-	return (0);
-}
-
-int
-ikev2_parse_e(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ibuf		*e = NULL;
-	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
-	struct iked_message	 emsg;
-	u_int8_t		*buf;
-	size_t			 len;
-	int			 ret = -1;
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld);
-
-	if ((e = ibuf_new(buf, len)) == NULL)
-		goto done;
-
-	if ((e = ikev2_message_decrypt(env, msg->msg_sa,
-	    msg->msg_data, e)) == NULL)
-		goto done;
-
-	/*
-	 * Parse decrypted payload
-	 */
-	bzero(&emsg, sizeof(emsg));
-	memcpy(&emsg, msg, sizeof(*msg));
-	emsg.msg_data = e;
-	emsg.msg_decrypted = msg;
-	TAILQ_INIT(&emsg.msg_proposals);
-
-	ret = ikev2_parse_payloads(env, &emsg, 0, ibuf_size(e),
-	    pld->pld_nextpayload, 0);
-
- done:
-	ibuf_release(e);
-
-	return (ret);
-}
-
-int
-ikev2_parse_cp(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct ikev2_cp		 cp;
-	struct ikev2_cfg	*cfg;
-	u_int8_t		*buf;
-	size_t			 len, i;
-	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
-	struct iked_sa		*sa = msg->msg_sa;
-
-	memcpy(&cp, msgbuf + offset, sizeof(cp));
-	offset += sizeof(cp);
-
-	buf = msgbuf + offset;
-	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cp);
-
-	log_debug("%s: type %s",
-	    __func__, print_map(cp.cp_type, ikev2_cp_map), len);
-	print_hex(buf, 0, len);
-
-	for (i = 0; i < len;) {
-		cfg = (struct ikev2_cfg *)(buf + i);
-
-		log_debug("%s: %s 0x%04x length %d", __func__,
-		    print_map(betoh16(cfg->cfg_type), ikev2_cfg_map),
-		    betoh16(cfg->cfg_type),
-		    betoh16(cfg->cfg_length));
-
-		i += betoh16(cfg->cfg_length) + sizeof(*cfg);
-	}
-
-	if (msg->msg_response)
-		return (0);
-
-	if (sa)
-		sa->sa_cp = cp.cp_type;
-
-	return (0);
-}
-
-int
-ikev2_parse_eap(struct iked *env, struct ikev2_payload *pld,
-    struct iked_message *msg, off_t offset)
-{
-	struct eap_header		*hdr;
-	struct eap_message		*eap = NULL;
-	struct iked_sa			*sa = msg->msg_sa;
-	size_t				 len;
-
-	if ((hdr = ibuf_seek(msg->msg_data, offset, sizeof(*hdr))) == NULL) {
-		log_debug("%s: failed to get EAP header", __func__);
-		return (-1);
-	}
-
-	len = betoh16(hdr->eap_length);
-
-	if (len < sizeof(*eap)) {
-		log_info("%s: %s id %d length %d", __func__,
-		    print_map(hdr->eap_code, eap_code_map),
-		    hdr->eap_id, betoh16(hdr->eap_length));
-	} else {
-		/* Now try to get the indicated length */
-		if ((eap = ibuf_seek(msg->msg_data, offset, len)) == NULL) {
-			log_debug("%s: invalid EAP length", __func__);
-			return (-1);
-		}
-
-		log_info("%s: %s id %d length %d EAP-%s", __func__,
-		    print_map(eap->eap_code, eap_code_map),
-		    eap->eap_id, betoh16(eap->eap_length),
-		    print_map(eap->eap_type, eap_type_map));
-	}
-
-	if (eap_parse(env, sa, hdr, msg->msg_response) == -1)
-		return (-1);
-
-	return (0);
-}
-
-int
 ikev2_sa_negotiate(struct iked_sa *sa, struct iked_proposals *local,
     struct iked_proposals *peer, u_int8_t protoid)
 {
diff --git a/sbin/iked/ikev2.h b/sbin/iked/ikev2.h
index 13278811f1a..a918ba8afba 100644
--- a/sbin/iked/ikev2.h
+++ b/sbin/iked/ikev2.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ikev2.h,v 1.1 2010/06/03 16:41:12 reyk Exp $	*/
+/*	$OpenBSD: ikev2.h,v 1.2 2010/06/10 08:29:47 reyk Exp $	*/
 /*	$vantronix: ikev2.h,v 1.27 2010/05/19 12:20:30 reyk Exp $	*/
 
 /*
@@ -75,20 +75,9 @@ extern size_t ikev2_default_nesp_transforms;
 extern struct iked_constmap ikev2_state_map[];
 
 /*
- * IKE header (partially compatible with IKEv1)
+ * IKEv2 definitions of the IKE header
  */
 
-struct ike_header {
-	u_int64_t	 ike_ispi;		/* IKE_SA Initiator SPI */
-	u_int64_t	 ike_rspi;		/* IKE_SA Responder SPI */
-	u_int8_t	 ike_nextpayload;	/* Next payload type */
-	u_int8_t	 ike_version;		/* Major/Minor version number */
-	u_int8_t	 ike_exchange;		/* Exchange type */
-	u_int8_t	 ike_flags;		/* Message options */
-	u_int32_t	 ike_msgid;		/* Message identifier */
-	u_int32_t	 ike_length;		/* Total message length */
-} __packed;
-
 /* IKEv2 exchange types */
 #define IKEV2_EXCHANGE_IKE_SA_INIT		34	/* Initial Exchange */
 #define IKEV2_EXCHANGE_IKE_AUTH			35	/* Authentication */
@@ -504,12 +493,4 @@ struct ikev2_cfg {
 
 extern struct iked_constmap ikev2_cfg_map[];
 
-/*
- * Internal structures
- */
-
-struct ikev2_message {
-	struct ike_header	 im_header;
-};
-
 #endif /* _IKEV2_H */
diff --git a/sbin/iked/ikev2_msg.c b/sbin/iked/ikev2_msg.c
new file mode 100644
index 00000000000..85d82138539
--- /dev/null
+++ b/sbin/iked/ikev2_msg.c
@@ -0,0 +1,788 @@
+/*	$OpenBSD: ikev2_msg.c,v 1.1 2010/06/10 08:29:47 reyk Exp $	*/
+/*	$vantronix: ikev2.c,v 1.101 2010/06/03 07:57:33 reyk Exp $	*/
+
+/*
+ * Copyright (c) 2010 Reyk Floeter <reyk@vantronix.net>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <sys/param.h>
+#include <sys/types.h>
+#include <sys/queue.h>
+#include <sys/socket.h>
+#include <sys/wait.h>
+#include <sys/uio.h>
+
+#include <netinet/in.h>
+#include <netinet/ip_ipsp.h>
+#include <arpa/inet.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <getopt.h>
+#include <signal.h>
+#include <errno.h>
+#include <err.h>
+#include <pwd.h>
+#include <event.h>
+
+#include <openssl/sha.h>
+#include <openssl/evp.h>
+
+#include "iked.h"
+#include "ikev2.h"
+#include "eap.h"
+#include "dh.h"
+
+void
+ikev2_msg_cb(int fd, short event, void *arg)
+{
+	struct iked_socket	*sock = arg;
+	struct iked		*env = sock->sock_env;
+	struct iked_message	 msg;
+	struct ike_header	 hdr;
+	u_int32_t		 natt = 0x00000000;
+	u_int8_t		 buf[IKED_MSGBUF_MAX];
+	ssize_t			 len;
+	off_t			 off;
+	struct iovec		 iov[2];
+
+	bzero(&msg, sizeof(msg));
+	bzero(buf, sizeof(buf));
+
+	msg.msg_peerlen = sizeof(msg.msg_peer);
+	msg.msg_locallen = sizeof(msg.msg_local);
+	memcpy(&msg.msg_local, &sock->sock_addr, sizeof(sock->sock_addr));
+
+	if ((len = recvfromto(fd, buf, sizeof(buf), 0,
+	    (struct sockaddr*)&msg.msg_peer, &msg.msg_peerlen,
+	    (struct sockaddr*)&msg.msg_local, &msg.msg_locallen)) <
+	    (ssize_t)sizeof(natt))
+		return;
+
+	if (socket_getport(&msg.msg_local) == IKED_NATT_PORT) {
+		if (bcmp(&natt, buf, sizeof(natt)) != 0)
+			return;
+		msg.msg_natt = 1;
+		off = sizeof(natt);
+	} else
+		off = 0;
+
+	if ((size_t)(len - off) <= sizeof(hdr))
+		return;
+	memcpy(&hdr, buf + off, sizeof(hdr));
+
+	if ((msg.msg_data = ibuf_new(buf + off, len - off)) == NULL)
+		return;
+
+	if (hdr.ike_version == IKEV1_VERSION) {
+		iov[0].iov_base = &msg;
+		iov[0].iov_len = sizeof(msg);
+		iov[1].iov_base = buf;
+		iov[1].iov_len = len;
+
+		imsg_composev_proc(env, PROC_IKEV1, IMSG_IKE_MESSAGE, -1,
+		    iov, 2);
+		goto done;
+	}
+	TAILQ_INIT(&msg.msg_proposals);
+
+	msg.msg_fd = fd;
+	ikev2_recv(env, &msg);
+
+ done:
+	message_cleanup(env, &msg);
+}
+
+struct ibuf *
+ikev2_msg_init(struct iked *env, struct iked_message *msg,
+    struct sockaddr_storage *peer, socklen_t peerlen,
+    struct sockaddr_storage *local, socklen_t locallen, int response)
+{
+	bzero(msg, sizeof(*msg));
+	memcpy(&msg->msg_peer, peer, peerlen);
+	msg->msg_peerlen = peerlen;
+	memcpy(&msg->msg_local, local, locallen);
+	msg->msg_locallen = locallen;
+	msg->msg_response = response ? 1 : 0;
+	msg->msg_fd = -1;
+	msg->msg_data = ibuf_static();
+
+	return (msg->msg_data);
+}
+
+int
+ikev2_msg_valid_ike_sa(struct iked *env, struct ike_header *oldhdr,
+    struct iked_message *msg)
+{
+#if 0
+	/* XXX Disabled, see comment below */
+	struct iked_message		 resp;
+	struct ike_header		*hdr;
+	struct ikev2_payload		*pld;
+	struct ikev2_notify		*n;
+	struct ibuf			*buf;
+	struct iked_sa			 sa;
+#endif
+
+	if (msg->msg_sa != NULL && msg->msg_policy != NULL)
+		return (0);
+
+#if 0
+	/*
+	 * XXX Sending INVALID_IKE_SPIs notifications is disabled
+	 * XXX because it is not mandatory and ignored by most
+	 * XXX implementations.  We might want to enable it in
+	 * XXX combination with a rate-limitation to avoid DoS situations.
+	 */
+
+	/* Fail without error message */
+	if (msg->msg_response || msg->msg_policy == NULL)
+		return (-1);
+
+	/* Invalid IKE SA, return notification */
+	if ((buf = ikev2_msg_init(env, &resp,
+	    &msg->msg_peer, msg->msg_peerlen,
+	    &msg->msg_local, msg->msg_locallen, 1)) == NULL)
+		goto done;
+
+	bzero(&sa, sizeof(sa));
+	if ((oldhdr->ike_flags & IKEV2_FLAG_INITIATOR) == 0)
+		sa.sa_hdr.sh_initiator = 1;
+	sa.sa_hdr.sh_ispi = betoh64(oldhdr->ike_ispi);
+	sa.sa_hdr.sh_rspi = betoh64(oldhdr->ike_rspi);
+
+	/* IKE header */
+	if ((hdr = ikev2_add_header(buf, &sa, betoh32(oldhdr->ike_msgid),
+	    IKEV2_PAYLOAD_NOTIFY, IKEV2_EXCHANGE_INFORMATIONAL,
+	    IKEV2_FLAG_RESPONSE)) == NULL)
+		goto done;
+
+	/* SA payload */
+	if ((pld = ikev2_add_payload(buf)) == NULL)
+		goto done;
+	if ((n = ibuf_advance(buf, sizeof(*n))) == NULL)
+		goto done;
+	n->n_protoid = IKEV2_SAPROTO_IKE;
+	n->n_spisize = 0;
+	n->n_type = htobe16(IKEV2_N_INVALID_IKE_SPI);
+
+	if (ikev2_next_payload(pld, sizeof(*n), IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
+		goto done;
+
+	(void)ikev2_pld_parse(env, hdr, &resp, 0);
+	(void)ikev2_msg_send(env, msg->msg_fd, &resp);
+
+ done:
+	message_cleanup(env, &resp);
+#endif
+
+	/* Always fail */
+	return (-1);
+}
+
+int
+ikev2_msg_send(struct iked *env, int fd, struct iked_message *msg)
+{
+	struct ibuf	*buf = msg->msg_data;
+	u_int32_t	 natt = 0x00000000;
+	struct ike_header	*hdr;
+
+	if (buf == NULL || (hdr = ibuf_seek(msg->msg_data,
+	    msg->msg_offset, sizeof(*hdr))) == NULL)
+		return (-1);
+
+	log_info("%s: %s to %s from %s", __func__,
+	    print_map(hdr->ike_exchange, ikev2_exchange_map),
+	    print_host(&msg->msg_peer, NULL, 0),
+	    print_host(&msg->msg_local, NULL, 0));
+
+	if (msg->msg_natt || (msg->msg_sa && msg->msg_sa->sa_natt)) {
+		if (ibuf_prepend(buf, &natt, sizeof(natt)) == -1) {
+			log_debug("%s: failed to set NAT-T", __func__);
+			return (-1);
+		}
+	}
+	if ((sendto(fd, ibuf_data(buf), ibuf_size(buf), 0,
+	    (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen)) == -1) {
+		log_warn("%s: sendto", __func__);
+		return (-1);
+	}
+
+	return (0);
+}
+
+u_int32_t
+ikev2_msg_id(struct iked *env, struct iked_sa *sa, int response)
+{
+	if (response)
+		return (sa->sa_msgid);
+
+	if (++sa->sa_msgid == UINT32_MAX) {
+		/* XXX we should close and renegotiate the connection now */
+		log_debug("%s: IKEv2 message sequence overflow", __func__);
+	}
+
+	return (sa->sa_msgid);
+}
+
+struct ibuf *
+ikev2_msg_encrypt(struct iked *env, struct iked_sa *sa, struct ibuf *src)
+{
+	size_t			 len, ivlen, encrlen, integrlen, blocklen,
+				    outlen;
+	u_int8_t		*buf, pad = 0, *ptr;
+	struct ibuf		*integr, *encr, *dst = NULL, *out = NULL;
+
+	buf = ibuf_data(src);
+	len = ibuf_size(src);
+
+	log_debug("%s: decrypted length %d", __func__, len);
+	print_hex(buf, 0, len);
+
+	if (sa == NULL ||
+	    sa->sa_encr == NULL ||
+	    sa->sa_integr == NULL) {
+		log_debug("%s: invalid SA", __func__);
+		goto done;
+	}
+
+	if (sa->sa_hdr.sh_initiator) {
+		encr = sa->sa_key_iencr;
+		integr = sa->sa_key_iauth;
+	} else {
+		encr = sa->sa_key_rencr;
+		integr = sa->sa_key_rauth;
+	}
+
+	blocklen = cipher_length(sa->sa_encr);
+	ivlen = cipher_ivlength(sa->sa_encr);
+	integrlen = hash_length(sa->sa_integr);
+	encrlen = roundup(len + sizeof(pad), blocklen);
+	pad = encrlen - (len + sizeof(pad));
+
+	/*
+	 * Pad the payload and encrypt it
+	 */
+	if (pad) {
+		if ((ptr = ibuf_advance(src, pad)) == NULL)
+			goto done;
+		arc4random_buf(ptr, pad);
+	}
+	if (ibuf_add(src, &pad, sizeof(pad)) != 0)
+		goto done;
+
+	log_debug("%s: padded length %d", __func__, ibuf_size(src));
+	print_hex(ibuf_data(src), 0, ibuf_size(src));
+
+	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
+	cipher_setiv(sa->sa_encr, NULL, 0);	/* new IV */
+	cipher_init_encrypt(sa->sa_encr);
+
+	if ((dst = ibuf_dup(sa->sa_encr->encr_iv)) == NULL)
+		goto done;
+
+	if ((out = ibuf_new(NULL,
+	    cipher_outlength(sa->sa_encr, encrlen))) == NULL)
+		goto done;
+
+	outlen = ibuf_size(out);
+	cipher_update(sa->sa_encr,
+	    ibuf_data(src), encrlen, ibuf_data(out), &outlen);
+
+	if (outlen && ibuf_add(dst, ibuf_data(out), outlen) != 0)
+		goto done;
+
+	outlen = cipher_outlength(sa->sa_encr, 0);
+	cipher_final(sa->sa_encr, out->buf, &outlen);
+	if (outlen)
+		ibuf_add(dst, out->buf, outlen);
+
+	if ((ptr = ibuf_advance(dst, integrlen)) == NULL)
+		goto done;
+	bzero(ptr, integrlen);
+
+	log_debug("%s: length %d, padding %d, output length %d",
+	    __func__, len + sizeof(pad), pad, ibuf_size(dst));
+	print_hex(ibuf_data(dst), 0, ibuf_size(dst));
+
+	ibuf_release(src);
+	ibuf_release(out);
+	return (dst);
+ done:
+	ibuf_release(src);
+	ibuf_release(out);
+	ibuf_release(dst);
+	return (NULL);
+}
+
+int
+ikev2_msg_integr(struct iked *env, struct iked_sa *sa, struct ibuf *src)
+{
+	int			 ret = -1;
+	size_t			 integrlen, tmplen;
+	struct ibuf		*integr, *prf, *tmp = NULL;
+	u_int8_t		*ptr;
+
+	log_debug("%s: message length %d", __func__, ibuf_size(src));
+	print_hex(ibuf_data(src), 0, ibuf_size(src));
+
+	if (sa == NULL ||
+	    sa->sa_integr == NULL) {
+		log_debug("%s: invalid SA", __func__);
+		return (-1);
+	}
+
+	if (sa->sa_hdr.sh_initiator) {
+		integr = sa->sa_key_iauth;
+		prf = sa->sa_key_iprf;
+	} else {
+		integr = sa->sa_key_rauth;
+		prf = sa->sa_key_rprf;
+	}
+
+	integrlen = hash_length(sa->sa_integr);
+
+	log_debug("%s: integrity checksum length %d", __func__,
+	    integrlen);
+
+	/*
+	 * Validate packet checksum
+	 */
+	if ((tmp = ibuf_new(NULL, hash_keylength(sa->sa_integr))) == NULL)
+		goto done;
+
+	hash_setkey(sa->sa_integr, ibuf_data(integr), ibuf_size(integr));
+	hash_init(sa->sa_integr);
+	hash_update(sa->sa_integr, ibuf_data(src),
+	    ibuf_size(src) - integrlen);
+	hash_final(sa->sa_integr, ibuf_data(tmp), &tmplen);
+
+	if (tmplen != integrlen) {
+		log_debug("%s: hash failure", __func__);
+		goto done;
+	}
+
+	if ((ptr = ibuf_seek(src,
+	    ibuf_size(src) - integrlen, integrlen)) == NULL)
+		goto done;
+	memcpy(ptr, ibuf_data(tmp), tmplen);
+
+	print_hex(ibuf_data(tmp), 0, ibuf_size(tmp));
+
+	ret = 0;
+ done:
+	ibuf_release(tmp);
+
+	return (ret);
+}
+
+struct ibuf *
+ikev2_msg_decrypt(struct iked *env, struct iked_sa *sa,
+    struct ibuf *msg, struct ibuf *src)
+{
+	size_t			 ivlen, encrlen, integrlen, blocklen,
+				    outlen, tmplen;
+	u_int8_t		 pad, *ptr;
+	struct ibuf		*integr, *encr, *tmp = NULL, *out = NULL;
+	off_t			 ivoff, encroff, integroff;
+
+	if (sa == NULL ||
+	    sa->sa_encr == NULL ||
+	    sa->sa_integr == NULL) {
+		log_debug("%s: invalid SA", __func__);
+		print_hex(ibuf_data(src), 0, ibuf_size(src));
+		goto done;
+	}
+
+	if (!sa->sa_hdr.sh_initiator) {
+		encr = sa->sa_key_iencr;
+		integr = sa->sa_key_iauth;
+	} else {
+		encr = sa->sa_key_rencr;
+		integr = sa->sa_key_rauth;
+	}
+
+	blocklen = cipher_length(sa->sa_encr);
+	ivlen = cipher_ivlength(sa->sa_encr);
+	ivoff = 0;
+	integrlen = hash_length(sa->sa_integr);
+	integroff = ibuf_size(src) - integrlen;
+	encroff = ivlen;
+	encrlen = ibuf_size(src) - integrlen - ivlen;
+
+	log_debug("%s: IV length %d", __func__, ivlen);
+	print_hex(ibuf_data(src), 0, ivlen);
+	log_debug("%s: encrypted payload length %d", __func__, encrlen);
+	print_hex(ibuf_data(src), encroff, encrlen);
+	log_debug("%s: integrity checksum length %d", __func__, integrlen);
+	print_hex(ibuf_data(src), integroff, integrlen);
+
+	/*
+	 * Validate packet checksum
+	 */
+	if ((tmp = ibuf_new(NULL, ibuf_length(integr))) == NULL)
+		goto done;
+
+	hash_setkey(sa->sa_integr, integr->buf, ibuf_length(integr));
+	hash_init(sa->sa_integr);
+	hash_update(sa->sa_integr, ibuf_data(msg),
+	    ibuf_size(msg) - integrlen);
+	hash_final(sa->sa_integr, tmp->buf, &tmplen);
+
+	if (memcmp(tmp->buf, ibuf_data(src) + integroff, integrlen) != 0) {
+		log_debug("%s: integrity check failed", __func__);
+		goto done;
+	}
+
+	log_debug("%s: integrity check succeeded", __func__, tmplen);
+	print_hex(tmp->buf, 0, tmplen);
+
+	ibuf_release(tmp);
+	tmp = NULL;
+
+	/*
+	 * Decrypt the payload and strip any padding
+	 */
+	if ((encrlen % blocklen) != 0) {
+		log_debug("%s: unaligned encrypted payload", __func__);
+		goto done;
+	}
+
+	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
+	cipher_setiv(sa->sa_encr, ibuf_data(src) + ivoff, ivlen);
+	cipher_init_decrypt(sa->sa_encr);
+
+	if ((out = ibuf_new(NULL, cipher_outlength(sa->sa_encr,
+	    encrlen))) == NULL)
+		goto done;
+
+	outlen = ibuf_length(out);
+	/* XXX why does it need encrlen + blocklen to work correctly? */
+	cipher_update(sa->sa_encr,
+	    ibuf_data(src) + encroff, encrlen + blocklen,
+	    ibuf_data(out), &outlen);
+	cipher_final(sa->sa_encr, ibuf_seek(out, outlen, blocklen), &tmplen);
+	if (tmplen)
+		outlen += tmplen;
+
+	/*
+	 * XXX 
+	 * XXX the padding is wrong
+	 * XXX
+	 */
+	ptr = ibuf_seek(out, outlen - 1, 1);
+	pad = *ptr;
+
+	log_debug("%s: decrypted payload length %d/%d padding %d",
+	    __func__, outlen, encrlen, pad);
+	print_hex(ibuf_data(out), 0, ibuf_size(out));
+
+	if (ibuf_setsize(out, outlen) != 0)
+		goto done;
+
+	ibuf_release(src);
+	return (out);
+ done:
+	ibuf_release(tmp);
+	ibuf_release(out);
+	ibuf_release(src);
+	return (NULL);
+}
+
+int
+ikev2_msg_send_encrypt(struct iked *env, struct iked_sa *sa,
+    struct ibuf **ep, u_int8_t exchange, u_int8_t firstpayload, int response)
+{
+	struct iked_message		 resp;
+	struct ike_header		*hdr;
+	struct ikev2_payload		*pld;
+	struct ibuf			*buf, *e = *ep;
+	int				 ret = -1;
+
+	if ((buf = ikev2_msg_init(env, &resp,
+	    &sa->sa_peer.addr, sa->sa_peer.addr.ss_len,
+	    &sa->sa_local.addr, sa->sa_local.addr.ss_len, 1)) == NULL)
+		goto done;
+
+	/* IKE header */
+	if ((hdr = ikev2_add_header(buf, sa,
+	    ikev2_msg_id(env, sa, response),
+	    IKEV2_PAYLOAD_E, exchange,
+	    response ? IKEV2_FLAG_RESPONSE : 0)) == NULL)
+		goto done;
+
+	if ((pld = ikev2_add_payload(buf)) == NULL)
+		goto done;
+
+	/* Encrypt message and add as an E payload */
+	if ((e = ikev2_msg_encrypt(env, sa, e)) == NULL) {
+		log_debug("%s: encryption failed", __func__);
+		goto done;
+	}
+	if (ibuf_cat(buf, e) != 0)
+		goto done;
+	if (ikev2_next_payload(pld, ibuf_size(e), firstpayload) == -1)
+		goto done;
+
+	if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
+		goto done;
+
+	/* Add integrity checksum (HMAC) */
+	if (ikev2_msg_integr(env, sa, buf) != 0) {
+		log_debug("%s: integrity checksum failed", __func__);
+		goto done;
+	}
+
+	resp.msg_data = buf;
+	resp.msg_sa = sa;
+	TAILQ_INIT(&resp.msg_proposals);
+
+	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
+	(void)ikev2_pld_parse(env, hdr, &resp, 0);
+	sa->sa_hdr.sh_initiator = sa->sa_hdr.sh_initiator ? 0 : 1;
+
+	ret = ikev2_msg_send(env, sa->sa_fd, &resp);
+
+ done:
+	/* e is cleaned up by the calling function */
+	*ep = e;
+	message_cleanup(env, &resp);
+
+	return (ret);
+}
+
+struct ibuf *
+ikev2_msg_auth(struct iked *env, struct iked_sa *sa, int response)
+{
+	struct ibuf		*authmsg = NULL, *nonce, *prfkey, *buf;
+	u_int8_t		*ptr;
+	struct iked_id		*id;
+	size_t			 tmplen;
+
+	/*
+	 * Create the payload to be signed/MAC'ed for AUTH
+	 */
+
+	if (!response) {
+		if ((nonce = sa->sa_rnonce) == NULL ||
+		    (sa->sa_iid.id_type == 0) ||
+		    (prfkey = sa->sa_key_iprf) == NULL ||
+		    (buf = sa->sa_1stmsg) == NULL)
+			return (NULL);
+		id = &sa->sa_iid;
+	} else {
+		if ((nonce = sa->sa_inonce) == NULL ||
+		    (sa->sa_rid.id_type == 0) ||
+		    (prfkey = sa->sa_key_rprf) == NULL ||
+		    (buf = sa->sa_2ndmsg) == NULL)
+			return (NULL);
+		id = &sa->sa_rid;
+	}
+
+	if ((authmsg = ibuf_dup(buf)) == NULL)
+		return (NULL);
+	if (ibuf_cat(authmsg, nonce) != 0)
+		goto fail;
+
+	if ((hash_setkey(sa->sa_prf, ibuf_data(prfkey),
+	    ibuf_size(prfkey))) == NULL)
+		goto fail;
+
+	if ((ptr = ibuf_advance(authmsg,
+	    hash_length(sa->sa_prf))) == NULL)
+		goto fail;
+
+	hash_init(sa->sa_prf);
+	hash_update(sa->sa_prf, ibuf_data(id->id_buf), ibuf_size(id->id_buf));
+	hash_final(sa->sa_prf, ptr, &tmplen);
+
+	if (tmplen != hash_length(sa->sa_prf))
+		goto fail;
+
+	log_debug("%s: %s auth data length %d",
+	    __func__, response ? "responder" : "initiator",
+	    ibuf_size(authmsg));
+	print_hex(ibuf_data(authmsg), 0, ibuf_size(authmsg));
+
+	return (authmsg);
+
+ fail:
+	ibuf_release(authmsg);
+	return (NULL);
+}
+
+int
+ikev2_msg_authverify(struct iked *env, struct iked_sa *sa,
+    struct iked_auth *auth, u_int8_t *buf, size_t len, struct ibuf *authmsg)
+{
+	u_int8_t			*key, *psk = NULL;
+	ssize_t				 keylen;
+	struct iked_id			*id;
+	struct iked_dsa			*dsa = NULL;
+	int				 ret = -1;
+	u_int8_t			 keytype;
+
+	if (sa->sa_hdr.sh_initiator)
+		id = &sa->sa_rcert;
+	else
+		id = &sa->sa_icert;
+
+	if ((dsa = dsa_verify_new(auth->auth_method, sa->sa_prf)) == NULL) {
+		log_debug("%s: invalid auth method", __func__);
+		return (-1);
+	}
+
+	switch (auth->auth_method) {
+	case IKEV2_AUTH_SHARED_KEY_MIC:
+		if (!auth->auth_length) {
+			log_debug("%s: no pre-shared key found", __func__);
+			goto done;
+		}
+		if ((keylen = ikev2_psk(sa, auth->auth_data,
+		    auth->auth_length, &psk)) == -1) {
+			log_debug("%s: failed to get PSK", __func__);
+			goto done;
+		}
+		key = psk;
+		keytype = 0;
+		break;
+	default:
+		if (id == NULL) {
+			log_debug("%s: no cert found", __func__);
+			goto done;
+		}
+		key = ibuf_data(id->id_buf);
+		keylen = ibuf_size(id->id_buf);
+		keytype = id->id_type;
+		break;
+	}
+
+	log_debug("%s: method %s keylen %d type %s", __func__,
+	    print_map(auth->auth_method, ikev2_auth_map), keylen,
+	    print_map(id->id_type, ikev2_cert_map));
+
+	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
+	    dsa_init(dsa) != 0 ||
+	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
+		log_debug("%s: failed to compute digital signature", __func__);
+		goto done;
+	}
+
+	if ((ret = dsa_verify_final(dsa, buf, len)) == 0) {
+		log_debug("%s: authentication successful", __func__);
+		sa_state(env, sa, IKEV2_STATE_AUTH_SUCCESS);
+
+		if (!sa->sa_policy->pol_auth.auth_eap &&
+		    auth->auth_method == IKEV2_AUTH_SHARED_KEY_MIC)
+			sa_state(env, sa, IKEV2_STATE_VALID);
+	} else {
+		log_debug("%s: authentication failed", __func__);
+		sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
+	}
+
+ done:
+	if (psk != NULL)
+		free(psk);
+	dsa_free(dsa);
+
+	return (ret);
+}
+
+int
+ikev2_msg_authsign(struct iked *env, struct iked_sa *sa,
+    struct iked_auth *auth, struct ibuf *authmsg)
+{
+	u_int8_t			*key, *psk = NULL;
+	ssize_t				 keylen;
+	struct iked_hash		*prf = sa->sa_prf;
+	struct iked_id			*id;
+	struct iked_dsa			*dsa = NULL;
+	struct ibuf			*buf;
+	int				 ret = -1;
+	u_int8_t			 keytype;
+
+	if (sa->sa_hdr.sh_initiator)
+		id = &sa->sa_icert;
+	else
+		id = &sa->sa_rcert;
+
+	if ((dsa = dsa_sign_new(auth->auth_method, prf)) == NULL) {
+		log_debug("%s: invalid auth method", __func__);
+		return (-1);
+	}
+
+	switch (auth->auth_method) {
+	case IKEV2_AUTH_SHARED_KEY_MIC:
+		if (!auth->auth_length) {
+			log_debug("%s: no pre-shared key found", __func__);
+			goto done;
+		}
+		if ((keylen = ikev2_psk(sa, auth->auth_data,
+		    auth->auth_length, &psk)) == -1) {
+			log_debug("%s: failed to get PSK", __func__);
+			goto done;
+		}
+		key = psk;
+		keytype = 0;
+		break;
+	default:
+		if (id == NULL) {
+			log_debug("%s: no cert found", __func__);
+			goto done;
+		}
+		key = ibuf_data(id->id_buf);
+		keylen = ibuf_size(id->id_buf);
+		keytype = id->id_type;
+		break;
+	}
+
+	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
+	    dsa_init(dsa) != 0 ||
+	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
+		log_debug("%s: failed to compute digital signature", __func__);
+		goto done;
+	}
+
+	ibuf_release(sa->sa_localauth.id_buf);
+	sa->sa_localauth.id_buf = NULL;
+
+	if ((buf = ibuf_new(NULL, dsa_length(dsa))) == NULL) {
+		log_debug("%s: failed to get auth buffer", __func__);
+		goto done;
+	}
+
+	if ((ret = dsa_sign_final(dsa,
+	    ibuf_data(buf), ibuf_size(buf))) == -1) {
+		log_debug("%s: failed to create auth signature", __func__);
+		ibuf_release(buf);
+		goto done;
+	}
+
+	sa->sa_localauth.id_type = auth->auth_method;
+	sa->sa_localauth.id_buf = buf;
+
+	ret = 0;
+ done:
+	if (psk != NULL)
+		free(psk);
+	dsa_free(dsa);
+
+	return (ret);
+}
diff --git a/sbin/iked/ikev2_pld.c b/sbin/iked/ikev2_pld.c
new file mode 100644
index 00000000000..e75d81a6c91
--- /dev/null
+++ b/sbin/iked/ikev2_pld.c
@@ -0,0 +1,1115 @@
+/*	$OpenBSD: ikev2_pld.c,v 1.1 2010/06/10 08:29:47 reyk Exp $	*/
+/*	$vantronix: ikev2.c,v 1.101 2010/06/03 07:57:33 reyk Exp $	*/
+
+/*
+ * Copyright (c) 2010 Reyk Floeter <reyk@vantronix.net>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <sys/param.h>
+#include <sys/types.h>
+#include <sys/queue.h>
+#include <sys/socket.h>
+#include <sys/wait.h>
+#include <sys/uio.h>
+
+#include <netinet/in.h>
+#include <netinet/ip_ipsp.h>
+#include <arpa/inet.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <getopt.h>
+#include <signal.h>
+#include <errno.h>
+#include <err.h>
+#include <pwd.h>
+#include <event.h>
+
+#include <openssl/sha.h>
+#include <openssl/evp.h>
+
+#include "iked.h"
+#include "ikev2.h"
+#include "eap.h"
+#include "dh.h"
+
+int	 ikev2_pld_payloads(struct iked *, struct iked_message *,
+	    off_t, size_t, u_int, int);
+int	 ikev2_pld_sa(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_xform(struct iked *, struct ikev2_sa_proposal *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_attr(struct iked *, struct ikev2_transform *,
+	    struct iked_message *, off_t, int);
+int	 ikev2_pld_ke(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_id(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t, u_int);
+int	 ikev2_pld_cert(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_certreq(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_nonce(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_notify(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_delete(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_ts(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t, u_int);
+int	 ikev2_pld_auth(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_e(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_cp(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+int	 ikev2_pld_eap(struct iked *, struct ikev2_payload *,
+	    struct iked_message *, off_t);
+
+int
+ikev2_pld_parse(struct iked *env, struct ike_header *hdr,
+    struct iked_message *msg, off_t offset)
+{
+	log_debug("%s: header ispi %s rspi %s"
+	    " nextpayload %s version 0x%02x exchange %s flags 0x%02x"
+	    " msgid %d length %d response %d", __func__,
+	    print_spi(betoh64(hdr->ike_ispi), 8),
+	    print_spi(betoh64(hdr->ike_rspi), 8),
+	    print_map(hdr->ike_nextpayload, ikev2_payload_map),
+	    hdr->ike_version,
+	    print_map(hdr->ike_exchange, ikev2_exchange_map),
+	    hdr->ike_flags,
+	    betoh32(hdr->ike_msgid),
+	    betoh32(hdr->ike_length),
+	    msg->msg_response);
+
+	if (ibuf_size(msg->msg_data) < betoh32(hdr->ike_length)) {
+		log_debug("%s: short message", __func__);
+		return (-1);
+	}
+
+	offset += sizeof(*hdr);
+
+	return (ikev2_pld_payloads(env, msg, offset,
+	    betoh32(hdr->ike_length), hdr->ike_nextpayload, 0));
+}
+
+int
+ikev2_pld_payloads(struct iked *env, struct iked_message *msg,
+    off_t offset, size_t length, u_int payload, int quick)
+{
+	struct ikev2_payload	 pld;
+	u_int			 e;
+	int			 ret;
+	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
+
+	/* Check if message was decrypted in an E payload */
+	e = msg->msg_decrypted ? IKED_E : 0;
+
+	if (quick)
+		print_debug("%s: %spayloads", __func__,
+		    e ? "decrypted " : "");
+	else
+		ikev2_pld_payloads(env, msg, offset, length, payload, 1);
+
+	while (payload != 0 && offset < (off_t)length) {
+		memcpy(&pld, msgbuf + offset, sizeof(pld));
+
+		if (quick)
+			print_debug(" %s",
+			    print_map(payload, ikev2_payload_map));
+		else
+			log_debug("%s: %spayload %s"
+			    " nextpayload %s critical 0x%02x length %d",
+			    __func__, e ? "decrypted " : "",
+			    print_map(payload, ikev2_payload_map),
+			    print_map(pld.pld_nextpayload, ikev2_payload_map),
+			    pld.pld_reserved & IKEV2_CRITICAL_PAYLOAD,
+			    betoh16(pld.pld_length));
+
+		offset += sizeof(pld);
+		ret = 0;
+
+		if (quick)
+			goto next;
+
+		switch (payload | e) {
+		case IKEV2_PAYLOAD_SA:
+		case IKEV2_PAYLOAD_SA | IKED_E:
+			ret = ikev2_pld_sa(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_KE:
+			ret = ikev2_pld_ke(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_IDi | IKED_E:
+		case IKEV2_PAYLOAD_IDr | IKED_E:
+			ret = ikev2_pld_id(env, &pld, msg, offset, payload);
+			break;
+		case IKEV2_PAYLOAD_CERT | IKED_E:
+			ret = ikev2_pld_cert(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_CERTREQ:
+		case IKEV2_PAYLOAD_CERTREQ | IKED_E:
+			ret = ikev2_pld_certreq(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_AUTH | IKED_E:
+			ret = ikev2_pld_auth(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_NONCE:
+		case IKEV2_PAYLOAD_NONCE | IKED_E:
+			ret = ikev2_pld_nonce(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_NOTIFY:
+		case IKEV2_PAYLOAD_NOTIFY | IKED_E:
+			ret = ikev2_pld_notify(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_DELETE | IKED_E:
+			ret = ikev2_pld_delete(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_TSi | IKED_E:
+		case IKEV2_PAYLOAD_TSr | IKED_E:
+			ret = ikev2_pld_ts(env, &pld, msg, offset, payload);
+			break;
+		case IKEV2_PAYLOAD_E:
+			ret = ikev2_pld_e(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_CP | IKED_E:
+			ret = ikev2_pld_cp(env, &pld, msg, offset);
+			break;
+		case IKEV2_PAYLOAD_EAP | IKED_E:
+			ret = ikev2_pld_eap(env, &pld, msg, offset);
+			break;
+		default:
+			print_hex(msgbuf, offset,
+			    betoh16(pld.pld_length) - sizeof(pld));
+			break;
+		}
+
+		if (ret != 0 && !msg->msg_response) {
+			(void)ikev2_send_informational(env, msg);
+			return (-1);
+		}
+
+		/* Encrypted payload must appear last */
+		if (payload == IKEV2_PAYLOAD_E)
+			return (0);
+
+ next:
+		payload = pld.pld_nextpayload;
+		offset += betoh16(pld.pld_length) - sizeof(pld);
+	}
+
+	if (quick)
+		print_debug("\n");
+
+	return (0);
+}
+
+int
+ikev2_pld_sa(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_sa_proposal	 sap;
+	struct iked_proposal		*prop = NULL;
+	u_int32_t			 spi32;
+	u_int64_t			 spi = 0, spi64;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+	struct iked_sa			*sa = msg->msg_sa;
+
+	memcpy(&sap, msgbuf + offset, sizeof(sap));
+	offset += sizeof(sap);
+
+	if (sap.sap_spisize) {
+		switch (sap.sap_spisize) {
+		case 4:
+			memcpy(&spi32, msgbuf + offset, 4);
+			spi = betoh32(spi32);
+			break;
+		case 8:
+			memcpy(&spi64, msgbuf + offset, 8);
+			spi = betoh64(spi64);
+			break;
+		default:
+			log_debug("%s: unsupported SPI size %d",
+			    __func__, sap.sap_spisize);
+			return (-1);
+		}
+
+		offset += sap.sap_spisize;
+	}
+
+	log_debug("%s: more %d reserved %d length %d"
+	    " proposal #%d protoid %s spisize %d xforms %d spi %s",
+	    __func__, sap.sap_more, sap.sap_reserved,
+	    betoh16(sap.sap_length), sap.sap_proposalnr,
+	    print_map(sap.sap_protoid, ikev2_saproto_map), sap.sap_spisize,
+	    sap.sap_transforms, print_spi(spi, sap.sap_spisize));
+
+	if (!msg->msg_response) {
+		if ((msg->msg_prop = config_add_proposal(&msg->msg_proposals,
+		    sap.sap_proposalnr, sap.sap_protoid)) == NULL) {
+			log_debug("%s: invalid proposal", __func__);
+			return (-1);
+		}
+		prop = msg->msg_prop;
+		prop->prop_localspi.spi_size = sap.sap_spisize;
+		prop->prop_peerspi.spi = spi;
+	}
+
+	/*
+	 * Parse the attached transforms
+	 */
+	if (ikev2_pld_xform(env, &sap, msg, offset) != 0) {
+		log_debug("%s: invalid proposal transforms", __func__);
+		return (-1);
+	}
+
+	if (msg->msg_response)
+		return (0);
+
+	/* XXX we need a better way to get this */
+	if (ikev2_sa_negotiate(sa,
+	    &msg->msg_policy->pol_proposals,
+	    &msg->msg_proposals, msg->msg_decrypted ?
+	    IKEV2_SAPROTO_ESP : IKEV2_SAPROTO_IKE) != 0) {
+		log_debug("%s: no proposal chosen", __func__);
+		msg->msg_error = IKEV2_N_NO_PROPOSAL_CHOSEN;
+		return (-1);
+	} else if (sa_stateok(sa, IKEV2_STATE_SA_INIT))
+		sa_stateflags(sa, IKED_REQ_SA);
+
+	return (0);
+}
+
+int
+ikev2_pld_xform(struct iked *env, struct ikev2_sa_proposal *sap,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_transform		 xfrm;
+	char				 id[BUFSIZ];
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&xfrm, msgbuf + offset, sizeof(xfrm));
+
+	switch (xfrm.xfrm_type) {
+	case IKEV2_XFORMTYPE_ENCR:
+		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
+		    ikev2_xformencr_map), sizeof(id));
+		break;
+	case IKEV2_XFORMTYPE_PRF:
+		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
+		    ikev2_xformprf_map), sizeof(id));
+		break;
+	case IKEV2_XFORMTYPE_INTEGR:
+		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
+		    ikev2_xformauth_map), sizeof(id));
+		break;
+	case IKEV2_XFORMTYPE_DH:
+		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
+		    ikev2_xformdh_map), sizeof(id));
+		break;
+	case IKEV2_XFORMTYPE_ESN:
+		strlcpy(id, print_map(betoh16(xfrm.xfrm_id),
+		    ikev2_xformesn_map), sizeof(id));
+		break;
+	default:
+		snprintf(id, sizeof(id), "<%d>", betoh16(xfrm.xfrm_id));
+		break;
+	}
+
+	log_debug("%s: more %d reserved %d length %d"
+	    " type %s id %s",
+	    __func__, xfrm.xfrm_more, xfrm.xfrm_reserved,
+	    betoh16(xfrm.xfrm_length),
+	    print_map(xfrm.xfrm_type, ikev2_xformtype_map), id);
+
+	/*
+	 * Parse transform attributes, if available
+	 */
+	msg->msg_attrlength = 0;
+	if ((u_int)betoh16(xfrm.xfrm_length) > sizeof(xfrm))
+		ikev2_pld_attr(env, &xfrm, msg, offset + sizeof(xfrm),
+		    betoh16(xfrm.xfrm_length) - sizeof(xfrm));
+
+	if (!msg->msg_response) {
+		if (config_add_transform(msg->msg_prop, xfrm.xfrm_type,
+		    betoh16(xfrm.xfrm_id), msg->msg_attrlength,
+		    msg->msg_attrlength) == NULL) {
+			log_debug("%s: failed to add transform", __func__);
+			return (-1);
+		}
+	}
+
+	/* Next transform */
+	offset += betoh16(xfrm.xfrm_length);
+	if (xfrm.xfrm_more == IKEV2_XFORM_MORE)
+		ikev2_pld_xform(env, sap, msg, offset);
+
+	return (0);
+}
+
+int
+ikev2_pld_attr(struct iked *env, struct ikev2_transform *xfrm,
+    struct iked_message *msg, off_t offset, int total)
+{
+	struct ikev2_attribute		 attr;
+	u_int				 type;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&attr, msgbuf + offset, sizeof(attr));
+
+	type = betoh16(attr.attr_type) & ~IKEV2_ATTRAF_TV;
+
+	log_debug("%s: attribute type %s length %d total %d",
+	    __func__, print_map(type, ikev2_attrtype_map),
+	    betoh16(attr.attr_length), total);
+
+	if (betoh16(attr.attr_type) & IKEV2_ATTRAF_TV) {
+		/* Type-Value attribute */
+		offset += sizeof(attr);
+		total -= sizeof(attr);
+
+		if (type == IKEV2_ATTRTYPE_KEY_LENGTH)
+			msg->msg_attrlength = betoh16(attr.attr_length);
+	} else {
+		/* Type-Length-Value attribute */
+		print_hex(msgbuf, offset + sizeof(attr),
+		    betoh16(attr.attr_length) - sizeof(attr));
+		offset += betoh16(attr.attr_length);
+		total -= betoh16(attr.attr_length);
+	}
+
+	if (total > 0) {
+		/* Next attribute */
+		ikev2_pld_attr(env, xfrm, msg, offset, total);
+	}
+
+	return (0);
+}
+
+int
+ikev2_pld_ke(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_keyexchange	 kex;
+	u_int8_t			*buf;
+	size_t				 len;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&kex, msgbuf + offset, sizeof(kex));
+
+	log_debug("%s: dh group %s reserved %d",
+	    __func__,
+	    print_map(betoh16(kex.kex_dhgroup), ikev2_xformdh_map),
+	    betoh16(kex.kex_reserved));
+
+	buf = msgbuf + offset + sizeof(kex);
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(kex);
+
+	print_hex(buf, 0, len);
+
+	if (!msg->msg_response) {
+		if ((msg->msg_sa->sa_dhiexchange =
+		    ibuf_new(buf, len)) == NULL) {
+			log_debug("%s: failed to get exchange", __func__);
+			return (-1);
+		}
+	}
+
+	return (0);
+}
+
+int
+ikev2_pld_id(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset, u_int payload)
+{
+	u_int8_t			*ptr;
+	struct ikev2_id			 id;
+	size_t				 len;
+	struct iked_id			*idp, idb;
+	struct iked_sa			*sa = msg->msg_sa;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+	struct ibuf			*authmsg;
+	char				 idstr[IKED_ID_SIZE];
+
+	memcpy(&id, msgbuf + offset, sizeof(id));
+	bzero(&idb, sizeof(idb));
+
+	/* Don't strip the Id payload header */
+	ptr = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld);
+
+	idb.id_type = id.id_type;
+	if ((idb.id_buf = ibuf_new(ptr, len)) == NULL)
+		return (-1);
+
+	if (print_id(&idb, sizeof(id), idstr, sizeof(idstr)) == -1) {
+		log_debug("%s: malformed id", __func__);
+		return (-1);
+	}
+
+	log_debug("%s: id %s/%s length %d",
+	    __func__, print_map(id.id_type, ikev2_id_map), idstr, len);
+
+	if (msg->msg_response) {
+		ibuf_release(idb.id_buf);
+		return (0);
+	}
+
+	if (sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDr) {
+		idp = &sa->sa_rid;
+	} else if (!sa->sa_hdr.sh_initiator && payload == IKEV2_PAYLOAD_IDi) {
+		idp = &sa->sa_iid;
+	} else {
+		log_debug("%s: unexpected id payload", __func__);
+		return (0);
+	}
+
+	ibuf_release(idp->id_buf);
+	idp->id_buf = idb.id_buf;
+	idp->id_type = idb.id_type;
+
+	if ((authmsg = ikev2_msg_auth(env, sa,
+	    !sa->sa_hdr.sh_initiator)) == NULL) {
+		log_debug("%s: failed to get response auth data", __func__);
+		return (-1);
+	}
+
+	ca_setauth(env, sa, authmsg, PROC_CERT);
+
+	return (0);
+}
+
+int
+ikev2_pld_cert(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_cert		 cert;
+	u_int8_t			*buf;
+	size_t				 len;
+	struct iked_sa			*sa = msg->msg_sa;
+	struct iked_id			*certid, *id;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&cert, msgbuf + offset, sizeof(cert));
+	offset += sizeof(cert);
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
+
+	log_debug("%s: type %s length %d",
+	    __func__, print_map(cert.cert_type, ikev2_cert_map), len);
+
+	print_hex(buf, 0, len);
+
+	if (msg->msg_response)
+		return (0);
+
+	if (!sa->sa_hdr.sh_initiator && !msg->msg_response) {
+		certid = &sa->sa_icert;
+		id = &sa->sa_iid;
+	} else if (sa->sa_hdr.sh_initiator && msg->msg_response) {
+		certid = &sa->sa_rcert;
+		id = &sa->sa_rid;
+	} else
+		return (0);	/* ignore */
+
+	if ((certid->id_buf = ibuf_new(buf, len)) == NULL) {
+		log_debug("%s: failed to save cert", __func__);
+		return (-1);
+	}
+	certid->id_type = cert.cert_type;
+
+	ca_setcert(env, &msg->msg_sa->sa_hdr, id, cert.cert_type,
+	    buf, len, PROC_CERT);
+
+	return (0);
+}
+
+int
+ikev2_pld_certreq(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_cert		 cert;
+	u_int8_t			*buf;
+	size_t				 len;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&cert, msgbuf + offset, sizeof(cert));
+	offset += sizeof(cert);
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cert);
+
+	log_debug("%s: type %s signatures length %d",
+	    __func__, print_map(cert.cert_type, ikev2_cert_map), len);
+	print_hex(buf, 0, len);
+
+	if (msg->msg_response)
+		return (0);
+
+	if (!len || (len % SHA_DIGEST_LENGTH) != 0) {
+		log_debug("%s: invalid certificate request", __func__);
+		return (-1);
+	}
+
+	if (msg->msg_sa == NULL)
+		return (-1);
+
+	/* Optional certreq for PSK */
+	msg->msg_sa->sa_staterequire |= IKED_REQ_CERT;
+
+	ca_setreq(env, &msg->msg_sa->sa_hdr, cert.cert_type,
+	    buf, len, PROC_CERT);
+
+	return (0);
+}
+
+int
+ikev2_pld_auth(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_auth		 auth;
+	struct iked_auth		 ikeauth;
+	u_int8_t			*buf;
+	size_t				 len;
+	struct ibuf			*authmsg;
+	struct iked_sa			*sa = msg->msg_sa;
+	struct iked_policy		*policy = sa->sa_policy;
+	int				 ret = -1;
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&auth, msgbuf + offset, sizeof(auth));
+	offset += sizeof(auth);
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(auth);
+
+	log_debug("%s: method %s length %d",
+	    __func__, print_map(auth.auth_method, ikev2_auth_map), len);
+
+	print_hex(buf, 0, len);
+
+	if (msg->msg_response)
+		return (0);
+
+	memcpy(&ikeauth, &policy->pol_auth, sizeof(ikeauth));
+
+	if (policy->pol_auth.auth_eap && sa->sa_eapmsk != NULL) {
+		/* The initiator EAP auth is a PSK derived from the MSK */
+		ikeauth.auth_method = IKEV2_AUTH_SHARED_KEY_MIC;
+
+		/* Copy session key as PSK */
+		memcpy(ikeauth.auth_data, ibuf_data(sa->sa_eapmsk),
+		    ibuf_size(sa->sa_eapmsk));
+		ikeauth.auth_length = ibuf_size(sa->sa_eapmsk);
+	}
+	if (auth.auth_method != ikeauth.auth_method) {
+		log_debug("%s: method %s required", __func__,
+		    print_map(ikeauth.auth_method, ikev2_auth_map));
+		return (-1);
+	}
+
+	/* The AUTH payload indicates if the responder wants EAP or not */
+	if (!sa_stateok(sa, IKEV2_STATE_EAP))
+		sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
+
+	if ((authmsg = ikev2_msg_auth(env, sa,
+	    sa->sa_hdr.sh_initiator)) == NULL) {
+		log_debug("%s: failed to get auth data", __func__);
+		return (-1);
+	}
+
+	ret = ikev2_msg_authverify(env, sa, &ikeauth, buf, len,
+	    authmsg);
+
+	ibuf_release(authmsg);
+	authmsg = NULL;
+
+	if (ret != 0)
+		goto done;
+
+	if (sa->sa_eapmsk != NULL) {
+		if ((authmsg = ikev2_msg_auth(env, sa,
+		    !sa->sa_hdr.sh_initiator)) == NULL) {
+			log_debug("%s: failed to get auth data", __func__);
+			return (-1);
+		}
+
+		/* 2nd AUTH for EAP messages */
+		if ((ret = ikev2_msg_authsign(env, sa,
+		    &ikeauth, authmsg)) != 0)
+			goto done;
+
+		sa_state(env, sa, IKEV2_STATE_EAP_VALID);
+	}
+
+ done:
+	ibuf_release(authmsg);
+
+	return (ret);
+}
+
+int
+ikev2_pld_nonce(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	size_t		 len;
+	u_int8_t	*buf;
+	u_int8_t	*msgbuf = ibuf_data(msg->msg_data);
+	struct iked_sa	*sa = msg->msg_sa;
+	struct ibuf	*localnonce, *peernonce;
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld);
+	print_hex(buf, 0, len);
+
+	if (!msg->msg_response) {
+		if ((peernonce = ibuf_new(buf, len)) == NULL) {
+			log_debug("%s: failed to get peer nonce", __func__);
+			return (-1);
+		}
+		if ((localnonce =
+		    ibuf_random(IKED_NONCE_SIZE)) == NULL) {
+			log_debug("%s: failed to get local nonce", __func__);
+			ibuf_release(peernonce);
+			return (-1);
+		}
+
+		ibuf_release(sa->sa_inonce);
+		ibuf_release(sa->sa_rnonce);
+
+		log_debug("%s: updating nonces", __func__);
+
+		if (sa->sa_hdr.sh_initiator) {
+			sa->sa_inonce = localnonce;
+			sa->sa_rnonce = peernonce;
+		} else {
+			sa->sa_inonce = peernonce;
+			sa->sa_rnonce = localnonce;
+		}
+	}
+
+	return (0);
+}
+
+int
+ikev2_pld_notify(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_notify	*n;
+	u_int8_t		*buf, md[SHA_DIGEST_LENGTH];
+	size_t			 len;
+	u_int16_t		 type;
+	u_int32_t		 spi32;
+	u_int64_t		 spi64;
+	struct iked_spi		*rekey;
+	
+	if ((n = ibuf_seek(msg->msg_data, offset, sizeof(*n))) == NULL)
+		return (-1);
+	type = betoh16(n->n_type);
+
+	log_debug("%s: protoid %s spisize %d type %s",
+	    __func__,
+	    print_map(n->n_protoid, ikev2_saproto_map), n->n_spisize,
+	    print_map(type, ikev2_n_map));
+
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(*n);
+	if ((buf = ibuf_seek(msg->msg_data, offset + sizeof(*n), len)) == NULL)
+		return (-1);
+
+	print_hex(buf, 0, len);
+
+	switch (type) {
+	case IKEV2_N_NAT_DETECTION_SOURCE_IP:
+	case IKEV2_N_NAT_DETECTION_DESTINATION_IP:
+		if (ikev2_nat_detection(msg, md, sizeof(md), type,
+		    msg->msg_response) == -1)
+			return (-1);
+		if (len != sizeof(md) || memcmp(buf, md, len) != 0) {
+			log_debug("%s: %s detected NAT, enabling "
+			    "UDP encapsulation", __func__,
+			    print_map(type, ikev2_n_map));
+
+			/*
+			 * Enable UDP encapsulation of ESP packages if
+			 * the check detected NAT.
+			 */
+			if (msg->msg_sa != NULL)
+				msg->msg_sa->sa_udpencap = 1;
+		}
+		print_hex(md, 0, sizeof(md));
+		break;
+	case IKEV2_N_REKEY_SA:
+		if (len != n->n_spisize) {
+			log_debug("%s: malformed notification", __func__);
+			return (-1);
+		}
+		if (msg->msg_decrypted)
+			rekey = &msg->msg_decrypted->msg_rekey;
+		else
+			rekey = &msg->msg_rekey;
+		if (rekey->spi != 0) {
+			log_debug("%s: rekeying of multiple SAs not supported",
+			    __func__);
+			return (-1);
+		}
+		switch (n->n_spisize) {
+		case 4:
+			memcpy(&spi32, buf, len);
+			rekey->spi = betoh32(spi32);
+			break;
+		case 8:
+			memcpy(&spi64, buf, len);
+			rekey->spi = betoh64(spi64);
+			break;
+		default:
+			log_debug("%s: invalid spi size %d", __func__,
+			    n->n_spisize);
+			return (-1);
+		}
+		rekey->spi_size = n->n_spisize;
+		rekey->spi_protoid = n->n_protoid;
+
+		log_debug("%s: rekey %s spi %s", __func__,
+		    print_map(n->n_protoid, ikev2_saproto_map),
+		    print_spi(rekey->spi, n->n_spisize));
+		break;
+	}
+
+	return (0);
+}
+
+int
+ikev2_pld_delete(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_delete	*del, *localdel;
+	u_int64_t		 spi64, spi = 0, *localspi = NULL;
+	u_int32_t		 spi32;
+	size_t			 len, i, cnt, sz, found = 0, failed = 0;
+	u_int8_t		*buf, firstpayload = 0;
+	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
+	struct iked_sa		*sa = msg->msg_sa;
+	struct ibuf		*resp = NULL;
+	int			 ret = -1;
+
+	if ((del = ibuf_seek(msg->msg_data, offset, sizeof(*del))) == NULL)
+		return (-1);
+	cnt = betoh16(del->del_nspi);
+	sz = del->del_spisize;
+
+	log_debug("%s: protoid %s spisize %d nspi %d",
+	    __func__, print_map(del->del_protoid, ikev2_saproto_map),
+	    sz, cnt);
+
+	buf = msgbuf + offset + sizeof(*del);
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(*del);
+
+	print_hex(buf, 0, len);
+
+	switch (sz) {
+	case 4:
+	case 8:
+		break;
+	default:
+		if (!msg->msg_response &&
+		    del->del_protoid == IKEV2_SAPROTO_IKE) {
+			sa_state(env, sa, IKEV2_STATE_DELETE);
+			return (0);
+		}
+		log_debug("%s: invalid SPI size", __func__);
+		return (-1);
+	}
+
+	if ((len / sz) != cnt) {
+		log_debug("%s: invalid payload length %d/%d != %d",
+		    __func__, len, sz, cnt);
+		return (-1);
+	}
+
+	if (!msg->msg_response &&
+	    (localspi = calloc(cnt, sizeof(u_int64_t))) == NULL) {
+		log_warn("%s", __func__);
+		return (-1);
+	}
+
+	for (i = 0; i < cnt; i++) {
+		/* XXX delete SAs */
+		switch (sz) {
+		case 4:
+			memcpy(&spi32, buf + (i * sz), sizeof(spi32));
+			spi = betoh32(spi32);
+			break;
+		case 8:
+			memcpy(&spi64, buf + (i * sz), sizeof(spi64));
+			spi = betoh64(spi64);
+			break;
+		}
+		if (msg->msg_response) {
+			log_debug("%s: spi %s", __func__, print_spi(spi, sz));
+			continue;
+		}
+
+		if (ikev2_childsa_delete(env, sa,
+		    del->del_protoid, spi, &localspi[i], 0) == -1)
+			failed++;
+		else
+			found++;
+	}
+
+	if (msg->msg_response)
+		return (0);
+
+	if ((resp = ibuf_static()) == NULL)
+		goto done;
+
+	if (found) {
+		if ((localdel = ibuf_advance(resp, sizeof(*localdel))) == NULL)
+			goto done;
+
+		firstpayload = IKEV2_PAYLOAD_DELETE;
+		localdel->del_protoid = del->del_protoid;
+		localdel->del_spisize = del->del_spisize;
+		localdel->del_nspi = htobe16(found);
+
+		for (i = 0; i < cnt; i++) {
+			if (!localspi[i])
+				continue;
+
+			switch (sz) {
+			case 4:
+				spi32 = htobe32(localspi[i]);
+				if (ibuf_add(resp, &spi32, sizeof(spi32)) != 0)
+					goto done;
+				break;
+			case 8:
+				spi64 = htobe64(localspi[i]);
+				if (ibuf_add(resp, &spi64, sizeof(spi64)) != 0)
+					goto done;
+				break;
+			}
+		}
+
+	}
+
+	if (found) {
+		ret = ikev2_send_ike_e(env, sa, resp,
+		    firstpayload, IKEV2_EXCHANGE_INFORMATIONAL, 1);
+	} else {
+		/* XXX should we send an INVALID_SPI notification? */
+		ret = 0;
+	}
+
+ done:
+	if (localspi != NULL)
+		free(localspi);
+	ibuf_release(resp);
+	return (ret);
+}
+
+int
+ikev2_pld_ts(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset, u_int payload)
+{
+	u_int8_t			*ptr;
+	struct ikev2_tsp		 tsp;
+	struct ikev2_ts			 ts;
+	size_t				 len, i;
+	struct sockaddr_in		 s4;
+	struct sockaddr_in6		 s6;
+	u_int8_t			 buf[2][128];
+	u_int8_t			*msgbuf = ibuf_data(msg->msg_data);
+
+	memcpy(&tsp, msgbuf + offset, sizeof(tsp));
+	offset += sizeof(tsp);
+
+	ptr = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(tsp);
+
+	log_debug("%s: count %d length %d", __func__,
+	    tsp.tsp_count, len);
+
+	for (i = 0; i < tsp.tsp_count; i++) {
+		memcpy(&ts, msgbuf + offset, sizeof(ts));
+
+		log_debug("%s: type %s protoid %u length %d "
+		    "startport %u endport %u", __func__,
+		    print_map(ts.ts_type, ikev2_ts_map),
+		    ts.ts_protoid, betoh16(ts.ts_length),
+		    betoh16(ts.ts_startport),
+		    betoh16(ts.ts_endport));
+
+		switch (ts.ts_type) {
+		case IKEV2_TS_IPV4_ADDR_RANGE:
+			bzero(&s4, sizeof(s4));
+			s4.sin_family = AF_INET;
+			s4.sin_len = sizeof(s4);
+			memcpy(&s4.sin_addr.s_addr,
+			    msgbuf + offset + sizeof(ts), 4);
+			print_host((struct sockaddr_storage *)&s4,
+			    (char *)buf[0], sizeof(buf[0]));
+			memcpy(&s4.sin_addr.s_addr,
+			    msgbuf + offset + sizeof(ts) + 4, 4);
+			print_host((struct sockaddr_storage *)&s4,
+			    (char *)buf[1], sizeof(buf[1]));
+			log_debug("%s: start %s end %s", __func__,
+			    buf[0], buf[1]);
+			break;
+		case IKEV2_TS_IPV6_ADDR_RANGE:
+			bzero(&s6, sizeof(s6));
+			s6.sin6_family = AF_INET6;
+			s6.sin6_len = sizeof(s6);
+			memcpy(&s6.sin6_addr,
+			    msgbuf + offset + sizeof(ts), 16);
+			print_host((struct sockaddr_storage *)&s6,
+			    (char *)buf[0], sizeof(buf[0]));
+			memcpy(&s6.sin6_addr,
+			    msgbuf + offset + sizeof(ts) + 16, 16);
+			print_host((struct sockaddr_storage *)&s6,
+			    (char *)buf[1], sizeof(buf[1]));
+			log_debug("%s: start %s end %s", __func__,
+			    buf[0], buf[1]);
+			break;
+		default:
+			break;
+		}
+
+		offset += betoh16(ts.ts_length);
+	}
+
+	return (0);
+}
+
+int
+ikev2_pld_e(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ibuf		*e = NULL;
+	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
+	struct iked_message	 emsg;
+	u_int8_t		*buf;
+	size_t			 len;
+	int			 ret = -1;
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld);
+
+	if ((e = ibuf_new(buf, len)) == NULL)
+		goto done;
+
+	if ((e = ikev2_msg_decrypt(env, msg->msg_sa,
+	    msg->msg_data, e)) == NULL)
+		goto done;
+
+	/*
+	 * Parse decrypted payload
+	 */
+	bzero(&emsg, sizeof(emsg));
+	memcpy(&emsg, msg, sizeof(*msg));
+	emsg.msg_data = e;
+	emsg.msg_decrypted = msg;
+	TAILQ_INIT(&emsg.msg_proposals);
+
+	ret = ikev2_pld_payloads(env, &emsg, 0, ibuf_size(e),
+	    pld->pld_nextpayload, 0);
+
+ done:
+	ibuf_release(e);
+
+	return (ret);
+}
+
+int
+ikev2_pld_cp(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct ikev2_cp		 cp;
+	struct ikev2_cfg	*cfg;
+	u_int8_t		*buf;
+	size_t			 len, i;
+	u_int8_t		*msgbuf = ibuf_data(msg->msg_data);
+	struct iked_sa		*sa = msg->msg_sa;
+
+	memcpy(&cp, msgbuf + offset, sizeof(cp));
+	offset += sizeof(cp);
+
+	buf = msgbuf + offset;
+	len = betoh16(pld->pld_length) - sizeof(*pld) - sizeof(cp);
+
+	log_debug("%s: type %s",
+	    __func__, print_map(cp.cp_type, ikev2_cp_map), len);
+	print_hex(buf, 0, len);
+
+	for (i = 0; i < len;) {
+		cfg = (struct ikev2_cfg *)(buf + i);
+
+		log_debug("%s: %s 0x%04x length %d", __func__,
+		    print_map(betoh16(cfg->cfg_type), ikev2_cfg_map),
+		    betoh16(cfg->cfg_type),
+		    betoh16(cfg->cfg_length));
+
+		i += betoh16(cfg->cfg_length) + sizeof(*cfg);
+	}
+
+	if (msg->msg_response)
+		return (0);
+
+	if (sa)
+		sa->sa_cp = cp.cp_type;
+
+	return (0);
+}
+
+int
+ikev2_pld_eap(struct iked *env, struct ikev2_payload *pld,
+    struct iked_message *msg, off_t offset)
+{
+	struct eap_header		*hdr;
+	struct eap_message		*eap = NULL;
+	struct iked_sa			*sa = msg->msg_sa;
+	size_t				 len;
+
+	if ((hdr = ibuf_seek(msg->msg_data, offset, sizeof(*hdr))) == NULL) {
+		log_debug("%s: failed to get EAP header", __func__);
+		return (-1);
+	}
+
+	len = betoh16(hdr->eap_length);
+
+	if (len < sizeof(*eap)) {
+		log_info("%s: %s id %d length %d", __func__,
+		    print_map(hdr->eap_code, eap_code_map),
+		    hdr->eap_id, betoh16(hdr->eap_length));
+	} else {
+		/* Now try to get the indicated length */
+		if ((eap = ibuf_seek(msg->msg_data, offset, len)) == NULL) {
+			log_debug("%s: invalid EAP length", __func__);
+			return (-1);
+		}
+
+		log_info("%s: %s id %d length %d EAP-%s", __func__,
+		    print_map(eap->eap_code, eap_code_map),
+		    eap->eap_id, betoh16(eap->eap_length),
+		    print_map(eap->eap_type, eap_type_map));
+	}
+
+	if (eap_parse(env, sa, hdr, msg->msg_response) == -1)
+		return (-1);
+
+	return (0);
+}