xref: /openbsd/sbin/iked/ikev2_msg.c (revision 771fbea0)
1 /*	$OpenBSD: ikev2_msg.c,v 1.77 2020/10/29 21:49:58 tobhe Exp $	*/
2 
3 /*
4  * Copyright (c) 2019 Tobias Heider <tobias.heider@stusta.de>
5  * Copyright (c) 2010-2013 Reyk Floeter <reyk@openbsd.org>
6  *
7  * Permission to use, copy, modify, and distribute this software for any
8  * purpose with or without fee is hereby granted, provided that the above
9  * copyright notice and this permission notice appear in all copies.
10  *
11  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
12  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
13  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
14  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
15  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
16  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
17  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
18  */
19 
20 #include <sys/param.h>	/* roundup */
21 #include <sys/queue.h>
22 #include <sys/socket.h>
23 #include <sys/uio.h>
24 
25 #include <netinet/in.h>
26 #include <arpa/inet.h>
27 
28 #include <stdlib.h>
29 #include <stdio.h>
30 #include <syslog.h>
31 #include <unistd.h>
32 #include <string.h>
33 #include <signal.h>
34 #include <errno.h>
35 #include <err.h>
36 #include <event.h>
37 
38 #include <openssl/sha.h>
39 #include <openssl/evp.h>
40 
41 #include "iked.h"
42 #include "ikev2.h"
43 #include "eap.h"
44 #include "dh.h"
45 
46 void	 ikev1_recv(struct iked *, struct iked_message *);
47 void	 ikev2_msg_response_timeout(struct iked *, void *);
48 void	 ikev2_msg_retransmit_timeout(struct iked *, void *);
49 int	 ikev2_check_frag_oversize(struct iked_sa *sa, struct ibuf *buf);
50 int	 ikev2_send_encrypted_fragments(struct iked *env, struct iked_sa *sa,
51 	    struct ibuf *in,uint8_t exchange, uint8_t firstpayload, int response);
52 int	 ikev2_msg_encrypt_prepare(struct iked_sa *, struct ikev2_payload *,
53 	    struct ibuf*, struct ibuf *, struct ike_header *, uint8_t, int);
54 
55 void
56 ikev2_msg_cb(int fd, short event, void *arg)
57 {
58 	struct iked_socket	*sock = arg;
59 	struct iked		*env = sock->sock_env;
60 	struct iked_message	 msg;
61 	struct ike_header	 hdr;
62 	uint32_t		 natt = 0x00000000;
63 	uint8_t			 buf[IKED_MSGBUF_MAX];
64 	ssize_t			 len;
65 	off_t			 off;
66 
67 	bzero(&msg, sizeof(msg));
68 	bzero(buf, sizeof(buf));
69 
70 	msg.msg_peerlen = sizeof(msg.msg_peer);
71 	msg.msg_locallen = sizeof(msg.msg_local);
72 	msg.msg_parent = &msg;
73 	memcpy(&msg.msg_local, &sock->sock_addr, sizeof(sock->sock_addr));
74 
75 	if ((len = recvfromto(fd, buf, sizeof(buf), 0,
76 	    (struct sockaddr *)&msg.msg_peer, &msg.msg_peerlen,
77 	    (struct sockaddr *)&msg.msg_local, &msg.msg_locallen)) <
78 	    (ssize_t)sizeof(natt))
79 		return;
80 
81 	if (socket_getport((struct sockaddr *)&msg.msg_local) ==
82 	    env->sc_nattport) {
83 		if (memcmp(&natt, buf, sizeof(natt)) != 0)
84 			return;
85 		msg.msg_natt = 1;
86 		off = sizeof(natt);
87 	} else
88 		off = 0;
89 
90 	if ((size_t)(len - off) <= sizeof(hdr))
91 		return;
92 	memcpy(&hdr, buf + off, sizeof(hdr));
93 
94 	if ((msg.msg_data = ibuf_new(buf + off, len - off)) == NULL)
95 		return;
96 
97 	TAILQ_INIT(&msg.msg_proposals);
98 	SIMPLEQ_INIT(&msg.msg_certreqs);
99 	msg.msg_fd = fd;
100 
101 	if (hdr.ike_version == IKEV1_VERSION)
102 		ikev1_recv(env, &msg);
103 	else
104 		ikev2_recv(env, &msg);
105 
106 	ikev2_msg_cleanup(env, &msg);
107 }
108 
109 void
110 ikev1_recv(struct iked *env, struct iked_message *msg)
111 {
112 	struct ike_header	*hdr;
113 
114 	if (ibuf_size(msg->msg_data) <= sizeof(*hdr)) {
115 		log_debug("%s: short message", __func__);
116 		return;
117 	}
118 
119 	hdr = (struct ike_header *)ibuf_data(msg->msg_data);
120 
121 	log_debug("%s: header ispi %s rspi %s"
122 	    " nextpayload %u version 0x%02x exchange %u flags 0x%02x"
123 	    " msgid %u length %u", __func__,
124 	    print_spi(betoh64(hdr->ike_ispi), 8),
125 	    print_spi(betoh64(hdr->ike_rspi), 8),
126 	    hdr->ike_nextpayload,
127 	    hdr->ike_version,
128 	    hdr->ike_exchange,
129 	    hdr->ike_flags,
130 	    betoh32(hdr->ike_msgid),
131 	    betoh32(hdr->ike_length));
132 
133 	log_debug("%s: IKEv1 not supported", __func__);
134 }
135 
136 struct ibuf *
137 ikev2_msg_init(struct iked *env, struct iked_message *msg,
138     struct sockaddr_storage *peer, socklen_t peerlen,
139     struct sockaddr_storage *local, socklen_t locallen, int response)
140 {
141 	bzero(msg, sizeof(*msg));
142 	memcpy(&msg->msg_peer, peer, peerlen);
143 	msg->msg_peerlen = peerlen;
144 	memcpy(&msg->msg_local, local, locallen);
145 	msg->msg_locallen = locallen;
146 	msg->msg_response = response ? 1 : 0;
147 	msg->msg_fd = -1;
148 	msg->msg_data = ibuf_static();
149 	msg->msg_e = 0;
150 	msg->msg_parent = msg;	/* has to be set */
151 	TAILQ_INIT(&msg->msg_proposals);
152 
153 	return (msg->msg_data);
154 }
155 
156 struct iked_message *
157 ikev2_msg_copy(struct iked *env, struct iked_message *msg)
158 {
159 	struct iked_message		*m = NULL;
160 	struct ibuf			*buf;
161 	size_t				 len;
162 	void				*ptr;
163 
164 	if (ibuf_size(msg->msg_data) < msg->msg_offset)
165 		return (NULL);
166 	len = ibuf_size(msg->msg_data) - msg->msg_offset;
167 
168 	if ((ptr = ibuf_seek(msg->msg_data, msg->msg_offset, len)) == NULL ||
169 	    (m = malloc(sizeof(*m))) == NULL ||
170 	    (buf = ikev2_msg_init(env, m, &msg->msg_peer, msg->msg_peerlen,
171 	     &msg->msg_local, msg->msg_locallen, msg->msg_response)) == NULL ||
172 	    ibuf_add(buf, ptr, len))
173 		return (NULL);
174 
175 	m->msg_fd = msg->msg_fd;
176 	m->msg_msgid = msg->msg_msgid;
177 	m->msg_offset = msg->msg_offset;
178 	m->msg_sa = msg->msg_sa;
179 
180 	return (m);
181 }
182 
183 void
184 ikev2_msg_cleanup(struct iked *env, struct iked_message *msg)
185 {
186 	struct iked_certreq	*cr;
187 
188 	if (msg == msg->msg_parent) {
189 		ibuf_release(msg->msg_nonce);
190 		ibuf_release(msg->msg_ke);
191 		ibuf_release(msg->msg_auth.id_buf);
192 		ibuf_release(msg->msg_id.id_buf);
193 		ibuf_release(msg->msg_cert.id_buf);
194 		ibuf_release(msg->msg_cookie);
195 		ibuf_release(msg->msg_cookie2);
196 		ibuf_release(msg->msg_del_buf);
197 		free(msg->msg_eap.eam_user);
198 		free(msg->msg_cp_addr);
199 		free(msg->msg_cp_addr6);
200 
201 		msg->msg_nonce = NULL;
202 		msg->msg_ke = NULL;
203 		msg->msg_auth.id_buf = NULL;
204 		msg->msg_id.id_buf = NULL;
205 		msg->msg_cert.id_buf = NULL;
206 		msg->msg_cookie = NULL;
207 		msg->msg_cookie2 = NULL;
208 		msg->msg_del_buf = NULL;
209 		msg->msg_eap.eam_user = NULL;
210 		msg->msg_cp_addr = NULL;
211 		msg->msg_cp_addr6 = NULL;
212 
213 		config_free_proposals(&msg->msg_proposals, 0);
214 		while ((cr = SIMPLEQ_FIRST(&msg->msg_certreqs))) {
215 			ibuf_release(cr->cr_data);
216 			SIMPLEQ_REMOVE_HEAD(&msg->msg_certreqs, cr_entry);
217 			free(cr);
218 		}
219 	}
220 
221 	if (msg->msg_data != NULL) {
222 		ibuf_release(msg->msg_data);
223 		msg->msg_data = NULL;
224 	}
225 }
226 
227 int
228 ikev2_msg_valid_ike_sa(struct iked *env, struct ike_header *oldhdr,
229     struct iked_message *msg)
230 {
231 	if (msg->msg_sa != NULL && msg->msg_policy != NULL) {
232 		if (msg->msg_sa->sa_state == IKEV2_STATE_CLOSED)
233 			return (-1);
234 		/*
235 		 * Only permit informational requests from initiator
236 		 * on closing SAs (for DELETE).
237 		 */
238 		if (msg->msg_sa->sa_state == IKEV2_STATE_CLOSING) {
239 			if (((oldhdr->ike_flags &
240 			    (IKEV2_FLAG_INITIATOR|IKEV2_FLAG_RESPONSE)) ==
241 			    IKEV2_FLAG_INITIATOR) &&
242 			    (oldhdr->ike_exchange ==
243 			    IKEV2_EXCHANGE_INFORMATIONAL))
244 				return (0);
245 			return (-1);
246 		}
247 		return (0);
248 	}
249 
250 	/* Always fail */
251 	return (-1);
252 }
253 
254 int
255 ikev2_msg_send(struct iked *env, struct iked_message *msg)
256 {
257 	struct iked_sa		*sa = msg->msg_sa;
258 	struct ibuf		*buf = msg->msg_data;
259 	uint32_t		 natt = 0x00000000;
260 	int			 isnatt = 0;
261 	uint8_t			 exchange, flags;
262 	struct ike_header	*hdr;
263 	struct iked_message	*m;
264 
265 	if (buf == NULL || (hdr = ibuf_seek(msg->msg_data,
266 	    msg->msg_offset, sizeof(*hdr))) == NULL)
267 		return (-1);
268 
269 	isnatt = (msg->msg_natt || (sa && sa->sa_natt));
270 
271 	exchange = hdr->ike_exchange;
272 	flags = hdr->ike_flags;
273 	logit(exchange == IKEV2_EXCHANGE_INFORMATIONAL ?  LOG_DEBUG : LOG_INFO,
274 	    "%ssend %s %s %u peer %s local %s, %ld bytes%s",
275 	    SPI_IH(hdr),
276 	    print_map(exchange, ikev2_exchange_map),
277 	    (flags & IKEV2_FLAG_RESPONSE) ? "res" : "req",
278 	    betoh32(hdr->ike_msgid),
279 	    print_host((struct sockaddr *)&msg->msg_peer, NULL, 0),
280 	    print_host((struct sockaddr *)&msg->msg_local, NULL, 0),
281 	    ibuf_length(buf), isnatt ? ", NAT-T" : "");
282 
283 	if (isnatt) {
284 		if (ibuf_prepend(buf, &natt, sizeof(natt)) == -1) {
285 			log_debug("%s: failed to set NAT-T", __func__);
286 			return (-1);
287 		}
288 	}
289 
290 	if (sendtofrom(msg->msg_fd, ibuf_data(buf), ibuf_size(buf), 0,
291 	    (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen,
292 	    (struct sockaddr *)&msg->msg_local, msg->msg_locallen) == -1) {
293 		log_warn("%s: sendtofrom", __func__);
294 		if (sa != NULL && errno == EADDRNOTAVAIL) {
295 			sa_state(env, sa, IKEV2_STATE_CLOSING);
296 			timer_del(env, &sa->sa_timer);
297 			timer_set(env, &sa->sa_timer,
298 			    ikev2_ike_sa_timeout, sa);
299 			timer_add(env, &sa->sa_timer,
300 			    IKED_IKE_SA_DELETE_TIMEOUT);
301 		}
302 	}
303 
304 	if (sa == NULL)
305 		return (0);
306 
307 	if ((m = ikev2_msg_copy(env, msg)) == NULL) {
308 		log_debug("%s: failed to copy a message", __func__);
309 		return (-1);
310 	}
311 	m->msg_exchange = exchange;
312 
313 	if (flags & IKEV2_FLAG_RESPONSE) {
314 		TAILQ_INSERT_TAIL(&sa->sa_responses, m, msg_entry);
315 		timer_set(env, &m->msg_timer, ikev2_msg_response_timeout, m);
316 		timer_add(env, &m->msg_timer, IKED_RESPONSE_TIMEOUT);
317 	} else {
318 		TAILQ_INSERT_TAIL(&sa->sa_requests, m, msg_entry);
319 		timer_set(env, &m->msg_timer, ikev2_msg_retransmit_timeout, m);
320 		timer_add(env, &m->msg_timer, IKED_RETRANSMIT_TIMEOUT);
321 	}
322 
323 	return (0);
324 }
325 
326 uint32_t
327 ikev2_msg_id(struct iked *env, struct iked_sa *sa)
328 {
329 	uint32_t		id = sa->sa_reqid;
330 
331 	if (++sa->sa_reqid == UINT32_MAX) {
332 		/* XXX we should close and renegotiate the connection now */
333 		log_debug("%s: IKEv2 message sequence overflow", __func__);
334 	}
335 	return (id);
336 }
337 
338 /*
339  * Calculate the final sizes of the IKEv2 header and the encrypted payload
340  * header.  This must be done before encryption to make sure the correct
341  * headers are authenticated.
342  */
343 int
344 ikev2_msg_encrypt_prepare(struct iked_sa *sa, struct ikev2_payload *pld,
345     struct ibuf *buf, struct ibuf *e, struct ike_header *hdr,
346     uint8_t firstpayload, int fragmentation)
347 {
348 	size_t	 len, ivlen, encrlen, integrlen, blocklen, pldlen, outlen;
349 
350 	if (sa == NULL ||
351 	    sa->sa_encr == NULL ||
352 	    sa->sa_integr == NULL) {
353 		log_debug("%s: invalid SA", __func__);
354 		return (-1);
355 	}
356 
357 	len = ibuf_size(e);
358 	blocklen = cipher_length(sa->sa_encr);
359 	integrlen = hash_length(sa->sa_integr);
360 	ivlen = cipher_ivlength(sa->sa_encr);
361 	encrlen = roundup(len + 1, blocklen);
362 	outlen = cipher_outlength(sa->sa_encr, encrlen);
363 	pldlen = ivlen + outlen + integrlen;
364 
365 	if (ikev2_next_payload(pld,
366 	    pldlen + (fragmentation ? sizeof(struct ikev2_frag_payload) : 0),
367 	    firstpayload) == -1)
368 		return (-1);
369 	if (ikev2_set_header(hdr, ibuf_size(buf) + pldlen - sizeof(*hdr)) == -1)
370 		return (-1);
371 
372 	return (0);
373 }
374 
375 struct ibuf *
376 ikev2_msg_encrypt(struct iked *env, struct iked_sa *sa, struct ibuf *src,
377     struct ibuf *aad)
378 {
379 	size_t			 len, encrlen, integrlen, blocklen,
380 				    outlen;
381 	uint8_t			*buf, pad = 0, *ptr;
382 	struct ibuf		*encr, *dst = NULL, *out = NULL;
383 
384 	buf = ibuf_data(src);
385 	len = ibuf_size(src);
386 
387 	log_debug("%s: decrypted length %zu", __func__, len);
388 	print_hex(buf, 0, len);
389 
390 	if (sa == NULL ||
391 	    sa->sa_encr == NULL ||
392 	    sa->sa_integr == NULL) {
393 		log_debug("%s: invalid SA", __func__);
394 		goto done;
395 	}
396 
397 	if (sa->sa_hdr.sh_initiator)
398 		encr = sa->sa_key_iencr;
399 	else
400 		encr = sa->sa_key_rencr;
401 
402 	blocklen = cipher_length(sa->sa_encr);
403 	integrlen = hash_length(sa->sa_integr);
404 	encrlen = roundup(len + sizeof(pad), blocklen);
405 	pad = encrlen - (len + sizeof(pad));
406 
407 	/*
408 	 * Pad the payload and encrypt it
409 	 */
410 	if (pad) {
411 		if ((ptr = ibuf_advance(src, pad)) == NULL)
412 			goto done;
413 		arc4random_buf(ptr, pad);
414 	}
415 	if (ibuf_add(src, &pad, sizeof(pad)) != 0)
416 		goto done;
417 
418 	log_debug("%s: padded length %zu", __func__, ibuf_size(src));
419 	print_hex(ibuf_data(src), 0, ibuf_size(src));
420 
421 	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
422 	cipher_setiv(sa->sa_encr, NULL, 0);	/* XXX ivlen */
423 	if (cipher_init_encrypt(sa->sa_encr) == -1) {
424 		log_info("%s: error initiating cipher.", __func__);
425 		goto done;
426 	}
427 
428 	if ((dst = ibuf_dup(sa->sa_encr->encr_iv)) == NULL)
429 		goto done;
430 
431 	if ((out = ibuf_new(NULL,
432 	    cipher_outlength(sa->sa_encr, encrlen))) == NULL)
433 		goto done;
434 
435 	outlen = ibuf_size(out);
436 
437 	/* Add AAD for AEAD ciphers */
438 	if (sa->sa_integr->hash_isaead)
439 		cipher_aad(sa->sa_encr, ibuf_data(aad),
440 		    ibuf_length(aad), &outlen);
441 
442 	if (cipher_update(sa->sa_encr, ibuf_data(src), encrlen,
443 	    ibuf_data(out), &outlen) == -1) {
444 		log_info("%s: error updating cipher.", __func__);
445 		goto done;
446 	}
447 
448 	if (cipher_final(sa->sa_encr) == -1) {
449 		log_info("%s: encryption failed.", __func__);
450 		goto done;
451 	}
452 
453 	if (outlen && ibuf_add(dst, ibuf_data(out), outlen) != 0)
454 		goto done;
455 
456 	if ((ptr = ibuf_advance(dst, integrlen)) == NULL)
457 		goto done;
458 	explicit_bzero(ptr, integrlen);
459 
460 	log_debug("%s: length %zu, padding %d, output length %zu",
461 	    __func__, len + sizeof(pad), pad, ibuf_size(dst));
462 	print_hex(ibuf_data(dst), 0, ibuf_size(dst));
463 
464 	ibuf_release(src);
465 	ibuf_release(out);
466 	return (dst);
467  done:
468 	ibuf_release(src);
469 	ibuf_release(out);
470 	ibuf_release(dst);
471 	return (NULL);
472 }
473 
474 int
475 ikev2_msg_integr(struct iked *env, struct iked_sa *sa, struct ibuf *src)
476 {
477 	int			 ret = -1;
478 	size_t			 integrlen, tmplen;
479 	struct ibuf		*integr, *tmp = NULL;
480 	uint8_t			*ptr;
481 
482 	log_debug("%s: message length %zu", __func__, ibuf_size(src));
483 	print_hex(ibuf_data(src), 0, ibuf_size(src));
484 
485 	if (sa == NULL ||
486 	    sa->sa_encr == NULL ||
487 	    sa->sa_integr == NULL) {
488 		log_debug("%s: invalid SA", __func__);
489 		return (-1);
490 	}
491 
492 	integrlen = hash_length(sa->sa_integr);
493 	log_debug("%s: integrity checksum length %zu", __func__,
494 	    integrlen);
495 
496 	/*
497 	 * Validate packet checksum
498 	 */
499 	if ((tmp = ibuf_new(NULL, hash_keylength(sa->sa_integr))) == NULL)
500 		goto done;
501 
502 	if (!sa->sa_integr->hash_isaead) {
503 		if (sa->sa_hdr.sh_initiator)
504 			integr = sa->sa_key_iauth;
505 		else
506 			integr = sa->sa_key_rauth;
507 
508 		hash_setkey(sa->sa_integr, ibuf_data(integr),
509 		    ibuf_size(integr));
510 		hash_init(sa->sa_integr);
511 		hash_update(sa->sa_integr, ibuf_data(src),
512 		    ibuf_size(src) - integrlen);
513 		hash_final(sa->sa_integr, ibuf_data(tmp), &tmplen);
514 
515 		if (tmplen != integrlen) {
516 			log_debug("%s: hash failure", __func__);
517 			goto done;
518 		}
519 	} else {
520 		/* Append AEAD tag */
521 		if (cipher_gettag(sa->sa_encr, ibuf_data(tmp), ibuf_size(tmp)))
522 			goto done;
523 	}
524 
525 	if ((ptr = ibuf_seek(src,
526 	    ibuf_size(src) - integrlen, integrlen)) == NULL)
527 		goto done;
528 	memcpy(ptr, ibuf_data(tmp), integrlen);
529 
530 	print_hex(ibuf_data(tmp), 0, ibuf_size(tmp));
531 
532 	ret = 0;
533  done:
534 	ibuf_release(tmp);
535 
536 	return (ret);
537 }
538 
539 struct ibuf *
540 ikev2_msg_decrypt(struct iked *env, struct iked_sa *sa,
541     struct ibuf *msg, struct ibuf *src)
542 {
543 	ssize_t			 ivlen, encrlen, integrlen, blocklen,
544 				    outlen, tmplen;
545 	uint8_t			 pad = 0, *ptr, *integrdata;
546 	struct ibuf		*integr, *encr, *tmp = NULL, *out = NULL;
547 	off_t			 ivoff, encroff, integroff;
548 
549 	if (sa == NULL ||
550 	    sa->sa_encr == NULL ||
551 	    sa->sa_integr == NULL) {
552 		log_debug("%s: invalid SA", __func__);
553 		print_hex(ibuf_data(src), 0, ibuf_size(src));
554 		goto done;
555 	}
556 
557 	if (!sa->sa_hdr.sh_initiator) {
558 		encr = sa->sa_key_iencr;
559 		integr = sa->sa_key_iauth;
560 	} else {
561 		encr = sa->sa_key_rencr;
562 		integr = sa->sa_key_rauth;
563 	}
564 
565 	blocklen = cipher_length(sa->sa_encr);
566 	ivlen = cipher_ivlength(sa->sa_encr);
567 	ivoff = 0;
568 	integrlen = hash_length(sa->sa_integr);
569 	integroff = ibuf_size(src) - integrlen;
570 	encroff = ivlen;
571 	encrlen = ibuf_size(src) - integrlen - ivlen;
572 
573 	if (encrlen < 0 || integroff < 0) {
574 		log_debug("%s: invalid integrity value", __func__);
575 		goto done;
576 	}
577 
578 	log_debug("%s: IV length %zd", __func__, ivlen);
579 	print_hex(ibuf_data(src), 0, ivlen);
580 	log_debug("%s: encrypted payload length %zd", __func__, encrlen);
581 	print_hex(ibuf_data(src), encroff, encrlen);
582 	log_debug("%s: integrity checksum length %zd", __func__, integrlen);
583 	print_hex(ibuf_data(src), integroff, integrlen);
584 
585 	/*
586 	 * Validate packet checksum
587 	 */
588 	if (!sa->sa_integr->hash_isaead) {
589 		if ((tmp = ibuf_new(NULL, hash_keylength(sa->sa_integr))) == NULL)
590 			goto done;
591 
592 		hash_setkey(sa->sa_integr, integr->buf, ibuf_length(integr));
593 		hash_init(sa->sa_integr);
594 		hash_update(sa->sa_integr, ibuf_data(msg),
595 		    ibuf_size(msg) - integrlen);
596 		hash_final(sa->sa_integr, tmp->buf, &tmplen);
597 
598 		integrdata = ibuf_seek(src, integroff, integrlen);
599 		if (integrdata == NULL)
600 			goto done;
601 		if (memcmp(tmp->buf, integrdata, integrlen) != 0) {
602 			log_debug("%s: integrity check failed", __func__);
603 			goto done;
604 		}
605 
606 		log_debug("%s: integrity check succeeded", __func__);
607 		print_hex(tmp->buf, 0, tmplen);
608 
609 		ibuf_release(tmp);
610 		tmp = NULL;
611 	}
612 
613 	/*
614 	 * Decrypt the payload and strip any padding
615 	 */
616 	if ((encrlen % blocklen) != 0) {
617 		log_debug("%s: unaligned encrypted payload", __func__);
618 		goto done;
619 	}
620 
621 	cipher_setkey(sa->sa_encr, encr->buf, ibuf_length(encr));
622 	cipher_setiv(sa->sa_encr, ibuf_data(src) + ivoff, ivlen);
623 	if (cipher_init_decrypt(sa->sa_encr) == -1) {
624 		log_info("%s: error initiating cipher.", __func__);
625 		goto done;
626 	}
627 
628 	/* Set AEAD tag */
629 	if (sa->sa_integr->hash_isaead) {
630 		integrdata = ibuf_seek(src, integroff, integrlen);
631 		if (integrdata == NULL)
632 			goto done;
633 		if (cipher_settag(sa->sa_encr, integrdata, integrlen)) {
634 			log_info("%s: failed to set tag.", __func__);
635 			goto done;
636 		}
637 	}
638 
639 	if ((out = ibuf_new(NULL, cipher_outlength(sa->sa_encr,
640 	    encrlen))) == NULL)
641 		goto done;
642 
643 	/*
644 	 * Add additional authenticated data for AEAD ciphers
645 	 */
646 	if (sa->sa_integr->hash_isaead) {
647 		log_debug("%s: AAD length %zu", __func__, ibuf_length(msg) - ibuf_length(src));
648 		print_hex(ibuf_data(msg), 0, ibuf_length(msg) - ibuf_length(src));
649 		cipher_aad(sa->sa_encr, ibuf_data(msg),
650 		    ibuf_length(msg) - ibuf_length(src), &outlen);
651 	}
652 
653 	if ((outlen = ibuf_length(out)) != 0) {
654 		if (cipher_update(sa->sa_encr, ibuf_data(src) + encroff,
655 		    encrlen, ibuf_data(out), &outlen) == -1) {
656 			log_info("%s: error updating cipher.", __func__);
657 			goto done;
658 		}
659 
660 		ptr = ibuf_seek(out, outlen - 1, 1);
661 		pad = *ptr;
662 	}
663 
664 	if (cipher_final(sa->sa_encr) == -1) {
665 		log_info("%s: decryption failed.", __func__);
666 		goto done;
667 	}
668 
669 	log_debug("%s: decrypted payload length %zd/%zd padding %d",
670 	    __func__, outlen, encrlen, pad);
671 	print_hex(ibuf_data(out), 0, ibuf_size(out));
672 
673 	/* Strip padding and padding length */
674 	if (ibuf_setsize(out, outlen - pad - 1) != 0)
675 		goto done;
676 
677 	ibuf_release(src);
678 	return (out);
679  done:
680 	ibuf_release(tmp);
681 	ibuf_release(out);
682 	ibuf_release(src);
683 	return (NULL);
684 }
685 
686 int
687 ikev2_check_frag_oversize(struct iked_sa *sa, struct ibuf *buf) {
688 	size_t		len = ibuf_length(buf);
689 	sa_family_t	sa_fam;
690 	size_t		max;
691 	size_t		ivlen, integrlen, blocklen;
692 
693 	if (sa == NULL ||
694 	    sa->sa_encr == NULL ||
695 	    sa->sa_integr == NULL) {
696 		log_debug("%s: invalid SA", __func__);
697 		return (-1);
698 	}
699 
700 	sa_fam = ((struct sockaddr *)&sa->sa_local.addr)->sa_family;
701 
702 	max = sa_fam == AF_INET ? IKEV2_MAXLEN_IPV4_FRAG
703 	    : IKEV2_MAXLEN_IPV6_FRAG;
704 
705 	blocklen = cipher_length(sa->sa_encr);
706 	ivlen = cipher_ivlength(sa->sa_encr);
707 	integrlen = hash_length(sa->sa_integr);
708 
709 	/* Estimated maximum packet size (with 0 < padding < blocklen) */
710 	return ((len + ivlen + blocklen + integrlen) >= max) && sa->sa_frag;
711 }
712 
713 int
714 ikev2_msg_send_encrypt(struct iked *env, struct iked_sa *sa, struct ibuf **ep,
715     uint8_t exchange, uint8_t firstpayload, int response)
716 {
717 	struct iked_message		 resp;
718 	struct ike_header		*hdr;
719 	struct ikev2_payload		*pld;
720 	struct ibuf			*buf, *e = *ep;
721 	int				 ret = -1;
722 
723 	/* Check if msg needs to be fragmented */
724 	if (ikev2_check_frag_oversize(sa, e)) {
725 		return ikev2_send_encrypted_fragments(env, sa, e, exchange,
726 		    firstpayload, response);
727 	}
728 
729 	if ((buf = ikev2_msg_init(env, &resp, &sa->sa_peer.addr,
730 	    sa->sa_peer.addr.ss_len, &sa->sa_local.addr,
731 	    sa->sa_local.addr.ss_len, response)) == NULL)
732 		goto done;
733 
734 	resp.msg_msgid = response ? sa->sa_msgid_current : ikev2_msg_id(env, sa);
735 
736 	/* IKE header */
737 	if ((hdr = ikev2_add_header(buf, sa, resp.msg_msgid, IKEV2_PAYLOAD_SK,
738 	    exchange, response ? IKEV2_FLAG_RESPONSE : 0)) == NULL)
739 		goto done;
740 
741 	if ((pld = ikev2_add_payload(buf)) == NULL)
742 		goto done;
743 
744 	if (ikev2_msg_encrypt_prepare(sa, pld, buf, e, hdr, firstpayload, 0) == -1)
745 		goto done;
746 
747 	/* Encrypt message and add as an E payload */
748 	if ((e = ikev2_msg_encrypt(env, sa, e, buf)) == NULL) {
749 		log_debug("%s: encryption failed", __func__);
750 		goto done;
751 	}
752 	if (ibuf_cat(buf, e) != 0)
753 		goto done;
754 
755 	/* Add integrity checksum (HMAC) */
756 	if (ikev2_msg_integr(env, sa, buf) != 0) {
757 		log_debug("%s: integrity checksum failed", __func__);
758 		goto done;
759 	}
760 
761 	resp.msg_data = buf;
762 	resp.msg_sa = sa;
763 	resp.msg_fd = sa->sa_fd;
764 	TAILQ_INIT(&resp.msg_proposals);
765 
766 	(void)ikev2_pld_parse(env, hdr, &resp, 0);
767 
768 	ret = ikev2_msg_send(env, &resp);
769 
770  done:
771 	/* e is cleaned up by the calling function */
772 	*ep = e;
773 	ikev2_msg_cleanup(env, &resp);
774 
775 	return (ret);
776 }
777 
778 int
779 ikev2_send_encrypted_fragments(struct iked *env, struct iked_sa *sa,
780     struct ibuf *in, uint8_t exchange, uint8_t firstpayload, int response) {
781 	struct iked_message		 resp;
782 	struct ibuf			*buf, *e = NULL;
783 	struct ike_header		*hdr;
784 	struct ikev2_payload		*pld;
785 	struct ikev2_frag_payload	*frag;
786 	sa_family_t			 sa_fam;
787 	size_t				 ivlen, integrlen, blocklen;
788 	size_t 				 max_len, left,  offset=0;
789 	size_t				 frag_num = 1, frag_total;
790 	uint8_t				*data;
791 	uint32_t			 msgid;
792 	int 				 ret = -1;
793 
794 	if (sa == NULL ||
795 	    sa->sa_encr == NULL ||
796 	    sa->sa_integr == NULL) {
797 		log_debug("%s: invalid SA", __func__);
798 		goto done;
799 	}
800 
801 	sa_fam = ((struct sockaddr *)&sa->sa_local.addr)->sa_family;
802 
803 	left = ibuf_length(in);
804 
805 	/* Calculate max allowed size of a fragments payload */
806 	blocklen = cipher_length(sa->sa_encr);
807 	ivlen = cipher_ivlength(sa->sa_encr);
808 	integrlen = hash_length(sa->sa_integr);
809 	max_len = (sa_fam == AF_INET ? IKEV2_MAXLEN_IPV4_FRAG
810 	    : IKEV2_MAXLEN_IPV6_FRAG)
811                   - ivlen - blocklen - integrlen;
812 
813 	/* Total number of fragments to send */
814 	frag_total = (left / max_len) + 1;
815 
816 	msgid = response ? sa->sa_msgid_current : ikev2_msg_id(env, sa);
817 
818 	while (frag_num <= frag_total) {
819 		if ((buf = ikev2_msg_init(env, &resp, &sa->sa_peer.addr,
820 		    sa->sa_peer.addr.ss_len, &sa->sa_local.addr,
821 		    sa->sa_local.addr.ss_len, response)) == NULL)
822 			goto done;
823 
824 		resp.msg_msgid = msgid;
825 
826 		/* IKE header */
827 		if ((hdr = ikev2_add_header(buf, sa, resp.msg_msgid,
828 		    IKEV2_PAYLOAD_SKF, exchange, response ? IKEV2_FLAG_RESPONSE
829 		        : 0)) == NULL)
830 			goto done;
831 
832 		/* Payload header */
833 		if ((pld = ikev2_add_payload(buf)) == NULL)
834 			goto done;
835 
836 		/* Fragment header */
837 		if ((frag = ibuf_advance(buf, sizeof(*frag))) == NULL) {
838 			log_debug("%s: failed to add SKF fragment header",
839 			    __func__);
840 			goto done;
841 		}
842 		frag->frag_num = htobe16(frag_num);
843 		frag->frag_total = htobe16(frag_total);
844 
845 		/* Encrypt message and add as an E payload */
846 		data = ibuf_seek(in, offset, 0);
847 		if ((e = ibuf_new(data, MIN(left, max_len))) == NULL) {
848 			goto done;
849 		}
850 
851 		if (ikev2_msg_encrypt_prepare(sa, pld, buf, e, hdr,
852 		    firstpayload, 1) == -1)
853 			goto done;
854 
855 		if ((e = ikev2_msg_encrypt(env, sa, e, buf)) == NULL) {
856 			log_debug("%s: encryption failed", __func__);
857 			goto done;
858 		}
859 		if (ibuf_cat(buf, e) != 0)
860 			goto done;
861 
862 		/* Add integrity checksum (HMAC) */
863 		if (ikev2_msg_integr(env, sa, buf) != 0) {
864 			log_debug("%s: integrity checksum failed", __func__);
865 			goto done;
866 		}
867 
868 		log_debug("%s: Fragment %zu of %zu has size of %zu bytes.",
869 		    __func__, frag_num, frag_total,
870 		    ibuf_size(buf) - sizeof(*hdr));
871 		print_hex(ibuf_data(buf), 0,  ibuf_size(buf));
872 
873 		resp.msg_data = buf;
874 		resp.msg_sa = sa;
875 		resp.msg_fd = sa->sa_fd;
876 		TAILQ_INIT(&resp.msg_proposals);
877 
878 		if (ikev2_msg_send(env, &resp) == -1)
879 			goto done;
880 
881 		offset += MIN(left, max_len);
882 		left -= MIN(left, max_len);
883 		frag_num++;
884 
885 		/* MUST be zero after first fragment */
886 		firstpayload = 0;
887 
888 		ikev2_msg_cleanup(env, &resp);
889 		ibuf_release(e);
890 		e = NULL;
891 	}
892 
893 	return 0;
894 done:
895 	ikev2_msg_cleanup(env, &resp);
896 	ibuf_release(e);
897 	return ret;
898 }
899 
900 struct ibuf *
901 ikev2_msg_auth(struct iked *env, struct iked_sa *sa, int response)
902 {
903 	struct ibuf		*authmsg = NULL, *nonce, *prfkey, *buf;
904 	uint8_t			*ptr;
905 	struct iked_id		*id;
906 	size_t			 tmplen;
907 
908 	/*
909 	 * Create the payload to be signed/MAC'ed for AUTH
910 	 */
911 
912 	if (!response) {
913 		if ((nonce = sa->sa_rnonce) == NULL ||
914 		    (sa->sa_iid.id_type == 0) ||
915 		    (prfkey = sa->sa_key_iprf) == NULL ||
916 		    (buf = sa->sa_1stmsg) == NULL)
917 			return (NULL);
918 		id = &sa->sa_iid;
919 	} else {
920 		if ((nonce = sa->sa_inonce) == NULL ||
921 		    (sa->sa_rid.id_type == 0) ||
922 		    (prfkey = sa->sa_key_rprf) == NULL ||
923 		    (buf = sa->sa_2ndmsg) == NULL)
924 			return (NULL);
925 		id = &sa->sa_rid;
926 	}
927 
928 	if ((authmsg = ibuf_dup(buf)) == NULL)
929 		return (NULL);
930 	if (ibuf_cat(authmsg, nonce) != 0)
931 		goto fail;
932 
933 	if ((hash_setkey(sa->sa_prf, ibuf_data(prfkey),
934 	    ibuf_size(prfkey))) == NULL)
935 		goto fail;
936 
937 	/* require non-truncating hash */
938 	if (hash_keylength(sa->sa_prf) != hash_length(sa->sa_prf))
939 		goto fail;
940 
941 	if ((ptr = ibuf_advance(authmsg, hash_keylength(sa->sa_prf))) == NULL)
942 		goto fail;
943 
944 	hash_init(sa->sa_prf);
945 	hash_update(sa->sa_prf, ibuf_data(id->id_buf), ibuf_size(id->id_buf));
946 	hash_final(sa->sa_prf, ptr, &tmplen);
947 
948 	if (tmplen != hash_length(sa->sa_prf))
949 		goto fail;
950 
951 	log_debug("%s: %s auth data length %zu",
952 	    __func__, response ? "responder" : "initiator",
953 	    ibuf_size(authmsg));
954 	print_hex(ibuf_data(authmsg), 0, ibuf_size(authmsg));
955 
956 	return (authmsg);
957 
958  fail:
959 	ibuf_release(authmsg);
960 	return (NULL);
961 }
962 
963 int
964 ikev2_msg_authverify(struct iked *env, struct iked_sa *sa,
965     struct iked_auth *auth, uint8_t *buf, size_t len, struct ibuf *authmsg)
966 {
967 	uint8_t				*key, *psk = NULL;
968 	ssize_t				 keylen;
969 	struct iked_id			*id;
970 	struct iked_dsa			*dsa = NULL;
971 	int				 ret = -1;
972 	uint8_t				 keytype;
973 
974 	if (sa->sa_hdr.sh_initiator)
975 		id = &sa->sa_rcert;
976 	else
977 		id = &sa->sa_icert;
978 
979 	if ((dsa = dsa_verify_new(auth->auth_method, sa->sa_prf)) == NULL) {
980 		log_debug("%s: invalid auth method", __func__);
981 		return (-1);
982 	}
983 
984 	switch (auth->auth_method) {
985 	case IKEV2_AUTH_SHARED_KEY_MIC:
986 		if (!auth->auth_length) {
987 			log_debug("%s: no pre-shared key found", __func__);
988 			goto done;
989 		}
990 		if ((keylen = ikev2_psk(sa, auth->auth_data,
991 		    auth->auth_length, &psk)) == -1) {
992 			log_debug("%s: failed to get PSK", __func__);
993 			goto done;
994 		}
995 		key = psk;
996 		keytype = 0;
997 		break;
998 	default:
999 		if (!id->id_type || !ibuf_length(id->id_buf)) {
1000 			log_debug("%s: no cert found", __func__);
1001 			goto done;
1002 		}
1003 		key = ibuf_data(id->id_buf);
1004 		keylen = ibuf_size(id->id_buf);
1005 		keytype = id->id_type;
1006 		break;
1007 	}
1008 
1009 	log_debug("%s: method %s keylen %zd type %s", __func__,
1010 	    print_map(auth->auth_method, ikev2_auth_map), keylen,
1011 	    print_map(id->id_type, ikev2_cert_map));
1012 
1013 	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
1014 	    dsa_init(dsa, buf, len) != 0 ||
1015 	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
1016 		log_debug("%s: failed to compute digital signature", __func__);
1017 		goto done;
1018 	}
1019 
1020 	if ((ret = dsa_verify_final(dsa, buf, len)) == 0) {
1021 		log_debug("%s: authentication successful", __func__);
1022 		sa_state(env, sa, IKEV2_STATE_AUTH_SUCCESS);
1023 		sa_stateflags(sa, IKED_REQ_AUTHVALID);
1024 	} else {
1025 		log_debug("%s: authentication failed", __func__);
1026 		sa_state(env, sa, IKEV2_STATE_AUTH_REQUEST);
1027 	}
1028 
1029  done:
1030 	free(psk);
1031 	dsa_free(dsa);
1032 
1033 	return (ret);
1034 }
1035 
1036 int
1037 ikev2_msg_authsign(struct iked *env, struct iked_sa *sa,
1038     struct iked_auth *auth, struct ibuf *authmsg)
1039 {
1040 	uint8_t				*key, *psk = NULL;
1041 	ssize_t				 keylen, siglen;
1042 	struct iked_hash		*prf = sa->sa_prf;
1043 	struct iked_id			*id;
1044 	struct iked_dsa			*dsa = NULL;
1045 	struct ibuf			*buf;
1046 	int				 ret = -1;
1047 	uint8_t			 keytype;
1048 
1049 	if (sa->sa_hdr.sh_initiator)
1050 		id = &sa->sa_icert;
1051 	else
1052 		id = &sa->sa_rcert;
1053 
1054 	if ((dsa = dsa_sign_new(auth->auth_method, prf)) == NULL) {
1055 		log_debug("%s: invalid auth method", __func__);
1056 		return (-1);
1057 	}
1058 
1059 	switch (auth->auth_method) {
1060 	case IKEV2_AUTH_SHARED_KEY_MIC:
1061 		if (!auth->auth_length) {
1062 			log_debug("%s: no pre-shared key found", __func__);
1063 			goto done;
1064 		}
1065 		if ((keylen = ikev2_psk(sa, auth->auth_data,
1066 		    auth->auth_length, &psk)) == -1) {
1067 			log_debug("%s: failed to get PSK", __func__);
1068 			goto done;
1069 		}
1070 		key = psk;
1071 		keytype = 0;
1072 		break;
1073 	default:
1074 		if (id == NULL) {
1075 			log_debug("%s: no cert found", __func__);
1076 			goto done;
1077 		}
1078 		key = ibuf_data(id->id_buf);
1079 		keylen = ibuf_size(id->id_buf);
1080 		keytype = id->id_type;
1081 		break;
1082 	}
1083 
1084 	if (dsa_setkey(dsa, key, keylen, keytype) == NULL ||
1085 	    dsa_init(dsa, NULL, 0) != 0 ||
1086 	    dsa_update(dsa, ibuf_data(authmsg), ibuf_size(authmsg))) {
1087 		log_debug("%s: failed to compute digital signature", __func__);
1088 		goto done;
1089 	}
1090 
1091 	ibuf_release(sa->sa_localauth.id_buf);
1092 	sa->sa_localauth.id_buf = NULL;
1093 
1094 	if ((buf = ibuf_new(NULL, dsa_length(dsa))) == NULL) {
1095 		log_debug("%s: failed to get auth buffer", __func__);
1096 		goto done;
1097 	}
1098 
1099 	if ((siglen = dsa_sign_final(dsa,
1100 	    ibuf_data(buf), ibuf_size(buf))) < 0) {
1101 		log_debug("%s: failed to create auth signature", __func__);
1102 		ibuf_release(buf);
1103 		goto done;
1104 	}
1105 
1106 	if (ibuf_setsize(buf, siglen) < 0) {
1107 		log_debug("%s: failed to set auth signature size to %zd",
1108 		    __func__, siglen);
1109 		ibuf_release(buf);
1110 		goto done;
1111 	}
1112 
1113 	sa->sa_localauth.id_type = auth->auth_method;
1114 	sa->sa_localauth.id_buf = buf;
1115 
1116 	ret = 0;
1117  done:
1118 	free(psk);
1119 	dsa_free(dsa);
1120 
1121 	return (ret);
1122 }
1123 
1124 int
1125 ikev2_msg_frompeer(struct iked_message *msg)
1126 {
1127 	struct iked_sa		*sa = msg->msg_sa;
1128 	struct ike_header	*hdr;
1129 
1130 	msg = msg->msg_parent;
1131 
1132 	if (sa == NULL ||
1133 	    (hdr = ibuf_seek(msg->msg_data, 0, sizeof(*hdr))) == NULL)
1134 		return (0);
1135 
1136 	if (!sa->sa_hdr.sh_initiator &&
1137 	    (hdr->ike_flags & IKEV2_FLAG_INITIATOR))
1138 		return (1);
1139 	else if (sa->sa_hdr.sh_initiator &&
1140 	    (hdr->ike_flags & IKEV2_FLAG_INITIATOR) == 0)
1141 		return (1);
1142 
1143 	return (0);
1144 }
1145 
1146 struct iked_socket *
1147 ikev2_msg_getsocket(struct iked *env, int af, int natt)
1148 {
1149 	switch (af) {
1150 	case AF_INET:
1151 		return (env->sc_sock4[natt ? 1 : 0]);
1152 	case AF_INET6:
1153 		return (env->sc_sock6[natt ? 1 : 0]);
1154 	}
1155 
1156 	log_debug("%s: af socket %d not available", __func__, af);
1157 	return (NULL);
1158 }
1159 
1160 void
1161 ikev2_msg_prevail(struct iked *env, struct iked_msgqueue *queue,
1162     struct iked_message *msg)
1163 {
1164 	struct iked_message	*m, *mtmp;
1165 
1166 	TAILQ_FOREACH_SAFE(m, queue, msg_entry, mtmp) {
1167 		if (m->msg_msgid < msg->msg_msgid)
1168 			ikev2_msg_dispose(env, queue, m);
1169 	}
1170 }
1171 
1172 void
1173 ikev2_msg_dispose(struct iked *env, struct iked_msgqueue *queue,
1174     struct iked_message *msg)
1175 {
1176 	TAILQ_REMOVE(queue, msg, msg_entry);
1177 	timer_del(env, &msg->msg_timer);
1178 	ikev2_msg_cleanup(env, msg);
1179 	free(msg);
1180 }
1181 
1182 void
1183 ikev2_msg_flushqueue(struct iked *env, struct iked_msgqueue *queue)
1184 {
1185 	struct iked_message	*m = NULL;
1186 
1187 	while ((m = TAILQ_FIRST(queue)) != NULL)
1188 		ikev2_msg_dispose(env, queue, m);
1189 }
1190 
1191 struct iked_message *
1192 ikev2_msg_lookup(struct iked *env, struct iked_msgqueue *queue,
1193     struct iked_message *msg, struct ike_header *hdr)
1194 {
1195 	struct iked_message	*m = NULL;
1196 
1197 	TAILQ_FOREACH(m, queue, msg_entry) {
1198 		if (m->msg_msgid == msg->msg_msgid &&
1199 		    m->msg_exchange == hdr->ike_exchange)
1200 			break;
1201 	}
1202 
1203 	return (m);
1204 }
1205 
1206 void
1207 ikev2_msg_lookup_dispose_all(struct iked *env, struct iked_msgqueue *queue,
1208     struct iked_message *msg, struct ike_header *hdr)
1209 {
1210 	struct iked_message	*m = NULL, *tmp = NULL;
1211 
1212 	TAILQ_FOREACH_SAFE(m, queue, msg_entry, tmp) {
1213 		if (m->msg_msgid == msg->msg_msgid &&
1214 		    m->msg_exchange == hdr->ike_exchange) {
1215 			TAILQ_REMOVE(queue, m, msg_entry);
1216 			timer_del(env, &m->msg_timer);
1217 			ikev2_msg_cleanup(env, m);
1218 			free(m);
1219 		}
1220 	}
1221 }
1222 
1223 int
1224 ikev2_msg_lookup_retransmit_all(struct iked *env, struct iked_msgqueue *queue,
1225     struct iked_message *msg, struct ike_header *hdr, struct iked_sa *sa)
1226 {
1227 	struct iked_message	*m = NULL, *tmp = NULL;
1228 	int count = 0;
1229 
1230 	TAILQ_FOREACH_SAFE(m, queue, msg_entry, tmp) {
1231 		if (m->msg_msgid == msg->msg_msgid &&
1232 		    m->msg_exchange == hdr->ike_exchange) {
1233 			if (ikev2_msg_retransmit_response(env, sa, m))
1234 				return -1;
1235 			count++;
1236 		}
1237 	}
1238 	return count;
1239 }
1240 
1241 int
1242 ikev2_msg_retransmit_response(struct iked *env, struct iked_sa *sa,
1243     struct iked_message *msg)
1244 {
1245 	if (sendtofrom(msg->msg_fd, ibuf_data(msg->msg_data),
1246 	    ibuf_size(msg->msg_data), 0,
1247 	    (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen,
1248 	    (struct sockaddr *)&msg->msg_local, msg->msg_locallen) == -1) {
1249 		log_warn("%s: sendtofrom", __func__);
1250 		return (-1);
1251 	}
1252 	log_info("%sretransmit %s res %u local %s peer %s",
1253 	    SPI_SA(sa, NULL),
1254 	    print_map(msg->msg_exchange, ikev2_exchange_map),
1255 	    msg->msg_msgid,
1256 	    print_host((struct sockaddr *)&msg->msg_local, NULL, 0),
1257 	    print_host((struct sockaddr *)&msg->msg_peer, NULL, 0));
1258 
1259 	timer_add(env, &msg->msg_timer, IKED_RESPONSE_TIMEOUT);
1260 	return (0);
1261 }
1262 
1263 void
1264 ikev2_msg_response_timeout(struct iked *env, void *arg)
1265 {
1266 	struct iked_message	*msg = arg;
1267 	struct iked_sa		*sa = msg->msg_sa;
1268 
1269 	ikev2_msg_dispose(env, &sa->sa_responses, msg);
1270 }
1271 
1272 void
1273 ikev2_msg_retransmit_timeout(struct iked *env, void *arg)
1274 {
1275 	struct iked_message	*msg = arg;
1276 	struct iked_sa		*sa = msg->msg_sa;
1277 
1278 	if (msg->msg_tries < IKED_RETRANSMIT_TRIES) {
1279 		if (sendtofrom(msg->msg_fd, ibuf_data(msg->msg_data),
1280 		    ibuf_size(msg->msg_data), 0,
1281 		    (struct sockaddr *)&msg->msg_peer, msg->msg_peerlen,
1282 		    (struct sockaddr *)&msg->msg_local,
1283 		    msg->msg_locallen) == -1) {
1284 			log_warn("%s: sendtofrom", __func__);
1285 			ikev2_ike_sa_setreason(sa, "retransmit failed");
1286 			sa_free(env, sa);
1287 			return;
1288 		}
1289 		/* Exponential timeout */
1290 		timer_add(env, &msg->msg_timer,
1291 		    IKED_RETRANSMIT_TIMEOUT * (2 << (msg->msg_tries++)));
1292 		log_info("%sretransmit %d %s req %u peer %s local %s",
1293 		    SPI_SA(sa, NULL),
1294 		    msg->msg_tries,
1295 		    print_map(msg->msg_exchange, ikev2_exchange_map),
1296 		    msg->msg_msgid,
1297 		    print_host((struct sockaddr *)&msg->msg_peer, NULL, 0),
1298 		    print_host((struct sockaddr *)&msg->msg_local, NULL, 0));
1299 	} else {
1300 		log_debug("%s: retransmit limit reached for req %u",
1301 		    __func__, msg->msg_msgid);
1302 		ikev2_ike_sa_setreason(sa, "retransmit limit reached");
1303 		sa_free(env, sa);
1304 	}
1305 }
1306