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