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