tools/testing/selftests/net/nettest.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* nettest - used for functional tests of networking APIs
   3  *
   4  * Copyright (c) 2013-2019 David Ahern <dsahern@gmail.com>. All rights reserved.
   5  */
   6
   7 #define _GNU_SOURCE
   8 #include <features.h>
   9 #include <sys/types.h>
  10 #include <sys/ioctl.h>
  11 #include <sys/socket.h>
  12 #include <sys/wait.h>
  13 #include <linux/tcp.h>
  14 #include <linux/udp.h>
  15 #include <arpa/inet.h>
  16 #include <net/if.h>
  17 #include <netinet/in.h>
  18 #include <netinet/ip.h>
  19 #include <netdb.h>
  20 #include <fcntl.h>
  21 #include <libgen.h>
  22 #include <limits.h>
  23 #include <sched.h>
  24 #include <stdarg.h>
  25 #include <stdio.h>
  26 #include <stdlib.h>
  27 #include <string.h>
  28 #include <unistd.h>
  29 #include <time.h>
  30 #include <errno.h>
  31 #include <getopt.h>
  32
  33 #include <linux/xfrm.h>
  34 #include <linux/ipsec.h>
  35 #include <linux/pfkeyv2.h>
  36
  37 #ifndef IPV6_UNICAST_IF
  38 #define IPV6_UNICAST_IF         76
  39 #endif
  40 #ifndef IPV6_MULTICAST_IF
  41 #define IPV6_MULTICAST_IF       17
  42 #endif
  43
  44 #define DEFAULT_PORT 12345
  45
  46 #define NS_PREFIX "/run/netns/"
  47
  48 #ifndef MAX
  49 #define MAX(a, b)  ((a) > (b) ? (a) : (b))
  50 #endif
  51 #ifndef MIN
  52 #define MIN(a, b)  ((a) < (b) ? (a) : (b))
  53 #endif
  54
  55 struct sock_args {
  56         /* local address */
  57         const char *local_addr_str;
  58         const char *client_local_addr_str;
  59         union {
  60                 struct in_addr  in;
  61                 struct in6_addr in6;
  62         } local_addr;
  63
  64         /* remote address */
  65         const char *remote_addr_str;
  66         union {
  67                 struct in_addr  in;
  68                 struct in6_addr in6;
  69         } remote_addr;
  70         int scope_id;  /* remote scope; v6 send only */
  71
  72         struct in_addr grp;     /* multicast group */
  73
  74         unsigned int has_local_ip:1,
  75                      has_remote_ip:1,
  76                      has_grp:1,
  77                      has_expected_laddr:1,
  78                      has_expected_raddr:1,
  79                      bind_test_only:1;
  80
  81         unsigned short port;
  82
  83         int type;      /* DGRAM, STREAM, RAW */
  84         int protocol;
  85         int version;   /* AF_INET/AF_INET6 */
  86
  87         int use_setsockopt;
  88         int use_freebind;
  89         int use_cmsg;
  90         const char *dev;
  91         const char *server_dev;
  92         int ifindex;
  93
  94         const char *clientns;
  95         const char *serverns;
  96
  97         const char *password;
  98         const char *client_pw;
  99         /* prefix for MD5 password */
 100         const char *md5_prefix_str;
 101         union {
 102                 struct sockaddr_in v4;
 103                 struct sockaddr_in6 v6;
 104         } md5_prefix;
 105         unsigned int prefix_len;
 106         /* 0: default, -1: force off, +1: force on */
 107         int bind_key_ifindex;
 108
 109         /* expected addresses and device index for connection */
 110         const char *expected_dev;
 111         const char *expected_server_dev;
 112         int expected_ifindex;
 113
 114         /* local address */
 115         const char *expected_laddr_str;
 116         union {
 117                 struct in_addr  in;
 118                 struct in6_addr in6;
 119         } expected_laddr;
 120
 121         /* remote address */
 122         const char *expected_raddr_str;
 123         union {
 124                 struct in_addr  in;
 125                 struct in6_addr in6;
 126         } expected_raddr;
 127
 128         /* ESP in UDP encap test */
 129         int use_xfrm;
 130 };
 131
 132 static int server_mode;
 133 static unsigned int prog_timeout = 5;
 134 static unsigned int interactive;
 135 static int iter = 1;
 136 static char *msg = "Hello world!";
 137 static int msglen;
 138 static int quiet;
 139 static int try_broadcast = 1;
 140
 141 static char *timestamp(char *timebuf, int buflen)
 142 {
 143         time_t now;
 144
 145         now = time(NULL);
 146         if (strftime(timebuf, buflen, "%T", localtime(&now)) == 0) {
 147                 memset(timebuf, 0, buflen);
 148                 strncpy(timebuf, "00:00:00", buflen-1);
 149         }
 150
 151         return timebuf;
 152 }
 153
 154 static void log_msg(const char *format, ...)
 155 {
 156         char timebuf[64];
 157         va_list args;
 158
 159         if (quiet)
 160                 return;
 161
 162         fprintf(stdout, "%s %s:",
 163                 timestamp(timebuf, sizeof(timebuf)),
 164                 server_mode ? "server" : "client");
 165         va_start(args, format);
 166         vfprintf(stdout, format, args);
 167         va_end(args);
 168
 169         fflush(stdout);
 170 }
 171
 172 static void log_error(const char *format, ...)
 173 {
 174         char timebuf[64];
 175         va_list args;
 176
 177         if (quiet)
 178                 return;
 179
 180         fprintf(stderr, "%s %s:",
 181                 timestamp(timebuf, sizeof(timebuf)),
 182                 server_mode ? "server" : "client");
 183         va_start(args, format);
 184         vfprintf(stderr, format, args);
 185         va_end(args);
 186
 187         fflush(stderr);
 188 }
 189
 190 static void log_err_errno(const char *fmt, ...)
 191 {
 192         char timebuf[64];
 193         va_list args;
 194
 195         if (quiet)
 196                 return;
 197
 198         fprintf(stderr, "%s %s: ",
 199                 timestamp(timebuf, sizeof(timebuf)),
 200                 server_mode ? "server" : "client");
 201         va_start(args, fmt);
 202         vfprintf(stderr, fmt, args);
 203         va_end(args);
 204
 205         fprintf(stderr, ": %d: %s\n", errno, strerror(errno));
 206         fflush(stderr);
 207 }
 208
 209 static void log_address(const char *desc, struct sockaddr *sa)
 210 {
 211         char addrstr[64];
 212
 213         if (quiet)
 214                 return;
 215
 216         if (sa->sa_family == AF_INET) {
 217                 struct sockaddr_in *s = (struct sockaddr_in *) sa;
 218
 219                 log_msg("%s %s:%d\n",
 220                         desc,
 221                         inet_ntop(AF_INET, &s->sin_addr, addrstr,
 222                                   sizeof(addrstr)),
 223                         ntohs(s->sin_port));
 224
 225         } else if (sa->sa_family == AF_INET6) {
 226                 struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
 227
 228                 log_msg("%s [%s]:%d\n",
 229                         desc,
 230                         inet_ntop(AF_INET6, &s6->sin6_addr, addrstr,
 231                                   sizeof(addrstr)),
 232                         ntohs(s6->sin6_port));
 233         }
 234
 235         fflush(stdout);
 236 }
 237
 238 static int switch_ns(const char *ns)
 239 {
 240         char path[PATH_MAX];
 241         int fd, ret;
 242
 243         if (geteuid())
 244                 log_error("warning: likely need root to set netns %s!\n", ns);
 245
 246         snprintf(path, sizeof(path), "%s%s", NS_PREFIX, ns);
 247         fd = open(path, 0);
 248         if (fd < 0) {
 249                 log_err_errno("Failed to open netns path; can not switch netns");
 250                 return 1;
 251         }
 252
 253         ret = setns(fd, CLONE_NEWNET);
 254         close(fd);
 255
 256         return ret;
 257 }
 258
 259 static int tcp_md5sig(int sd, void *addr, socklen_t alen, struct sock_args *args)
 260 {
 261         int keylen = strlen(args->password);
 262         struct tcp_md5sig md5sig = {};
 263         int opt = TCP_MD5SIG;
 264         int rc;
 265
 266         md5sig.tcpm_keylen = keylen;
 267         memcpy(md5sig.tcpm_key, args->password, keylen);
 268
 269         if (args->prefix_len) {
 270                 opt = TCP_MD5SIG_EXT;
 271                 md5sig.tcpm_flags |= TCP_MD5SIG_FLAG_PREFIX;
 272
 273                 md5sig.tcpm_prefixlen = args->prefix_len;
 274                 addr = &args->md5_prefix;
 275         }
 276         memcpy(&md5sig.tcpm_addr, addr, alen);
 277
 278         if ((args->ifindex && args->bind_key_ifindex >= 0) || args->bind_key_ifindex >= 1) {
 279                 opt = TCP_MD5SIG_EXT;
 280                 md5sig.tcpm_flags |= TCP_MD5SIG_FLAG_IFINDEX;
 281
 282                 md5sig.tcpm_ifindex = args->ifindex;
 283                 log_msg("TCP_MD5SIG_FLAG_IFINDEX set tcpm_ifindex=%d\n", md5sig.tcpm_ifindex);
 284         } else {
 285                 log_msg("TCP_MD5SIG_FLAG_IFINDEX off\n", md5sig.tcpm_ifindex);
 286         }
 287
 288         rc = setsockopt(sd, IPPROTO_TCP, opt, &md5sig, sizeof(md5sig));
 289         if (rc < 0) {
 290                 /* ENOENT is harmless. Returned when a password is cleared */
 291                 if (errno == ENOENT)
 292                         rc = 0;
 293                 else
 294                         log_err_errno("setsockopt(TCP_MD5SIG)");
 295         }
 296
 297         return rc;
 298 }
 299
 300 static int tcp_md5_remote(int sd, struct sock_args *args)
 301 {
 302         struct sockaddr_in sin = {
 303                 .sin_family = AF_INET,
 304         };
 305         struct sockaddr_in6 sin6 = {
 306                 .sin6_family = AF_INET6,
 307         };
 308         void *addr;
 309         int alen;
 310
 311         switch (args->version) {
 312         case AF_INET:
 313                 sin.sin_port = htons(args->port);
 314                 sin.sin_addr = args->md5_prefix.v4.sin_addr;
 315                 addr = &sin;
 316                 alen = sizeof(sin);
 317                 break;
 318         case AF_INET6:
 319                 sin6.sin6_port = htons(args->port);
 320                 sin6.sin6_addr = args->md5_prefix.v6.sin6_addr;
 321                 addr = &sin6;
 322                 alen = sizeof(sin6);
 323                 break;
 324         default:
 325                 log_error("unknown address family\n");
 326                 exit(1);
 327         }
 328
 329         if (tcp_md5sig(sd, addr, alen, args))
 330                 return -1;
 331
 332         return 0;
 333 }
 334
 335 static int get_ifidx(const char *ifname)
 336 {
 337         struct ifreq ifdata;
 338         int sd, rc;
 339
 340         if (!ifname || *ifname == '\0')
 341                 return -1;
 342
 343         memset(&ifdata, 0, sizeof(ifdata));
 344
 345         strcpy(ifdata.ifr_name, ifname);
 346
 347         sd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
 348         if (sd < 0) {
 349                 log_err_errno("socket failed");
 350                 return -1;
 351         }
 352
 353         rc = ioctl(sd, SIOCGIFINDEX, (char *)&ifdata);
 354         close(sd);
 355         if (rc != 0) {
 356                 log_err_errno("ioctl(SIOCGIFINDEX) failed");
 357                 return -1;
 358         }
 359
 360         return ifdata.ifr_ifindex;
 361 }
 362
 363 static int bind_to_device(int sd, const char *name)
 364 {
 365         int rc;
 366
 367         rc = setsockopt(sd, SOL_SOCKET, SO_BINDTODEVICE, name, strlen(name)+1);
 368         if (rc < 0)
 369                 log_err_errno("setsockopt(SO_BINDTODEVICE)");
 370
 371         return rc;
 372 }
 373
 374 static int get_bind_to_device(int sd, char *name, size_t len)
 375 {
 376         int rc;
 377         socklen_t optlen = len;
 378
 379         name[0] = '\0';
 380         rc = getsockopt(sd, SOL_SOCKET, SO_BINDTODEVICE, name, &optlen);
 381         if (rc < 0)
 382                 log_err_errno("setsockopt(SO_BINDTODEVICE)");
 383
 384         return rc;
 385 }
 386
 387 static int check_device(int sd, struct sock_args *args)
 388 {
 389         int ifindex = 0;
 390         char name[32];
 391
 392         if (get_bind_to_device(sd, name, sizeof(name)))
 393                 *name = '\0';
 394         else
 395                 ifindex = get_ifidx(name);
 396
 397         log_msg("    bound to device %s/%d\n",
 398                 *name ? name : "<none>", ifindex);
 399
 400         if (!args->expected_ifindex)
 401                 return 0;
 402
 403         if (args->expected_ifindex != ifindex) {
 404                 log_error("Device index mismatch: expected %d have %d\n",
 405                           args->expected_ifindex, ifindex);
 406                 return 1;
 407         }
 408
 409         log_msg("Device index matches: expected %d have %d\n",
 410                 args->expected_ifindex, ifindex);
 411
 412         return 0;
 413 }
 414
 415 static int set_pktinfo_v4(int sd)
 416 {
 417         int one = 1;
 418         int rc;
 419
 420         rc = setsockopt(sd, SOL_IP, IP_PKTINFO, &one, sizeof(one));
 421         if (rc < 0 && rc != -ENOTSUP)
 422                 log_err_errno("setsockopt(IP_PKTINFO)");
 423
 424         return rc;
 425 }
 426
 427 static int set_recvpktinfo_v6(int sd)
 428 {
 429         int one = 1;
 430         int rc;
 431
 432         rc = setsockopt(sd, SOL_IPV6, IPV6_RECVPKTINFO, &one, sizeof(one));
 433         if (rc < 0 && rc != -ENOTSUP)
 434                 log_err_errno("setsockopt(IPV6_RECVPKTINFO)");
 435
 436         return rc;
 437 }
 438
 439 static int set_recverr_v4(int sd)
 440 {
 441         int one = 1;
 442         int rc;
 443
 444         rc = setsockopt(sd, SOL_IP, IP_RECVERR, &one, sizeof(one));
 445         if (rc < 0 && rc != -ENOTSUP)
 446                 log_err_errno("setsockopt(IP_RECVERR)");
 447
 448         return rc;
 449 }
 450
 451 static int set_recverr_v6(int sd)
 452 {
 453         int one = 1;
 454         int rc;
 455
 456         rc = setsockopt(sd, SOL_IPV6, IPV6_RECVERR, &one, sizeof(one));
 457         if (rc < 0 && rc != -ENOTSUP)
 458                 log_err_errno("setsockopt(IPV6_RECVERR)");
 459
 460         return rc;
 461 }
 462
 463 static int set_unicast_if(int sd, int ifindex, int version)
 464 {
 465         int opt = IP_UNICAST_IF;
 466         int level = SOL_IP;
 467         int rc;
 468
 469         ifindex = htonl(ifindex);
 470
 471         if (version == AF_INET6) {
 472                 opt = IPV6_UNICAST_IF;
 473                 level = SOL_IPV6;
 474         }
 475         rc = setsockopt(sd, level, opt, &ifindex, sizeof(ifindex));
 476         if (rc < 0)
 477                 log_err_errno("setsockopt(IP_UNICAST_IF)");
 478
 479         return rc;
 480 }
 481
 482 static int set_multicast_if(int sd, int ifindex)
 483 {
 484         struct ip_mreqn mreq = { .imr_ifindex = ifindex };
 485         int rc;
 486
 487         rc = setsockopt(sd, SOL_IP, IP_MULTICAST_IF, &mreq, sizeof(mreq));
 488         if (rc < 0)
 489                 log_err_errno("setsockopt(IP_MULTICAST_IF)");
 490
 491         return rc;
 492 }
 493
 494 static int set_membership(int sd, uint32_t grp, uint32_t addr, int ifindex)
 495 {
 496         uint32_t if_addr = addr;
 497         struct ip_mreqn mreq;
 498         int rc;
 499
 500         if (addr == htonl(INADDR_ANY) && !ifindex) {
 501                 log_error("Either local address or device needs to be given for multicast membership\n");
 502                 return -1;
 503         }
 504
 505         mreq.imr_multiaddr.s_addr = grp;
 506         mreq.imr_address.s_addr = if_addr;
 507         mreq.imr_ifindex = ifindex;
 508
 509         rc = setsockopt(sd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
 510         if (rc < 0) {
 511                 log_err_errno("setsockopt(IP_ADD_MEMBERSHIP)");
 512                 return -1;
 513         }
 514
 515         return 0;
 516 }
 517
 518 static int set_freebind(int sd, int version)
 519 {
 520         unsigned int one = 1;
 521         int rc = 0;
 522
 523         switch (version) {
 524         case AF_INET:
 525                 if (setsockopt(sd, SOL_IP, IP_FREEBIND, &one, sizeof(one))) {
 526                         log_err_errno("setsockopt(IP_FREEBIND)");
 527                         rc = -1;
 528                 }
 529                 break;
 530         case AF_INET6:
 531                 if (setsockopt(sd, SOL_IPV6, IPV6_FREEBIND, &one, sizeof(one))) {
 532                         log_err_errno("setsockopt(IPV6_FREEBIND");
 533                         rc = -1;
 534                 }
 535                 break;
 536         }
 537
 538         return rc;
 539 }
 540
 541 static int set_broadcast(int sd)
 542 {
 543         unsigned int one = 1;
 544         int rc = 0;
 545
 546         if (setsockopt(sd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)) != 0) {
 547                 log_err_errno("setsockopt(SO_BROADCAST)");
 548                 rc = -1;
 549         }
 550
 551         return rc;
 552 }
 553
 554 static int set_reuseport(int sd)
 555 {
 556         unsigned int one = 1;
 557         int rc = 0;
 558
 559         if (setsockopt(sd, SOL_SOCKET, SO_REUSEPORT, &one, sizeof(one)) != 0) {
 560                 log_err_errno("setsockopt(SO_REUSEPORT)");
 561                 rc = -1;
 562         }
 563
 564         return rc;
 565 }
 566
 567 static int set_reuseaddr(int sd)
 568 {
 569         unsigned int one = 1;
 570         int rc = 0;
 571
 572         if (setsockopt(sd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) {
 573                 log_err_errno("setsockopt(SO_REUSEADDR)");
 574                 rc = -1;
 575         }
 576
 577         return rc;
 578 }
 579
 580 static int str_to_uint(const char *str, int min, int max, unsigned int *value)
 581 {
 582         int number;
 583         char *end;
 584
 585         errno = 0;
 586         number = (unsigned int) strtoul(str, &end, 0);
 587
 588         /* entire string should be consumed by conversion
 589          * and value should be between min and max
 590          */
 591         if (((*end == '\0') || (*end == '\n')) && (end != str) &&
 592             (errno != ERANGE) && (min <= number) && (number <= max)) {
 593                 *value = number;
 594                 return 0;
 595         }
 596
 597         return -1;
 598 }
 599
 600 static int resolve_devices(struct sock_args *args)
 601 {
 602         if (args->dev) {
 603                 args->ifindex = get_ifidx(args->dev);
 604                 if (args->ifindex < 0) {
 605                         log_error("Invalid device name\n");
 606                         return 1;
 607                 }
 608         }
 609
 610         if (args->expected_dev) {
 611                 unsigned int tmp;
 612
 613                 if (str_to_uint(args->expected_dev, 0, INT_MAX, &tmp) == 0) {
 614                         args->expected_ifindex = (int)tmp;
 615                 } else {
 616                         args->expected_ifindex = get_ifidx(args->expected_dev);
 617                         if (args->expected_ifindex < 0) {
 618                                 fprintf(stderr, "Invalid expected device\n");
 619                                 return 1;
 620                         }
 621                 }
 622         }
 623
 624         return 0;
 625 }
 626
 627 static int expected_addr_match(struct sockaddr *sa, void *expected,
 628                                const char *desc)
 629 {
 630         char addrstr[64];
 631         int rc = 0;
 632
 633         if (sa->sa_family == AF_INET) {
 634                 struct sockaddr_in *s = (struct sockaddr_in *) sa;
 635                 struct in_addr *exp_in = (struct in_addr *) expected;
 636
 637                 if (s->sin_addr.s_addr != exp_in->s_addr) {
 638                         log_error("%s address does not match expected %s\n",
 639                                   desc,
 640                                   inet_ntop(AF_INET, exp_in,
 641                                             addrstr, sizeof(addrstr)));
 642                         rc = 1;
 643                 }
 644         } else if (sa->sa_family == AF_INET6) {
 645                 struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
 646                 struct in6_addr *exp_in = (struct in6_addr *) expected;
 647
 648                 if (memcmp(&s6->sin6_addr, exp_in, sizeof(*exp_in))) {
 649                         log_error("%s address does not match expected %s\n",
 650                                   desc,
 651                                   inet_ntop(AF_INET6, exp_in,
 652                                             addrstr, sizeof(addrstr)));
 653                         rc = 1;
 654                 }
 655         } else {
 656                 log_error("%s address does not match expected - unknown family\n",
 657                           desc);
 658                 rc = 1;
 659         }
 660
 661         if (!rc)
 662                 log_msg("%s address matches expected\n", desc);
 663
 664         return rc;
 665 }
 666
 667 static int show_sockstat(int sd, struct sock_args *args)
 668 {
 669         struct sockaddr_in6 local_addr, remote_addr;
 670         socklen_t alen = sizeof(local_addr);
 671         struct sockaddr *sa;
 672         const char *desc;
 673         int rc = 0;
 674
 675         desc = server_mode ? "server local:" : "client local:";
 676         sa = (struct sockaddr *) &local_addr;
 677         if (getsockname(sd, sa, &alen) == 0) {
 678                 log_address(desc, sa);
 679
 680                 if (args->has_expected_laddr) {
 681                         rc = expected_addr_match(sa, &args->expected_laddr,
 682                                                  "local");
 683                 }
 684         } else {
 685                 log_err_errno("getsockname failed");
 686         }
 687
 688         sa = (struct sockaddr *) &remote_addr;
 689         desc = server_mode ? "server peer:" : "client peer:";
 690         if (getpeername(sd, sa, &alen) == 0) {
 691                 log_address(desc, sa);
 692
 693                 if (args->has_expected_raddr) {
 694                         rc |= expected_addr_match(sa, &args->expected_raddr,
 695                                                  "remote");
 696                 }
 697         } else {
 698                 log_err_errno("getpeername failed");
 699         }
 700
 701         return rc;
 702 }
 703
 704 enum addr_type {
 705         ADDR_TYPE_LOCAL,
 706         ADDR_TYPE_REMOTE,
 707         ADDR_TYPE_MCAST,
 708         ADDR_TYPE_EXPECTED_LOCAL,
 709         ADDR_TYPE_EXPECTED_REMOTE,
 710         ADDR_TYPE_MD5_PREFIX,
 711 };
 712
 713 static int convert_addr(struct sock_args *args, const char *_str,
 714                         enum addr_type atype)
 715 {
 716         int pfx_len_max = args->version == AF_INET6 ? 128 : 32;
 717         int family = args->version;
 718         char *str, *dev, *sep;
 719         struct in6_addr *in6;
 720         struct in_addr  *in;
 721         const char *desc;
 722         void *addr;
 723         int rc = 0;
 724
 725         str = strdup(_str);
 726         if (!str)
 727                 return -ENOMEM;
 728
 729         switch (atype) {
 730         case ADDR_TYPE_LOCAL:
 731                 desc = "local";
 732                 addr = &args->local_addr;
 733                 break;
 734         case ADDR_TYPE_REMOTE:
 735                 desc = "remote";
 736                 addr = &args->remote_addr;
 737                 break;
 738         case ADDR_TYPE_MCAST:
 739                 desc = "mcast grp";
 740                 addr = &args->grp;
 741                 break;
 742         case ADDR_TYPE_EXPECTED_LOCAL:
 743                 desc = "expected local";
 744                 addr = &args->expected_laddr;
 745                 break;
 746         case ADDR_TYPE_EXPECTED_REMOTE:
 747                 desc = "expected remote";
 748                 addr = &args->expected_raddr;
 749                 break;
 750         case ADDR_TYPE_MD5_PREFIX:
 751                 desc = "md5 prefix";
 752                 if (family == AF_INET) {
 753                         args->md5_prefix.v4.sin_family = AF_INET;
 754                         addr = &args->md5_prefix.v4.sin_addr;
 755                 } else if (family == AF_INET6) {
 756                         args->md5_prefix.v6.sin6_family = AF_INET6;
 757                         addr = &args->md5_prefix.v6.sin6_addr;
 758                 } else
 759                         return 1;
 760
 761                 sep = strchr(str, '/');
 762                 if (sep) {
 763                         *sep = '\0';
 764                         sep++;
 765                         if (str_to_uint(sep, 1, pfx_len_max,
 766                                         &args->prefix_len) != 0) {
 767                                 fprintf(stderr, "Invalid port\n");
 768                                 return 1;
 769                         }
 770                 } else {
 771                         args->prefix_len = 0;
 772                 }
 773                 break;
 774         default:
 775                 log_error("unknown address type\n");
 776                 exit(1);
 777         }
 778
 779         switch (family) {
 780         case AF_INET:
 781                 in  = (struct in_addr *) addr;
 782                 if (str) {
 783                         if (inet_pton(AF_INET, str, in) == 0) {
 784                                 log_error("Invalid %s IP address\n", desc);
 785                                 rc = -1;
 786                                 goto out;
 787                         }
 788                 } else {
 789                         in->s_addr = htonl(INADDR_ANY);
 790                 }
 791                 break;
 792
 793         case AF_INET6:
 794                 dev = strchr(str, '%');
 795                 if (dev) {
 796                         *dev = '\0';
 797                         dev++;
 798                 }
 799
 800                 in6 = (struct in6_addr *) addr;
 801                 if (str) {
 802                         if (inet_pton(AF_INET6, str, in6) == 0) {
 803                                 log_error("Invalid %s IPv6 address\n", desc);
 804                                 rc = -1;
 805                                 goto out;
 806                         }
 807                 } else {
 808                         *in6 = in6addr_any;
 809                 }
 810                 if (dev) {
 811                         args->scope_id = get_ifidx(dev);
 812                         if (args->scope_id < 0) {
 813                                 log_error("Invalid scope on %s IPv6 address\n",
 814                                           desc);
 815                                 rc = -1;
 816                                 goto out;
 817                         }
 818                 }
 819                 break;
 820
 821         default:
 822                 log_error("Invalid address family\n");
 823         }
 824
 825 out:
 826         free(str);
 827         return rc;
 828 }
 829
 830 static int validate_addresses(struct sock_args *args)
 831 {
 832         if (args->local_addr_str &&
 833             convert_addr(args, args->local_addr_str, ADDR_TYPE_LOCAL) < 0)
 834                 return 1;
 835
 836         if (args->remote_addr_str &&
 837             convert_addr(args, args->remote_addr_str, ADDR_TYPE_REMOTE) < 0)
 838                 return 1;
 839
 840         if (args->md5_prefix_str &&
 841             convert_addr(args, args->md5_prefix_str,
 842                          ADDR_TYPE_MD5_PREFIX) < 0)
 843                 return 1;
 844
 845         if (args->expected_laddr_str &&
 846             convert_addr(args, args->expected_laddr_str,
 847                          ADDR_TYPE_EXPECTED_LOCAL))
 848                 return 1;
 849
 850         if (args->expected_raddr_str &&
 851             convert_addr(args, args->expected_raddr_str,
 852                          ADDR_TYPE_EXPECTED_REMOTE))
 853                 return 1;
 854
 855         return 0;
 856 }
 857
 858 static int get_index_from_cmsg(struct msghdr *m)
 859 {
 860         struct cmsghdr *cm;
 861         int ifindex = 0;
 862         char buf[64];
 863
 864         for (cm = (struct cmsghdr *)CMSG_FIRSTHDR(m);
 865              m->msg_controllen != 0 && cm;
 866              cm = (struct cmsghdr *)CMSG_NXTHDR(m, cm)) {
 867
 868                 if (cm->cmsg_level == SOL_IP &&
 869                     cm->cmsg_type == IP_PKTINFO) {
 870                         struct in_pktinfo *pi;
 871
 872                         pi = (struct in_pktinfo *)(CMSG_DATA(cm));
 873                         inet_ntop(AF_INET, &pi->ipi_addr, buf, sizeof(buf));
 874                         ifindex = pi->ipi_ifindex;
 875                 } else if (cm->cmsg_level == SOL_IPV6 &&
 876                            cm->cmsg_type == IPV6_PKTINFO) {
 877                         struct in6_pktinfo *pi6;
 878
 879                         pi6 = (struct in6_pktinfo *)(CMSG_DATA(cm));
 880                         inet_ntop(AF_INET6, &pi6->ipi6_addr, buf, sizeof(buf));
 881                         ifindex = pi6->ipi6_ifindex;
 882                 }
 883         }
 884
 885         if (ifindex) {
 886                 log_msg("    pktinfo: ifindex %d dest addr %s\n",
 887                         ifindex, buf);
 888         }
 889         return ifindex;
 890 }
 891
 892 static int send_msg_no_cmsg(int sd, void *addr, socklen_t alen)
 893 {
 894         int err;
 895
 896 again:
 897         err = sendto(sd, msg, msglen, 0, addr, alen);
 898         if (err < 0) {
 899                 if (errno == EACCES && try_broadcast) {
 900                         try_broadcast = 0;
 901                         if (!set_broadcast(sd))
 902                                 goto again;
 903                         errno = EACCES;
 904                 }
 905
 906                 log_err_errno("sendto failed");
 907                 return 1;
 908         }
 909
 910         return 0;
 911 }
 912
 913 static int send_msg_cmsg(int sd, void *addr, socklen_t alen,
 914                          int ifindex, int version)
 915 {
 916         unsigned char cmsgbuf[64];
 917         struct iovec iov[2];
 918         struct cmsghdr *cm;
 919         struct msghdr m;
 920         int err;
 921
 922         iov[0].iov_base = msg;
 923         iov[0].iov_len = msglen;
 924         m.msg_iov = iov;
 925         m.msg_iovlen = 1;
 926         m.msg_name = (caddr_t)addr;
 927         m.msg_namelen = alen;
 928
 929         memset(cmsgbuf, 0, sizeof(cmsgbuf));
 930         cm = (struct cmsghdr *)cmsgbuf;
 931         m.msg_control = (caddr_t)cm;
 932
 933         if (version == AF_INET) {
 934                 struct in_pktinfo *pi;
 935
 936                 cm->cmsg_level = SOL_IP;
 937                 cm->cmsg_type = IP_PKTINFO;
 938                 cm->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
 939                 pi = (struct in_pktinfo *)(CMSG_DATA(cm));
 940                 pi->ipi_ifindex = ifindex;
 941
 942                 m.msg_controllen = cm->cmsg_len;
 943
 944         } else if (version == AF_INET6) {
 945                 struct in6_pktinfo *pi6;
 946
 947                 cm->cmsg_level = SOL_IPV6;
 948                 cm->cmsg_type = IPV6_PKTINFO;
 949                 cm->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
 950
 951                 pi6 = (struct in6_pktinfo *)(CMSG_DATA(cm));
 952                 pi6->ipi6_ifindex = ifindex;
 953
 954                 m.msg_controllen = cm->cmsg_len;
 955         }
 956
 957 again:
 958         err = sendmsg(sd, &m, 0);
 959         if (err < 0) {
 960                 if (errno == EACCES && try_broadcast) {
 961                         try_broadcast = 0;
 962                         if (!set_broadcast(sd))
 963                                 goto again;
 964                         errno = EACCES;
 965                 }
 966
 967                 log_err_errno("sendmsg failed");
 968                 return 1;
 969         }
 970
 971         return 0;
 972 }
 973
 974
 975 static int send_msg(int sd, void *addr, socklen_t alen, struct sock_args *args)
 976 {
 977         if (args->type == SOCK_STREAM) {
 978                 if (write(sd, msg, msglen) < 0) {
 979                         log_err_errno("write failed sending msg to peer");
 980                         return 1;
 981                 }
 982         } else if (args->ifindex && args->use_cmsg) {
 983                 if (send_msg_cmsg(sd, addr, alen, args->ifindex, args->version))
 984                         return 1;
 985         } else {
 986                 if (send_msg_no_cmsg(sd, addr, alen))
 987                         return 1;
 988         }
 989
 990         log_msg("Sent message:\n");
 991         log_msg("    %.24s%s\n", msg, msglen > 24 ? " ..." : "");
 992
 993         return 0;
 994 }
 995
 996 static int socket_read_dgram(int sd, struct sock_args *args)
 997 {
 998         unsigned char addr[sizeof(struct sockaddr_in6)];
 999         struct sockaddr *sa = (struct sockaddr *) addr;
1000         socklen_t alen = sizeof(addr);
1001         struct iovec iov[2];
1002         struct msghdr m = {
1003                 .msg_name = (caddr_t)addr,
1004                 .msg_namelen = alen,
1005                 .msg_iov = iov,
1006                 .msg_iovlen = 1,
1007         };
1008         unsigned char cmsgbuf[256];
1009         struct cmsghdr *cm = (struct cmsghdr *)cmsgbuf;
1010         char buf[16*1024];
1011         int ifindex;
1012         int len;
1013
1014         iov[0].iov_base = (caddr_t)buf;
1015         iov[0].iov_len = sizeof(buf);
1016
1017         memset(cmsgbuf, 0, sizeof(cmsgbuf));
1018         m.msg_control = (caddr_t)cm;
1019         m.msg_controllen = sizeof(cmsgbuf);
1020
1021         len = recvmsg(sd, &m, 0);
1022         if (len == 0) {
1023                 log_msg("peer closed connection.\n");
1024                 return 0;
1025         } else if (len < 0) {
1026                 log_msg("failed to read message: %d: %s\n",
1027                         errno, strerror(errno));
1028                 return -1;
1029         }
1030
1031         buf[len] = '\0';
1032
1033         log_address("Message from:", sa);
1034         log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1035
1036         ifindex = get_index_from_cmsg(&m);
1037         if (args->expected_ifindex) {
1038                 if (args->expected_ifindex != ifindex) {
1039                         log_error("Device index mismatch: expected %d have %d\n",
1040                                   args->expected_ifindex, ifindex);
1041                         return -1;
1042                 }
1043                 log_msg("Device index matches: expected %d have %d\n",
1044                         args->expected_ifindex, ifindex);
1045         }
1046
1047         if (!interactive && server_mode) {
1048                 if (sa->sa_family == AF_INET6) {
1049                         struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
1050                         struct in6_addr *in6 = &s6->sin6_addr;
1051
1052                         if (IN6_IS_ADDR_V4MAPPED(in6)) {
1053                                 const uint32_t *pa = (uint32_t *) &in6->s6_addr;
1054                                 struct in_addr in4;
1055                                 struct sockaddr_in *sin;
1056
1057                                 sin = (struct sockaddr_in *) addr;
1058                                 pa += 3;
1059                                 in4.s_addr = *pa;
1060                                 sin->sin_addr = in4;
1061                                 sin->sin_family = AF_INET;
1062                                 if (send_msg_cmsg(sd, addr, alen,
1063                                                   ifindex, AF_INET) < 0)
1064                                         goto out_err;
1065                         }
1066                 }
1067 again:
1068                 iov[0].iov_len = len;
1069
1070                 if (args->version == AF_INET6) {
1071                         struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
1072
1073                         if (args->dev) {
1074                                 /* avoid PKTINFO conflicts with bindtodev */
1075                                 if (sendto(sd, buf, len, 0,
1076                                            (void *) addr, alen) < 0)
1077                                         goto out_err;
1078                         } else {
1079                                 /* kernel is allowing scope_id to be set to VRF
1080                                  * index for LLA. for sends to global address
1081                                  * reset scope id
1082                                  */
1083                                 s6->sin6_scope_id = ifindex;
1084                                 if (sendmsg(sd, &m, 0) < 0)
1085                                         goto out_err;
1086                         }
1087                 } else {
1088                         int err;
1089
1090                         err = sendmsg(sd, &m, 0);
1091                         if (err < 0) {
1092                                 if (errno == EACCES && try_broadcast) {
1093                                         try_broadcast = 0;
1094                                         if (!set_broadcast(sd))
1095                                                 goto again;
1096                                         errno = EACCES;
1097                                 }
1098                                 goto out_err;
1099                         }
1100                 }
1101                 log_msg("Sent message:\n");
1102                 log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1103         }
1104
1105         return 1;
1106 out_err:
1107         log_err_errno("failed to send msg to peer");
1108         return -1;
1109 }
1110
1111 static int socket_read_stream(int sd)
1112 {
1113         char buf[1024];
1114         int len;
1115
1116         len = read(sd, buf, sizeof(buf)-1);
1117         if (len == 0) {
1118                 log_msg("client closed connection.\n");
1119                 return 0;
1120         } else if (len < 0) {
1121                 log_msg("failed to read message\n");
1122                 return -1;
1123         }
1124
1125         buf[len] = '\0';
1126         log_msg("Incoming message:\n");
1127         log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1128
1129         if (!interactive && server_mode) {
1130                 if (write(sd, buf, len) < 0) {
1131                         log_err_errno("failed to send buf");
1132                         return -1;
1133                 }
1134                 log_msg("Sent message:\n");
1135                 log_msg("     %.24s%s\n", buf, len > 24 ? " ..." : "");
1136         }
1137
1138         return 1;
1139 }
1140
1141 static int socket_read(int sd, struct sock_args *args)
1142 {
1143         if (args->type == SOCK_STREAM)
1144                 return socket_read_stream(sd);
1145
1146         return socket_read_dgram(sd, args);
1147 }
1148
1149 static int stdin_to_socket(int sd, int type, void *addr, socklen_t alen)
1150 {
1151         char buf[1024];
1152         int len;
1153
1154         if (fgets(buf, sizeof(buf), stdin) == NULL)
1155                 return 0;
1156
1157         len = strlen(buf);
1158         if (type == SOCK_STREAM) {
1159                 if (write(sd, buf, len) < 0) {
1160                         log_err_errno("failed to send buf");
1161                         return -1;
1162                 }
1163         } else {
1164                 int err;
1165
1166 again:
1167                 err = sendto(sd, buf, len, 0, addr, alen);
1168                 if (err < 0) {
1169                         if (errno == EACCES && try_broadcast) {
1170                                 try_broadcast = 0;
1171                                 if (!set_broadcast(sd))
1172                                         goto again;
1173                                 errno = EACCES;
1174                         }
1175                         log_err_errno("failed to send msg to peer");
1176                         return -1;
1177                 }
1178         }
1179         log_msg("Sent message:\n");
1180         log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1181
1182         return 1;
1183 }
1184
1185 static void set_recv_attr(int sd, int version)
1186 {
1187         if (version == AF_INET6) {
1188                 set_recvpktinfo_v6(sd);
1189                 set_recverr_v6(sd);
1190         } else {
1191                 set_pktinfo_v4(sd);
1192                 set_recverr_v4(sd);
1193         }
1194 }
1195
1196 static int msg_loop(int client, int sd, void *addr, socklen_t alen,
1197                     struct sock_args *args)
1198 {
1199         struct timeval timeout = { .tv_sec = prog_timeout }, *ptval = NULL;
1200         fd_set rfds;
1201         int nfds;
1202         int rc;
1203
1204         if (args->type != SOCK_STREAM)
1205                 set_recv_attr(sd, args->version);
1206
1207         if (msg) {
1208                 msglen = strlen(msg);
1209
1210                 /* client sends first message */
1211                 if (client) {
1212                         if (send_msg(sd, addr, alen, args))
1213                                 return 1;
1214                 }
1215                 if (!interactive) {
1216                         ptval = &timeout;
1217                         if (!prog_timeout)
1218                                 timeout.tv_sec = 5;
1219                 }
1220         }
1221
1222         nfds = interactive ? MAX(fileno(stdin), sd)  + 1 : sd + 1;
1223         while (1) {
1224                 FD_ZERO(&rfds);
1225                 FD_SET(sd, &rfds);
1226                 if (interactive)
1227                         FD_SET(fileno(stdin), &rfds);
1228
1229                 rc = select(nfds, &rfds, NULL, NULL, ptval);
1230                 if (rc < 0) {
1231                         if (errno == EINTR)
1232                                 continue;
1233
1234                         rc = 1;
1235                         log_err_errno("select failed");
1236                         break;
1237                 } else if (rc == 0) {
1238                         log_error("Timed out waiting for response\n");
1239                         rc = 2;
1240                         break;
1241                 }
1242
1243                 if (FD_ISSET(sd, &rfds)) {
1244                         rc = socket_read(sd, args);
1245                         if (rc < 0) {
1246                                 rc = 1;
1247                                 break;
1248                         }
1249                         if (rc == 0)
1250                                 break;
1251                 }
1252
1253                 rc = 0;
1254
1255                 if (FD_ISSET(fileno(stdin), &rfds)) {
1256                         if (stdin_to_socket(sd, args->type, addr, alen) <= 0)
1257                                 break;
1258                 }
1259
1260                 if (interactive)
1261                         continue;
1262
1263                 if (iter != -1) {
1264                         --iter;
1265                         if (iter == 0)
1266                                 break;
1267                 }
1268
1269                 log_msg("Going into quiet mode\n");
1270                 quiet = 1;
1271
1272                 if (client) {
1273                         if (send_msg(sd, addr, alen, args)) {
1274                                 rc = 1;
1275                                 break;
1276                         }
1277                 }
1278         }
1279
1280         return rc;
1281 }
1282
1283 static int msock_init(struct sock_args *args, int server)
1284 {
1285         uint32_t if_addr = htonl(INADDR_ANY);
1286         struct sockaddr_in laddr = {
1287                 .sin_family = AF_INET,
1288                 .sin_port = htons(args->port),
1289         };
1290         int one = 1;
1291         int sd;
1292
1293         if (!server && args->has_local_ip)
1294                 if_addr = args->local_addr.in.s_addr;
1295
1296         sd = socket(PF_INET, SOCK_DGRAM, 0);
1297         if (sd < 0) {
1298                 log_err_errno("socket");
1299                 return -1;
1300         }
1301
1302         if (setsockopt(sd, SOL_SOCKET, SO_REUSEADDR,
1303                        (char *)&one, sizeof(one)) < 0) {
1304                 log_err_errno("Setting SO_REUSEADDR error");
1305                 goto out_err;
1306         }
1307
1308         if (setsockopt(sd, SOL_SOCKET, SO_BROADCAST,
1309                        (char *)&one, sizeof(one)) < 0)
1310                 log_err_errno("Setting SO_BROADCAST error");
1311
1312         if (args->dev && bind_to_device(sd, args->dev) != 0)
1313                 goto out_err;
1314         else if (args->use_setsockopt &&
1315                  set_multicast_if(sd, args->ifindex))
1316                 goto out_err;
1317
1318         laddr.sin_addr.s_addr = if_addr;
1319
1320         if (bind(sd, (struct sockaddr *) &laddr, sizeof(laddr)) < 0) {
1321                 log_err_errno("bind failed");
1322                 goto out_err;
1323         }
1324
1325         if (server &&
1326             set_membership(sd, args->grp.s_addr,
1327                            args->local_addr.in.s_addr, args->ifindex))
1328                 goto out_err;
1329
1330         return sd;
1331 out_err:
1332         close(sd);
1333         return -1;
1334 }
1335
1336 static int msock_server(struct sock_args *args)
1337 {
1338         return msock_init(args, 1);
1339 }
1340
1341 static int msock_client(struct sock_args *args)
1342 {
1343         return msock_init(args, 0);
1344 }
1345
1346 static int bind_socket(int sd, struct sock_args *args)
1347 {
1348         struct sockaddr_in serv_addr = {
1349                 .sin_family = AF_INET,
1350         };
1351         struct sockaddr_in6 serv6_addr = {
1352                 .sin6_family = AF_INET6,
1353         };
1354         void *addr;
1355         socklen_t alen;
1356
1357         if (!args->has_local_ip && args->type == SOCK_RAW)
1358                 return 0;
1359
1360         switch (args->version) {
1361         case AF_INET:
1362                 serv_addr.sin_port = htons(args->port);
1363                 serv_addr.sin_addr = args->local_addr.in;
1364                 addr = &serv_addr;
1365                 alen = sizeof(serv_addr);
1366                 break;
1367
1368         case AF_INET6:
1369                 serv6_addr.sin6_port = htons(args->port);
1370                 serv6_addr.sin6_addr = args->local_addr.in6;
1371                 addr = &serv6_addr;
1372                 alen = sizeof(serv6_addr);
1373                 break;
1374
1375         default:
1376                 log_error("Invalid address family\n");
1377                 return -1;
1378         }
1379
1380         if (bind(sd, addr, alen) < 0) {
1381                 log_err_errno("error binding socket");
1382                 return -1;
1383         }
1384
1385         return 0;
1386 }
1387
1388 static int config_xfrm_policy(int sd, struct sock_args *args)
1389 {
1390         struct xfrm_userpolicy_info policy = {};
1391         int type = UDP_ENCAP_ESPINUDP;
1392         int xfrm_af = IP_XFRM_POLICY;
1393         int level = SOL_IP;
1394
1395         if (args->type != SOCK_DGRAM) {
1396                 log_error("Invalid socket type. Only DGRAM could be used for XFRM\n");
1397                 return 1;
1398         }
1399
1400         policy.action = XFRM_POLICY_ALLOW;
1401         policy.sel.family = args->version;
1402         if (args->version == AF_INET6) {
1403                 xfrm_af = IPV6_XFRM_POLICY;
1404                 level = SOL_IPV6;
1405         }
1406
1407         policy.dir = XFRM_POLICY_OUT;
1408         if (setsockopt(sd, level, xfrm_af, &policy, sizeof(policy)) < 0)
1409                 return 1;
1410
1411         policy.dir = XFRM_POLICY_IN;
1412         if (setsockopt(sd, level, xfrm_af, &policy, sizeof(policy)) < 0)
1413                 return 1;
1414
1415         if (setsockopt(sd, IPPROTO_UDP, UDP_ENCAP, &type, sizeof(type)) < 0) {
1416                 log_err_errno("Failed to set xfrm encap");
1417                 return 1;
1418         }
1419
1420         return 0;
1421 }
1422
1423 static int lsock_init(struct sock_args *args)
1424 {
1425         long flags;
1426         int sd;
1427
1428         sd = socket(args->version, args->type, args->protocol);
1429         if (sd < 0) {
1430                 log_err_errno("Error opening socket");
1431                 return  -1;
1432         }
1433
1434         if (set_reuseaddr(sd) != 0)
1435                 goto err;
1436
1437         if (set_reuseport(sd) != 0)
1438                 goto err;
1439
1440         if (args->dev && bind_to_device(sd, args->dev) != 0)
1441                 goto err;
1442         else if (args->use_setsockopt &&
1443                  set_unicast_if(sd, args->ifindex, args->version))
1444                 goto err;
1445
1446         if (args->use_freebind && set_freebind(sd, args->version))
1447                 goto err;
1448
1449         if (bind_socket(sd, args))
1450                 goto err;
1451
1452         if (args->bind_test_only)
1453                 goto out;
1454
1455         if (args->type == SOCK_STREAM && listen(sd, 1) < 0) {
1456                 log_err_errno("listen failed");
1457                 goto err;
1458         }
1459
1460         flags = fcntl(sd, F_GETFL);
1461         if ((flags < 0) || (fcntl(sd, F_SETFL, flags|O_NONBLOCK) < 0)) {
1462                 log_err_errno("Failed to set non-blocking option");
1463                 goto err;
1464         }
1465
1466         if (fcntl(sd, F_SETFD, FD_CLOEXEC) < 0)
1467                 log_err_errno("Failed to set close-on-exec flag");
1468
1469         if (args->use_xfrm && config_xfrm_policy(sd, args)) {
1470                 log_err_errno("Failed to set xfrm policy");
1471                 goto err;
1472         }
1473
1474 out:
1475         return sd;
1476
1477 err:
1478         close(sd);
1479         return -1;
1480 }
1481
1482 static void ipc_write(int fd, int message)
1483 {
1484         /* Not in both_mode, so there's no process to signal */
1485         if (fd < 0)
1486                 return;
1487
1488         if (write(fd, &message, sizeof(message)) < 0)
1489                 log_err_errno("Failed to send client status");
1490 }
1491
1492 static int do_server(struct sock_args *args, int ipc_fd)
1493 {
1494         /* ipc_fd = -1 if no parent process to signal */
1495         struct timeval timeout = { .tv_sec = prog_timeout }, *ptval = NULL;
1496         unsigned char addr[sizeof(struct sockaddr_in6)] = {};
1497         socklen_t alen = sizeof(addr);
1498         int lsd, csd = -1;
1499
1500         fd_set rfds;
1501         int rc;
1502
1503         if (args->serverns) {
1504                 if (switch_ns(args->serverns)) {
1505                         log_error("Could not set server netns to %s\n",
1506                                   args->serverns);
1507                         goto err_exit;
1508                 }
1509                 log_msg("Switched server netns\n");
1510         }
1511
1512         args->dev = args->server_dev;
1513         args->expected_dev = args->expected_server_dev;
1514         if (resolve_devices(args) || validate_addresses(args))
1515                 goto err_exit;
1516
1517         if (prog_timeout)
1518                 ptval = &timeout;
1519
1520         if (args->has_grp)
1521                 lsd = msock_server(args);
1522         else
1523                 lsd = lsock_init(args);
1524
1525         if (lsd < 0)
1526                 goto err_exit;
1527
1528         if (args->bind_test_only) {
1529                 close(lsd);
1530                 ipc_write(ipc_fd, 1);
1531                 return 0;
1532         }
1533
1534         if (args->type != SOCK_STREAM) {
1535                 ipc_write(ipc_fd, 1);
1536                 rc = msg_loop(0, lsd, (void *) addr, alen, args);
1537                 close(lsd);
1538                 return rc;
1539         }
1540
1541         if (args->password && tcp_md5_remote(lsd, args)) {
1542                 close(lsd);
1543                 goto err_exit;
1544         }
1545
1546         ipc_write(ipc_fd, 1);
1547         while (1) {
1548                 log_msg("waiting for client connection.\n");
1549                 FD_ZERO(&rfds);
1550                 FD_SET(lsd, &rfds);
1551
1552                 rc = select(lsd+1, &rfds, NULL, NULL, ptval);
1553                 if (rc == 0) {
1554                         rc = 2;
1555                         break;
1556                 }
1557
1558                 if (rc < 0) {
1559                         if (errno == EINTR)
1560                                 continue;
1561
1562                         log_err_errno("select failed");
1563                         break;
1564                 }
1565
1566                 if (FD_ISSET(lsd, &rfds)) {
1567
1568                         csd = accept(lsd, (void *) addr, &alen);
1569                         if (csd < 0) {
1570                                 log_err_errno("accept failed");
1571                                 break;
1572                         }
1573
1574                         rc = show_sockstat(csd, args);
1575                         if (rc)
1576                                 break;
1577
1578                         rc = check_device(csd, args);
1579                         if (rc)
1580                                 break;
1581                 }
1582
1583                 rc = msg_loop(0, csd, (void *) addr, alen, args);
1584                 close(csd);
1585
1586                 if (!interactive)
1587                         break;
1588         }
1589
1590         close(lsd);
1591
1592         return rc;
1593 err_exit:
1594         ipc_write(ipc_fd, 0);
1595         return 1;
1596 }
1597
1598 static int wait_for_connect(int sd)
1599 {
1600         struct timeval _tv = { .tv_sec = prog_timeout }, *tv = NULL;
1601         fd_set wfd;
1602         int val = 0, sz = sizeof(val);
1603         int rc;
1604
1605         FD_ZERO(&wfd);
1606         FD_SET(sd, &wfd);
1607
1608         if (prog_timeout)
1609                 tv = &_tv;
1610
1611         rc = select(FD_SETSIZE, NULL, &wfd, NULL, tv);
1612         if (rc == 0) {
1613                 log_error("connect timed out\n");
1614                 return -2;
1615         } else if (rc < 0) {
1616                 log_err_errno("select failed");
1617                 return -3;
1618         }
1619
1620         if (getsockopt(sd, SOL_SOCKET, SO_ERROR, &val, (socklen_t *)&sz) < 0) {
1621                 log_err_errno("getsockopt(SO_ERROR) failed");
1622                 return -4;
1623         }
1624
1625         if (val != 0) {
1626                 log_error("connect failed: %d: %s\n", val, strerror(val));
1627                 return -1;
1628         }
1629
1630         return 0;
1631 }
1632
1633 static int connectsock(void *addr, socklen_t alen, struct sock_args *args)
1634 {
1635         int sd, rc = -1;
1636         long flags;
1637
1638         sd = socket(args->version, args->type, args->protocol);
1639         if (sd < 0) {
1640                 log_err_errno("Failed to create socket");
1641                 return -1;
1642         }
1643
1644         flags = fcntl(sd, F_GETFL);
1645         if ((flags < 0) || (fcntl(sd, F_SETFL, flags|O_NONBLOCK) < 0)) {
1646                 log_err_errno("Failed to set non-blocking option");
1647                 goto err;
1648         }
1649
1650         if (set_reuseport(sd) != 0)
1651                 goto err;
1652
1653         if (args->dev && bind_to_device(sd, args->dev) != 0)
1654                 goto err;
1655         else if (args->use_setsockopt &&
1656                  set_unicast_if(sd, args->ifindex, args->version))
1657                 goto err;
1658
1659         if (args->has_local_ip && bind_socket(sd, args))
1660                 goto err;
1661
1662         if (args->type != SOCK_STREAM)
1663                 goto out;
1664
1665         if (args->password && tcp_md5sig(sd, addr, alen, args))
1666                 goto err;
1667
1668         if (args->bind_test_only)
1669                 goto out;
1670
1671         if (connect(sd, addr, alen) < 0) {
1672                 if (errno != EINPROGRESS) {
1673                         log_err_errno("Failed to connect to remote host");
1674                         rc = -1;
1675                         goto err;
1676                 }
1677                 rc = wait_for_connect(sd);
1678                 if (rc < 0)
1679                         goto err;
1680         }
1681 out:
1682         return sd;
1683
1684 err:
1685         close(sd);
1686         return rc;
1687 }
1688
1689 static int do_client(struct sock_args *args)
1690 {
1691         struct sockaddr_in sin = {
1692                 .sin_family = AF_INET,
1693         };
1694         struct sockaddr_in6 sin6 = {
1695                 .sin6_family = AF_INET6,
1696         };
1697         void *addr;
1698         int alen;
1699         int rc = 0;
1700         int sd;
1701
1702         if (!args->has_remote_ip && !args->has_grp) {
1703                 fprintf(stderr, "remote IP or multicast group not given\n");
1704                 return 1;
1705         }
1706
1707         if (args->clientns) {
1708                 if (switch_ns(args->clientns)) {
1709                         log_error("Could not set client netns to %s\n",
1710                                   args->clientns);
1711                         return 1;
1712                 }
1713                 log_msg("Switched client netns\n");
1714         }
1715
1716         args->local_addr_str = args->client_local_addr_str;
1717         if (resolve_devices(args) || validate_addresses(args))
1718                 return 1;
1719
1720         if ((args->use_setsockopt || args->use_cmsg) && !args->ifindex) {
1721                 fprintf(stderr, "Device binding not specified\n");
1722                 return 1;
1723         }
1724         if (args->use_setsockopt || args->use_cmsg)
1725                 args->dev = NULL;
1726
1727         switch (args->version) {
1728         case AF_INET:
1729                 sin.sin_port = htons(args->port);
1730                 if (args->has_grp)
1731                         sin.sin_addr = args->grp;
1732                 else
1733                         sin.sin_addr = args->remote_addr.in;
1734                 addr = &sin;
1735                 alen = sizeof(sin);
1736                 break;
1737         case AF_INET6:
1738                 sin6.sin6_port = htons(args->port);
1739                 sin6.sin6_addr = args->remote_addr.in6;
1740                 sin6.sin6_scope_id = args->scope_id;
1741                 addr = &sin6;
1742                 alen = sizeof(sin6);
1743                 break;
1744         }
1745
1746         args->password = args->client_pw;
1747
1748         if (args->has_grp)
1749                 sd = msock_client(args);
1750         else
1751                 sd = connectsock(addr, alen, args);
1752
1753         if (sd < 0)
1754                 return -sd;
1755
1756         if (args->bind_test_only)
1757                 goto out;
1758
1759         if (args->type == SOCK_STREAM) {
1760                 rc = show_sockstat(sd, args);
1761                 if (rc != 0)
1762                         goto out;
1763         }
1764
1765         rc = msg_loop(1, sd, addr, alen, args);
1766
1767 out:
1768         close(sd);
1769
1770         return rc;
1771 }
1772
1773 static char *random_msg(int len)
1774 {
1775         int i, n = 0, olen = len + 1;
1776         char *m;
1777
1778         if (len <= 0)
1779                 return NULL;
1780
1781         m = malloc(olen);
1782         if (!m)
1783                 return NULL;
1784
1785         while (len > 26) {
1786                 i = snprintf(m + n, olen - n, "%.26s",
1787                              "abcdefghijklmnopqrstuvwxyz");
1788                 n += i;
1789                 len -= i;
1790         }
1791         i = snprintf(m + n, olen - n, "%.*s", len,
1792                      "abcdefghijklmnopqrstuvwxyz");
1793         return m;
1794 }
1795
1796 static int ipc_child(int fd, struct sock_args *args)
1797 {
1798         char *outbuf, *errbuf;
1799         int rc = 1;
1800
1801         outbuf = malloc(4096);
1802         errbuf = malloc(4096);
1803         if (!outbuf || !errbuf) {
1804                 fprintf(stderr, "server: Failed to allocate buffers for stdout and stderr\n");
1805                 goto out;
1806         }
1807
1808         setbuffer(stdout, outbuf, 4096);
1809         setbuffer(stderr, errbuf, 4096);
1810
1811         server_mode = 1; /* to tell log_msg in case we are in both_mode */
1812
1813         /* when running in both mode, address validation applies
1814          * solely to client side
1815          */
1816         args->has_expected_laddr = 0;
1817         args->has_expected_raddr = 0;
1818
1819         rc = do_server(args, fd);
1820
1821 out:
1822         free(outbuf);
1823         free(errbuf);
1824
1825         return rc;
1826 }
1827
1828 static int ipc_parent(int cpid, int fd, struct sock_args *args)
1829 {
1830         int client_status;
1831         int status;
1832         int buf;
1833
1834         /* do the client-side function here in the parent process,
1835          * waiting to be told when to continue
1836          */
1837         if (read(fd, &buf, sizeof(buf)) <= 0) {
1838                 log_err_errno("Failed to read IPC status from status");
1839                 return 1;
1840         }
1841         if (!buf) {
1842                 log_error("Server failed; can not continue\n");
1843                 return 1;
1844         }
1845         log_msg("Server is ready\n");
1846
1847         client_status = do_client(args);
1848         log_msg("parent is done!\n");
1849
1850         if (kill(cpid, 0) == 0)
1851                 kill(cpid, SIGKILL);
1852
1853         wait(&status);
1854         return client_status;
1855 }
1856
1857 #define GETOPT_STR  "sr:l:c:p:t:g:P:DRn:M:X:m:d:I:BN:O:SCi6xL:0:1:2:3:Fbqf"
1858 #define OPT_FORCE_BIND_KEY_IFINDEX 1001
1859 #define OPT_NO_BIND_KEY_IFINDEX 1002
1860
1861 static struct option long_opts[] = {
1862         {"force-bind-key-ifindex", 0, 0, OPT_FORCE_BIND_KEY_IFINDEX},
1863         {"no-bind-key-ifindex", 0, 0, OPT_NO_BIND_KEY_IFINDEX},
1864         {0, 0, 0, 0}
1865 };
1866
1867 static void print_usage(char *prog)
1868 {
1869         printf(
1870         "usage: %s OPTS\n"
1871         "Required:\n"
1872         "    -r addr       remote address to connect to (client mode only)\n"
1873         "    -p port       port to connect to (client mode)/listen on (server mode)\n"
1874         "                  (default: %d)\n"
1875         "    -s            server mode (default: client mode)\n"
1876         "    -t            timeout seconds (default: none)\n"
1877         "\n"
1878         "Optional:\n"
1879         "    -B            do both client and server via fork and IPC\n"
1880         "    -N ns         set client to network namespace ns (requires root)\n"
1881         "    -O ns         set server to network namespace ns (requires root)\n"
1882         "    -F            Restart server loop\n"
1883         "    -6            IPv6 (default is IPv4)\n"
1884         "    -P proto      protocol for socket: icmp, ospf (default: none)\n"
1885         "    -D|R          datagram (D) / raw (R) socket (default stream)\n"
1886         "    -l addr       local address to bind to in server mode\n"
1887         "    -c addr       local address to bind to in client mode\n"
1888         "    -x            configure XFRM policy on socket\n"
1889         "\n"
1890         "    -d dev        bind socket to given device name\n"
1891         "    -I dev        bind socket to given device name - server mode\n"
1892         "    -S            use setsockopt (IP_UNICAST_IF or IP_MULTICAST_IF)\n"
1893         "                  to set device binding\n"
1894         "    -f            bind socket with the IP[V6]_FREEBIND option\n"
1895         "    -C            use cmsg and IP_PKTINFO to specify device binding\n"
1896         "\n"
1897         "    -L len        send random message of given length\n"
1898         "    -n num        number of times to send message\n"
1899         "\n"
1900         "    -M password   use MD5 sum protection\n"
1901         "    -X password   MD5 password for client mode\n"
1902         "    -m prefix/len prefix and length to use for MD5 key\n"
1903         "    --no-bind-key-ifindex: Force TCP_MD5SIG_FLAG_IFINDEX off\n"
1904         "    --force-bind-key-ifindex: Force TCP_MD5SIG_FLAG_IFINDEX on\n"
1905         "        (default: only if -I is passed)\n"
1906         "\n"
1907         "    -g grp        multicast group (e.g., 239.1.1.1)\n"
1908         "    -i            interactive mode (default is echo and terminate)\n"
1909         "\n"
1910         "    -0 addr       Expected local address\n"
1911         "    -1 addr       Expected remote address\n"
1912         "    -2 dev        Expected device name (or index) to receive packet\n"
1913         "    -3 dev        Expected device name (or index) to receive packets - server mode\n"
1914         "\n"
1915         "    -b            Bind test only.\n"
1916         "    -q            Be quiet. Run test without printing anything.\n"
1917         , prog, DEFAULT_PORT);
1918 }
1919
1920 int main(int argc, char *argv[])
1921 {
1922         struct sock_args args = {
1923                 .version = AF_INET,
1924                 .type    = SOCK_STREAM,
1925                 .port    = DEFAULT_PORT,
1926         };
1927         struct protoent *pe;
1928         int both_mode = 0;
1929         unsigned int tmp;
1930         int forever = 0;
1931         int fd[2];
1932         int cpid;
1933
1934         /* process inputs */
1935         extern char *optarg;
1936         int rc = 0;
1937
1938         /*
1939          * process input args
1940          */
1941
1942         while ((rc = getopt_long(argc, argv, GETOPT_STR, long_opts, NULL)) != -1) {
1943                 switch (rc) {
1944                 case 'B':
1945                         both_mode = 1;
1946                         break;
1947                 case 's':
1948                         server_mode = 1;
1949                         break;
1950                 case 'F':
1951                         forever = 1;
1952                         break;
1953                 case 'l':
1954                         args.has_local_ip = 1;
1955                         args.local_addr_str = optarg;
1956                         break;
1957                 case 'r':
1958                         args.has_remote_ip = 1;
1959                         args.remote_addr_str = optarg;
1960                         break;
1961                 case 'c':
1962                         args.has_local_ip = 1;
1963                         args.client_local_addr_str = optarg;
1964                         break;
1965                 case 'p':
1966                         if (str_to_uint(optarg, 1, 65535, &tmp) != 0) {
1967                                 fprintf(stderr, "Invalid port\n");
1968                                 return 1;
1969                         }
1970                         args.port = (unsigned short) tmp;
1971                         break;
1972                 case 't':
1973                         if (str_to_uint(optarg, 0, INT_MAX,
1974                                         &prog_timeout) != 0) {
1975                                 fprintf(stderr, "Invalid timeout\n");
1976                                 return 1;
1977                         }
1978                         break;
1979                 case 'D':
1980                         args.type = SOCK_DGRAM;
1981                         break;
1982                 case 'R':
1983                         args.type = SOCK_RAW;
1984                         args.port = 0;
1985                         if (!args.protocol)
1986                                 args.protocol = IPPROTO_RAW;
1987                         break;
1988                 case 'P':
1989                         pe = getprotobyname(optarg);
1990                         if (pe) {
1991                                 args.protocol = pe->p_proto;
1992                         } else {
1993                                 if (str_to_uint(optarg, 0, 0xffff, &tmp) != 0) {
1994                                         fprintf(stderr, "Invalid protocol\n");
1995                                         return 1;
1996                                 }
1997                                 args.protocol = tmp;
1998                         }
1999                         break;
2000                 case 'n':
2001                         iter = atoi(optarg);
2002                         break;
2003                 case 'N':
2004                         args.clientns = optarg;
2005                         break;
2006                 case 'O':
2007                         args.serverns = optarg;
2008                         break;
2009                 case 'L':
2010                         msg = random_msg(atoi(optarg));
2011                         break;
2012                 case 'M':
2013                         args.password = optarg;
2014                         break;
2015                 case OPT_FORCE_BIND_KEY_IFINDEX:
2016                         args.bind_key_ifindex = 1;
2017                         break;
2018                 case OPT_NO_BIND_KEY_IFINDEX:
2019                         args.bind_key_ifindex = -1;
2020                         break;
2021                 case 'X':
2022                         args.client_pw = optarg;
2023                         break;
2024                 case 'm':
2025                         args.md5_prefix_str = optarg;
2026                         break;
2027                 case 'S':
2028                         args.use_setsockopt = 1;
2029                         break;
2030                 case 'f':
2031                         args.use_freebind = 1;
2032                         break;
2033                 case 'C':
2034                         args.use_cmsg = 1;
2035                         break;
2036                 case 'd':
2037                         args.dev = optarg;
2038                         break;
2039                 case 'I':
2040                         args.server_dev = optarg;
2041                         break;
2042                 case 'i':
2043                         interactive = 1;
2044                         break;
2045                 case 'g':
2046                         args.has_grp = 1;
2047                         if (convert_addr(&args, optarg, ADDR_TYPE_MCAST) < 0)
2048                                 return 1;
2049                         args.type = SOCK_DGRAM;
2050                         break;
2051                 case '6':
2052                         args.version = AF_INET6;
2053                         break;
2054                 case 'b':
2055                         args.bind_test_only = 1;
2056                         break;
2057                 case '0':
2058                         args.has_expected_laddr = 1;
2059                         args.expected_laddr_str = optarg;
2060                         break;
2061                 case '1':
2062                         args.has_expected_raddr = 1;
2063                         args.expected_raddr_str = optarg;
2064                         break;
2065                 case '2':
2066                         args.expected_dev = optarg;
2067                         break;
2068                 case '3':
2069                         args.expected_server_dev = optarg;
2070                         break;
2071                 case 'q':
2072                         quiet = 1;
2073                         break;
2074                 case 'x':
2075                         args.use_xfrm = 1;
2076                         break;
2077                 default:
2078                         print_usage(argv[0]);
2079                         return 1;
2080                 }
2081         }
2082
2083         if (args.password &&
2084             ((!args.has_remote_ip && !args.md5_prefix_str) ||
2085               args.type != SOCK_STREAM)) {
2086                 log_error("MD5 passwords apply to TCP only and require a remote ip for the password\n");
2087                 return 1;
2088         }
2089
2090         if (args.md5_prefix_str && !args.password) {
2091                 log_error("Prefix range for MD5 protection specified without a password\n");
2092                 return 1;
2093         }
2094
2095         if (iter == 0) {
2096                 fprintf(stderr, "Invalid number of messages to send\n");
2097                 return 1;
2098         }
2099
2100         if (args.type == SOCK_STREAM && !args.protocol)
2101                 args.protocol = IPPROTO_TCP;
2102         if (args.type == SOCK_DGRAM && !args.protocol)
2103                 args.protocol = IPPROTO_UDP;
2104
2105         if ((args.type == SOCK_STREAM || args.type == SOCK_DGRAM) &&
2106              args.port == 0) {
2107                 fprintf(stderr, "Invalid port number\n");
2108                 return 1;
2109         }
2110
2111         if ((both_mode || !server_mode) && !args.has_grp &&
2112             !args.has_remote_ip && !args.has_local_ip) {
2113                 fprintf(stderr,
2114                         "Local (server mode) or remote IP (client IP) required\n");
2115                 return 1;
2116         }
2117
2118         if (interactive) {
2119                 prog_timeout = 0;
2120                 msg = NULL;
2121         }
2122
2123         if (both_mode) {
2124                 if (pipe(fd) < 0) {
2125                         perror("pipe");
2126                         exit(1);
2127                 }
2128
2129                 cpid = fork();
2130                 if (cpid < 0) {
2131                         perror("fork");
2132                         exit(1);
2133                 }
2134                 if (cpid)
2135                         return ipc_parent(cpid, fd[0], &args);
2136
2137                 return ipc_child(fd[1], &args);
2138         }
2139
2140         if (server_mode) {
2141                 do {
2142                         rc = do_server(&args, -1);
2143                 } while (forever);
2144
2145                 return rc;
2146         }
2147         return do_client(&args);
2148 }