tests/echo_srv.c

   1 #include "config.h"
   2
   3 #include <errno.h>
   4
   5 #include <sys/types.h>
   6 #include <sys/socket.h>
   7 #include <sys/stat.h>
   8 #include <sys/wait.h>
   9
  10 #include <arpa/inet.h>
  11 #include <netinet/in.h>
  12 #include <netinet/tcp.h>
  13 #ifdef HAVE_NETINET_TCP_FSM_H
  14 #include <netinet/tcp_fsm.h>
  15 #endif
  16 #include <netdb.h>
  17 #include <resolv.h>
  18
  19 #include <fcntl.h>
  20 #include <getopt.h>
  21 #include <unistd.h>
  22
  23 #include <string.h>
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdbool.h>
  27
  28 #ifndef PIDFILE
  29 #define PIDFILE     "echo_srv.pid"
  30 #endif  /* PIDFILE */
  31
  32 #define ECHO_SRV_IPV4 "127.0.0.10"
  33 /* socket wrapper IPv6 prefix  fd00::5357:5fxx */
  34 #define ECHO_SRV_IPV6 "fd00::5357:5f0a"
  35
  36 #define DFL_PORT    7
  37 #define BACKLOG     16
  38
  39 #ifndef BUFSIZE
  40 #define BUFSIZE     0x20000 /* 128K */
  41 #endif /* BUFSIZE */
  42
  43 #ifndef discard_const
  44 #define discard_const(ptr) ((void *)((uintptr_t)(ptr)))
  45 #endif
  46
  47 #ifndef discard_const_p
  48 #define discard_const_p(type, ptr) ((type *)discard_const(ptr))
  49 #endif
  50
  51 #ifndef ZERO_STRUCT
  52 #define ZERO_STRUCT(x) memset((char *)&(x), 0, sizeof(x))
  53 #endif
  54
  55 struct torture_address {
  56         socklen_t sa_socklen;
  57         union {
  58                 struct sockaddr s;
  59                 struct sockaddr_in in;
  60 #ifdef HAVE_IPV6
  61                 struct sockaddr_in6 in6;
  62 #endif
  63                 struct sockaddr_storage ss;
  64         } sa;
  65 };
  66
  67 struct echo_srv_opts {
  68     int port;
  69     int socktype;
  70     bool daemon;
  71     char *bind;
  72     const char *pidfile;
  73 };
  74
  75 #ifdef HAVE_STRUCT_MSGHDR_MSG_CONTROL
  76
  77 #if defined(IP_PKTINFO) || defined(IP_RECVDSTADDR) || defined(IPV6_PKTINFO)
  78 union pktinfo {
  79 #ifdef HAVE_STRUCT_IN6_PKTINFO
  80         struct in6_pktinfo pkt6;
  81 #endif
  82 #ifdef HAVE_STRUCT_IN_PKTINFO
  83         struct in_pktinfo pkt4;
  84 #elif defined(IP_RECVDSTADDR)
  85         struct in_addr pkt4;
  86 #endif
  87         char c;
  88 };
  89
  90 #define HAVE_UNION_PKTINFO 1
  91 #endif /* IP_PKTINFO || IP_RECVDSTADDR || IPV6_PKTINFO */
  92
  93 static const char *echo_server_address(int family)
  94 {
  95         switch (family) {
  96         case AF_INET: {
  97                 const char *ip4 = getenv("TORTURE_SERVER_ADDRESS_IPV4");
  98
  99                 if (ip4 != NULL && ip4[0] != '\0') {
 100                         return ip4;
 101                 }
 102
 103                 return ECHO_SRV_IPV4;
 104         }
 105 #ifdef HAVE_IPV6
 106         case AF_INET6: {
 107                 const char *ip6 = getenv("TORTURE_SERVER_ADDRESS_IPV6");
 108
 109                 if (ip6 != NULL && ip6[0] != '\0') {
 110                         return ip6;
 111                 }
 112
 113                 return ECHO_SRV_IPV6;
 114         }
 115 #endif
 116         default:
 117                 return NULL;
 118         }
 119
 120         return NULL;
 121 }
 122 #endif /* HAVE_STRUCT_MSGHDR_MSG_CONTROL */
 123
 124 static void _assert_return_code(int rc,
 125                                 int err,
 126                                 const char * const file,
 127                                 const int line)
 128 {
 129         if (rc < 0) {
 130                 fprintf(stderr, "Fatal error: %s\n", strerror(err));
 131                 fprintf(stderr, "%s:%d", file, line);
 132
 133                 abort();
 134         }
 135 }
 136 #define assert_return_code(rc, err) \
 137         _assert_return_code(rc, err, __FILE__, __LINE__)
 138
 139
 140 static int pidfile(const char *path)
 141 {
 142     int err;
 143     int fd;
 144     char pid_str[32] = { 0 };
 145     ssize_t nwritten;
 146     size_t len;
 147
 148     fd = open(path, O_RDONLY, 0644);
 149     err = errno;
 150     if (fd != -1) {
 151         close(fd);
 152         return EEXIST;
 153     } else if (err != ENOENT) {
 154         return err;
 155     }
 156
 157     fd = open(path, O_CREAT | O_WRONLY | O_EXCL, 0644);
 158     err = errno;
 159     if (fd == -1) {
 160         return err;
 161     }
 162
 163     snprintf(pid_str, sizeof(pid_str) -1, "%u\n", (unsigned int) getpid());
 164     len = strlen(pid_str);
 165
 166     nwritten = write(fd, pid_str, len);
 167     close(fd);
 168     if (nwritten != (ssize_t)len) {
 169         return EIO;
 170     }
 171
 172     return 0;
 173 }
 174
 175 static int become_daemon(void)
 176 {
 177     int ret;
 178     pid_t child_pid;
 179     int fd;
 180     int i;
 181
 182     if (getppid() == 1) {
 183         return 0;
 184     }
 185
 186     child_pid = fork();
 187     if (child_pid == -1) {
 188         ret = errno;
 189         perror("fork");
 190         return ret;
 191     } else if (child_pid > 0) {
 192         exit(0);
 193     }
 194
 195     /* If a working directory was defined, go there */
 196 #ifdef WORKING_DIR
 197     chdir(WORKING_DIR);
 198 #endif
 199
 200     ret = setsid();
 201     if (ret == -1) {
 202         ret = errno;
 203         perror("setsid");
 204         return ret;
 205     }
 206
 207     for (i = 0; i < 3; i++) {
 208         close(i);
 209
 210         fd = open("/dev/null", O_RDWR, 0);
 211         if (fd < 0) {
 212             fd = open("/dev/null", O_WRONLY, 0);
 213         }
 214         if (fd < 0) {
 215             ret = errno;
 216             perror("Can't open /dev/null");
 217             return ret;
 218         }
 219         if (fd != i) {
 220             perror("Didn't get correct fd");
 221             close(fd);
 222             return EINVAL;
 223         }
 224     }
 225
 226     umask(0177);
 227     return 0;
 228 }
 229
 230 static void set_sock_pktinfo(int sock, int family)
 231 {
 232         int sockopt = 1;
 233         int option = 0;
 234         int proto = 0;
 235         int rc;
 236
 237         switch(family) {
 238         case AF_INET:
 239                 proto = IPPROTO_IP;
 240 #ifdef IP_PKTINFO
 241                 option = IP_PKTINFO;
 242 #elif IP_RECVDSTADDR
 243                 option = IP_RECVDSTADDR;
 244 #else
 245                 return;
 246 #endif /* IP_PKTINFO */
 247                 break;
 248 #ifdef HAVE_IPV6
 249 #ifdef IPV6_RECVPKTINFO
 250         case AF_INET6:
 251                 proto = IPPROTO_IPV6;
 252                 option = IPV6_RECVPKTINFO;
 253                 break;
 254 #endif /* IPV6_RECVPKTINFO */
 255 #endif /* HAVE_IPV6 */
 256         default:
 257                 return;
 258         }
 259
 260         rc = setsockopt(sock, proto, option, &sockopt, sizeof(sockopt));
 261         assert_return_code(rc, errno);
 262 }
 263
 264 /* Returns 0 on success, errno on failure. If successful,
 265  * sock is a ready to use socket */
 266 static int setup_srv(struct echo_srv_opts *opts, int *_sock)
 267 {
 268     struct addrinfo hints;
 269     struct addrinfo *res, *ri;
 270     char svc[6];
 271     int ret;
 272     int sock;
 273
 274     memset(&hints, 0, sizeof(hints));
 275     hints.ai_family = AF_UNSPEC;
 276     hints.ai_socktype = opts->socktype;
 277     hints.ai_flags = AI_PASSIVE;
 278
 279     snprintf(svc, sizeof(svc), "%d", opts->port);
 280
 281     ret = getaddrinfo(opts->bind, svc, &hints, &res);
 282     if (ret != 0) {
 283         return errno;
 284     }
 285
 286     for (ri = res; ri != NULL; ri = ri->ai_next) {
 287         sock = socket(ri->ai_family, ri->ai_socktype,
 288                       ri->ai_protocol);
 289         if (sock == -1) {
 290             ret = errno;
 291             freeaddrinfo(res);
 292             perror("socket");
 293             return ret;
 294         }
 295
 296         if (ri->ai_socktype == SOCK_DGRAM) {
 297             set_sock_pktinfo(sock, ri->ai_family);
 298         }
 299
 300         ret = bind(sock, ri->ai_addr, ri->ai_addrlen);
 301         if (ret == 0) {
 302             break;
 303         }
 304
 305         close(sock);
 306     }
 307     freeaddrinfo(res);
 308
 309     if (ri == NULL) {
 310         fprintf(stderr, "Could not bind\n");
 311         return EFAULT;
 312     }
 313
 314     if (opts->socktype == SOCK_STREAM) {
 315         ret = listen(sock, BACKLOG);
 316         if (ret == -1) {
 317             ret = errno;
 318             close(sock);
 319             perror("listen");
 320             return ret;
 321         }
 322 #ifdef TCP_INFO
 323         {
 324             struct tcp_info info;
 325             socklen_t optlen = sizeof(info);
 326
 327             ZERO_STRUCT(info);
 328             ret = getsockopt(sock, IPPROTO_TCP, TCP_INFO, &info, &optlen);
 329             if (ret == -1) {
 330                 ret = errno;
 331                 perror("TCP_INFO failed");
 332                 close(sock);
 333                 return ret;
 334             }
 335 #ifdef HAVE_NETINET_TCP_FSM_H
 336 /* This is FreeBSD */
 337 # define __TCP_LISTEN TCPS_LISTEN
 338 #else
 339 /* This is Linux */
 340 # define __TCP_LISTEN TCP_LISTEN
 341 #endif
 342             if (info.tcpi_state != __TCP_LISTEN) {
 343                 errno = ret = ERANGE;
 344                 perror("not __TCP_LISTEN => ERANGE...");
 345                 close(sock);
 346                 return ret;
 347             }
 348         }
 349 #endif /* TCP_INFO */
 350     }
 351
 352     *_sock = sock;
 353     return 0;
 354 }
 355
 356 static int socket_dup(int s)
 357 {
 358     struct torture_address cli_addr1 = {
 359         .sa_socklen = sizeof(struct sockaddr_storage),
 360     };
 361     struct torture_address srv_addr1 = {
 362         .sa_socklen = sizeof(struct sockaddr_storage),
 363     };
 364
 365     struct torture_address cli_addr2 = {
 366         .sa_socklen = sizeof(struct sockaddr_storage),
 367     };
 368     struct torture_address srv_addr2 = {
 369         .sa_socklen = sizeof(struct sockaddr_storage),
 370     };
 371
 372     struct torture_address cli_addr3 = {
 373         .sa_socklen = sizeof(struct sockaddr_storage),
 374     };
 375     struct torture_address srv_addr3 = {
 376         .sa_socklen = sizeof(struct sockaddr_storage),
 377     };
 378
 379     int s2;
 380     int rc;
 381
 382     rc = getsockname(s, &srv_addr1.sa.s, &srv_addr1.sa_socklen);
 383     if (rc == -1) {
 384         perror("getsockname");
 385         return -1;
 386     }
 387
 388     rc = getpeername(s, &cli_addr1.sa.s, &cli_addr1.sa_socklen);
 389     if (rc == -1) {
 390         perror("getpeername");
 391         return -1;
 392     }
 393
 394     if (cli_addr1.sa.ss.ss_family != srv_addr1.sa.ss.ss_family) {
 395         perror("client/server family mismatch");
 396         return -1;
 397     }
 398
 399     /* Test dup */
 400     s2 = dup(s);
 401     if (s2 == -1) {
 402         perror("dup");
 403         return -1;
 404     }
 405     close(s);
 406
 407     rc = getsockname(s2, &srv_addr2.sa.s, &srv_addr2.sa_socklen);
 408     if (rc == -1) {
 409         perror("getsockname");
 410         close(s2);
 411         return -1;
 412     }
 413
 414     rc = getpeername(s2, &cli_addr2.sa.s, &cli_addr2.sa_socklen);
 415     if (rc == -1) {
 416         perror("getpeername");
 417         close(s2);
 418         return -1;
 419     }
 420
 421     if (cli_addr1.sa_socklen != cli_addr2.sa_socklen ||
 422         srv_addr1.sa_socklen != srv_addr2.sa_socklen) {
 423         perror("length mismatch");
 424         close(s2);
 425         return -1;
 426     }
 427
 428     switch(cli_addr1.sa.ss.ss_family) {
 429     case AF_INET: {
 430         rc = memcmp(&cli_addr1.sa.in, &cli_addr2.sa.in, sizeof(struct sockaddr_in));
 431         if (rc != 0) {
 432             perror("client mismatch");
 433         }
 434
 435         rc = memcmp(&srv_addr1.sa.in, &srv_addr2.sa.in, sizeof(struct sockaddr_in));
 436         if (rc != 0) {
 437             perror("server mismatch");
 438         }
 439         break;
 440     }
 441 #ifdef HAVE_IPV6
 442     case AF_INET6: {
 443         rc = memcmp(&cli_addr1.sa.in6, &cli_addr2.sa.in6, sizeof(struct sockaddr_in6));
 444         if (rc != 0) {
 445             perror("client mismatch");
 446         }
 447
 448         rc = memcmp(&srv_addr1.sa.in6, &srv_addr2.sa.in6, sizeof(struct sockaddr_in6));
 449         if (rc != 0) {
 450             perror("server mismatch");
 451         }
 452         break;
 453     }
 454 #endif
 455     default:
 456         perror("family mismatch");
 457         close(s2);
 458         return -1;
 459     }
 460
 461     /* Test dup2 */
 462     s = dup2(s2, s);
 463     close(s2);
 464     if (s == -1) {
 465         perror("dup");
 466         return -1;
 467     }
 468
 469     rc = getsockname(s, &srv_addr3.sa.s, &srv_addr3.sa_socklen);
 470     if (rc == -1) {
 471         perror("getsockname");
 472         close(s);
 473         return -1;
 474     }
 475
 476     rc = getpeername(s, &cli_addr3.sa.s, &cli_addr3.sa_socklen);
 477     if (rc == -1) {
 478         perror("getpeername");
 479         close(s);
 480         return -1;
 481     }
 482
 483     if (cli_addr2.sa_socklen != cli_addr3.sa_socklen ||
 484         srv_addr2.sa_socklen != srv_addr3.sa_socklen) {
 485         perror("length mismatch");
 486         close(s);
 487         return -1;
 488     }
 489
 490     switch(cli_addr2.sa.ss.ss_family) {
 491     case AF_INET: {
 492         rc = memcmp(&cli_addr1.sa.in, &cli_addr2.sa.in, sizeof(struct sockaddr_in));
 493         if (rc != 0) {
 494             perror("client mismatch");
 495         }
 496
 497         rc = memcmp(&srv_addr1.sa.in, &srv_addr2.sa.in, sizeof(struct sockaddr_in));
 498         if (rc != 0) {
 499             perror("server mismatch");
 500         }
 501         break;
 502     }
 503 #ifdef HAVE_IPV6
 504     case AF_INET6: {
 505         rc = memcmp(&cli_addr1.sa.in6, &cli_addr2.sa.in6, sizeof(struct sockaddr_in6));
 506         if (rc != 0) {
 507             perror("client mismatch");
 508         }
 509
 510         rc = memcmp(&srv_addr1.sa.in6, &srv_addr2.sa.in6, sizeof(struct sockaddr_in6));
 511         if (rc != 0) {
 512             perror("server mismatch");
 513         }
 514         break;
 515     }
 516 #endif
 517     default:
 518         perror("family mismatch");
 519         close(s);
 520         return -1;
 521     }
 522
 523     return s;
 524 }
 525
 526 static void echo_tcp(int sock)
 527 {
 528     struct torture_address addr = {
 529         .sa_socklen = sizeof(struct sockaddr_storage),
 530     };
 531
 532     char buf[BUFSIZE];
 533     ssize_t bret;
 534
 535     int client_sock = -1;
 536     int s;
 537     pid_t pid;
 538
 539     while (1) {
 540         waitpid(-1, NULL, WNOHANG);
 541         s = accept(sock, &addr.sa.s, &addr.sa_socklen);
 542         if (s == -1 && errno == ECONNABORTED) {
 543             continue;
 544         }
 545         if (s == -1) {
 546             perror("accept");
 547             goto done;
 548         }
 549
 550         pid = fork();
 551         if (pid == -1) {
 552             perror("fork");
 553         } else if (pid == 0) {
 554             close(sock);
 555             client_sock = socket_dup(s);
 556             if (client_sock == -1) {
 557                 perror("socket_dup");
 558                 goto done;
 559             }
 560
 561             while (1) {
 562                 bret = recv(client_sock, buf, BUFSIZE, 0);
 563                 if (bret == -1) {
 564                     perror("recv");
 565                     goto done;
 566                 } else if (bret == 0) {
 567                     break;
 568                 }
 569
 570                 bret = send(client_sock, buf, bret, 0);
 571                 if (bret == -1) {
 572                     perror("send");
 573                     goto done;
 574                 }
 575             }
 576             close(s);
 577             exit(0);
 578         }
 579         close(s);
 580     }
 581
 582 done:
 583     if (client_sock != -1) {
 584         close(client_sock);
 585     }
 586 }
 587
 588 static ssize_t echo_udp_recv_from_to(int sock,
 589                                      void *buf, size_t buflen, int flags,
 590                                      struct torture_address *from,
 591                                      struct torture_address *to)
 592 {
 593         struct msghdr rmsg;
 594         struct iovec riov;
 595         ssize_t ret;
 596
 597 #if defined(HAVE_STRUCT_MSGHDR_MSG_CONTROL) && defined(HAVE_UNION_PKTINFO)
 598         size_t cmlen = CMSG_LEN(sizeof(union pktinfo));
 599         char cmsg[cmlen];
 600 #endif /* HAVE_STRUCT_MSGHDR_MSG_CONTROL */
 601
 602         riov.iov_base = buf;
 603         riov.iov_len = buflen;
 604
 605         ZERO_STRUCT(rmsg);
 606
 607         rmsg.msg_name = &from->sa.s;
 608         rmsg.msg_namelen = from->sa_socklen;
 609
 610         rmsg.msg_iov = &riov;
 611         rmsg.msg_iovlen = 1;
 612
 613 #if defined(HAVE_STRUCT_MSGHDR_MSG_CONTROL) && defined(HAVE_UNION_PKTINFO)
 614         memset(cmsg, 0, cmlen);
 615
 616         rmsg.msg_control = cmsg;
 617         rmsg.msg_controllen = cmlen;
 618 #endif
 619
 620         ret = recvmsg(sock, &rmsg, flags);
 621         if (ret < 0) {
 622                 return ret;
 623         }
 624         from->sa_socklen = rmsg.msg_namelen;
 625
 626 #if defined(HAVE_STRUCT_MSGHDR_MSG_CONTROL) && defined(HAVE_UNION_PKTINFO)
 627         if (rmsg.msg_controllen > 0) {
 628                 struct cmsghdr *cmsgptr;
 629
 630                 cmsgptr = CMSG_FIRSTHDR(&rmsg);
 631                 while (cmsgptr != NULL) {
 632                         const char *p;
 633
 634 #if defined(IP_PKTINFO) && defined(HAVE_STRUCT_IN_PKTINFO)
 635                         if (cmsgptr->cmsg_level == IPPROTO_IP &&
 636                                         cmsgptr->cmsg_type == IP_PKTINFO) {
 637                                 char ip[INET_ADDRSTRLEN] = { 0 };
 638                                 struct in_pktinfo *pkt;
 639                                 void *cmsg_cast_ptr = CMSG_DATA(cmsgptr);
 640
 641                                 pkt = (struct in_pktinfo *)cmsg_cast_ptr;
 642
 643                                 to->sa.in.sin_family = AF_INET;
 644                                 to->sa.in.sin_addr = pkt->ipi_addr;
 645                                 to->sa_socklen = sizeof(struct sockaddr_in);
 646
 647                                 p = inet_ntop(AF_INET, &to->sa.in.sin_addr, ip, sizeof(ip));
 648                                 if (p == 0) {
 649                                         fprintf(stderr, "Failed to convert IP address");
 650                                         abort();
 651                                 }
 652
 653                                 if (strcmp(ip, echo_server_address(AF_INET)) != 0) {
 654                                         fprintf(stderr, "Wrong IP received");
 655                                         abort();
 656                                 }
 657                         }
 658 #endif /* IP_PKTINFO */
 659 #ifdef IP_RECVDSTADDR
 660                         if (cmsgptr->cmsg_level == IPPROTO_IP &&
 661                             cmsgptr->cmsg_type == IP_RECVDSTADDR) {
 662                                 char ip[INET_ADDRSTRLEN] = { 0 };
 663                                 struct in_addr *addr;
 664                                 void *cmsg_cast_ptr = CMSG_DATA(cmsgptr);
 665
 666                                 addr = (struct in_addr *)cmsg_cast_ptr;
 667
 668                                 to->sa.in.sin_family = AF_INET;
 669                                 to->sa.in.sin_addr = *addr;
 670                                 to->sa_socklen = sizeof(struct sockaddr_in);
 671
 672                                 p = inet_ntop(AF_INET, &to->sa.in.sin_addr, ip, sizeof(ip));
 673                                 if (p == 0) {
 674                                         fprintf(stderr, "Failed to convert IP address");
 675                                         abort();
 676                                 }
 677
 678                                 if (strcmp(ip, echo_server_address(AF_INET)) != 0) {
 679                                         fprintf(stderr, "Wrong IP received");
 680                                         abort();
 681                                 }
 682                         }
 683 #endif /* IP_RECVDSTADDR */
 684 #if defined(IPV6_PKTINFO) && defined(HAVE_STRUCT_IN6_PKTINFO)
 685                         if (cmsgptr->cmsg_level == IPPROTO_IPV6 &&
 686                                         cmsgptr->cmsg_type == IPV6_PKTINFO) {
 687                                 char ip[INET6_ADDRSTRLEN] = { 0 };
 688                                 struct in6_pktinfo *pkt6;
 689                                 void *cmsg_cast_ptr = CMSG_DATA(cmsgptr);
 690
 691                                 pkt6 = (struct in6_pktinfo *)cmsg_cast_ptr;
 692
 693                                 to->sa.in6.sin6_family = AF_INET6;
 694                                 to->sa.in6.sin6_addr = pkt6->ipi6_addr;
 695
 696                                 p = inet_ntop(AF_INET6, &to->sa.in6.sin6_addr, ip, sizeof(ip));
 697                                 if (p == 0) {
 698                                         fprintf(stderr, "Failed to convert IP address");
 699                                         abort();
 700                                 }
 701
 702                                 if (strcmp(ip, echo_server_address(AF_INET6)) != 0) {
 703                                         fprintf(stderr, "Wrong IP received");
 704                                         abort();
 705                                 }
 706                         }
 707 #endif /* IPV6_PKTINFO */
 708                         cmsgptr = CMSG_NXTHDR(&rmsg, cmsgptr);
 709                 }
 710         } else {
 711                 fprintf(stderr, "Failed to receive pktinfo");
 712                 abort();
 713         }
 714 #endif /* HAVE_STRUCT_MSGHDR_MSG_CONTROL && HAVE_UNION_PKTINFO */
 715
 716         return ret;
 717 }
 718
 719 static ssize_t echo_udp_send_to_from(int sock,
 720                                      void *buf, size_t buflen, int flags,
 721                                      struct sockaddr *to, socklen_t tolen,
 722                                      struct sockaddr *from, socklen_t fromlen)
 723 {
 724         struct msghdr msg;
 725         struct iovec iov;
 726         ssize_t ret;
 727
 728 #if defined(HAVE_STRUCT_MSGHDR_MSG_CONTROL) && defined(HAVE_UNION_PKTINFO)
 729         size_t clen = CMSG_SPACE(sizeof(union pktinfo));
 730         char cbuf[clen];
 731         struct cmsghdr *cmsgptr;
 732 #else
 733         (void)from; /* unused */
 734         (void)fromlen; /* unused */
 735 #endif /* HAVE_STRUCT_MSGHDR_MSG_CONTROL && HAVE_UNION_PKTINFO */
 736
 737         iov.iov_base = buf;
 738         iov.iov_len = buflen;
 739
 740         ZERO_STRUCT(msg);
 741
 742         msg.msg_name = to;
 743         msg.msg_namelen = tolen;
 744
 745         msg.msg_iov = &iov;
 746         msg.msg_iovlen = 1;
 747
 748 #if defined(HAVE_STRUCT_MSGHDR_MSG_CONTROL) && defined(HAVE_UNION_PKTINFO)
 749         memset(cbuf, 0, clen);
 750
 751         msg.msg_control = cbuf;
 752         msg.msg_controllen = clen;
 753
 754         cmsgptr = CMSG_FIRSTHDR(&msg);
 755         msg.msg_controllen = 0;
 756
 757         switch (from->sa_family) {
 758 #if defined(IP_PKTINFO) || defined(IP_SENDSRCADDR)
 759         case AF_INET: {
 760                 void *cmsg_cast_ptr = CMSG_DATA(cmsgptr);
 761 #ifdef IP_PKTINFO
 762                 struct in_pktinfo *p = (struct in_pktinfo *)cmsg_cast_ptr;
 763 #elif defined(IP_SENDSRCADDR)
 764                 struct in_addr *p = (struct in_addr *)cmsg_cast_ptr;
 765 #endif
 766                 const struct sockaddr_in *from4 =
 767                         (const struct sockaddr_in *)(const void *)from;
 768
 769                 if (fromlen != sizeof(struct sockaddr_in)) {
 770                         break;
 771                 }
 772
 773                 cmsgptr->cmsg_level = IPPROTO_IP;
 774 #ifdef IP_PKTINFO
 775                 cmsgptr->cmsg_type = IP_PKTINFO;
 776                 p->ipi_spec_dst = from4->sin_addr;
 777 #elif defined(IP_SENDSRCADDR)
 778                 cmsgptr->cmsg_type = IP_SENDSRCADDR;
 779                 *p = from4->sin_addr;
 780 #endif
 781                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(*p));
 782
 783                 msg.msg_controllen = CMSG_SPACE(sizeof(*p));
 784
 785                 break;
 786         }
 787 #endif /* IP_PKTINFO || IP_SENDSRCADDR */
 788 #ifdef IPV6_PKTINFO
 789         case AF_INET6: {
 790         void *cast_ptr = CMSG_DATA(cmsgptr);
 791                 struct in6_pktinfo *p = (struct in6_pktinfo *)cast_ptr;
 792                 const struct sockaddr_in6 *from6 =
 793                         (const struct sockaddr_in6 *)(const void *)from;
 794
 795                 if (fromlen != sizeof(struct sockaddr_in6)) {
 796                         break;
 797                 }
 798
 799                 cmsgptr->cmsg_level = IPPROTO_IPV6;
 800                 cmsgptr->cmsg_type = IPV6_PKTINFO;
 801                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
 802
 803                 p->ipi6_addr = from6->sin6_addr;
 804
 805                 msg.msg_controllen = CMSG_SPACE(sizeof(struct in6_pktinfo));
 806
 807                 break;
 808         }
 809 #endif /* IPV6_PKTINFO */
 810         default:
 811                 break;
 812         }
 813 #endif /* HAVE_STRUCT_MSGHDR_MSG_CONTROL && HAVE_UNION_PKTINFO */
 814
 815         ret = sendmsg(sock, &msg, flags);
 816
 817         return ret;
 818 }
 819
 820 static void echo_udp(int sock)
 821 {
 822     struct torture_address saddr = {
 823         .sa_socklen = sizeof(struct sockaddr_storage),
 824     };
 825     struct torture_address daddr = {
 826         .sa_socklen = sizeof(struct sockaddr_storage),
 827     };
 828     ssize_t bret;
 829     char buf[BUFSIZE];
 830
 831     while (1) {
 832         bret = echo_udp_recv_from_to(sock,
 833                                      buf,
 834                                      BUFSIZE,
 835                                      0,
 836                                      &saddr,
 837                                      &daddr);
 838         if (bret == -1) {
 839             perror("recvfrom");
 840             continue;
 841         }
 842
 843         bret = echo_udp_send_to_from(sock,
 844                                      buf,
 845                                      bret,
 846                                      0,
 847                                      &saddr.sa.s,
 848                                      saddr.sa_socklen,
 849                                      &daddr.sa.s,
 850                                      daddr.sa_socklen);
 851         if (bret == -1) {
 852             perror("sendto");
 853             continue;
 854         }
 855     }
 856 }
 857
 858 static void echo(int sock, struct echo_srv_opts *opts)
 859 {
 860     switch (opts->socktype) {
 861         case SOCK_STREAM:
 862             echo_tcp(sock);
 863             return;
 864         case SOCK_DGRAM:
 865             echo_udp(sock);
 866             return;
 867         default:
 868             fprintf(stderr, "Unsupported protocol\n");
 869             return;
 870     }
 871 }
 872
 873 int main(int argc, char **argv)
 874 {
 875     int ret;
 876     int sock = -1;
 877     struct echo_srv_opts opts;
 878     int opt;
 879     int optindex;
 880     static struct option long_options[] = {
 881         { discard_const_p(char, "tcp"),         no_argument,            0,  't' },
 882         { discard_const_p(char, "udp"),         no_argument,            0,  'u' },
 883         { discard_const_p(char, "bind-addr"),   required_argument,      0,  'b' },
 884         { discard_const_p(char, "port"),        required_argument,      0,  'p' },
 885         { discard_const_p(char, "daemon"),      no_argument,            0,  'D' },
 886         { discard_const_p(char, "pid"),         required_argument,      0,  0 },
 887         {0,             0,                                              0,  0 }
 888     };
 889
 890     opts.port = DFL_PORT;
 891     opts.socktype = SOCK_STREAM;
 892     opts.bind = NULL;
 893     opts.pidfile = PIDFILE;
 894     opts.daemon = false;
 895
 896     while ((opt = getopt_long(argc, argv, "Dutp:b:",
 897                               long_options, &optindex)) != -1) {
 898         switch (opt) {
 899             case 0:
 900                 if (optindex == 5) {
 901                     opts.pidfile = optarg;
 902                 }
 903                 break;
 904             case 'p':
 905                 opts.port = atoi(optarg);
 906                 break;
 907             case 'b':
 908                 opts.bind = optarg;
 909                 break;
 910             case 'u':
 911                 opts.socktype = SOCK_DGRAM;
 912                 break;
 913             case 't':
 914                 opts.socktype = SOCK_STREAM;
 915                 break;
 916             case 'D':
 917                 opts.daemon = true;
 918                 break;
 919             default: /* '?' */
 920                 fprintf(stderr, "Usage: %s [-p port] [-u] [-t] [-b bind_addr] " \
 921                                 "[-D] [--pid pidfile]\n"
 922                                 "-t makes the server listen on TCP\n"
 923                                 "-u makes the server listen on UDP\n"
 924                                 "-D tells the server to become a daemon and " \
 925                                 "write a PIDfile\n"
 926                                 "The default port is 7, the default PIDfile is " \
 927                                 "echo_srv.pid in the current directory\n",
 928                                 argv[0]);
 929                 ret = 1;
 930                 goto done;
 931         }
 932     }
 933
 934     if (opts.daemon) {
 935         ret = become_daemon();
 936         if (ret != 0) {
 937             fprintf(stderr, "Cannot become daemon: %s\n", strerror(ret));
 938             goto done;
 939         }
 940     }
 941
 942     ret = setup_srv(&opts, &sock);
 943     if (ret != 0) {
 944         fprintf(stderr, "Cannot setup server: %s\n", strerror(ret));
 945         goto done;
 946     }
 947
 948     if (opts.daemon && opts.pidfile != NULL) {
 949         ret = pidfile(opts.pidfile);
 950         if (ret != 0) {
 951             fprintf(stderr, "Cannot create pidfile %s: %s\n",
 952                     opts.pidfile, strerror(ret));
 953             goto done;
 954         }
 955     }
 956
 957     echo(sock, &opts);
 958     if (sock >= 0) {
 959         close(sock);
 960     }
 961
 962     if (opts.daemon && opts.pidfile != NULL) {
 963         unlink(opts.pidfile);
 964     }
 965
 966     ret = 0;
 967 done:
 968     return ret;
 969 }