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