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