Import iked, a new implementation of the IKEv2 protocol.

iked(8) is an automatic keying daemon for IPsec, like isakmpd(8), that
IPsec creates flows and SAs automatically.  Unlike isakmpd, iked(8)
implements the newer IKEv2 protocol instead of IKEv1/ISAKMP.  The
daemon is still work-in-progress and not enabled in the builds, but is
already able to establish IKEv2 sessions with some other IKEv2
implementations as a responder.

with lots of help and debugging by jsg@
ok deraadt@

1 files changed, 4419 insertions, 0 deletions

diff --git a/sbin/iked/ikev2.c b/sbin/iked/ikev2.c
new file mode 100644
index 00000000000..b5872edaad8
--- /dev/null
+++ b/sbin/iked/ikev2.c
@@ -0,0 +1,4419 @@
+/*	$OpenBSD: ikev2.c,v 1.1 2010/06/03 16:41:12 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_dispatch_parent(int, struct iked_proc *, struct imsg *);
+int	 ikev2_dispatch_ikev1(int, struct iked_proc *, struct imsg *);
+int	 ikev2_dispatch_cert(int, struct iked_proc *, struct imsg *);
+
+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 *);
+
+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);
+ssize_t	 ikev2_add_ts(struct ibuf *, u_int, struct iked_sa *);
+int	 ikev2_add_data(struct ibuf *, void *, size_t);
+int	 ikev2_add_buf(struct ibuf *buf, struct ibuf *);
+
+static struct iked_proc procs[] = {
+	{ "parent",	PROC_PARENT,	ikev2_dispatch_parent },
+	{ "ikev1",	PROC_IKEV1,	ikev2_dispatch_ikev1 },
+	{ "certstore",	PROC_CERT,	ikev2_dispatch_cert }
+};
+
+pid_t
+ikev2(struct iked *env, struct iked_proc *p)
+{
+	return (run_proc(env, p, procs, nitems(procs), NULL, NULL));
+}
+
+int
+ikev2_dispatch_parent(int fd, struct iked_proc *p, struct imsg *imsg)
+{
+	struct iked		*env = p->env;
+
+	switch (imsg->hdr.type) {
+	case IMSG_CTL_RESET:
+		return (config_getreset(env, imsg));
+	case IMSG_UDP_SOCKET:
+		return (config_getsocket(env, imsg, ikev2_message_cb));
+	case IMSG_PFKEY_SOCKET:
+		return (config_getpfkey(env, imsg));
+	case IMSG_CFG_POLICY:
+		return (config_getpolicy(env, imsg));
+	case IMSG_CFG_USER:
+		return (config_getuser(env, imsg));
+	default:
+		break;
+	}
+
+	return (-1);
+}
+
+int
+ikev2_dispatch_ikev1(int fd, struct iked_proc *p, struct imsg *imsg)
+{
+	struct iked		*env = p->env;
+	struct iked_message	 msg;
+	u_int8_t		*buf;
+	ssize_t			 len;
+
+	switch (imsg->hdr.type) {
+	case IMSG_IKE_MESSAGE:
+		log_debug("%s: message", __func__);
+		IMSG_SIZE_CHECK(imsg, &msg);
+		memcpy(&msg, imsg->data, sizeof(msg));
+
+		len = IMSG_DATA_SIZE(imsg) - sizeof(msg);
+		buf = (u_int8_t *)imsg->data + sizeof(msg);
+		if (len <= 0 || (msg.msg_data = ibuf_new(buf, len)) == NULL) {
+			log_debug("%s: short message", __func__);
+			return (0);
+		}
+
+		log_debug("%s: message length %d", __func__, len);
+
+		ikev2_message_recv(env, &msg);
+		message_cleanup(env, &msg);
+		return (0);
+	default:
+		break;
+	}
+
+	return (-1);
+}
+
+int
+ikev2_dispatch_cert(int fd, struct iked_proc *p, struct imsg *imsg)
+{
+	struct iked		*env = p->env;
+	struct iked_sahdr	 sh;
+	struct iked_sa		*sa;
+	u_int8_t		 type;
+	u_int8_t		*ptr;
+	size_t			 len;
+	struct iked_id		*id = NULL;
+
+	switch (imsg->hdr.type) {
+	case IMSG_CERTREQ:
+		IMSG_SIZE_CHECK(imsg, &type);
+
+		ptr = imsg->data;
+		memcpy(&type, ptr, sizeof(type));
+		ptr += sizeof(type);
+
+		ibuf_release(env->sc_certreq);
+		env->sc_certreqtype = type;
+		env->sc_certreq = ibuf_new(ptr,
+		    IMSG_DATA_SIZE(imsg) - sizeof(type));
+
+		log_debug("%s: updated local CERTREQ signatures length %d",
+		    __func__, ibuf_length(env->sc_certreq));
+
+		break;
+	case IMSG_CERTVALID:
+	case IMSG_CERTINVALID:
+		memcpy(&sh, imsg->data, sizeof(sh));
+		memcpy(&type, (u_int8_t *)imsg->data + sizeof(sh),
+		    sizeof(type));
+
+		/* Ignore invalid or unauthenticated SAs */
+		if ((sa = sa_lookup(env,
+		    sh.sh_ispi, sh.sh_rspi, sh.sh_initiator)) == NULL ||
+		    sa->sa_state < IKEV2_STATE_EAP)
+			break;
+
+		if (imsg->hdr.type == IMSG_CERTVALID) {
+			log_debug("%s: peer certificate is valid", __func__);
+			sa_state(env, sa, IKEV2_STATE_VALID);
+		} else
+			log_debug("%s: peer certificate is invalid", __func__);
+
+		sa_stateflags(sa, IKED_REQ_VALID);
+
+		if (ikev2_resp_ike_auth(env, sa) != 0)
+			log_debug("%s: failed to send auth response", __func__);
+		break;
+	case IMSG_CERT:
+		if ((sa = ikev2_getimsgdata(env, imsg,
+		    &sh, &type, &ptr, &len)) == NULL) {
+			log_debug("%s: invalid cert reply");
+			break;
+		}
+
+		if (sh.sh_initiator)
+			id = &sa->sa_icert;
+		else
+			id = &sa->sa_rcert;
+
+		id->id_type = type;
+		ibuf_release(id->id_buf);
+		id->id_buf = NULL;
+
+		if (type != IKEV2_CERT_NONE) {
+			if (len <= 0 ||
+			    (id->id_buf = ibuf_new(ptr, len)) == NULL) {
+				log_debug("%s: failed to get cert payload",
+				    __func__);
+				break;
+			}
+		}
+
+		log_debug("%s: cert type %d length %d", __func__,
+		    id->id_type, ibuf_length(id->id_buf));
+
+		sa_stateflags(sa, IKED_REQ_CERT);
+
+		if (ikev2_resp_ike_auth(env, sa) != 0)
+			log_debug("%s: failed to send auth response", __func__);
+		break;
+	case IMSG_AUTH:
+		if ((sa = ikev2_getimsgdata(env, imsg,
+		    &sh, &type, &ptr, &len)) == NULL) {
+			log_debug("%s: invalid auth reply", __func__);
+			break;
+		}
+
+		log_debug("%s: AUTH type %d len %d", __func__, type, len);
+
+		id = &sa->sa_localauth;
+		id->id_type = type;
+		ibuf_release(id->id_buf);
+
+		if (type != IKEV2_AUTH_NONE) {
+			if (len <= 0 ||
+			    (id->id_buf = ibuf_new(ptr, len)) == NULL) {
+				log_debug("%s: failed to get auth payload",
+				    __func__);
+				break;
+			}
+		}
+
+		sa_stateflags(sa, IKED_REQ_AUTH);
+
+		if (ikev2_resp_ike_auth(env, sa) != 0)
+			log_debug("%s: failed to send auth response", __func__);
+		break;
+	default:
+		return (-1);
+	}
+
+	return (0);
+}
+
+struct iked_sa *
+ikev2_getimsgdata(struct iked *env, struct imsg *imsg, struct iked_sahdr *sh,
+    u_int8_t *type, u_int8_t **buf, size_t *size)
+{
+	u_int8_t	*ptr;
+	size_t		 len;
+	struct iked_sa	*sa;
+
+	IMSG_SIZE_CHECK(imsg, sh);
+
+	ptr = imsg->data;
+	len = IMSG_DATA_SIZE(imsg) - sizeof(*sh) - sizeof(*type);
+	memcpy(sh, ptr, sizeof(*sh));
+	memcpy(type, ptr + sizeof(*sh), sizeof(*type));
+
+	sa = sa_lookup(env, sh->sh_ispi, sh->sh_rspi, sh->sh_initiator);
+
+	log_debug("%s: imsg %d rspi %s ispi %s initiator %d sa %s"
+	    " type %d data length %d",
+	    __func__, imsg->hdr.type,
+	    print_spi(sh->sh_rspi, 8),
+	    print_spi(sh->sh_ispi, 8),
+	    sh->sh_initiator,
+	    sa == NULL ? "invalid" : "valid", *type, len);
+
+	if (sa == NULL)
+		return (NULL);
+
+	*buf = ptr + sizeof(*sh) + sizeof(*type);
+	*size = len;
+
+	return (sa);
+}
+
+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)
+{
+	u_int			 initiator;
+	struct iked_sa		*sa;
+	struct ike_header	*hdr;
+
+	hdr = ibuf_seek(msg->msg_data, msg->msg_offset, sizeof(*hdr));
+
+	if (hdr == NULL || (ssize_t)ibuf_size(msg->msg_data) <
+	    (betoh32(hdr->ike_length) - msg->msg_offset))
+		return;
+
+	initiator = (hdr->ike_flags & IKEV2_FLAG_INITIATOR) ? 0 : 1;
+	msg->msg_sa = sa_lookup(env,
+	    betoh64(hdr->ike_ispi), betoh64(hdr->ike_rspi),
+	    initiator);
+	if (policy_lookup(env, msg) != 0)
+		return;
+
+	log_info("%s: %s from %s to %s policy '%s', %ld bytes", __func__,
+	    print_map(hdr->ike_exchange, ikev2_exchange_map),
+	    print_host(&msg->msg_peer, NULL, 0),
+	    print_host(&msg->msg_local, NULL, 0),
+	    msg->msg_policy->pol_name,
+	    ibuf_size(msg->msg_data));
+
+	switch (hdr->ike_exchange) {
+	case IKEV2_EXCHANGE_IKE_SA_INIT:
+		if (msg->msg_sa != NULL) {
+			log_debug("%s: SA already exists", __func__);
+			return;
+		}
+		if ((msg->msg_sa = sa_new(env,
+		    betoh64(hdr->ike_ispi), betoh64(hdr->ike_rspi),
+		    initiator, msg->msg_policy)) == NULL) {
+			log_debug("%s: failed to get new SA", __func__);
+			return;
+		}
+		break;
+	case IKEV2_EXCHANGE_IKE_AUTH:
+		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+			return;
+		if (sa_stateok(msg->msg_sa, IKEV2_STATE_VALID)) {
+			log_debug("%s: already authenticated", __func__);
+			return;
+		}
+		break;
+	case IKEV2_EXCHANGE_CREATE_CHILD_SA:
+		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+			return;
+		break;
+	case IKEV2_EXCHANGE_INFORMATIONAL:
+		if (ikev2_message_valid_ike_sa(env, hdr, msg) == -1)
+			return;
+		break;
+	default:
+		log_debug("%s: unsupported exchange: %s", __func__,
+		    print_map(hdr->ike_exchange, ikev2_exchange_map));
+		return;
+	}
+
+	if (msg->msg_sa != NULL) {
+		sa = msg->msg_sa;
+
+		log_debug("%s: updating msg, natt %d", __func__, msg->msg_natt);
+
+		if (msg->msg_natt)
+			sa->sa_natt = 1;
+
+		if (betoh32(hdr->ike_msgid) < sa->sa_msgid) {
+			log_debug("%s: invalid sequence number", __func__);
+			return;
+		} else
+			sa->sa_msgid = betoh32(hdr->ike_msgid);
+
+		if (msg->msg_peerlen > sizeof(sa->sa_peer)) {
+			log_debug("%s: invalid peer address",
+			    __func__);
+			return;
+		}
+		sa->sa_peer.addr_af = msg->msg_peer.ss_family;
+		sa->sa_peer.addr.ss_len = msg->msg_peerlen;
+		sa->sa_peer.addr_port =
+		    htons(socket_getport(&msg->msg_peer));
+		socket_af((struct sockaddr *)&sa->sa_peer.addr,
+		    sa->sa_peer.addr_port);
+		memcpy(&sa->sa_peer.addr, &msg->msg_peer,
+		    sizeof(sa->sa_peer.addr));
+
+		if (msg->msg_locallen > sizeof(sa->sa_local)) {
+			log_debug("%s: invalid local address",
+			    __func__);
+			return;
+		}
+		sa->sa_local.addr_af = msg->msg_local.ss_family;
+		sa->sa_local.addr.ss_len = msg->msg_locallen;
+		sa->sa_local.addr_port =
+		    htons(socket_getport(&msg->msg_local));
+		socket_af((struct sockaddr *)&sa->sa_local.addr,
+		    sa->sa_local.addr_port);
+		memcpy(&sa->sa_local.addr, &msg->msg_local,
+		    sizeof(sa->sa_local.addr));
+		sa->sa_fd = msg->msg_fd;
+
+		log_debug("%s: updated SA peer %s local %s", __func__,
+		    print_host(&sa->sa_peer.addr, NULL, 0),
+		    print_host(&sa->sa_local.addr, NULL, 0));
+	}
+
+	if (ikev2_parse_message(env, hdr, msg, msg->msg_offset) != 0) {
+		log_debug("%s: failed to parse message", __func__);
+		return;
+	}
+
+	if (msg->msg_response)
+		return;
+
+	switch (hdr->ike_exchange) {
+	case IKEV2_EXCHANGE_IKE_SA_INIT:
+		sa = msg->msg_sa;
+
+		if (sa == NULL) {
+			log_debug("%s: invalid sa", __func__);
+			return;
+		}
+
+		sa_state(env, sa, IKEV2_STATE_SA_INIT);
+
+		if ((ibuf_length(sa->sa_inonce) < IKED_NONCE_MIN) ||
+		    (ibuf_length(sa->sa_rnonce) < IKED_NONCE_MIN)) {
+			log_debug("%s: invalid nonce", __func__);
+			sa_free(env, sa);
+			return;
+		}
+		if (ikev2_sa_keys(sa) != 0) {
+			log_debug("%s: failed to get IKE SA keys", __func__);
+			sa_free(env, sa);
+			return;
+		}
+
+		if (ikev2_policy2id(&msg->msg_policy->pol_localid,
+		    &sa->sa_rid, 1) != 0) {
+			log_debug("%s: failed to get responder id", __func__);
+			return;
+		}
+		ibuf_release(sa->sa_1stmsg);
+		if ((sa->sa_1stmsg = ibuf_new(ibuf_data(msg->msg_data),
+		    ibuf_size(msg->msg_data))) == NULL) {
+			log_debug("%s: failed to copy 1st message", __func__);
+			return;
+		}
+
+		if (ikev2_resp_ike_sa_init(env, msg) != 0) {
+			log_debug("%s: failed to send init response", __func__);
+			return;
+		}
+		break;
+	case IKEV2_EXCHANGE_IKE_AUTH:
+		sa = msg->msg_sa;
+
+		if (!sa_stateok(sa, IKEV2_STATE_AUTH_REQUEST) &&
+		    sa->sa_policy->pol_auth.auth_eap)
+			sa_state(env, sa, IKEV2_STATE_EAP);
+
+		if (ikev2_resp_ike_auth(env, sa) != 0) {
+			log_debug("%s: failed to send auth response", __func__);
+			return;
+		}
+		break;
+	case IKEV2_EXCHANGE_CREATE_CHILD_SA:
+		(void)ikev2_resp_create_child_sa(env, msg);
+		break;
+	default:
+		break;
+	}
+
+	if (msg->msg_sa && msg->msg_sa->sa_state == IKEV2_STATE_DELETE)
+		sa_free(env, msg->msg_sa);
+}
+
+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;
+	struct iked_static_id	 localpid;
+	char			 idstr[IKED_ID_SIZE];
+	struct in_addr		 in4;
+	struct in6_addr		 in6;
+
+	/* Fixup the local Id if not specified */
+	if (srcid && polid->id_type == 0) {
+		polid = &localpid;
+		bzero(polid, sizeof(*polid));
+
+		/* Create a default local ID based on our FQDN */
+		polid->id_type = IKEV2_ID_FQDN;
+		if (gethostname((char *)polid->id_data,
+		    sizeof(polid->id_data)) != 0)
+			return (-1);
+		polid->id_length =
+		    strlen((char *)polid->id_data); /* excluding NUL */
+	}
+
+	if (!polid->id_length)
+		return (-1);
+
+	/* Create an IKEv2 ID payload */
+	bzero(&hdr, sizeof(hdr));
+	hdr.id_type = id->id_type = polid->id_type;
+
+	if ((id->id_buf = ibuf_new(&hdr, sizeof(hdr))) == NULL)
+		return (-1);
+
+	switch (id->id_type) {
+	case IKEV2_ID_IPV4_ADDR:
+		if (inet_pton(AF_INET, polid->id_data, &in4) != 1 ||
+		    ibuf_add(id->id_buf, &in4, sizeof(in4)) != 0) {
+			ibuf_release(id->id_buf);
+			return (-1);
+		}
+		break;
+	case IKEV2_ID_IPV6_ADDR:
+		if (inet_pton(AF_INET6, polid->id_data, &in6) != 1 ||
+		    ibuf_add(id->id_buf, &in6, sizeof(in6)) != 0) {
+			ibuf_release(id->id_buf);
+			return (-1);
+		}
+		break;
+	default:
+		if (ibuf_add(id->id_buf,
+		    polid->id_data, polid->id_length) != 0) {
+			ibuf_release(id->id_buf);
+			return (-1);
+		}
+		break;
+	}
+
+	if (print_id(id, sizeof(hdr), idstr, sizeof(idstr)) == -1)
+		return (-1);
+
+	log_debug("%s: %s %s/%s length %d", __func__,
+	    srcid ? "srcid" : "dstid",
+	    print_map(id->id_type, ikev2_id_map),
+	    idstr, ibuf_size(id->id_buf));
+
+	return (0);
+}
+
+struct ike_header *
+ikev2_add_header(struct ibuf *buf, struct iked_sa *sa,
+    u_int32_t msgid, u_int8_t nextpayload,
+    u_int8_t exchange, u_int8_t flags)
+{
+	struct ike_header	*hdr;
+
+	if ((hdr = ibuf_advance(buf, sizeof(*hdr))) == NULL) {
+		log_debug("%s: failed to add header", __func__);
+		return (NULL);
+	}
+
+	hdr->ike_ispi = htobe64(sa->sa_hdr.sh_ispi);
+	hdr->ike_rspi = htobe64(sa->sa_hdr.sh_rspi);
+	hdr->ike_nextpayload = nextpayload;
+	hdr->ike_version = IKEV2_VERSION;
+	hdr->ike_exchange = exchange;
+	hdr->ike_msgid = htobe32(msgid);
+	hdr->ike_length = htobe32(sizeof(*hdr));
+	hdr->ike_flags = flags;
+
+	if (sa->sa_hdr.sh_initiator)
+		hdr->ike_flags |= IKEV2_FLAG_INITIATOR;
+
+	return (hdr);
+}
+
+int
+ikev2_set_header(struct ike_header *hdr, size_t length)
+{
+	u_int32_t	 hdrlength = sizeof(*hdr) + length;
+
+	if (hdrlength > UINT32_MAX) {
+		log_debug("%s: message too long", __func__);
+		return (-1);
+	}
+
+	hdr->ike_length = htobe32(sizeof(*hdr) + length);
+
+	return (0);
+}
+
+struct ikev2_payload *
+ikev2_add_payload(struct ibuf *buf)
+{
+	struct ikev2_payload	*pld;
+
+	if ((pld = ibuf_advance(buf, sizeof(*pld))) == NULL) {
+		log_debug("%s: failed to add payload", __func__);
+		return (NULL);
+	}
+
+	pld->pld_nextpayload = IKEV2_PAYLOAD_NONE;
+	pld->pld_length = sizeof(*pld);
+
+	return (pld);
+}
+
+ssize_t
+ikev2_add_ts(struct ibuf *buf, u_int type, struct iked_sa *sa)
+{
+	struct iked_policy	*pol = sa->sa_policy;
+	struct ikev2_tsp	*tsp;
+	struct ikev2_ts		*ts;
+	struct iked_flow	*flow;
+	struct iked_addr	*addr;
+	u_int8_t		*ptr;
+	size_t			 len = 0;
+	u_int32_t		 av[4], bv[4], mv[4];
+	struct sockaddr_in	*in4;
+	struct sockaddr_in6	*in6;
+
+	if ((tsp = ibuf_advance(buf, sizeof(*tsp))) == NULL)
+		return (-1);
+	tsp->tsp_count = pol->pol_nflows;
+	len = sizeof(*tsp);
+
+	TAILQ_FOREACH(flow, &pol->pol_flows, flow_entry) {
+		if ((ts = ibuf_advance(buf, sizeof(*ts))) == NULL)
+			return (-1);
+
+		if (type == IKEV2_PAYLOAD_TSi)
+			addr = &flow->flow_src;
+		else if (type == IKEV2_PAYLOAD_TSr)
+			addr = &flow->flow_dst;
+		else
+			return (-1);
+
+		ts->ts_protoid = flow->flow_ipproto;
+
+		if (addr->addr_port) {
+			ts->ts_startport = addr->addr_port;
+			ts->ts_endport = addr->addr_port;
+		} else {
+			ts->ts_startport = 0;
+			ts->ts_endport = 0xffff;
+		}
+
+		switch (addr->addr_af) {
+		case AF_INET:
+			ts->ts_type = IKEV2_TS_IPV4_ADDR_RANGE;
+			ts->ts_length = htobe16(sizeof(*ts) + 8);
+
+			if ((ptr = ibuf_advance(buf, 8)) == NULL)
+				return (-1);
+
+			in4 = (struct sockaddr_in *)&addr->addr;
+			if (addr->addr_net) {
+				/* Convert IPv4 network to address range */
+				mv[0] = prefixlen2mask(addr->addr_mask);
+				av[0] = in4->sin_addr.s_addr & mv[0];
+				bv[0] = in4->sin_addr.s_addr | ~mv[0];
+			} else
+				av[0] = bv[0] = in4->sin_addr.s_addr;
+
+			memcpy(ptr, &av[0], 4);
+			memcpy(ptr + 4, &bv[0], 4);
+			break;
+		case AF_INET6:
+			ts->ts_type = IKEV2_TS_IPV6_ADDR_RANGE;
+			ts->ts_length = htobe16(sizeof(*ts) + 32);
+
+			if ((ptr = ibuf_advance(buf, 32)) == NULL)
+				return (-1);
+
+			in6 = (struct sockaddr_in6 *)&addr->addr;
+
+			memcpy(&av, &in6->sin6_addr.s6_addr, 16);
+			memcpy(&bv, &in6->sin6_addr.s6_addr, 16);
+			if (addr->addr_net) {
+				/* Convert IPv6 network to address range */
+				prefixlen2mask6(addr->addr_mask, mv);
+				av[0] &= mv[0];
+				av[1] &= mv[1];
+				av[2] &= mv[2];
+				av[3] &= mv[3];
+				bv[0] |= ~mv[0];
+				bv[1] |= ~mv[1];
+				bv[2] |= ~mv[2];
+				bv[3] |= ~mv[3];
+			}
+
+			memcpy(ptr, &av, 16);
+			memcpy(ptr + 16, &bv, 16);
+			break;
+		}
+
+		len += betoh16(ts->ts_length);
+	}
+
+	return (len);
+}
+
+int
+ikev2_next_payload(struct ikev2_payload *pld, size_t length,
+    u_int8_t nextpayload)
+{
+	size_t	 pldlength = sizeof(*pld) + length;
+
+	if (pldlength > UINT16_MAX) {
+		log_debug("%s: payload too long", __func__);
+		return (-1);
+	}
+
+	log_debug("%s: length %d nextpayload %s",
+	    __func__, pldlength, print_map(nextpayload, ikev2_payload_map));
+
+	pld->pld_length = htobe16(pldlength);
+	pld->pld_nextpayload = nextpayload;
+
+	return (0);
+}
+
+ssize_t
+ikev2_nat_detection(struct iked_message *msg, void *ptr, size_t len,
+    u_int type, int response)
+{
+	EVP_MD_CTX		 ctx;
+	struct ike_header	*hdr;
+	u_int8_t		 md[SHA_DIGEST_LENGTH];
+	u_int			 mdlen = sizeof(md);
+	struct iked_sa		*sa = msg->msg_sa;
+	struct sockaddr_in	*in4;
+	struct sockaddr_in6	*in6;
+	ssize_t			 ret = -1;
+	struct sockaddr_storage	*ss;
+	u_int64_t		 rspi, ispi;
+
+	if (ptr == NULL)
+		return (mdlen);
+
+	if (!response || sa == NULL) {
+		if ((hdr = ibuf_seek(msg->msg_data, 0, sizeof(*hdr))) == NULL)
+			return (-1);
+		ispi = hdr->ike_ispi;
+		rspi = hdr->ike_rspi;
+	} else {
+		ispi = htobe64(sa->sa_hdr.sh_ispi);
+		rspi = htobe64(sa->sa_hdr.sh_rspi);
+	}
+
+	EVP_MD_CTX_init(&ctx);
+	EVP_DigestInit_ex(&ctx, EVP_sha1(), NULL);
+
+	switch (type) {
+	case IKEV2_N_NAT_DETECTION_SOURCE_IP:
+		if (response)
+			ss = &msg->msg_local;
+		else
+			ss = &msg->msg_peer;
+
+		log_debug("%s: source %s %s %s", __func__,
+		    print_spi(betoh64(ispi), 8),
+		    print_spi(betoh64(rspi), 8),
+		    print_host(ss, NULL, 0));
+		break;
+	case IKEV2_N_NAT_DETECTION_DESTINATION_IP:
+		if (response)
+			ss = &msg->msg_peer;
+		else
+			ss = &msg->msg_local;
+
+		log_debug("%s: destination %s %s %s", __func__,
+		    print_spi(betoh64(ispi), 8),
+		    print_spi(betoh64(rspi), 8),
+		    print_host(ss, NULL, 0));
+		break;
+	default:
+		goto done;
+	}
+
+	EVP_DigestUpdate(&ctx, &ispi, sizeof(ispi));
+	EVP_DigestUpdate(&ctx, &rspi, sizeof(rspi));
+
+	switch (ss->ss_family) {
+	case AF_INET:
+		in4 = (struct sockaddr_in *)ss;
+		EVP_DigestUpdate(&ctx, &in4->sin_addr.s_addr,
+		    sizeof(in4->sin_addr.s_addr));
+		EVP_DigestUpdate(&ctx, &in4->sin_port,
+		    sizeof(in4->sin_port));
+		break;
+	case AF_INET6:
+		in6 = (struct sockaddr_in6 *)ss;
+		EVP_DigestUpdate(&ctx, &in6->sin6_addr.s6_addr,
+		    sizeof(in6->sin6_addr.s6_addr));
+		EVP_DigestUpdate(&ctx, &in6->sin6_port,
+		    sizeof(in6->sin6_port));
+		break;
+	default:
+		goto done;
+	}
+
+	EVP_DigestFinal_ex(&ctx, md, &mdlen);
+
+	if (len < mdlen)
+		goto done;
+
+	memcpy(ptr, md, mdlen);
+	ret = mdlen;
+ done:
+	EVP_MD_CTX_cleanup(&ctx);
+
+	return (ret);
+}
+
+ssize_t
+ikev2_add_cp(struct iked *env, struct iked_sa *sa, struct ibuf *buf)
+{
+	struct iked_policy	*pol = sa->sa_policy;
+	struct ikev2_cp		*cp;
+	struct ikev2_cfg	*cfg;
+	struct iked_cfg		*ikecfg;
+	u_int			 i;
+	size_t			 len;
+	struct sockaddr_in	*in4;
+	struct sockaddr_in6	*in6;
+	u_int8_t		 prefixlen;
+
+	if ((cp = ibuf_advance(buf, sizeof(*cp))) == NULL)
+		return (-1);
+	len = sizeof(*cp);
+
+	switch (sa->sa_cp) {
+	case IKEV2_CP_REQUEST:
+		cp->cp_type = IKEV2_CP_REPLY;
+		break;
+	case IKEV2_CP_REPLY:
+	case IKEV2_CP_SET:
+	case IKEV2_CP_ACK:
+		/* Not yet supported */
+		return (-1);
+	}
+
+	for (i = 0; i < pol->pol_ncfg; i++) {
+		ikecfg = &pol->pol_cfg[i];
+		if (ikecfg->cfg_action != cp->cp_type)
+			continue;
+
+		if ((cfg = ibuf_advance(buf, sizeof(*cfg))) == NULL)
+			return (-1);
+
+		cfg->cfg_type = htobe16(ikecfg->cfg_type);
+		len += sizeof(*cfg);
+
+		switch (ikecfg->cfg_type) {
+		case IKEV2_CFG_INTERNAL_IP4_ADDRESS:
+		case IKEV2_CFG_INTERNAL_IP4_NETMASK:
+		case IKEV2_CFG_INTERNAL_IP4_DNS:
+		case IKEV2_CFG_INTERNAL_IP4_NBNS:
+		case IKEV2_CFG_INTERNAL_IP4_DHCP:
+		case IKEV2_CFG_INTERNAL_IP4_SERVER:
+			/* 4 bytes IPv4 address */
+			in4 = (struct sockaddr_in *)&ikecfg->cfg.address.addr;
+			cfg->cfg_length = htobe16(4);
+			if (ibuf_add(buf, &in4->sin_addr.s_addr, 4) == -1)
+				return (-1);
+			len += 4;
+			break;
+		case IKEV2_CFG_INTERNAL_IP6_DNS:
+		case IKEV2_CFG_INTERNAL_IP6_NBNS:
+		case IKEV2_CFG_INTERNAL_IP6_DHCP:
+		case IKEV2_CFG_INTERNAL_IP6_SERVER:
+			/* 16 bytes IPv6 address */
+			in6 = (struct sockaddr_in6 *)&ikecfg->cfg.address;
+			cfg->cfg_length = htobe16(16);
+			if (ibuf_add(buf, &in6->sin6_addr.s6_addr, 16) == -1)
+				return (-1);
+			len += 16;
+			break;
+		case IKEV2_CFG_INTERNAL_IP6_ADDRESS:
+			/* 16 bytes IPv6 address + 1 byte prefix length */
+			in6 = (struct sockaddr_in6 *)&ikecfg->cfg.address.addr;
+			cfg->cfg_length = htobe16(17);
+			if (ibuf_add(buf, &in6->sin6_addr.s6_addr, 16) == -1)
+				return (-1);
+			if (ikecfg->cfg.address.addr_net)
+				prefixlen = ikecfg->cfg.address.addr_mask;
+			else
+				prefixlen = 128;
+			if (ibuf_add(buf, &prefixlen, 1) == -1)
+				return (-1);
+			len += 16 + 1;
+			break;
+		case IKEV2_CFG_APPLICATION_VERSION:
+			/* Reply with an empty string (non-NUL terminated) */
+			cfg->cfg_length = 0;
+			break;
+		}
+	}
+
+	return (len);
+}
+
+ssize_t
+ikev2_add_proposals(struct ibuf *buf, struct iked_proposals *proposals,
+    u_int8_t protoid)
+{
+	struct ikev2_sa_proposal	*sap;
+	struct iked_transform		*xform;
+	struct iked_proposal		*prop;
+	ssize_t				 i, length = 0, saplength, ret, n;
+	u_int64_t			 spi64;
+	u_int32_t			 spi32;
+
+	n = 0;
+	TAILQ_FOREACH(prop, proposals, prop_entry) {
+		if (prop->prop_protoid != protoid)
+			continue;
+
+		if ((sap = ibuf_advance(buf, sizeof(*sap))) == NULL) {
+			log_debug("%s: failed to add proposal", __func__);
+			return (-1);
+		}
+		sap->sap_proposalnr = prop->prop_id;
+		sap->sap_protoid = prop->prop_protoid;
+		sap->sap_spisize = prop->prop_localspi.spi_size;
+		sap->sap_transforms = prop->prop_nxforms;
+		saplength = sizeof(*sap);
+
+		switch (prop->prop_localspi.spi_size) {
+		case 4:
+			spi32 = htobe32(prop->prop_localspi.spi);
+			if (ibuf_add(buf, &spi32, sizeof(spi32)) != 0)
+				return (-1);
+			saplength += 4;
+			break;
+		case 8:
+			spi64 = htobe64(prop->prop_localspi.spi);
+			if (ibuf_add(buf, &spi64, sizeof(spi64)) != 0)
+				return (-1);
+			saplength += 8;
+			break;
+		default:
+			break;
+		}
+
+		for (i = 0; i < prop->prop_nxforms; i++) {
+			xform = prop->prop_xforms + i;
+
+			if ((ret = ikev2_add_transform(buf,
+			    i == prop->prop_nxforms - 1 ?
+			    IKEV2_XFORM_LAST : IKEV2_XFORM_MORE,
+			    xform->xform_type, xform->xform_id,
+			    xform->xform_length)) == -1)
+				return (-1);
+
+			saplength += ret;
+		}
+
+		sap->sap_length = htobe16(saplength);
+		length += saplength;
+	}
+
+	log_debug("%s: length %d", __func__, length);
+
+	return (length);
+}
+
+ssize_t
+ikev2_add_transform(struct ibuf *buf,
+    u_int8_t more, u_int8_t type, u_int16_t id, u_int16_t length)
+{
+	struct ikev2_transform	*xfrm;
+	struct ikev2_attribute	*attr;
+
+	if ((xfrm = ibuf_advance(buf, sizeof(*xfrm))) == NULL) {
+		log_debug("%s: failed to add transform", __func__);
+		return (-1);
+	}
+	xfrm->xfrm_more = more;
+	xfrm->xfrm_type = type;
+	xfrm->xfrm_id = htobe16(id);
+
+	if (length) {
+		xfrm->xfrm_length = htobe16(sizeof(*xfrm) + sizeof(*attr));
+
+		if ((attr = ibuf_advance(buf, sizeof(*attr))) == NULL) {
+			log_debug("%s: failed to add attribute", __func__);
+			return (-1);
+		}
+		attr->attr_type = htobe16(IKEV2_ATTRAF_TV |
+		    IKEV2_ATTRTYPE_KEY_LENGTH);
+		attr->attr_length = htobe16(length);
+	} else
+		xfrm->xfrm_length = htobe16(sizeof(*xfrm));
+
+	return (betoh16(xfrm->xfrm_length));
+}
+
+int
+ikev2_add_data(struct ibuf *buf, void *data, size_t length)
+{
+	void	*msgbuf;
+
+	if ((msgbuf = ibuf_advance(buf, length)) == NULL) {
+		log_debug("%s: failed", __func__);
+		return (-1);
+	}
+	memcpy(msgbuf, data, length);
+
+	return (0);
+}
+
+int
+ikev2_add_buf(struct ibuf *buf, struct ibuf *data)
+{
+	void	*msgbuf;
+
+	if ((msgbuf = ibuf_advance(buf, ibuf_size(data))) == NULL) {
+		log_debug("%s: failed", __func__);
+		return (-1);
+	}
+	memcpy(msgbuf, ibuf_data(data), ibuf_size(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)
+{
+	struct iked_message		 resp;
+	struct ike_header		*hdr;
+	struct ikev2_payload		*pld;
+	struct ikev2_cert		*cert;
+	struct ikev2_keyexchange	*ke;
+	struct ikev2_notify		*n;
+	struct iked_sa			*sa = msg->msg_sa;
+	struct ibuf			*buf;
+	struct group			*group;
+	u_int8_t			*ptr;
+	ssize_t				 len;
+	int				 ret = -1;
+
+	if ((buf = ikev2_message_init(env, &resp,
+	    &msg->msg_peer, msg->msg_peerlen,
+	    &msg->msg_local, msg->msg_locallen, 1)) == NULL)
+		goto done;
+
+	/* IKE header */
+	if ((hdr = ikev2_add_header(buf, sa, 0,
+	    IKEV2_PAYLOAD_SA, IKEV2_EXCHANGE_IKE_SA_INIT,
+	    IKEV2_FLAG_RESPONSE)) == NULL)
+		goto done;
+
+	/* SA payload */
+	if ((pld = ikev2_add_payload(buf)) == NULL)
+		goto done;
+	if ((len = ikev2_add_proposals(buf, &sa->sa_proposals,
+	    IKEV2_SAPROTO_IKE)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_KE) == -1)
+		goto done;
+
+	/* KE payload */
+	if ((pld = ikev2_add_payload(buf)) == NULL)
+		goto done;
+	if ((ke = ibuf_advance(buf, sizeof(*ke))) == NULL)
+		goto done;
+	if ((group = sa->sa_dhgroup) == NULL) {
+		log_debug("%s: invalid dh", __func__);
+		goto done;
+	}
+	ke->kex_dhgroup = htobe16(group->id);
+	if (ikev2_add_buf(buf, sa->sa_dhrexchange) == -1)
+		goto done;
+	len = sizeof(*ke) + dh_getlen(group);
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONCE) == -1)
+		goto done;
+
+	/* NONCE payload */
+	if ((pld = ikev2_add_payload(buf)) == NULL)
+		goto done;
+	if (ikev2_add_buf(buf, sa->sa_rnonce) == -1)
+		goto done;
+	len = ibuf_size(sa->sa_rnonce);
+
+	if ((env->sc_opts & IKED_OPT_NONATT) == 0 &&
+	    msg->msg_local.ss_family != AF_UNSPEC) {
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NOTIFY) == -1)
+			goto done;
+
+		/* NAT-T notify payloads */
+		if ((pld = ikev2_add_payload(buf)) == NULL)
+			goto done;
+		if ((n = ibuf_advance(buf, sizeof(*n))) == NULL)
+			goto done;
+		n->n_type = htobe16(IKEV2_N_NAT_DETECTION_SOURCE_IP);
+		len = ikev2_nat_detection(msg, NULL, 0, 0, 1);
+		if ((ptr = ibuf_advance(buf, len)) == NULL)
+			goto done;
+		if ((len = ikev2_nat_detection(msg, ptr, len,
+		    betoh16(n->n_type), 1)) == -1)
+			goto done;
+		len += sizeof(*n);
+
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NOTIFY) == -1)
+			goto done;
+
+		if ((pld = ikev2_add_payload(buf)) == NULL)
+			goto done;
+		if ((n = ibuf_advance(buf, sizeof(*n))) == NULL)
+			goto done;
+		n->n_type = htobe16(IKEV2_N_NAT_DETECTION_DESTINATION_IP);
+		len = ikev2_nat_detection(msg, NULL, 0, 0, 1);
+		if ((ptr = ibuf_advance(buf, len)) == NULL)
+			goto done;
+		if ((len = ikev2_nat_detection(msg, ptr, len,
+		    betoh16(n->n_type), 1)) == -1)
+			goto done;
+		len += sizeof(*n);
+	}
+
+	if (env->sc_certreqtype) {
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_CERTREQ) == -1)
+			goto done;
+
+		/* CERTREQ payload */
+		if ((pld = ikev2_add_payload(buf)) == NULL)
+			goto done;
+		if ((cert = ibuf_advance(buf, sizeof(*cert))) == NULL)
+			goto done;
+		cert->cert_type = env->sc_certreqtype;
+		if (ikev2_add_buf(buf, env->sc_certreq) == -1)
+			goto done;
+		len = ibuf_size(env->sc_certreq) + sizeof(*cert);
+	}
+
+	if (ikev2_next_payload(pld, len, 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);
+
+	ibuf_release(sa->sa_2ndmsg);
+	if ((sa->sa_2ndmsg = ibuf_dup(buf)) == NULL) {
+		log_debug("%s: failed to copy 2nd message", __func__);
+		goto done;
+	}
+
+	ret = ikev2_message_send(env, msg->msg_fd, &resp);
+
+ done:
+	message_cleanup(env, &resp);
+
+	return (ret);
+}
+
+int
+ikev2_resp_ike_auth(struct iked *env, struct iked_sa *sa)
+{
+	struct ikev2_payload		*pld;
+	struct ikev2_notify		*n;
+	struct ikev2_cert		*cert;
+	struct ikev2_auth		*auth;
+	struct iked_id			*id, *certid;
+	struct ibuf			*e = NULL;
+	u_int8_t			 firstpayload;
+	int				 ret = -1;
+	ssize_t				 len;
+
+	if (sa == NULL)
+		return (-1);
+
+	if (sa->sa_state == IKEV2_STATE_EAP)
+		return (ikev2_resp_ike_eap(env, sa, NULL));
+	else if (!sa_stateok(sa, IKEV2_STATE_VALID))
+		return (0);	/* ignore */
+
+	if (ikev2_childsa_negotiate(env, sa, NULL) == -1)
+		return (-1);
+
+	/* New encrypted message buffer */
+	if ((e = ibuf_static()) == NULL)
+		goto done;
+
+	if (!sa->sa_localauth.id_type) {
+		/* Downgrade the state */
+		sa_state(env, sa, IKEV2_STATE_AUTH_SUCCESS);
+	}
+
+	if (!sa_stateok(sa, IKEV2_STATE_VALID)) {
+		/* Notify payload */
+		if ((pld = ikev2_add_payload(e)) == NULL)
+			goto done;
+		firstpayload = IKEV2_PAYLOAD_NOTIFY;
+
+		if ((n = ibuf_advance(e, sizeof(*n))) == NULL)
+			goto done;
+		n->n_protoid = IKEV2_SAPROTO_IKE;	/* XXX ESP etc. */
+		n->n_spisize = 0;
+		n->n_type = htobe16(IKEV2_N_AUTHENTICATION_FAILED);
+		len = sizeof(*n);
+
+		goto send;
+	}
+
+	if (sa->sa_hdr.sh_initiator) {
+		id = &sa->sa_iid;
+		certid = &sa->sa_icert;
+	} else {
+		id = &sa->sa_rid;
+		certid = &sa->sa_rcert;
+	}
+
+	if (sa->sa_state != IKEV2_STATE_EAP_VALID) {
+		/* ID payload */
+		if ((pld = ikev2_add_payload(e)) == NULL)
+			goto done;
+		firstpayload = IKEV2_PAYLOAD_IDr;
+		if (ibuf_cat(e, id->id_buf) != 0)
+			goto done;
+		len = ibuf_size(id->id_buf);
+
+		/* CERT payload */
+		if ((sa->sa_staterequire & IKED_REQ_CERT) &&
+		    (certid->id_type != IKEV2_CERT_NONE)) {
+			if (ikev2_next_payload(pld, len,
+			    IKEV2_PAYLOAD_CERT) == -1)
+				goto done;
+
+			if ((pld = ikev2_add_payload(e)) == NULL)
+				goto done;
+			if ((cert = ibuf_advance(e, sizeof(*cert))) == NULL)
+				goto done;
+			cert->cert_type = certid->id_type;
+			if (ibuf_cat(e, certid->id_buf) != 0)
+				goto done;
+			len = ibuf_size(certid->id_buf) + sizeof(*cert);
+		}
+
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_AUTH) == -1)
+			goto done;
+	} else
+		firstpayload = IKEV2_PAYLOAD_AUTH;
+
+	/* AUTH payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((auth = ibuf_advance(e, sizeof(*auth))) == NULL)
+		goto done;
+	auth->auth_method = sa->sa_localauth.id_type;
+	if (ibuf_cat(e, sa->sa_localauth.id_buf) != 0)
+		goto done;
+	len = ibuf_size(sa->sa_localauth.id_buf) + sizeof(*auth);
+
+	/* CP payload */
+	if (sa->sa_cp) {
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_CP) == -1)
+			goto done;
+
+		if ((pld = ikev2_add_payload(e)) == NULL)
+			goto done;
+		if ((len = ikev2_add_cp(env, sa, e)) == -1)
+			goto done;
+	}
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_SA) == -1)
+		goto done;
+
+	/* SA payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = ikev2_add_proposals(e, &sa->sa_proposals,
+	    IKEV2_SAPROTO_ESP)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_TSi) == -1)
+		goto done;
+
+	/* TSi payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = ikev2_add_ts(e, IKEV2_PAYLOAD_TSi, sa)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_TSr) == -1)
+		goto done;
+
+	/* TSr payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = ikev2_add_ts(e, IKEV2_PAYLOAD_TSr, sa)) == -1)
+		goto done;
+
+ send:
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	ret = ikev2_message_send_encrypt(env, sa, &e,
+	    IKEV2_EXCHANGE_IKE_AUTH, firstpayload, 1);
+	if (ret == 0)
+		ret = ikev2_childsa_enable(env, sa);
+	if (ret == 0)
+		sa_state(env, sa, IKEV2_STATE_RUNNING);
+
+ done:
+	ibuf_release(e);
+
+	return (ret);
+}
+
+int
+ikev2_resp_ike_eap(struct iked *env, struct iked_sa *sa, struct ibuf *eapmsg)
+{
+	struct ikev2_payload		*pld;
+	struct ikev2_cert		*cert;
+	struct ikev2_auth		*auth;
+	struct iked_id			*id, *certid;
+	struct ibuf			*e = NULL;
+	u_int8_t			 firstpayload;
+	int				 ret = -1;
+	ssize_t				 len = 0;
+
+	if (!sa_stateok(sa, IKEV2_STATE_EAP))
+		return (0);
+
+	/* Responder only */
+	if (sa->sa_hdr.sh_initiator)
+		return (-1);
+
+	/* New encrypted message buffer */
+	if ((e = ibuf_static()) == NULL)
+		goto done;
+
+	id = &sa->sa_rid;
+	certid = &sa->sa_rcert;
+
+	/* ID payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	firstpayload = IKEV2_PAYLOAD_IDr;
+	if (ibuf_cat(e, id->id_buf) != 0)
+		goto done;
+	len = ibuf_size(id->id_buf);
+
+	if ((sa->sa_staterequire & IKED_REQ_CERT) &&
+	    (certid->id_type != IKEV2_CERT_NONE)) {
+		if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_CERT) == -1)
+			goto done;
+
+		/* CERT payload */
+		if ((pld = ikev2_add_payload(e)) == NULL)
+			goto done;
+		if ((cert = ibuf_advance(e, sizeof(*cert))) == NULL)
+			goto done;
+		cert->cert_type = certid->id_type;
+		if (ibuf_cat(e, certid->id_buf) != 0)
+			goto done;
+		len = ibuf_size(certid->id_buf) + sizeof(*cert);
+	}
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_AUTH) == -1)
+		goto done;
+
+	/* AUTH payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((auth = ibuf_advance(e, sizeof(*auth))) == NULL)
+		goto done;
+	auth->auth_method = sa->sa_localauth.id_type;
+	if (ibuf_cat(e, sa->sa_localauth.id_buf) != 0)
+		goto done;
+	len = ibuf_size(sa->sa_localauth.id_buf) + sizeof(*auth);
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_EAP) == -1)
+		goto done;
+
+	/* EAP payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = eap_identity_request(e)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	ret = ikev2_message_send_encrypt(env, sa, &e,
+	    IKEV2_EXCHANGE_IKE_AUTH, firstpayload, 1);
+
+ done:
+	ibuf_release(e);
+
+	return (ret);
+}
+
+int
+ikev2_send_ike_e(struct iked *env, struct iked_sa *sa, struct ibuf *buf,
+    u_int8_t firstpayload, u_int8_t exchange, int response)
+{
+	struct ikev2_payload		*pld;
+	struct ibuf			*e = NULL;
+	int				 ret = -1;
+
+	/* New encrypted message buffer */
+	if ((e = ibuf_static()) == NULL)
+		goto done;
+
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if (ibuf_cat(e, buf) != 0)
+		goto done;
+
+	if (ikev2_next_payload(pld, ibuf_size(buf), IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	ret = ikev2_message_send_encrypt(env, sa, &e,
+	    exchange, firstpayload, response);
+
+ done:
+	ibuf_release(e);
+
+	return (ret);
+}
+
+int
+ikev2_resp_create_child_sa(struct iked *env, struct iked_message *msg)
+{
+	struct ikev2_payload		*pld;
+	struct ibuf			*e = NULL, *nonce = NULL;
+	u_int				 firstpayload;
+	int				 ret = -1;
+	ssize_t				 len = 0;
+	struct iked_sa			*sa = msg->msg_sa;
+	struct iked_spi			*rekey = &msg->msg_rekey;
+
+	if (msg->msg_response)
+		return (0);
+
+	log_debug("%s: rekey %s spi %s", __func__,
+	    print_map(rekey->spi_protoid, ikev2_saproto_map),
+	    print_spi(rekey->spi, rekey->spi_size));
+
+	if (rekey->spi_protoid == 0) {
+		/* Default to IKE_SA if REKEY_SA was not notified */
+		rekey->spi_protoid = IKEV2_SAPROTO_IKE;
+		if (sa->sa_hdr.sh_initiator)
+			rekey->spi = sa->sa_hdr.sh_rspi;
+		else
+			rekey->spi = sa->sa_hdr.sh_ispi;
+		rekey->spi_size = 8;
+	}
+
+	if (ikev2_childsa_negotiate(env, sa, rekey) == -1)
+		return (-1);
+
+	if (sa->sa_hdr.sh_initiator)
+		nonce = sa->sa_inonce;
+	else
+		nonce = sa->sa_rnonce;
+
+	if ((e = ibuf_static()) == NULL)
+		return (-1);
+
+	/* SA payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	firstpayload = IKEV2_PAYLOAD_SA;
+	if ((len = ikev2_add_proposals(e, &sa->sa_proposals,
+	    IKEV2_SAPROTO_ESP)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONCE) == -1)
+		goto done;
+
+	/* NONCE payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if (ikev2_add_buf(e, nonce) == -1)
+		goto done;
+	len = ibuf_size(nonce);
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_TSi) == -1)
+		goto done;
+
+	/* TSi payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = ikev2_add_ts(e, IKEV2_PAYLOAD_TSi, sa)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_TSr) == -1)
+		goto done;
+
+	/* TSr payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+	if ((len = ikev2_add_ts(e, IKEV2_PAYLOAD_TSr, sa)) == -1)
+		goto done;
+
+	if (ikev2_next_payload(pld, len, IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	ret = ikev2_message_send_encrypt(env, sa, &e,
+	    IKEV2_EXCHANGE_CREATE_CHILD_SA, firstpayload, 1);
+	if (ret == 0)
+		ret = ikev2_childsa_enable(env, sa);
+
+ done:
+	ibuf_release(e);
+	return (ret);
+}
+
+int
+ikev2_send_informational(struct iked *env, struct iked_message *msg)
+{
+	struct iked_message		 resp;
+	struct ike_header		*hdr;
+	struct ikev2_payload		*pld;
+	struct ikev2_notify		*n;
+	struct iked_sa			*sa = msg->msg_sa, sah;
+	struct ibuf			*buf, *e = NULL;
+	int				 ret = -1;
+
+	if (msg->msg_error == 0)
+		return (0);
+
+	if ((buf = ikev2_message_init(env, &resp,
+	    &msg->msg_peer, msg->msg_peerlen,
+	    &msg->msg_local, msg->msg_locallen, 0)) == NULL)
+		goto done;
+
+	/* New encrypted message buffer */
+	if ((e = ibuf_static()) == NULL)
+		goto done;
+
+	/* NOTIFY payload */
+	if ((pld = ikev2_add_payload(e)) == NULL)
+		goto done;
+
+	if ((n = ibuf_advance(e, sizeof(*n))) == NULL)
+		goto done;
+	n->n_protoid = IKEV2_SAPROTO_IKE;	/* XXX ESP etc. */
+	n->n_spisize = 0;
+	n->n_type = htobe16(msg->msg_error);
+
+	switch (msg->msg_error) {
+	case IKEV2_N_INVALID_IKE_SPI:
+	case IKEV2_N_NO_PROPOSAL_CHOSEN:
+		break;
+	default:
+		log_debug("%s: unsupported notification %s", __func__,
+		    print_map(msg->msg_error, ikev2_n_map));
+		goto done;
+	}
+
+	if (ikev2_next_payload(pld, sizeof(*n), IKEV2_PAYLOAD_NONE) == -1)
+		goto done;
+
+	if (sa != NULL && msg->msg_decrypted) {
+		/* IKE header */
+		if ((hdr = ikev2_add_header(buf, sa,
+		    ikev2_message_id(env, sa, 0),
+		    IKEV2_PAYLOAD_E, IKEV2_EXCHANGE_INFORMATIONAL,
+		    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),
+		    IKEV2_PAYLOAD_NOTIFY) == -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;
+		}
+	} else {
+		if ((hdr = ibuf_seek(msg->msg_data, 0, sizeof(*hdr))) == NULL)
+			goto done;
+
+		bzero(&sah, sizeof(sah));
+		sah.sa_hdr.sh_rspi = betoh64(hdr->ike_rspi);
+		sah.sa_hdr.sh_ispi = betoh64(hdr->ike_ispi);
+		sah.sa_hdr.sh_initiator =
+		    hdr->ike_flags & IKEV2_FLAG_INITIATOR ? 0 : 1;
+		sa = &sah;
+
+		/* IKE header */
+		if ((hdr = ikev2_add_header(buf, &sah,
+		    ikev2_message_id(env, &sah, 0),
+		    IKEV2_PAYLOAD_NOTIFY, IKEV2_EXCHANGE_INFORMATIONAL,
+		    0)) == NULL)
+			goto done;
+		if (ibuf_cat(buf, e) != 0)
+			goto done;
+		if (ikev2_set_header(hdr, ibuf_size(buf) - sizeof(*hdr)) == -1)
+			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, msg->msg_fd, &resp);
+
+ done:
+	ibuf_release(e);
+	message_cleanup(env, &resp);
+
+	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)
+{
+	u_int8_t	*psk;
+	size_t		 psklen = -1;
+
+	if (hash_setkey(sa->sa_prf, data, length) == NULL)
+		return (-1);
+
+	if ((psk = calloc(1, hash_keylength(sa->sa_prf))) == NULL)
+		return (-1);
+
+	hash_init(sa->sa_prf);
+	hash_update(sa->sa_prf, IKEV2_KEYPAD, strlen(IKEV2_KEYPAD));
+	hash_final(sa->sa_prf, psk, &psklen);
+
+	*pskptr = psk;
+	return (psklen);
+}
+
+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 and and if we received
+			 * the IKE message on the NAT-T port.
+			 */
+			if (msg->msg_sa != NULL && msg->msg_sa->sa_natt)
+				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)
+{
+	struct iked_proposal	*ppeer = NULL, *plocal, *prop, values;
+	struct iked_transform	*tpeer, *tlocal;
+	struct iked_transform	*match[IKEV2_XFORMTYPE_MAX];
+	struct iked_transform	*chosen[IKEV2_XFORMTYPE_MAX];
+	u_int			 type, i, j, match_score, chosen_score = 0;
+
+	bzero(chosen, sizeof(chosen));
+	bzero(&values, sizeof(values));
+
+	TAILQ_FOREACH(plocal, local, prop_entry) {
+		if (plocal->prop_protoid != protoid)
+			continue;
+
+		TAILQ_FOREACH(ppeer, peer, prop_entry) {
+			bzero(match, sizeof(match));
+			match_score = 0;
+
+			if (ppeer->prop_protoid != plocal->prop_protoid)
+				continue;
+
+			for (i = 0; i < ppeer->prop_nxforms; i++) {
+				tpeer = ppeer->prop_xforms + i;
+				for (j = 0; j < plocal->prop_nxforms; j++) {
+					tlocal = plocal->prop_xforms + j;
+					if (!(tpeer->xform_type ==
+					    tlocal->xform_type &&
+					    tpeer->xform_id ==
+					    tlocal->xform_id &&
+					    tpeer->xform_length ==
+					    tlocal->xform_length))
+						continue;
+					if (tpeer->xform_type >
+					    IKEV2_XFORMTYPE_MAX)
+						continue;
+					type = tpeer->xform_type;
+
+					if (match[type] == NULL ||
+					    tlocal->xform_score <
+					    match[type]->xform_score) {
+						match[type] = tlocal;
+					} else
+						continue;
+
+					print_debug("%s: xform %d "
+					    "<-> %d (%d): %s %s (keylength %d <-> %d)", __func__,
+					    ppeer->prop_id, plocal->prop_id,
+					    tlocal->xform_score,
+					    print_map(type,
+					    ikev2_xformtype_map),
+					    print_map(tpeer->xform_id,
+					    tpeer->xform_map),
+					    tpeer->xform_keylength,
+					    tlocal->xform_keylength);
+					if (tpeer->xform_length)
+						print_debug(" %d",
+						    tpeer->xform_length);
+					print_debug("\n");
+				}
+			}
+
+			for (i = match_score = 0;
+			    i < IKEV2_XFORMTYPE_MAX; i++) {
+				if (protoid == IKEV2_SAPROTO_IKE &&
+				    (match[i] == NULL) && (
+				    (i == IKEV2_XFORMTYPE_ENCR) ||
+				    (i == IKEV2_XFORMTYPE_PRF) ||
+				    (i == IKEV2_XFORMTYPE_INTEGR) ||
+				    (i == IKEV2_XFORMTYPE_DH))) {
+					match_score = 0;
+					break;
+				} else if (protoid != IKEV2_SAPROTO_IKE &&
+				    (match[i] == NULL) && (
+				    (i == IKEV2_XFORMTYPE_ENCR) ||
+				    (i == IKEV2_XFORMTYPE_INTEGR) ||
+				    (i == IKEV2_XFORMTYPE_ESN))) {
+					match_score = 0;
+					break;
+				} else if (match[i] == NULL)
+					continue;
+
+				match_score += match[i]->xform_score;
+			}
+
+			log_debug("%s: score %d", __func__, match_score);
+			if (match_score != 0 &&
+			    (chosen_score == 0 || match_score < chosen_score)) {
+				chosen_score = match_score;
+				for (i = 0; i < IKEV2_XFORMTYPE_MAX; i++)
+					chosen[i] = match[i];
+				memcpy(&values, ppeer, sizeof(values));
+			}
+		}
+		if (chosen_score != 0)
+			break;
+	}
+
+	if (chosen_score == 0)
+		return (-1);
+	else if (sa == NULL)
+		return (0);
+
+	(void)config_free_proposals(&sa->sa_proposals, protoid);
+	prop = config_add_proposal(&sa->sa_proposals, values.prop_id, protoid);
+
+	if (values.prop_localspi.spi_size) {
+		prop->prop_peerspi = values.prop_peerspi;
+		prop->prop_localspi.spi_size = values.prop_localspi.spi_size;
+		prop->prop_localspi.spi = 0;
+	}
+
+	for (i = 0; i < IKEV2_XFORMTYPE_MAX; i++) {
+		if (chosen[i] == NULL)
+			continue;
+		print_debug("%s: score %d: %s %s",
+		    __func__, chosen[i]->xform_score,
+		    print_map(i, ikev2_xformtype_map),
+		    print_map(chosen[i]->xform_id, chosen[i]->xform_map));
+		if (chosen[i]->xform_length)
+			print_debug(" %d", chosen[i]->xform_length);
+		print_debug("\n");
+
+		if (config_add_transform(prop, chosen[i]->xform_type,
+		    chosen[i]->xform_id, chosen[i]->xform_length,
+		    chosen[i]->xform_keylength) == NULL)
+			break;
+	}
+
+	return (0);
+}
+
+int
+ikev2_sa_keys(struct iked_sa *sa)
+{
+	struct iked_transform	*xform;
+	struct iked_hash	*prf, *integr;
+	struct iked_cipher	*encr;
+	struct group		*group;
+	struct ibuf		*ninr, *dhsecret, *skeyseed, *s, *t;
+	size_t			 nonceminlen, ilen, rlen, tmplen;
+	u_int64_t		 ispi, rspi;
+	int			 ret = -1;
+
+	ninr = dhsecret = skeyseed = s = t = NULL;
+
+	if (sa->sa_encr == NULL) {
+		if ((xform = config_findtransform(&sa->sa_proposals,
+		    IKEV2_XFORMTYPE_ENCR)) == NULL) {
+			log_debug("%s: did not find encr transform", __func__);
+			return (-1);
+		}
+		if ((sa->sa_encr = cipher_new(xform->xform_type,
+		    xform->xform_id, xform->xform_length)) == NULL) {
+			log_debug("%s: failed to get encr", __func__);
+			return (-1);
+		}
+	}
+	encr = sa->sa_encr;
+
+	if (sa->sa_prf == NULL) {
+		if ((xform = config_findtransform(&sa->sa_proposals,
+		    IKEV2_XFORMTYPE_PRF)) == NULL) {
+			log_debug("%s: did not find prf transform", __func__);
+			return (-1);
+		}
+		if ((sa->sa_prf =
+		    hash_new(xform->xform_type, xform->xform_id)) == NULL) {
+			log_debug("%s: failed to get prf", __func__);
+			return (-1);
+		}
+	}
+	prf = sa->sa_prf;
+
+	if (sa->sa_integr == NULL) {
+		if ((xform = config_findtransform(&sa->sa_proposals,
+		    IKEV2_XFORMTYPE_INTEGR)) == NULL) {
+			log_debug("%s: did not find integr transform",
+			    __func__);
+			return (-1);
+		}
+		if ((sa->sa_integr =
+		    hash_new(xform->xform_type, xform->xform_id)) == NULL) {
+			log_debug("%s: failed to get integr", __func__);
+			return (-1);
+		}
+	}
+	integr = sa->sa_integr;
+
+	if (sa->sa_dhgroup == NULL) {
+		if ((xform = config_findtransform(&sa->sa_proposals,
+		    IKEV2_XFORMTYPE_DH)) == NULL) {
+			log_debug("%s: did not find dh transform", __func__);
+			return (-1);
+		}
+		if ((sa->sa_dhgroup =
+		    group_get(xform->xform_id)) == NULL) {
+			log_debug("%s: invalid dh", __func__);
+			return (-1);
+		}
+		if ((ssize_t)ibuf_length(sa->sa_dhiexchange) !=
+		    dh_getlen(sa->sa_dhgroup)) {
+			/* XXX send notification to peer */
+			log_debug("%s: dh mismatch %d", __func__,
+			    dh_getlen(sa->sa_dhgroup) * 8);
+			return (-1);
+		}
+	}
+	group = sa->sa_dhgroup;
+
+	if (!ibuf_length(sa->sa_dhrexchange)) {
+		if ((sa->sa_dhrexchange = ibuf_new(NULL,
+		    dh_getlen(group))) == NULL) {
+			log_debug("%s: failed to alloc dh exchange", __func__);
+			return (-1);
+		}
+		if (dh_create_exchange(group, sa->sa_dhrexchange->buf) == -1) {
+			log_debug("%s: failed to get dh exchange", __func__);
+			return (-1);
+		}
+	}
+
+	if (prf->hash_fixedkey)
+		nonceminlen = prf->hash_fixedkey;
+	else
+		nonceminlen = IKED_NONCE_MIN;
+
+	/* Nonces need a minimal size and should have an even length */
+	if (ibuf_length(sa->sa_inonce) < nonceminlen ||
+	    (ibuf_length(sa->sa_inonce) % 2) != 0 ||
+	    ibuf_length(sa->sa_rnonce) < nonceminlen ||
+	    (ibuf_length(sa->sa_rnonce) % 2) != 0) {
+		log_debug("%s: invalid nonces", __func__);
+		return (-1);
+	}
+
+	/*
+	 * First generate SKEEYSEED = prf(Ni | Nr, g^ir)
+	 */
+	if (prf->hash_fixedkey) {
+		/* Half of the key bits must come from Ni, and half from Nr */
+		ilen = prf->hash_fixedkey / 2;
+		rlen = prf->hash_fixedkey / 2;
+	} else {
+		/* Most PRF functions accept a variable-length key */
+		ilen = ibuf_length(sa->sa_inonce);
+		rlen = ibuf_length(sa->sa_rnonce);
+	}
+
+	if ((ninr = ibuf_new(sa->sa_inonce->buf, ilen)) == NULL ||
+	    ibuf_add(ninr, sa->sa_rnonce->buf, rlen) != 0) {
+		log_debug("%s: failed to get nonce key buffer", __func__);
+		goto done;
+	}
+	if ((hash_setkey(prf, ninr->buf, ibuf_length(ninr))) == NULL) {
+		log_debug("%s: failed to set prf key", __func__);
+		goto done;
+	}
+
+	if ((dhsecret = ibuf_new(NULL, dh_getlen(group))) == NULL) {
+		log_debug("%s: failed to alloc dh secret", __func__);
+		goto done;
+	}
+	if (dh_create_shared(group, dhsecret->buf,
+	    sa->sa_dhiexchange->buf) == -1) {
+		log_debug("%s: failed to get dh secret"
+		    " group %d len %d secret %d exchange %d", __func__,
+		    group->id, dh_getlen(group), ibuf_length(dhsecret),
+		    ibuf_length(sa->sa_dhiexchange));
+		goto done;
+	}
+
+	if ((skeyseed = ibuf_new(NULL, hash_length(prf))) == NULL) {
+		log_debug("%s: failed to get SKEYSEED buffer", __func__);
+		goto done;
+	}
+
+	tmplen = 0;
+	hash_init(prf);
+	hash_update(prf, dhsecret->buf, ibuf_length(dhsecret));
+	hash_final(prf, skeyseed->buf, &tmplen);
+
+	log_debug("%s: SKEYSEED with %d bytes", __func__, tmplen);
+	print_hex(skeyseed->buf, 0, tmplen);
+
+	if (ibuf_setsize(skeyseed, tmplen) == -1) {
+		log_debug("%s: failed to set keymaterial length", __func__);
+		goto done;
+	}
+
+	/*
+	 * Now generate the key material
+	 *
+	 * S = Ni | Nr | SPIi | SPIr
+	 */
+
+	/* S = Ni | Nr | SPIi | SPIr */
+	ilen = ibuf_length(sa->sa_inonce);
+	rlen = ibuf_length(sa->sa_rnonce);
+	ispi = htobe64(sa->sa_hdr.sh_ispi);
+	rspi = htobe64(sa->sa_hdr.sh_rspi);
+
+	if ((s = ibuf_new(sa->sa_inonce->buf, ilen)) == NULL ||
+	    ibuf_add(s, sa->sa_rnonce->buf, rlen) != 0 ||
+	    ibuf_add(s, &ispi, sizeof(ispi)) != 0 ||
+	    ibuf_add(s, &rspi, sizeof(rspi)) != 0) {
+		log_debug("%s: failed to set S buffer", __func__);
+		goto done;
+	}
+
+	log_debug("%s: S with %d bytes", __func__, ibuf_length(s));
+	print_hex(s->buf, 0, ibuf_length(s));
+
+	/*
+	 * Get the size of the key material we need and the number
+	 * of rounds we need to run the prf+ function.
+	 */
+	ilen = hash_length(prf) +	/* SK_d */
+	    hash_keylength(integr) +	/* SK_ai */
+	    hash_keylength(integr) +	/* SK_ar */
+	    cipher_keylength(encr) +	/* SK_ei */
+	    cipher_keylength(encr) +	/* SK_er */
+	    hash_keylength(prf) +	/* SK_pi */
+	    hash_keylength(prf);	/* SK_pr */
+
+	if ((t = ikev2_prfplus(prf, skeyseed, s, ilen)) == NULL) {
+		log_debug("%s: failed to get IKE SA key material", __func__);
+		goto done;
+	}
+
+	if ((sa->sa_key_d = ibuf_copy(t, hash_length(prf))) == NULL ||
+	    (sa->sa_key_iauth = ibuf_copy(t, hash_keylength(integr))) == NULL ||
+	    (sa->sa_key_rauth = ibuf_copy(t, hash_keylength(integr))) == NULL ||
+	    (sa->sa_key_iencr = ibuf_copy(t, cipher_keylength(encr))) == NULL ||
+	    (sa->sa_key_rencr = ibuf_copy(t, cipher_keylength(encr))) == NULL ||
+	    (sa->sa_key_iprf = ibuf_copy(t, hash_length(prf))) == NULL ||
+	    (sa->sa_key_rprf = ibuf_copy(t, hash_length(prf))) == NULL) {
+		log_debug("%s: failed to get SA keys", __func__);
+		goto done;
+	}
+
+	log_debug("%s: SK_d with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_d));
+	print_hex(sa->sa_key_d->buf, 0, ibuf_length(sa->sa_key_d));
+	log_debug("%s: SK_ai with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_iauth));
+	print_hex(sa->sa_key_iauth->buf, 0, ibuf_length(sa->sa_key_iauth));
+	log_debug("%s: SK_ar with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_rauth));
+	print_hex(sa->sa_key_rauth->buf, 0, ibuf_length(sa->sa_key_rauth));
+	log_debug("%s: SK_ei with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_iencr));
+	print_hex(sa->sa_key_iencr->buf, 0, ibuf_length(sa->sa_key_iencr));
+	log_debug("%s: SK_er with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_rencr));
+	print_hex(sa->sa_key_rencr->buf, 0, ibuf_length(sa->sa_key_rencr));
+	log_debug("%s: SK_pi with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_iprf));
+	print_hex(sa->sa_key_iprf->buf, 0, ibuf_length(sa->sa_key_iprf));
+	log_debug("%s: SK_pr with %d bytes", __func__,
+	    ibuf_length(sa->sa_key_rprf));
+	print_hex(sa->sa_key_rprf->buf, 0, ibuf_length(sa->sa_key_rprf));
+
+	ret = 0;
+
+ done:
+	ibuf_release(ninr);
+	ibuf_release(dhsecret);
+	ibuf_release(skeyseed);
+	ibuf_release(s);
+	ibuf_release(t);
+
+	return (ret);
+}
+
+struct ibuf *
+ikev2_prfplus(struct iked_hash *prf, struct ibuf *key, struct ibuf *seed,
+    size_t keymatlen)
+{
+	struct ibuf	*t = NULL, *t1 = NULL, *t2 = NULL;
+	size_t		 rlen, i, hashlen = 0;
+	u_int8_t	 pad = 0;
+
+	/*
+	 * prf+ (K, S) = T1 | T2 | T3 | T4 | ...
+	 *
+	 * T1 = prf (K, S | 0x01)
+	 * T2 = prf (K, T1 | S | 0x02)
+	 * T3 = prf (K, T2 | S | 0x03)
+	 * T4 = prf (K, T3 | S | 0x04)
+	 */
+
+	if ((hash_setkey(prf, ibuf_data(key), ibuf_size(key))) == NULL) {
+		log_debug("%s: failed to set prf+ key", __func__);
+		goto fail;
+	}
+
+	if ((t = ibuf_new(NULL, 0)) == NULL) {
+		log_debug("%s: failed to get T buffer", __func__);
+		goto fail;
+	}
+
+	rlen = roundup(keymatlen, hash_length(prf)) / hash_length(prf);
+	if (rlen > 255)
+		fatalx("ikev2_prfplus: key material too large");
+
+	for (i = 0; i < rlen; i++) {
+		if (t1 != NULL) {
+			t2 = ibuf_new(t1->buf, ibuf_length(t1));
+			ibuf_release(t1);
+		} else
+			t2 = ibuf_new(NULL, 0);
+		t1 = ibuf_new(NULL, hash_length(prf));
+
+		ibuf_add(t2, seed->buf, ibuf_length(seed));
+		pad = i + 1;
+		ibuf_add(t2, &pad, 1);
+
+		hash_init(prf);
+		hash_update(prf, t2->buf, ibuf_length(t2));
+		hash_final(prf, t1->buf, &hashlen);
+
+		if (hashlen != hash_length(prf))
+			fatalx("ikev2_prfplus: hash length mismatch");
+
+		ibuf_release(t2);
+		ibuf_add(t, t1->buf, ibuf_length(t1));
+
+		log_debug("%s: T%d with %d bytes", __func__,
+		    pad, ibuf_length(t1));
+		print_hex(t1->buf, 0, ibuf_length(t1));
+	}
+
+	log_debug("%s: Tn with %d bytes", __func__, ibuf_length(t));
+	print_hex(t->buf, 0, ibuf_length(t));
+
+	ibuf_release(t1);
+
+	return (t);
+
+ fail:
+	ibuf_release(t1);
+	ibuf_release(t);
+
+	return (NULL);
+}
+
+int
+ikev2_sa_tag(struct iked_sa *sa, struct iked_id *id)
+{
+	char	*format, *domain = NULL, *idrepl = NULL;
+	char	 idstr[IKED_ID_SIZE];
+	int	 ret = -1;
+	size_t	 len;
+
+	if (sa->sa_tag != NULL)
+		free(sa->sa_tag);
+	sa->sa_tag = NULL;
+	format = sa->sa_policy->pol_tag;
+
+	len = IKED_TAG_SIZE;
+	if ((sa->sa_tag = calloc(1, len)) == NULL) {
+		log_debug("%s: calloc", __func__);
+		goto fail;
+	}
+	if (strlcpy(sa->sa_tag, format, len) >= len) {
+		log_debug("%s: tag too long", __func__);
+		goto fail;
+	}
+
+	if (print_id(id, sizeof(struct ikev2_id),
+	    idstr, sizeof(idstr)) == -1) {
+		log_debug("%s: invalid id", __func__);
+		goto fail;
+	}
+
+	/* ASN.1 DER IDs are too long, use the CN part instead */
+	if (*idstr == '/' && (idrepl = strstr(idstr, "CN=")) != NULL) {
+		domain = strstr(idrepl, "emailAddress=");
+		idrepl[strcspn(idrepl, "/")] = '\0';
+	} else
+		idrepl = idstr;
+
+	if (strstr(format, "$id") != NULL) {
+		if (expand_string(sa->sa_tag, len, "$id", idrepl) != 0) {
+			log_debug("%s: failed to expand tag", __func__);
+			goto fail;
+		}
+	}
+
+	if (strstr(format, "$name") != NULL) {
+		if (expand_string(sa->sa_tag, len, "$name",
+		    sa->sa_policy->pol_name) != 0) {
+			log_debug("%s: failed to expand tag", __func__);
+			goto fail;
+		}
+	}
+
+	if (strstr(format, "$domain") != NULL) {
+		if (id->id_type == IKEV2_ID_FQDN)
+			domain = strchr(idrepl, '.');
+		else if (id->id_type == IKEV2_ID_RFC822_ADDR)
+			domain = strchr(idrepl, '@');
+		else if (*idstr == '/' && domain != NULL)
+			domain = strchr(domain, '@');
+		else
+			domain = NULL;
+		if (domain == NULL || strlen(domain) < 2) {
+			log_debug("%s: no valid domain in ID %s",
+			    __func__, idstr);
+			goto fail;
+		}
+		domain++;
+		if (expand_string(sa->sa_tag, len, "$domain", domain) != 0) {
+			log_debug("%s: failed to expand tag", __func__);
+			goto fail;
+		}
+	}
+
+	log_debug("%s: %s (%d)", __func__, sa->sa_tag, strlen(sa->sa_tag));
+
+	ret = 0;
+ fail:
+	if (ret != 0) {
+		free(sa->sa_tag);
+		sa->sa_tag = NULL;
+	}
+
+	return (ret);
+}
+
+int
+ikev2_childsa_negotiate(struct iked *env, struct iked_sa *sa,
+    struct iked_spi *rekey)
+{
+	struct iked_proposal	*prop;
+	struct iked_transform	*xform, *encrxf = NULL, *integrxf = NULL;
+	struct iked_childsa	*csa, *csb;
+	struct iked_flow	*flow, *flowa, *flowb;
+	struct ibuf		*keymat = NULL, *seed = NULL;
+	u_int			 i, mdlen;
+	size_t			 ilen = 0;
+	int			 ret = -1;
+	u_int32_t		 spi = 0, flowloaded = 0;
+	struct iked_id		*peerid, *localid;
+	EVP_MD_CTX		 ctx;
+	u_int8_t		 md[SHA_DIGEST_LENGTH];
+
+	if (!sa_stateok(sa, IKEV2_STATE_VALID))
+		return (-1);
+
+	if (sa->sa_hdr.sh_initiator) {
+		peerid = &sa->sa_rid;
+		localid = &sa->sa_iid;
+	} else {
+		localid = &sa->sa_rid;
+		peerid = &sa->sa_iid;
+	}
+
+	if (ikev2_sa_tag(sa, peerid) == -1)
+		return (-1);
+
+	sa_stateflags(sa, IKED_REQ_CHILDSA);
+
+	/* We need to determinate the key material length first */
+	TAILQ_FOREACH(prop, &sa->sa_proposals, prop_entry) {
+		if (prop->prop_protoid == IKEV2_SAPROTO_IKE)
+			continue;
+		log_debug("%s: proposal %d", __func__, prop->prop_id);
+		for (i = 0; i < prop->prop_nxforms; i++) {
+			xform = prop->prop_xforms + i;
+			xform->xform_keylength =
+			    keylength_xf(prop->prop_protoid,
+			    xform->xform_type, xform->xform_id);
+
+			switch (xform->xform_type) {
+			case IKEV2_XFORMTYPE_ENCR:
+			case IKEV2_XFORMTYPE_INTEGR:
+				if (xform->xform_length)
+					xform->xform_keylength =
+					    xform->xform_length;
+				ilen += xform->xform_keylength / 8;
+				break;
+			}
+		}
+	}
+
+	/* double key material length for inbound/outbound */
+	ilen *= 2;
+
+	log_debug("%s: key material length %d", __func__, ilen);
+
+	if ((seed = ibuf_dup(sa->sa_inonce)) == NULL ||
+	    ibuf_cat(seed, sa->sa_rnonce) != 0 ||
+	    (keymat = ikev2_prfplus(sa->sa_prf,
+	    sa->sa_key_d, seed, ilen)) == NULL) {
+		log_debug("%s: failed to get IKE SA key material", __func__);
+		goto done;
+	}
+
+	/*
+	 * Generate a hash of the negotiated flows to detect a possible
+	 * IKEv2 traffic selector re-negotiation.
+	 */
+	EVP_DigestInit(&ctx, EVP_sha1());
+	TAILQ_FOREACH(prop, &sa->sa_proposals, prop_entry) {
+		if (ikev2_valid_proposal(prop, NULL, NULL) != 0)
+			continue;
+
+		TAILQ_FOREACH(flow, &sa->sa_policy->pol_flows, flow_entry) {
+			/* Use the inbound flow to generate the hash */
+			i = IPSP_DIRECTION_IN;
+			EVP_DigestUpdate(&ctx, &i, sizeof(i));
+			EVP_DigestUpdate(&ctx, &flow->flow_src,
+			    sizeof(flow->flow_src));
+			EVP_DigestUpdate(&ctx, &flow->flow_dst,
+			    sizeof(flow->flow_dst));
+		}
+	}
+	mdlen = sizeof(sa->sa_flowhash);
+	EVP_DigestFinal(&ctx, md, &mdlen);
+
+	/* Check the existing flows */
+	if (rekey != NULL) {
+		if (memcmp(&sa->sa_flowhash, &md, sizeof(md)) == 0) {
+			flowloaded = 0;
+			TAILQ_FOREACH(flow, &sa->sa_flows,
+			    flow_entry) {
+				/* Mark the flow as unloaded */
+				if (flow->flow_peerspi == rekey->spi) {
+					flow->flow_loaded = 0;
+					flowloaded++;
+				}
+			}
+			log_debug("%s: keeping %d flows",
+			    __func__, flowloaded);
+		}
+
+		/* XXX Check ESP/AH/IKE */
+		if (ikev2_childsa_delete(env, sa,
+		    rekey->spi_protoid, rekey->spi, NULL, 1) == -1) {
+			log_debug("%s: failed to disable old SA %s",
+			    __func__, print_spi(rekey->spi, rekey->spi_size));
+			return (-1);
+		}
+	}
+
+	memcpy(&sa->sa_flowhash, &md, sizeof(md));
+
+	/* Create the new flows */
+	TAILQ_FOREACH(prop, &sa->sa_proposals, prop_entry) {
+		if (ikev2_valid_proposal(prop, NULL, NULL) != 0)
+			continue;
+
+		TAILQ_FOREACH(flow, &sa->sa_policy->pol_flows, flow_entry) {
+			if ((flowa = calloc(1, sizeof(*flowa))) == NULL) {
+				log_debug("%s: failed to get flow", __func__);
+				goto done;
+			}
+
+			memcpy(flowa, flow, sizeof(*flow));
+			flowa->flow_dir = IPSP_DIRECTION_IN;
+			flowa->flow_saproto = prop->prop_protoid;
+			flowa->flow_srcid = localid;
+			flowa->flow_dstid = peerid;
+			flowa->flow_local = &sa->sa_local;
+			flowa->flow_peer = &sa->sa_peer;
+			flowa->flow_ikesa = sa;
+			flowa->flow_peerspi = prop->prop_peerspi.spi;
+			if (flowloaded)
+				flowa->flow_loaded = 1;
+
+			TAILQ_INSERT_TAIL(&sa->sa_flows, flowa, flow_entry);
+
+			if ((flowb = calloc(1, sizeof(*flowb))) == NULL) {
+				log_debug("%s: failed to get flow", __func__);
+				goto done;
+			}
+
+			memcpy(flowb, flowa, sizeof(*flow));
+
+			flowb->flow_dir = IPSP_DIRECTION_OUT;
+			memcpy(&flowb->flow_src, &flow->flow_dst,
+			    sizeof(flow->flow_dst));
+			memcpy(&flowb->flow_dst, &flow->flow_src,
+			    sizeof(flow->flow_src));
+
+			TAILQ_INSERT_TAIL(&sa->sa_flows, flowb, flow_entry);
+		}
+	}
+
+	/* Create the CHILD SAs using the key material */
+	TAILQ_FOREACH(prop, &sa->sa_proposals, prop_entry) {
+		if (ikev2_valid_proposal(prop, &encrxf, &integrxf) != 0)
+			continue;
+
+		spi = 0;
+
+		if ((csa = calloc(1, sizeof(*csa))) == NULL) {
+			log_debug("%s: failed to get CHILD SA", __func__);
+			goto done;
+		}
+
+		csa->csa_saproto = prop->prop_protoid;
+		csa->csa_ikesa = sa;
+		csa->csa_srcid = localid;
+		csa->csa_dstid = peerid;
+		csa->csa_peerspi = prop->prop_peerspi.spi;
+		if (sa->sa_hdr.sh_initiator) {
+			/* Initiator -> Responder */
+			csa->csa_dir = IPSP_DIRECTION_OUT;
+			csa->csa_local = &sa->sa_local;
+			csa->csa_peer = &sa->sa_peer;
+
+			if ((ret = pfkey_sa_init(env->sc_pfkey,
+			    csa, &spi)) != 0)
+				goto done;
+			csa->csa_spi.spi = prop->prop_localspi.spi = spi;
+		} else {
+			/* Responder <- Initiator */
+			csa->csa_dir = IPSP_DIRECTION_IN;
+			csa->csa_spi.spi = prop->prop_peerspi.spi;
+			csa->csa_local = &sa->sa_peer;
+			csa->csa_peer = &sa->sa_local;
+		}
+
+		if ((csa->csa_encrkey = ibuf_copy(keymat,
+		    encrxf->xform_keylength / 8)) == NULL ||
+		    (csa->csa_integrkey = ibuf_copy(keymat,
+		    integrxf->xform_keylength / 8)) == NULL) {
+			log_debug("%s: failed to get CHILD SA keys", __func__);
+			childsa_free(csa);
+			goto done;
+		}
+		csa->csa_encrxf = encrxf;
+		csa->csa_integrxf = integrxf;
+
+		TAILQ_INSERT_TAIL(&sa->sa_childsas, csa, csa_entry);
+
+		if ((csb = calloc(1, sizeof(*csb))) == NULL) {
+			log_debug("%s: failed to get CHILD SA", __func__);
+			goto done;
+		}
+
+		memcpy(csb, csa, sizeof(*csb));
+		csb->csa_dir = csa->csa_dir == IPSP_DIRECTION_IN ?
+		    IPSP_DIRECTION_OUT : IPSP_DIRECTION_IN;
+		if (spi == 0) {
+			if ((ret = pfkey_sa_init(env->sc_pfkey,
+			    csa, &spi)) != 0)
+				goto done;
+			csa->csa_spi.spi = prop->prop_localspi.spi = spi;
+		}
+		csb->csa_local = csa->csa_peer;
+		csb->csa_peer = csa->csa_local;
+
+		if ((csb->csa_encrkey = ibuf_copy(keymat,
+		    encrxf->xform_keylength / 8)) == NULL ||
+		    (csb->csa_integrkey = ibuf_copy(keymat,
+		    integrxf->xform_keylength / 8)) == NULL) {
+			log_debug("%s: failed to get CHILD SA keys", __func__);
+			childsa_free(csb);
+			goto done;
+		}
+
+		TAILQ_INSERT_TAIL(&sa->sa_childsas, csb, csa_entry);
+	}
+
+	ret = 0;
+ done:
+	ibuf_release(keymat);
+	ibuf_release(seed);
+
+	return (ret);
+}
+
+int
+ikev2_childsa_enable(struct iked *env, struct iked_sa *sa)
+{
+	struct iked_childsa	*csa;
+	struct iked_flow	*flow;
+
+	TAILQ_FOREACH(csa, &sa->sa_childsas, csa_entry) {
+		if (csa->csa_rekey)
+			continue;
+
+		if (pfkey_sa_add(env->sc_pfkey, csa, NULL) != 0) {
+			log_debug("%s: failed to load CHILD SA spi %s",
+			    __func__, print_spi(csa->csa_spi.spi, 4));
+			return (-1);
+		}
+
+		log_debug("%s: loaded CHILD SA spi %s", __func__,
+		    print_spi(csa->csa_spi.spi, 4));
+	}
+
+	TAILQ_FOREACH(flow, &sa->sa_flows, flow_entry) { 
+		if (flow->flow_rekey)
+			continue;
+
+		if (pfkey_flow_add(env->sc_pfkey, flow) != 0) {
+			log_debug("%s: failed to load flow", __func__);
+			return (-1);
+		}
+
+		log_debug("%s: loaded flow %p", __func__, flow);
+	}
+
+	return (0);
+}
+
+int
+ikev2_childsa_delete(struct iked *env, struct iked_sa *sa, u_int8_t saproto,
+    u_int64_t spi, u_int64_t *spiptr, int rekey)
+{
+	struct iked_childsa	*csa, *nextcsa;
+	struct iked_flow	*flow, *nextflow;
+	int			 found = 0;
+	u_int64_t		 localspi = 0;
+	const char		*action;
+
+	if (spiptr)
+		*spiptr = 0;
+
+	action = rekey ? "disable" : "delete";
+
+	for (csa = TAILQ_FIRST(&sa->sa_childsas); csa != NULL; csa = nextcsa) {
+		nextcsa = TAILQ_NEXT(csa, csa_entry);
+
+		if (csa->csa_saproto != saproto ||
+		    csa->csa_peerspi != spi)
+			continue;
+
+		if (csa->csa_spi.spi != spi)
+			localspi = csa->csa_spi.spi;
+		if (pfkey_sa_delete(env->sc_pfkey, csa) != 0)
+			log_debug("%s: failed to %s CHILD SA spi %s",
+			    __func__, action, print_spi(csa->csa_spi.spi, 4));
+		else
+			log_debug("%s: %sd CHILD SA spi %s", __func__,
+			    action, print_spi(csa->csa_spi.spi, 4));
+		found++;
+
+		if (rekey) {
+			csa->csa_rekey = 1;
+			continue;
+		}
+		TAILQ_REMOVE(&sa->sa_childsas, csa, csa_entry);
+		childsa_free(csa);
+	}
+
+	for (flow = TAILQ_FIRST(&sa->sa_flows); flow != NULL; flow = nextflow) {
+		nextflow = TAILQ_NEXT(flow, flow_entry);
+
+		if (flow->flow_saproto != saproto ||
+		    flow->flow_peerspi != spi)
+			continue;
+		if (pfkey_flow_delete(env->sc_pfkey, flow) != 0)
+			log_debug("%s: failed to %s flow %p",
+			    __func__, action, flow);
+		else
+			log_debug("%s: %sd flow %p",
+			    __func__, action, flow);
+		found++;
+
+		if (rekey) {
+			flow->flow_rekey = 1;
+			continue;
+		}
+		TAILQ_REMOVE(&sa->sa_flows, flow, flow_entry);
+		flow_free(flow);
+	}
+
+	if (spiptr)
+		*spiptr = localspi;
+
+	return (found ? 0 : -1);
+}
+
+int
+ikev2_valid_proposal(struct iked_proposal *prop,
+    struct iked_transform **exf, struct iked_transform **ixf)
+{
+	struct iked_transform	*xform, *encrxf, *integrxf;
+	size_t			 i;
+
+	switch (prop->prop_protoid) {
+	case IKEV2_SAPROTO_ESP:
+	case IKEV2_SAPROTO_AH:
+		break;
+	default:
+		return (-1);
+	}
+
+	encrxf = integrxf = NULL;
+	for (i = 0; i < prop->prop_nxforms; i++) {
+		xform = prop->prop_xforms + i;
+		if (xform->xform_type ==
+		    IKEV2_XFORMTYPE_ENCR)
+			encrxf = xform;
+		else if (xform->xform_type ==
+		    IKEV2_XFORMTYPE_INTEGR)
+			integrxf = xform;
+	}
+	/* XXX support non-auth / non-enc proposals */
+	if (encrxf == NULL || integrxf == NULL)
+		return (-1);
+
+	if (exf)
+		*exf = encrxf;
+	if (ixf)
+		*ixf = integrxf;
+
+	return (0);
+}