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