1 /* $OpenBSD: util.c,v 1.13 2011/05/27 12:01:02 reyk Exp $ */ 2 /* $vantronix: util.c,v 1.39 2010/06/02 12:22:58 reyk Exp $ */ 3 4 /* 5 * Copyright (c) 2010 Reyk Floeter <reyk@vantronix.net> 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> 21 #include <sys/queue.h> 22 #include <sys/socket.h> 23 #include <sys/uio.h> 24 25 #include <net/if.h> 26 #include <netinet/in_systm.h> 27 #include <netinet/in.h> 28 #include <netinet/ip.h> 29 #include <netinet/tcp.h> 30 #include <arpa/inet.h> 31 #include <netdb.h> 32 33 #include <stdio.h> 34 #include <stdlib.h> 35 #include <unistd.h> 36 #include <string.h> 37 #include <errno.h> 38 #include <fcntl.h> 39 #include <ctype.h> 40 #include <event.h> 41 42 #include "iked.h" 43 #include "ikev2.h" 44 45 void 46 socket_set_blockmode(int fd, enum blockmodes bm) 47 { 48 int flags; 49 50 if ((flags = fcntl(fd, F_GETFL, 0)) == -1) 51 fatal("fcntl F_GETFL"); 52 53 if (bm == BM_NONBLOCK) 54 flags |= O_NONBLOCK; 55 else 56 flags &= ~O_NONBLOCK; 57 58 if ((flags = fcntl(fd, F_SETFL, flags)) == -1) 59 fatal("fcntl F_SETFL"); 60 } 61 62 int 63 socket_af(struct sockaddr *sa, in_port_t port) 64 { 65 errno = 0; 66 switch (sa->sa_family) { 67 case AF_INET: 68 ((struct sockaddr_in *)sa)->sin_port = port; 69 ((struct sockaddr_in *)sa)->sin_len = 70 sizeof(struct sockaddr_in); 71 break; 72 case AF_INET6: 73 ((struct sockaddr_in6 *)sa)->sin6_port = port; 74 ((struct sockaddr_in6 *)sa)->sin6_len = 75 sizeof(struct sockaddr_in6); 76 break; 77 default: 78 errno = EPFNOSUPPORT; 79 return (-1); 80 } 81 82 return (0); 83 } 84 85 in_port_t 86 socket_getport(struct sockaddr_storage *ss) 87 { 88 switch (ss->ss_family) { 89 case AF_INET: 90 return (ntohs(((struct sockaddr_in *)ss)->sin_port)); 91 case AF_INET6: 92 return (ntohs(((struct sockaddr_in6 *)ss)->sin6_port)); 93 default: 94 return (0); 95 } 96 97 /* NOTREACHED */ 98 return (0); 99 } 100 101 int 102 socket_bypass(int s, struct sockaddr *sa) 103 { 104 int v, *a; 105 int a4[] = { 106 IPPROTO_IP, 107 IP_AUTH_LEVEL, 108 IP_ESP_TRANS_LEVEL, 109 IP_ESP_NETWORK_LEVEL, 110 #ifdef IPV6_IPCOMP_LEVEL 111 IP_IPCOMP_LEVEL 112 #endif 113 }; 114 int a6[] = { 115 IPPROTO_IPV6, 116 IPV6_AUTH_LEVEL, 117 IPV6_ESP_TRANS_LEVEL, 118 IPV6_ESP_NETWORK_LEVEL, 119 #ifdef IPV6_IPCOMP_LEVEL 120 IPV6_IPCOMP_LEVEL 121 #endif 122 }; 123 124 switch (sa->sa_family) { 125 case AF_INET: 126 a = a4; 127 break; 128 case AF_INET6: 129 a = a6; 130 break; 131 default: 132 log_warn("%s: invalid address family", __func__); 133 return (-1); 134 } 135 136 v = IPSEC_LEVEL_BYPASS; 137 if (setsockopt(s, a[0], a[1], &v, sizeof(v)) == -1) { 138 log_warn("%s: AUTH_LEVEL", __func__); 139 return (-1); 140 } 141 if (setsockopt(s, a[0], a[2], &v, sizeof(v)) == -1) { 142 log_warn("%s: ESP_TRANS_LEVEL", __func__); 143 return (-1); 144 } 145 if (setsockopt(s, a[0], a[3], &v, sizeof(v)) == -1) { 146 log_warn("%s: ESP_NETWORK_LEVEL", __func__); 147 return (-1); 148 } 149 #ifdef IP_IPCOMP_LEVEL 150 if (setsockopt(s, a[0], a[4], &v, sizeof(v)) == -1) { 151 log_warn("%s: IPCOMP_LEVEL", __func__); 152 return (-1); 153 } 154 #endif 155 156 return (0); 157 } 158 159 int 160 udp_bind(struct sockaddr *sa, in_port_t port) 161 { 162 int s, val; 163 164 if (socket_af(sa, port) == -1) { 165 log_warn("%s: failed to set UDP port", __func__); 166 return (-1); 167 } 168 169 if ((s = socket(sa->sa_family, SOCK_DGRAM, IPPROTO_UDP)) == -1) { 170 log_warn("%s: failed to get UDP socket", __func__); 171 return (-1); 172 } 173 174 /* Skip IPsec processing (don't encrypt) for IKE messages */ 175 if (socket_bypass(s, sa) == -1) { 176 log_warn("%s: failed to bypass IPsec on IKE socket", 177 __func__); 178 goto bad; 179 } 180 181 val = 1; 182 if (setsockopt(s, SOL_SOCKET, SO_REUSEPORT, &val, sizeof(int)) == -1) { 183 log_warn("%s: failed to set reuseport", __func__); 184 goto bad; 185 } 186 val = 1; 187 if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(int)) == -1) { 188 log_warn("%s: failed to set reuseaddr", __func__); 189 goto bad; 190 } 191 192 if (sa->sa_family == AF_INET) { 193 val = 1; 194 if (setsockopt(s, IPPROTO_IP, IP_RECVDSTADDR, 195 &val, sizeof(int)) == -1) { 196 log_warn("%s: failed to set IPv4 packet info", 197 __func__); 198 goto bad; 199 } 200 } else { 201 val = 1; 202 if (setsockopt(s, IPPROTO_IPV6, IPV6_RECVPKTINFO, 203 &val, sizeof(int)) == -1) { 204 log_warn("%s: failed to set IPv6 packet info", 205 __func__); 206 goto bad; 207 } 208 } 209 210 if (bind(s, sa, sa->sa_len) == -1) { 211 log_warn("%s: failed to bind UDP socket", __func__); 212 goto bad; 213 } 214 215 return (s); 216 bad: 217 close(s); 218 return (-1); 219 } 220 221 int 222 sockaddr_cmp(struct sockaddr *a, struct sockaddr *b, int prefixlen) 223 { 224 struct sockaddr_in *a4, *b4; 225 struct sockaddr_in6 *a6, *b6; 226 u_int32_t av[4], bv[4], mv[4]; 227 228 if (a->sa_family == AF_UNSPEC || b->sa_family == AF_UNSPEC) 229 return (0); 230 else if (a->sa_family > b->sa_family) 231 return (1); 232 else if (a->sa_family < b->sa_family) 233 return (-1); 234 235 if (prefixlen == -1) 236 memset(&mv, 0xff, sizeof(mv)); 237 238 switch (a->sa_family) { 239 case AF_INET: 240 a4 = (struct sockaddr_in *)a; 241 b4 = (struct sockaddr_in *)b; 242 243 av[0] = a4->sin_addr.s_addr; 244 bv[0] = b4->sin_addr.s_addr; 245 if (prefixlen != -1) 246 mv[0] = prefixlen2mask(prefixlen); 247 248 if ((av[0] & mv[0]) > (bv[0] & mv[0])) 249 return (1); 250 if ((av[0] & mv[0]) < (bv[0] & mv[0])) 251 return (-1); 252 break; 253 case AF_INET6: 254 a6 = (struct sockaddr_in6 *)a; 255 b6 = (struct sockaddr_in6 *)b; 256 257 memcpy(&av, &a6->sin6_addr.s6_addr, 16); 258 memcpy(&bv, &b6->sin6_addr.s6_addr, 16); 259 if (prefixlen != -1) 260 prefixlen2mask6(prefixlen, mv); 261 262 if ((av[3] & mv[3]) > (bv[3] & mv[3])) 263 return (1); 264 if ((av[3] & mv[3]) < (bv[3] & mv[3])) 265 return (-1); 266 if ((av[2] & mv[2]) > (bv[2] & mv[2])) 267 return (1); 268 if ((av[2] & mv[2]) < (bv[2] & mv[2])) 269 return (-1); 270 if ((av[1] & mv[1]) > (bv[1] & mv[1])) 271 return (1); 272 if ((av[1] & mv[1]) < (bv[1] & mv[1])) 273 return (-1); 274 if ((av[0] & mv[0]) > (bv[0] & mv[0])) 275 return (1); 276 if ((av[0] & mv[0]) < (bv[0] & mv[0])) 277 return (-1); 278 break; 279 } 280 281 return (0); 282 } 283 284 ssize_t 285 recvfromto(int s, void *buf, size_t len, int flags, struct sockaddr *from, 286 socklen_t *fromlen, struct sockaddr *to, socklen_t *tolen) 287 { 288 struct iovec iov; 289 struct msghdr msg; 290 struct cmsghdr *cmsg; 291 struct in6_pktinfo *pkt6; 292 struct sockaddr_in *in; 293 struct sockaddr_in6 *in6; 294 ssize_t ret; 295 union { 296 struct cmsghdr hdr; 297 char buf[CMSG_SPACE(sizeof(struct sockaddr_storage))]; 298 } cmsgbuf; 299 300 bzero(&msg, sizeof(msg)); 301 bzero(&cmsgbuf.buf, sizeof(cmsgbuf.buf)); 302 303 iov.iov_base = buf; 304 iov.iov_len = len; 305 msg.msg_iov = &iov; 306 msg.msg_iovlen = 1; 307 msg.msg_name = from; 308 msg.msg_namelen = *fromlen; 309 msg.msg_control = &cmsgbuf.buf; 310 msg.msg_controllen = sizeof(cmsgbuf.buf); 311 312 if ((ret = recvmsg(s, &msg, 0)) == -1) 313 return (-1); 314 315 *fromlen = from->sa_len; 316 *tolen = 0; 317 318 if (getsockname(s, to, tolen) != 0) 319 *tolen = 0; 320 321 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; 322 cmsg = CMSG_NXTHDR(&msg, cmsg)) { 323 switch (from->sa_family) { 324 case AF_INET: 325 if (cmsg->cmsg_level == IPPROTO_IP && 326 cmsg->cmsg_type == IP_RECVDSTADDR) { 327 in = (struct sockaddr_in *)to; 328 in->sin_family = AF_INET; 329 in->sin_len = *tolen = sizeof(*in); 330 memcpy(&in->sin_addr, CMSG_DATA(cmsg), 331 sizeof(struct in_addr)); 332 } 333 break; 334 case AF_INET6: 335 if (cmsg->cmsg_level == IPPROTO_IPV6 && 336 cmsg->cmsg_type == IPV6_PKTINFO) { 337 in6 = (struct sockaddr_in6 *)to; 338 in6->sin6_family = AF_INET6; 339 in6->sin6_len = *tolen = sizeof(*in6); 340 pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg); 341 memcpy(&in6->sin6_addr, &pkt6->ipi6_addr, 342 sizeof(struct in6_addr)); 343 if (IN6_IS_ADDR_LINKLOCAL(&in6->sin6_addr)) 344 in6->sin6_scope_id = 345 pkt6->ipi6_ifindex; 346 } 347 break; 348 } 349 } 350 351 return (ret); 352 } 353 354 const char * 355 print_spi(u_int64_t spi, int size) 356 { 357 static char buf[IKED_CYCLE_BUFFERS][32]; 358 static int i = 0; 359 char *ptr; 360 361 ptr = buf[i]; 362 363 switch (size) { 364 case 4: 365 snprintf(ptr, 32, "0x%08x", (u_int32_t)spi); 366 break; 367 case 8: 368 snprintf(ptr, 32, "0x%016llx", spi); 369 break; 370 default: 371 snprintf(ptr, 32, "%llu", spi); 372 break; 373 } 374 375 if (++i >= IKED_CYCLE_BUFFERS) 376 i = 0; 377 378 return (ptr); 379 } 380 381 const char * 382 print_map(u_int type, struct iked_constmap *map) 383 { 384 u_int i; 385 static char buf[IKED_CYCLE_BUFFERS][32]; 386 static int idx = 0; 387 const char *name = NULL; 388 389 if (idx >= IKED_CYCLE_BUFFERS) 390 idx = 0; 391 bzero(buf[idx], sizeof(buf[idx])); 392 393 for (i = 0; map[i].cm_name != NULL; i++) { 394 if (map[i].cm_type == type) 395 name = map[i].cm_name; 396 } 397 398 if (name == NULL) 399 snprintf(buf[idx], sizeof(buf[idx]), "<UNKNOWN:%u>", type); 400 else 401 strlcpy(buf[idx], name, sizeof(buf[idx])); 402 403 return (buf[idx++]); 404 } 405 406 void 407 lc_string(char *str) 408 { 409 for (; *str != '\0'; str++) 410 *str = tolower(*str); 411 } 412 413 void 414 print_hex(u_int8_t *buf, off_t offset, size_t length) 415 { 416 u_int i; 417 extern int verbose; 418 419 if (verbose < 2 || !length) 420 return; 421 422 for (i = 0; i < length; i++) { 423 if (i && (i % 4) == 0) { 424 if ((i % 32) == 0) 425 print_debug("\n"); 426 else 427 print_debug(" "); 428 } 429 print_debug("%02x", buf[offset + i]); 430 } 431 print_debug("\n"); 432 } 433 434 void 435 print_hexval(u_int8_t *buf, off_t offset, size_t length) 436 { 437 u_int i; 438 extern int verbose; 439 440 if (verbose < 2 || !length) 441 return; 442 443 print_debug("0x"); 444 for (i = 0; i < length; i++) 445 print_debug("%02x", buf[offset + i]); 446 print_debug("\n"); 447 } 448 449 const char * 450 print_bits(u_short v, char *bits) 451 { 452 static char buf[IKED_CYCLE_BUFFERS][BUFSIZ]; 453 static int idx = 0; 454 u_int i, any = 0, j = 0; 455 char c; 456 457 if (!bits) 458 return (""); 459 460 if (++idx >= IKED_CYCLE_BUFFERS) 461 idx = 0; 462 463 bzero(buf[idx], sizeof(buf[idx])); 464 465 bits++; 466 while ((i = *bits++)) { 467 if (v & (1 << (i-1))) { 468 if (any) { 469 buf[idx][j++] = ','; 470 if (j >= sizeof(buf[idx])) 471 return (buf[idx]); 472 } 473 any = 1; 474 for (; (c = *bits) > 32; bits++) { 475 buf[idx][j++] = tolower(c); 476 if (j >= sizeof(buf[idx])) 477 return (buf[idx]); 478 } 479 } else 480 for (; *bits > 32; bits++) 481 ; 482 } 483 484 return (buf[idx]); 485 } 486 487 u_int8_t 488 mask2prefixlen(struct sockaddr *sa) 489 { 490 struct sockaddr_in *sa_in = (struct sockaddr_in *)sa; 491 in_addr_t ina = sa_in->sin_addr.s_addr; 492 493 if (ina == 0) 494 return (0); 495 else 496 return (33 - ffs(ntohl(ina))); 497 } 498 499 u_int8_t 500 mask2prefixlen6(struct sockaddr *sa) 501 { 502 struct sockaddr_in6 *sa_in6 = (struct sockaddr_in6 *)sa; 503 u_int8_t l = 0, *ap, *ep; 504 505 /* 506 * sin6_len is the size of the sockaddr so substract the offset of 507 * the possibly truncated sin6_addr struct. 508 */ 509 ap = (u_int8_t *)&sa_in6->sin6_addr; 510 ep = (u_int8_t *)sa_in6 + sa_in6->sin6_len; 511 for (; ap < ep; ap++) { 512 /* this "beauty" is adopted from sbin/route/show.c ... */ 513 switch (*ap) { 514 case 0xff: 515 l += 8; 516 break; 517 case 0xfe: 518 l += 7; 519 return (l); 520 case 0xfc: 521 l += 6; 522 return (l); 523 case 0xf8: 524 l += 5; 525 return (l); 526 case 0xf0: 527 l += 4; 528 return (l); 529 case 0xe0: 530 l += 3; 531 return (l); 532 case 0xc0: 533 l += 2; 534 return (l); 535 case 0x80: 536 l += 1; 537 return (l); 538 case 0x00: 539 return (l); 540 default: 541 return (0); 542 } 543 } 544 545 return (l); 546 } 547 548 u_int32_t 549 prefixlen2mask(u_int8_t prefixlen) 550 { 551 if (prefixlen == 0) 552 return (0); 553 554 if (prefixlen > 32) 555 prefixlen = 32; 556 557 return (htonl(0xffffffff << (32 - prefixlen))); 558 } 559 560 struct in6_addr * 561 prefixlen2mask6(u_int8_t prefixlen, u_int32_t *mask) 562 { 563 static struct in6_addr s6; 564 int i; 565 566 if (prefixlen > 128) 567 prefixlen = 128; 568 569 bzero(&s6, sizeof(s6)); 570 for (i = 0; i < prefixlen / 8; i++) 571 s6.s6_addr[i] = 0xff; 572 i = prefixlen % 8; 573 if (i) 574 s6.s6_addr[prefixlen / 8] = 0xff00 >> i; 575 576 memcpy(mask, &s6, sizeof(s6)); 577 578 return (&s6); 579 } 580 581 const char * 582 print_host(struct sockaddr_storage *ss, char *buf, size_t len) 583 { 584 static char sbuf[IKED_CYCLE_BUFFERS][NI_MAXHOST + 7]; 585 static int idx = 0; 586 char pbuf[7]; 587 in_port_t port; 588 589 if (buf == NULL) { 590 buf = sbuf[idx]; 591 len = sizeof(sbuf[idx]); 592 if (++idx >= IKED_CYCLE_BUFFERS) 593 idx = 0; 594 } 595 596 if (ss->ss_family == AF_UNSPEC) { 597 strlcpy(buf, "any", len); 598 return (buf); 599 } 600 601 if (getnameinfo((struct sockaddr *)ss, ss->ss_len, 602 buf, len, NULL, 0, NI_NUMERICHOST) != 0) { 603 buf[0] = '\0'; 604 return (NULL); 605 } 606 607 if ((port = socket_getport(ss)) != 0) { 608 snprintf(pbuf, sizeof(pbuf), ":%d", port); 609 (void)strlcat(buf, pbuf, len); 610 } 611 612 return (buf); 613 } 614 615 char * 616 get_string(u_int8_t *ptr, size_t len) 617 { 618 size_t i; 619 char *str; 620 621 for (i = 0; i < len; i++) 622 if (!isprint((char)ptr[i])) 623 break; 624 625 if ((str = calloc(1, i + 1)) == NULL) 626 return (NULL); 627 memcpy(str, ptr, i); 628 629 return (str); 630 } 631 632 const char * 633 print_proto(u_int8_t proto) 634 { 635 struct protoent *p; 636 static char buf[IKED_CYCLE_BUFFERS][BUFSIZ]; 637 static int idx = 0; 638 639 if (idx >= IKED_CYCLE_BUFFERS) 640 idx = 0; 641 642 if ((p = getprotobynumber(proto)) != NULL) 643 strlcpy(buf[idx], p->p_name, sizeof(buf[idx])); 644 else 645 snprintf(buf[idx], sizeof(buf), "%u", proto); 646 647 648 return (buf[idx++]); 649 } 650 651 int 652 expand_string(char *label, size_t len, const char *srch, const char *repl) 653 { 654 char *tmp; 655 char *p, *q; 656 657 if ((tmp = calloc(1, len)) == NULL) { 658 log_debug("expand_string: calloc"); 659 return (-1); 660 } 661 p = q = label; 662 while ((q = strstr(p, srch)) != NULL) { 663 *q = '\0'; 664 if ((strlcat(tmp, p, len) >= len) || 665 (strlcat(tmp, repl, len) >= len)) { 666 log_debug("expand_string: string too long"); 667 return (-1); 668 } 669 q += strlen(srch); 670 p = q; 671 } 672 if (strlcat(tmp, p, len) >= len) { 673 log_debug("expand_string: string too long"); 674 return (-1); 675 } 676 strlcpy(label, tmp, len); /* always fits */ 677 free(tmp); 678 679 return (0); 680 } 681 682 u_int8_t * 683 string2unicode(const char *ascii, size_t *outlen) 684 { 685 u_int8_t *uc = NULL; 686 size_t i, len = strlen(ascii); 687 688 if ((uc = calloc(1, (len * 2) + 2)) == NULL) 689 return (NULL); 690 691 for (i = 0; i < len; i++) { 692 /* XXX what about the byte order? */ 693 uc[i * 2] = ascii[i]; 694 } 695 *outlen = len * 2; 696 697 return (uc); 698 } 699