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