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