ipsec-tools/racoon/sockmisc.c

   1 /* $Id: sockmisc.c,v 1.17.4.4 2005/10/04 09:54:27 manubsd Exp $ */
   2
   3 /*
   4  * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project.
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  * 3. Neither the name of the project nor the names of its contributors
  16  *    may be used to endorse or promote products derived from this software
  17  *    without specific prior written permission.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
  20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22  * ARE DISCLAIMED.  IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
  23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  25  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  26  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  28  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  29  * SUCH DAMAGE.
  30  */
  31
  32 #include "config.h"
  33
  34 #include <sys/types.h>
  35 #include <sys/param.h>
  36 #include <sys/socket.h>
  37 #include <sys/uio.h>
  38
  39 #include <netinet/in.h>
  40 #ifndef HAVE_NETINET6_IPSEC
  41 #include <netinet/ipsec.h>
  42 #else
  43 #include <netinet6/ipsec.h>
  44 #endif
  45
  46 #if defined(INET6) && !defined(INET6_ADVAPI) && \
  47         defined(IP_RECVDSTADDR) && !defined(IPV6_RECVDSTADDR)
  48 #define IPV6_RECVDSTADDR IP_RECVDSTADDR
  49 #endif
  50
  51 #include <stdlib.h>
  52 #include <stdio.h>
  53 #include <string.h>
  54 #include <errno.h>
  55 #ifdef HAVE_UNISTD_H
  56 #include <unistd.h>
  57 #endif
  58 #include <fcntl.h>
  59
  60 #include "var.h"
  61 #include "misc.h"
  62 #include "plog.h"
  63 #include "sockmisc.h"
  64 #include "debug.h"
  65 #include "gcmalloc.h"
  66 #include "debugrm.h"
  67 #include "libpfkey.h"
  68
  69 #ifndef IP_IPSEC_POLICY
  70 #define IP_IPSEC_POLICY 16      /* XXX: from linux/in.h */
  71 #endif
  72
  73 #ifndef IPV6_IPSEC_POLICY
  74 #define IPV6_IPSEC_POLICY 34    /* XXX: from linux/???.h per
  75                                    "Tom Lendacky" <toml@us.ibm.com> */
  76 #endif
  77
  78 const int niflags = 0;
  79
  80 /*
  81  * compare two sockaddr without port number.
  82  * OUT: 0: equal.
  83  *      1: not equal.
  84  */
  85 int
  86 cmpsaddrwop(addr1, addr2)
  87         const struct sockaddr_storage *addr1;
  88         const struct sockaddr_storage *addr2;
  89 {
  90         caddr_t sa1, sa2;
  91
  92         if (addr1 == 0 && addr2 == 0)
  93                 return 0;
  94         if (addr1 == 0 || addr2 == 0)
  95                 return 1;
  96
  97         if (addr1->ss_len != addr2->ss_len
  98          || addr1->ss_family != addr2->ss_family)
  99                 return 1;
 100         switch (addr1->ss_family) {
 101         case AF_INET:
 102                 sa1 = (caddr_t)&((struct sockaddr_in *)addr1)->sin_addr;
 103                 sa2 = (caddr_t)&((struct sockaddr_in *)addr2)->sin_addr;
 104                 if (memcmp(sa1, sa2, sizeof(struct in_addr)) != 0)
 105                         return 1;
 106                 break;
 107 #ifdef INET6
 108         case AF_INET6:
 109                 sa1 = (caddr_t)&((struct sockaddr_in6 *)addr1)->sin6_addr;
 110                 sa2 = (caddr_t)&((struct sockaddr_in6 *)addr2)->sin6_addr;
 111                 if (memcmp(sa1, sa2, sizeof(struct in6_addr)) != 0)
 112                         return 1;
 113                 if (((struct sockaddr_in6 *)addr1)->sin6_scope_id !=
 114                     ((struct sockaddr_in6 *)addr2)->sin6_scope_id)
 115                         return 1;
 116                 break;
 117 #endif
 118         default:
 119                 return 1;
 120         }
 121
 122         return 0;
 123 }
 124
 125 /*
 126  * compare two sockaddr without port number using prefix.
 127  * OUT: 0: equal.
 128  *      1: not equal.
 129  */
 130 int
 131 cmpsaddrwop_withprefix(const struct sockaddr_storage *addr1, const struct sockaddr_storage *addr2, int prefix)
 132 {
 133     u_int32_t mask;
 134     int i;
 135
 136         if (addr1 == 0 && addr2 == 0)
 137                 return 0;
 138         if (addr1 == 0 || addr2 == 0)
 139                 return 1;
 140
 141         if (addr1->ss_len != addr2->ss_len
 142         || addr1->ss_family != addr2->ss_family)
 143                 return 1;
 144         switch (addr1->ss_family) {
 145         case AF_INET:
 146             mask = ~0;
 147             mask <<= 32-prefix;
 148             if ((((struct sockaddr_in *)addr1)->sin_addr.s_addr & htonl(mask)) !=
 149                 (((struct sockaddr_in *)addr2)->sin_addr.s_addr & htonl(mask)))
 150                 return 1;
 151             break;
 152 #ifdef INET6
 153         case AF_INET6:
 154             for (i = 0; i < 4; i++) {
 155                 if (prefix >= 32) {
 156                     mask = ~0;
 157                     prefix -= 32;
 158                 } else if (prefix == 0)
 159                     mask = 0;
 160                 else {
 161                     mask = ~0;
 162                     mask <<= 32-prefix;
 163                     prefix = 0;
 164                 }
 165                 if ((((struct sockaddr_in6 *)addr1)->sin6_addr.__u6_addr.__u6_addr32[i] & htonl(mask)) !=
 166                     (((struct sockaddr_in6 *)addr2)->sin6_addr.__u6_addr.__u6_addr32[i] & htonl(mask)))
 167                     return 1;
 168             }
 169             if (((struct sockaddr_in6 *)addr1)->sin6_scope_id !=
 170                 ((struct sockaddr_in6 *)addr2)->sin6_scope_id)
 171                 return 1;
 172             break;
 173 #endif
 174         default:
 175             return 1;
 176         }
 177
 178         return 0;
 179 }
 180
 181
 182 /*
 183  * compare two sockaddr with port, taking care wildcard.
 184  * addr1 is a subject address, addr2 is in a database entry.
 185  * OUT: 0: equal.
 186  *      1: not equal.
 187  */
 188 int
 189 cmpsaddrwild(addr1, addr2)
 190         const struct sockaddr_storage *addr1;
 191         const struct sockaddr_storage *addr2;
 192 {
 193         caddr_t sa1, sa2;
 194         u_short port1, port2;
 195
 196         if (addr1 == 0 && addr2 == 0)
 197                 return 0;
 198         if (addr1 == 0 || addr2 == 0)
 199                 return 1;
 200
 201         if (addr1->ss_len != addr2->ss_len
 202          || addr1->ss_family != addr2->ss_family)
 203                 return 1;
 204
 205         switch (addr1->ss_family) {
 206         case AF_INET:
 207                 sa1 = (caddr_t)&((struct sockaddr_in *)addr1)->sin_addr;
 208                 sa2 = (caddr_t)&((struct sockaddr_in *)addr2)->sin_addr;
 209                 port1 = ((struct sockaddr_in *)addr1)->sin_port;
 210                 port2 = ((struct sockaddr_in *)addr2)->sin_port;
 211                 if (!(port1 == IPSEC_PORT_ANY ||
 212                       port2 == IPSEC_PORT_ANY ||
 213                       port1 == port2))
 214                         return 1;
 215                 if (memcmp(sa1, sa2, sizeof(struct in_addr)) != 0)
 216                         return 1;
 217                 break;
 218 #ifdef INET6
 219         case AF_INET6:
 220                 sa1 = (caddr_t)&((struct sockaddr_in6 *)addr1)->sin6_addr;
 221                 sa2 = (caddr_t)&((struct sockaddr_in6 *)addr2)->sin6_addr;
 222                 port1 = ((struct sockaddr_in6 *)addr1)->sin6_port;
 223                 port2 = ((struct sockaddr_in6 *)addr2)->sin6_port;
 224                 if (!(port1 == IPSEC_PORT_ANY ||
 225                       port2 == IPSEC_PORT_ANY ||
 226                       port1 == port2))
 227                         return 1;
 228                 if (memcmp(sa1, sa2, sizeof(struct in6_addr)) != 0)
 229                         return 1;
 230                 if (((struct sockaddr_in6 *)addr1)->sin6_scope_id !=
 231                     ((struct sockaddr_in6 *)addr2)->sin6_scope_id)
 232                         return 1;
 233                 break;
 234 #endif
 235         default:
 236                 return 1;
 237         }
 238
 239         return 0;
 240 }
 241
 242 /*
 243  * compare two sockaddr with strict match on port.
 244  * OUT: 0: equal.
 245  *      1: not equal.
 246  */
 247 int
 248 cmpsaddrstrict(addr1, addr2)
 249         const struct sockaddr_storage *addr1;
 250         const struct sockaddr_storage *addr2;
 251 {
 252         caddr_t sa1, sa2;
 253         u_short port1, port2;
 254
 255         if (addr1 == 0 && addr2 == 0)
 256                 return 0;
 257         if (addr1 == 0 || addr2 == 0)
 258                 return 1;
 259
 260         if (addr1->ss_len != addr2->ss_len
 261          || addr1->ss_family != addr2->ss_family)
 262                 return 1;
 263
 264         switch (addr1->ss_family) {
 265         case AF_INET:
 266                 sa1 = (caddr_t)&((struct sockaddr_in *)addr1)->sin_addr;
 267                 sa2 = (caddr_t)&((struct sockaddr_in *)addr2)->sin_addr;
 268                 port1 = ((struct sockaddr_in *)addr1)->sin_port;
 269                 port2 = ((struct sockaddr_in *)addr2)->sin_port;
 270                 if (port1 != port2)
 271                         return 1;
 272                 if (memcmp(sa1, sa2, sizeof(struct in_addr)) != 0)
 273                         return 1;
 274                 break;
 275 #ifdef INET6
 276         case AF_INET6:
 277                 sa1 = (caddr_t)&((struct sockaddr_in6 *)addr1)->sin6_addr;
 278                 sa2 = (caddr_t)&((struct sockaddr_in6 *)addr2)->sin6_addr;
 279                 port1 = ((struct sockaddr_in6 *)addr1)->sin6_port;
 280                 port2 = ((struct sockaddr_in6 *)addr2)->sin6_port;
 281                 if (port1 != port2)
 282                         return 1;
 283                 if (memcmp(sa1, sa2, sizeof(struct in6_addr)) != 0)
 284                         return 1;
 285                 if (((struct sockaddr_in6 *)addr1)->sin6_scope_id !=
 286                     ((struct sockaddr_in6 *)addr2)->sin6_scope_id)
 287                         return 1;
 288                 break;
 289 #endif
 290         default:
 291                 return 1;
 292         }
 293
 294         return 0;
 295 }
 296
 297 /*
 298  * compare two sockaddr with strict match on port using prefix.
 299  * OUT: 0: equal.
 300  *      1: not equal.
 301  */
 302 int
 303 cmpsaddrstrict_withprefix(const struct sockaddr_storage *addr1, const struct sockaddr_storage *addr2, int prefix)
 304 {
 305         u_short port1, port2;
 306     u_int32_t mask;
 307     int i;
 308
 309         if (addr1 == 0 && addr2 == 0)
 310                 return 0;
 311         if (addr1 == 0 || addr2 == 0)
 312                 return 1;
 313
 314         if (addr1->ss_len != addr2->ss_len
 315         || addr1->ss_family != addr2->ss_family)
 316                 return 1;
 317
 318         switch (addr1->ss_family) {
 319         case AF_INET:
 320             port1 = ((struct sockaddr_in *)addr1)->sin_port;
 321             port2 = ((struct sockaddr_in *)addr2)->sin_port;
 322             if (port1 != port2)
 323                 return 1;
 324             mask = ~0;
 325             mask <<= 32-prefix;
 326             if ((((struct sockaddr_in *)addr1)->sin_addr.s_addr & htonl(mask)) !=
 327                 (((struct sockaddr_in *)addr2)->sin_addr.s_addr & htonl(mask)))
 328                 return 1;
 329             break;
 330 #ifdef INET6
 331         case AF_INET6:
 332             port1 = ((struct sockaddr_in6 *)addr1)->sin6_port;
 333             port2 = ((struct sockaddr_in6 *)addr2)->sin6_port;
 334             if (port1 != port2)
 335                 return 1;
 336             for (i = 0; i < 4; i++) {
 337                 if (prefix >= 32) {
 338                     mask = ~0;
 339                     prefix -= 32;
 340                 } else if (prefix == 0)
 341                     mask = 0;
 342                 else {
 343                     mask = ~0;
 344                     mask <<= 32-prefix;
 345                     prefix = 0;
 346                 }
 347                 if ((((struct sockaddr_in6 *)addr1)->sin6_addr.__u6_addr.__u6_addr32[i] & htonl(mask)) !=
 348                     (((struct sockaddr_in6 *)addr2)->sin6_addr.__u6_addr.__u6_addr32[i] & htonl(mask)))
 349                     return 1;
 350             }
 351             if (((struct sockaddr_in6 *)addr1)->sin6_scope_id !=
 352                 ((struct sockaddr_in6 *)addr2)->sin6_scope_id)
 353                 return 1;
 354             break;
 355 #endif
 356         default:
 357             return 1;
 358         }
 359
 360         return 0;
 361 }
 362
 363
 364 /* get local address against the destination. */
 365 struct sockaddr_storage *
 366 getlocaladdr(remote)
 367         struct sockaddr *remote;
 368 {
 369         struct sockaddr_storage *local;
 370         u_int local_len = sizeof(struct sockaddr);
 371         int s;  /* for dummy connection */
 372
 373         /* allocate buffer */
 374         if ((local = racoon_calloc(1, local_len)) == NULL) {
 375                 plog(LLV_ERROR, LOCATION, NULL,
 376                         "failed to get address buffer.\n");
 377                 goto err;
 378         }
 379
 380         /* get real interface received packet */
 381         if ((s = socket(remote->sa_family, SOCK_DGRAM, 0)) < 0) {
 382                 plog(LLV_ERROR, LOCATION, NULL,
 383                         "socket (%s)\n", strerror(errno));
 384                 goto err;
 385         }
 386
 387         if (fcntl(s, F_SETFL, O_NONBLOCK) == -1) {
 388                 plog(LLV_ERROR, LOCATION, NULL,
 389                          "failed to put localaddr socket in non-blocking mode\n");
 390         }
 391
 392         setsockopt_bypass(s, remote->sa_family);
 393
 394         if (connect(s, remote, sysdep_sa_len(remote)) < 0) {
 395                 plog(LLV_ERROR, LOCATION, NULL,
 396                         "connect (%s)\n", strerror(errno));
 397                 close(s);
 398                 goto err;
 399         }
 400
 401         if (getsockname(s, (struct sockaddr *)local, &local_len) < 0) {
 402                 plog(LLV_ERROR, LOCATION, NULL,
 403                         "getsockname (%s)\n", strerror(errno));
 404                 close(s);
 405                 return NULL;
 406         }
 407
 408         close(s);
 409         return local;
 410
 411     err:
 412         if (local != NULL)
 413                 racoon_free(local);
 414         return NULL;
 415 }
 416
 417 /*
 418  * Receive packet, with src/dst information.  It is assumed that necessary
 419  * setsockopt() have already performed on socket.
 420  */
 421 int
 422 recvfromto(s, buf, buflen, flags, from, fromlen, to, tolen)
 423         int s;
 424         void *buf;
 425         size_t buflen;
 426         int flags;
 427         struct sockaddr_storage *from;
 428         socklen_t *fromlen;
 429         struct sockaddr_storage *to;
 430         u_int *tolen;
 431 {
 432         int otolen;
 433         ssize_t len;
 434         struct sockaddr_storage ss;
 435         struct msghdr m;
 436         struct cmsghdr *cm, *cm_prev;
 437         struct iovec iov[2];
 438     u_int32_t cmsgbuf[256/sizeof(u_int32_t)];       // Wcast-align fix - force 32 bit alignment
 439 #if defined(INET6) && defined(INET6_ADVAPI)
 440         struct in6_pktinfo *pi;
 441 #endif /*INET6_ADVAPI*/
 442         struct sockaddr_in *sin;
 443 #ifdef INET6
 444         struct sockaddr_in6 *sin6;
 445 #endif
 446
 447         len = sizeof(ss);
 448         if (getsockname(s, (struct sockaddr *)&ss, &len) < 0) {
 449                 plog(LLV_ERROR, LOCATION, NULL,
 450                         "getsockname (%s)\n", strerror(errno));
 451                 return -1;
 452         }
 453
 454         m.msg_name = (caddr_t)from;
 455         m.msg_namelen = *fromlen;
 456         iov[0].iov_base = (caddr_t)buf;
 457         iov[0].iov_len = buflen;
 458         m.msg_iov = iov;
 459         m.msg_iovlen = 1;
 460         memset(cmsgbuf, 0, sizeof(cmsgbuf));
 461         cm = (struct cmsghdr *)cmsgbuf;
 462         m.msg_control = (caddr_t)cm;
 463         m.msg_controllen = sizeof(cmsgbuf);
 464         while ((len = recvmsg(s, &m, flags)) < 0) {
 465                 if (errno == EINTR)
 466                         continue;
 467                 plog(LLV_ERROR, LOCATION, NULL,
 468                         "recvmsg (%s)\n", strerror(errno));
 469                 return -1;
 470         }
 471         if (len == 0) {
 472                 return 0;
 473         }
 474         *fromlen = m.msg_namelen;
 475
 476         otolen = *tolen;
 477         *tolen = 0;
 478         for (cm = (struct cmsghdr *)CMSG_FIRSTHDR(&m), cm_prev = NULL;
 479              m.msg_controllen != 0 && cm && cm != cm_prev;
 480              cm_prev = cm, cm = (struct cmsghdr *)CMSG_NXTHDR(&m, cm)) {
 481 #if 0
 482                 plog(LLV_ERROR, LOCATION, NULL,
 483                         "cmsg %d %d\n", cm->cmsg_level, cm->cmsg_type);)
 484 #endif
 485 #if defined(INET6) && defined(INET6_ADVAPI)
 486                 if (ss.ss_family == AF_INET6
 487                  && cm->cmsg_level == IPPROTO_IPV6
 488                  && cm->cmsg_type == IPV6_PKTINFO
 489                  && otolen >= sizeof(*sin6)) {
 490                         pi = ALIGNED_CAST(struct in6_pktinfo *)(CMSG_DATA(cm));
 491                         *tolen = sizeof(*sin6);
 492                         sin6 = (struct sockaddr_in6 *)to;
 493                         memset(sin6, 0, sizeof(*sin6));
 494                         sin6->sin6_family = AF_INET6;
 495                         sin6->sin6_len = sizeof(*sin6);
 496                         memcpy(&sin6->sin6_addr, &pi->ipi6_addr,
 497                                 sizeof(sin6->sin6_addr));
 498                         /* XXX other cases, such as site-local? */
 499                         if (IN6_IS_ADDR_LINKLOCAL(&sin6->sin6_addr))
 500                                 sin6->sin6_scope_id = pi->ipi6_ifindex;
 501                         else
 502                                 sin6->sin6_scope_id = 0;
 503                         sin6->sin6_port =
 504                                 ((struct sockaddr_in6 *)&ss)->sin6_port;
 505                         otolen = -1;    /* "to" already set */
 506                         continue;
 507                 }
 508 #endif
 509 #if defined(INET6) && defined(IPV6_RECVDSTADDR)
 510                 if (ss.ss_family == AF_INET6
 511                       && cm->cmsg_level == IPPROTO_IPV6
 512                       && cm->cmsg_type == IPV6_RECVDSTADDR
 513                       && otolen >= sizeof(*sin6)) {
 514                         *tolen = sizeof(*sin6);
 515                         sin6 = (struct sockaddr_in6 *)to;
 516                         memset(sin6, 0, sizeof(*sin6));
 517                         sin6->sin6_family = AF_INET6;
 518                         sin6->sin6_len = sizeof(*sin6);
 519                         memcpy(&sin6->sin6_addr, CMSG_DATA(cm),
 520                                 sizeof(sin6->sin6_addr));
 521                         sin6->sin6_port =
 522                                 ((struct sockaddr_in6 *)&ss)->sin6_port;
 523                         otolen = -1;    /* "to" already set */
 524                         continue;
 525                 }
 526 #endif
 527                 if (ss.ss_family == AF_INET
 528                  && cm->cmsg_level == IPPROTO_IP
 529                  && cm->cmsg_type == IP_RECVDSTADDR
 530                  && otolen >= sizeof(*sin)) {
 531                         *tolen = sizeof(*sin);
 532                         sin = (struct sockaddr_in *)to;
 533                         memset(sin, 0, sizeof(*sin));
 534                         sin->sin_family = AF_INET;
 535                         sin->sin_len = sizeof(*sin);
 536                         memcpy(&sin->sin_addr, CMSG_DATA(cm),
 537                                 sizeof(sin->sin_addr));
 538                         sin->sin_port = ((struct sockaddr_in *)&ss)->sin_port;
 539                         otolen = -1;    /* "to" already set */
 540                         continue;
 541                 }
 542         }
 543
 544         return len;
 545 }
 546
 547 /* send packet, with fixing src/dst address pair. */
 548 int
 549 sendfromto(s, buf, buflen, src, dst, cnt)
 550         int s, cnt;
 551         const void *buf;
 552         size_t buflen;
 553         struct sockaddr_storage *src;
 554         struct sockaddr_storage *dst;
 555 {
 556         struct sockaddr_storage ss;
 557         int len;
 558         int i;
 559
 560         if (src->ss_family != dst->ss_family) {
 561                 plog(LLV_ERROR, LOCATION, NULL,
 562                         "address family mismatch\n");
 563                 return -1;
 564         }
 565
 566         len = sizeof(ss);
 567         if (getsockname(s, (struct sockaddr *)&ss, &len) < 0) {
 568                 plog(LLV_ERROR, LOCATION, NULL,
 569                         "getsockname (%s)\n", strerror(errno));
 570                 return -1;
 571         }
 572
 573         plog(LLV_DEBUG, LOCATION, NULL,
 574                 "sockname %s\n", saddr2str((struct sockaddr *)&ss));
 575         plog(LLV_DEBUG, LOCATION, NULL,
 576                 "send packet from %s\n", saddr2str((struct sockaddr *)src));
 577         plog(LLV_DEBUG, LOCATION, NULL,
 578                 "send packet to %s\n", saddr2str((struct sockaddr *)dst));
 579
 580         if (src->ss_family != ss.ss_family) {
 581                 plog(LLV_ERROR, LOCATION, NULL,
 582                         "address family mismatch\n");
 583                 return -1;
 584         }
 585
 586         switch (src->ss_family) {
 587 #if defined(INET6) && defined(INET6_ADVAPI)
 588         case AF_INET6:
 589             {
 590                 struct msghdr m;
 591                 struct cmsghdr *cm;
 592                 struct iovec iov[2];
 593         u_int32_t cmsgbuf[256/sizeof(u_int32_t)];   // Wcast-align fix - force 32 bit alignment
 594                 struct in6_pktinfo *pi;
 595                 int ifindex;
 596                 struct sockaddr_in6 src6, dst6;
 597
 598                 memcpy(&src6, src, sizeof(src6));
 599                 memcpy(&dst6, dst, sizeof(dst6));
 600
 601                 /* XXX take care of other cases, such as site-local */
 602                 ifindex = 0;
 603                 if (IN6_IS_ADDR_LINKLOCAL(&src6.sin6_addr)
 604                  || IN6_IS_ADDR_MULTICAST(&src6.sin6_addr)) {
 605                         ifindex = src6.sin6_scope_id;   /*???*/
 606                 }
 607
 608                 /* XXX some sanity check on dst6.sin6_scope_id */
 609
 610                 /* flowinfo for IKE?  mmm, maybe useful but for now make it 0 */
 611                 src6.sin6_flowinfo = dst6.sin6_flowinfo = 0;
 612
 613                 memset(&m, 0, sizeof(m));
 614                 m.msg_name = (caddr_t)&dst6;
 615                 m.msg_namelen = sizeof(dst6);
 616                 iov[0].iov_base = (char *)buf;
 617                 iov[0].iov_len = buflen;
 618                 m.msg_iov = iov;
 619                 m.msg_iovlen = 1;
 620
 621                 memset(cmsgbuf, 0, sizeof(cmsgbuf));
 622                 cm = (struct cmsghdr *)cmsgbuf;
 623                 m.msg_control = (caddr_t)cm;
 624                 m.msg_controllen = CMSG_SPACE(sizeof(struct in6_pktinfo));
 625
 626                 cm->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
 627                 cm->cmsg_level = IPPROTO_IPV6;
 628                 cm->cmsg_type = IPV6_PKTINFO;
 629                 pi = ALIGNED_CAST(struct in6_pktinfo *)CMSG_DATA(cm);
 630                 memcpy(&pi->ipi6_addr, &src6.sin6_addr, sizeof(src6.sin6_addr));
 631                 pi->ipi6_ifindex = ifindex;
 632
 633                 plog(LLV_DEBUG, LOCATION, NULL,
 634                         "src6 %s %d\n",
 635                         saddr2str((struct sockaddr *)&src6),
 636                         src6.sin6_scope_id);
 637                 plog(LLV_DEBUG, LOCATION, NULL,
 638                         "dst6 %s %d\n",
 639                         saddr2str((struct sockaddr *)&dst6),
 640                         dst6.sin6_scope_id);
 641
 642                 for (i = 0; i < cnt; i++) {
 643                         len = sendmsg(s, &m, 0 /*MSG_DONTROUTE*/);
 644                         if (len < 0) {
 645                                 plog(LLV_ERROR, LOCATION, NULL,
 646                                         "sendmsg (%s)\n", strerror(errno));
 647                                 if (errno != EHOSTUNREACH && errno != ENETDOWN && errno != ENETUNREACH) {
 648                                         return -1;
 649                                 }
 650                                 // <rdar://problem/6609744> treat these failures like
 651                                 // packet loss, in case the network interface is flaky
 652                                 len = 0;
 653                         }
 654                         plog(LLV_DEBUG, LOCATION, NULL,
 655                                 "%d times of %d bytes message will be sent "
 656                                 "to %s\n",
 657                                 i + 1, len, saddr2str((struct sockaddr *)dst));
 658                 }
 659                 plogdump(LLV_DEBUG, (char *)buf, buflen);
 660
 661                 return len;
 662             }
 663 #endif
 664         default:
 665             {
 666                 int needclose = 0;
 667                 int sendsock;
 668
 669                 if (ss.ss_family == src->ss_family && memcmp(&ss, src, sysdep_sa_len((struct sockaddr *)src)) == 0) {
 670                         sendsock = s;
 671                         needclose = 0;
 672                 } else {
 673                         int yes = 1;
 674                         /*
 675                          * Use newly opened socket for sending packets.
 676                          * NOTE: this is unsafe, because if the peer is quick enough
 677                          * the packet from the peer may be queued into sendsock.
 678                          * Better approach is to prepare bind'ed udp sockets for
 679                          * each of the interface addresses.
 680                          */
 681                         sendsock = socket(src->ss_family, SOCK_DGRAM, 0);
 682                         if (sendsock < 0) {
 683                                 plog(LLV_ERROR, LOCATION, NULL,
 684                                         "socket (%s)\n", strerror(errno));
 685                                 return -1;
 686                         }
 687                         if (fcntl(sendsock, F_SETFL, O_NONBLOCK) == -1) {
 688                                 plog(LLV_ERROR, LOCATION, NULL,
 689                                          "failed to put sendsock socket in non-blocking mode\n");
 690                         }
 691                         if (setsockopt(sendsock, SOL_SOCKET,
 692                                        SO_REUSEPORT,
 693                                        (void *)&yes, sizeof(yes)) < 0) {
 694                                 plog(LLV_ERROR, LOCATION, NULL,
 695                                         "setsockopt SO_REUSEPORT (%s)\n",
 696                                         strerror(errno));
 697                                 close(sendsock);
 698                                 return -1;
 699                         }
 700 #ifdef IPV6_USE_MIN_MTU
 701                         if (src->ss_family == AF_INET6 &&
 702                             setsockopt(sendsock, IPPROTO_IPV6, IPV6_USE_MIN_MTU,
 703                             (void *)&yes, sizeof(yes)) < 0) {
 704                                 plog(LLV_ERROR, LOCATION, NULL,
 705                                         "setsockopt IPV6_USE_MIN_MTU (%s)\n",
 706                                         strerror(errno));
 707                                 close(sendsock);
 708                                 return -1;
 709                         }
 710 #endif
 711                         if (setsockopt_bypass(sendsock, src->ss_family) < 0) {
 712                                 close(sendsock);
 713                                 return -1;
 714                         }
 715
 716                         if (bind(sendsock, (struct sockaddr *)src, sysdep_sa_len((struct sockaddr *)src)) < 0) {
 717                                 plog(LLV_ERROR, LOCATION, NULL,
 718                                         "bind 1 (%s)\n", strerror(errno));
 719                                 close(sendsock);
 720                                 return -1;
 721                         }
 722                         needclose = 1;
 723                 }
 724
 725                 for (i = 0; i < cnt; i++) {
 726                         len = sendto(sendsock, buf, buflen, 0, (struct sockaddr *)dst, sysdep_sa_len((struct sockaddr *)dst));
 727                         if (len < 0) {
 728                                 plog(LLV_ERROR, LOCATION, NULL,
 729                                         "sendto (%s)\n", strerror(errno));
 730                                 if (errno != EHOSTUNREACH && errno != ENETDOWN && errno != ENETUNREACH) {
 731                                         if (needclose)
 732                                                 close(sendsock);
 733                                         return -1;
 734                                 }
 735                                 plog(LLV_ERROR, LOCATION, NULL,
 736                                         "treating socket error (%s) like packet loss\n", strerror(errno));
 737                                 // else treat these failures like a packet loss
 738                                 len = 0;
 739                         }
 740                         plog(LLV_DEBUG, LOCATION, NULL,
 741                                 "%d times of %d bytes message will be sent "
 742                                 "to %s\n",
 743                                 i + 1, len, saddr2str((struct sockaddr *)dst));
 744                 }
 745                 plogdump(LLV_DEBUG, (char *)buf, buflen);
 746
 747                 if (needclose)
 748                         close(sendsock);
 749
 750                 return len;
 751             }
 752         }
 753 }
 754
 755 int
 756 setsockopt_bypass(so, family)
 757         int so, family;
 758 {
 759         int level;
 760         char *buf;
 761         char *policy;
 762
 763         switch (family) {
 764         case AF_INET:
 765                 level = IPPROTO_IP;
 766                 break;
 767 #ifdef INET6
 768         case AF_INET6:
 769                 level = IPPROTO_IPV6;
 770                 break;
 771 #endif
 772         default:
 773                 plog(LLV_ERROR, LOCATION, NULL,
 774                         "unsupported address family %d\n", family);
 775                 return -1;
 776         }
 777
 778         policy = "in bypass";
 779         buf = ipsec_set_policy(policy, strlen(policy));
 780         if (buf == NULL) {
 781                 plog(LLV_ERROR, LOCATION, NULL,
 782                         "ipsec_set_policy (%s)\n",
 783                         ipsec_strerror());
 784                 return -1;
 785         }
 786         if (setsockopt(so, level,
 787                        (level == IPPROTO_IP ?
 788                                  IP_IPSEC_POLICY : IPV6_IPSEC_POLICY),
 789                        buf, ipsec_get_policylen(buf)) < 0) {
 790                 plog(LLV_ERROR, LOCATION, NULL,
 791                         "setsockopt IP_IPSEC_POLICY (%s)\n",
 792                         strerror(errno));
 793                 return -1;
 794         }
 795         racoon_free(buf);
 796
 797         policy = "out bypass";
 798         buf = ipsec_set_policy(policy, strlen(policy));
 799         if (buf == NULL) {
 800                 plog(LLV_ERROR, LOCATION, NULL,
 801                         "ipsec_set_policy (%s)\n",
 802                         ipsec_strerror());
 803                 return -1;
 804         }
 805         if (setsockopt(so, level,
 806                        (level == IPPROTO_IP ?
 807                                  IP_IPSEC_POLICY : IPV6_IPSEC_POLICY),
 808                        buf, ipsec_get_policylen(buf)) < 0) {
 809                 plog(LLV_ERROR, LOCATION, NULL,
 810                         "setsockopt IP_IPSEC_POLICY (%s)\n",
 811                         strerror(errno));
 812                 return -1;
 813         }
 814         racoon_free(buf);
 815
 816         return 0;
 817 }
 818
 819 struct sockaddr_storage *
 820 newsaddr(len)
 821         int len;
 822 {
 823         struct sockaddr_storage *new;
 824
 825         if ((new = racoon_calloc(1, len)) == NULL) {
 826                 plog(LLV_ERROR, LOCATION, NULL,
 827                         "%s\n", strerror(errno));
 828                 goto out;
 829         }
 830         /* initial */
 831         new->ss_len = len;
 832 out:
 833         return new;
 834 }
 835
 836 struct sockaddr_storage *
 837 dupsaddr(src)
 838         struct sockaddr *src;
 839 {
 840         struct sockaddr_storage *dst;
 841
 842         dst = racoon_calloc(1, sysdep_sa_len(src));
 843         if (dst == NULL) {
 844                 plog(LLV_ERROR, LOCATION, NULL,
 845                         "%s\n", strerror(errno));
 846                 return NULL;
 847         }
 848
 849         memcpy(dst, src, sysdep_sa_len(src));
 850
 851         return dst;
 852 }
 853
 854 char *
 855 saddr2str(saddr)
 856         const struct sockaddr *saddr;
 857 {
 858         static char buf[NI_MAXHOST + NI_MAXSERV + 10];
 859         char addr[NI_MAXHOST], port[NI_MAXSERV];
 860
 861         if (saddr == NULL) {
 862                 buf[0] = '\0';
 863                 return buf;
 864         }
 865
 866         if (saddr->sa_family == AF_UNSPEC)
 867                 snprintf (buf, sizeof(buf), "%s", "anonymous");
 868         else {
 869                 GETNAMEINFO(saddr, addr, port);
 870                 snprintf(buf, sizeof(buf), "%s[%s]", addr, port);
 871         }
 872
 873         return buf;
 874 }
 875
 876 char *
 877 saddr2str_with_prefix(saddr, prefix)
 878 const struct sockaddr *saddr;
 879 int prefix;
 880 {
 881         static char buf[NI_MAXHOST + NI_MAXSERV + 10];
 882         char addr[NI_MAXHOST], port[NI_MAXSERV];
 883
 884         if (saddr == NULL) {
 885                 buf[0] = '\0';
 886                 return buf;
 887         }
 888
 889         if (saddr->sa_family == AF_UNSPEC)
 890                 snprintf (buf, sizeof(buf), "%s", "anonymous");
 891         else {
 892                 GETNAMEINFO(saddr, addr, port);
 893                 snprintf(buf, sizeof(buf), "%s/%d[%s]", addr, prefix, port);
 894         }
 895
 896         return buf;
 897 }
 898
 899
 900 char *
 901 saddrwop2str(saddr)
 902         const struct sockaddr *saddr;
 903 {
 904         static char buf[NI_MAXHOST + NI_MAXSERV + 10];
 905         char addr[NI_MAXHOST];
 906
 907         if (saddr == NULL) {
 908                 buf[0] = '\0';
 909                 return buf;
 910         }
 911
 912         GETNAMEINFO_NULL(saddr, addr);
 913         snprintf(buf, sizeof(buf), "%s", addr);
 914
 915         return buf;
 916 }
 917
 918 char *
 919 naddrwop2str(const struct netaddr *naddr)
 920 {
 921         static char buf[NI_MAXHOST + 10];
 922         static const struct sockaddr sa_any;    /* this is initialized to all zeros */
 923
 924         if (naddr == NULL) {
 925                 buf[0] = '\0';
 926                 return buf;
 927         }
 928
 929         if (memcmp(&naddr->sa, &sa_any, sizeof(sa_any)) == 0)
 930                 snprintf(buf, sizeof(buf), "%s", "any");
 931         else {
 932                 snprintf(buf, sizeof(buf), "%s", saddrwop2str((struct sockaddr *)&naddr->sa.sa));
 933                 snprintf(&buf[strlen(buf)], sizeof(buf) - strlen(buf), "/%ld", naddr->prefix);
 934         }
 935         return buf;
 936 }
 937
 938 char *
 939 naddrwop2str_fromto(const char *format, const struct netaddr *saddr,
 940                     const struct netaddr *daddr)
 941 {
 942         static char buf[2*(NI_MAXHOST + NI_MAXSERV + 10) + 100];
 943         char *src, *dst;
 944
 945         src = racoon_strdup(naddrwop2str(saddr));
 946         dst = racoon_strdup(naddrwop2str(daddr));
 947         STRDUP_FATAL(src);
 948         STRDUP_FATAL(dst);
 949         /* WARNING: Be careful about the format string! Don't
 950            ever pass in something that a user can modify!!! */
 951         snprintf (buf, sizeof(buf), format, src, dst);
 952         racoon_free (src);
 953         racoon_free (dst);
 954
 955         return buf;
 956 }
 957
 958 char *
 959 saddr2str_fromto(format, saddr, daddr)
 960         const char *format;
 961         const struct sockaddr *saddr;
 962         const struct sockaddr *daddr;
 963 {
 964         static char buf[2*(NI_MAXHOST + NI_MAXSERV + 10) + 100];
 965         char *src, *dst;
 966
 967         if (saddr) {
 968                 src = racoon_strdup(saddr2str(saddr));
 969                 STRDUP_FATAL(src);
 970         } else {
 971                 src = NULL;
 972         }
 973         if (daddr) {
 974                 dst = racoon_strdup(saddr2str(daddr));
 975                 STRDUP_FATAL(dst);
 976         } else {
 977                 dst = NULL;
 978         }
 979         /* WARNING: Be careful about the format string! Don't
 980            ever pass in something that a user can modify!!! */
 981         snprintf (buf, sizeof(buf), format, src? src:"[null]", dst? dst:"[null]");
 982         if (src) {
 983                 racoon_free (src);
 984         }
 985         if (dst) {
 986                 racoon_free (dst);
 987         }
 988
 989         return buf;
 990 }
 991
 992 struct sockaddr_storage *
 993 str2saddr(host, port)
 994         char *host;
 995         char *port;
 996 {
 997         struct addrinfo hints, *res;
 998         struct sockaddr_storage *saddr;
 999         int error;
1000
1001         memset(&hints, 0, sizeof(hints));
1002         hints.ai_family = PF_UNSPEC;
1003         hints.ai_socktype = SOCK_DGRAM;
1004         hints.ai_flags = AI_NUMERICHOST;
1005         error = getaddrinfo(host, port, &hints, &res);
1006         if (error != 0) {
1007                 plog(LLV_ERROR, LOCATION, NULL,
1008                         "getaddrinfo(%s%s%s): %s\n",
1009                         host, port ? "," : "", port ? port : "",
1010                         gai_strerror(error));
1011                 return NULL;
1012         }
1013         if (res->ai_next != NULL) {
1014                 plog(LLV_WARNING, LOCATION, NULL,
1015                         "getaddrinfo(%s%s%s): "
1016                         "resolved to multiple address, "
1017                         "taking the first one\n",
1018                         host, port ? "," : "", port ? port : "");
1019         }
1020         saddr = newsaddr(sizeof(*saddr));
1021         if (saddr == NULL) {
1022                 plog(LLV_ERROR, LOCATION, NULL,
1023                         "failed to allocate buffer.\n");
1024                 freeaddrinfo(res);
1025                 return NULL;
1026         }
1027         memcpy(saddr, res->ai_addr, res->ai_addrlen);
1028         freeaddrinfo(res);
1029
1030         return saddr;
1031 }
1032
1033 void
1034 mask_sockaddr(a, b, l)
1035         struct sockaddr_storage *a;
1036         const struct sockaddr_storage *b;
1037         size_t l;
1038 {
1039         size_t i;
1040         u_int8_t *p, alen;
1041
1042         switch (b->ss_family) {
1043         case AF_INET:
1044                 alen = sizeof(struct in_addr);
1045                 p = (u_int8_t *)&((struct sockaddr_in *)a)->sin_addr;
1046                 break;
1047 #ifdef INET6
1048         case AF_INET6:
1049                 alen = sizeof(struct in6_addr);
1050                 p = (u_int8_t *)&((struct sockaddr_in6 *)a)->sin6_addr;
1051                 break;
1052 #endif
1053         default:
1054                 plog(LLV_ERROR2, LOCATION, NULL,
1055                         "invalid address family: %d\n", b->ss_family);
1056                 exit(1);
1057         }
1058
1059         if ((alen << 3) < l) {
1060                 plog(LLV_ERROR2, LOCATION, NULL,
1061                         "unexpected inconsistency: %d %zu\n", b->ss_family, l);
1062                 exit(1);
1063         }
1064
1065         memcpy(a, b, sysdep_sa_len((struct sockaddr *)b));
1066         p[l / 8] &= (0xff00 >> (l % 8)) & 0xff;
1067         for (i = l / 8 + 1; i < alen; i++)
1068                 p[i] = 0x00;
1069 }
1070
1071 /* Compute a score describing how "accurate" a netaddr is for a given sockaddr.
1072  * Examples:
1073  *      Return values for address 10.20.30.40 [port 500] and given netaddresses...
1074  *              10.10.0.0/16    => -1   ... doesn't match
1075  *              0.0.0.0/0       => 0    ... matches, but only 0 bits.
1076  *              10.20.0.0/16    => 16   ... 16 bits match
1077  *              10.20.30.0/24   => 24   ... guess what ;-)
1078  *              10.20.30.40/32  => 32   ... whole address match
1079  *              10.20.30.40:500 => 33   ... both address and port match
1080  *              10.20.30.40:501 => -1   ... port doesn't match and isn't 0 (=any)
1081  */
1082 int
1083 naddr_score(const struct netaddr *naddr, const struct sockaddr_storage *saddr)
1084 {
1085         static const struct netaddr naddr_any;  /* initialized to all-zeros */
1086         struct sockaddr_storage sa;
1087         u_int16_t naddr_port, saddr_port;
1088         int port_score;
1089
1090         if (!naddr || !saddr) {
1091                 plog(LLV_ERROR, LOCATION, NULL,
1092                      "Call with null args: naddr=%p, saddr=%p\n",
1093                      naddr, saddr);
1094                 return -1;
1095         }
1096
1097         /* Wildcard address matches, but only 0 bits. */
1098         if (memcmp(naddr, &naddr_any, sizeof(naddr_any)) == 0)
1099                 return 0;
1100
1101         /* If families don't match we really can't do much... */
1102         if (naddr->sa.sa.ss_family != saddr->ss_family)
1103                 return -1;
1104
1105         /* If port check fail don't bother to check addresses. */
1106         naddr_port = extract_port(&naddr->sa.sa);
1107         saddr_port = extract_port(saddr);
1108         if (naddr_port == 0 || saddr_port == 0) /* wildcard match */
1109                 port_score = 0;
1110         else if (naddr_port == saddr_port)      /* exact match */
1111                 port_score = 1;
1112         else                                    /* mismatch :-) */
1113                 return -1;
1114
1115         /* Here it comes - compare network addresses. */
1116         mask_sockaddr(&sa, saddr, naddr->prefix);
1117         if (loglevel >= LLV_DEBUG) {    /* debug only */
1118                 char *a1, *a2, *a3;
1119                 a1 = racoon_strdup(naddrwop2str(naddr));
1120                 a2 = racoon_strdup(saddrwop2str((struct sockaddr *)saddr));
1121                 a3 = racoon_strdup(saddrwop2str((struct sockaddr *)&sa));
1122                 STRDUP_FATAL(a1);
1123                 STRDUP_FATAL(a2);
1124                 STRDUP_FATAL(a3);
1125                 plog(LLV_DEBUG, LOCATION, NULL,
1126                      "naddr=%s, saddr=%s (masked=%s)\n",
1127                      a1, a2, a3);
1128                 free(a1);
1129                 free(a2);
1130                 free(a3);
1131         }
1132         if (cmpsaddrwop(&sa, &naddr->sa.sa) == 0)
1133                 return naddr->prefix + port_score;
1134
1135         return -1;
1136 }
1137
1138 /* Some usefull functions for sockaddr_storage port manipulations. */
1139 u_int16_t
1140 extract_port (const struct sockaddr_storage *addr)
1141 {
1142   u_int16_t port = -1;
1143
1144   if (!addr)
1145     return port;
1146
1147   switch (addr->ss_family) {
1148     case AF_INET:
1149       port = ((struct sockaddr_in *)addr)->sin_port;
1150       break;
1151     case AF_INET6:
1152       port = ((struct sockaddr_in6 *)addr)->sin6_port;
1153       break;
1154     default:
1155       plog(LLV_ERROR, LOCATION, NULL, "unknown AF: %u\n", addr->ss_family);
1156       break;
1157   }
1158
1159   return ntohs(port);
1160 }
1161
1162 u_int16_t *
1163 get_port_ptr (struct sockaddr_storage *addr)
1164 {
1165   u_int16_t *port_ptr;
1166
1167   if (!addr)
1168     return NULL;
1169
1170   switch (addr->ss_family) {
1171     case AF_INET:
1172       port_ptr = &(((struct sockaddr_in *)addr)->sin_port);
1173       break;
1174     case AF_INET6:
1175       port_ptr = &(((struct sockaddr_in6 *)addr)->sin6_port);
1176       break;
1177     default:
1178       plog(LLV_ERROR, LOCATION, NULL, "unknown AF: %u\n", addr->ss_family);
1179       return NULL;
1180       break;
1181   }
1182
1183   return port_ptr;
1184 }
1185
1186 u_int16_t *
1187 set_port (struct sockaddr_storage *addr, u_int16_t new_port)
1188 {
1189   u_int16_t *port_ptr;
1190
1191   port_ptr = get_port_ptr (addr);
1192
1193   if (port_ptr)
1194     *port_ptr = htons(new_port);
1195
1196   return port_ptr;
1197 }