tools/testing/selftests/net/cmsg_sender.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 #include <errno.h>
   3 #include <error.h>
   4 #include <netdb.h>
   5 #include <stdbool.h>
   6 #include <stdio.h>
   7 #include <stdlib.h>
   8 #include <string.h>
   9 #include <time.h>
  10 #include <unistd.h>
  11 #include <linux/errqueue.h>
  12 #include <linux/icmp.h>
  13 #include <linux/icmpv6.h>
  14 #include <linux/net_tstamp.h>
  15 #include <linux/types.h>
  16 #include <linux/udp.h>
  17 #include <sys/socket.h>
  18
  19 #include "../kselftest.h"
  20
  21 enum {
  22         ERN_SUCCESS = 0,
  23         /* Well defined errors, callers may depend on these */
  24         ERN_SEND = 1,
  25         /* Informational, can reorder */
  26         ERN_HELP,
  27         ERN_SEND_SHORT,
  28         ERN_SOCK_CREATE,
  29         ERN_RESOLVE,
  30         ERN_CMSG_WR,
  31         ERN_SOCKOPT,
  32         ERN_GETTIME,
  33         ERN_RECVERR,
  34         ERN_CMSG_RD,
  35         ERN_CMSG_RCV,
  36 };
  37
  38 struct option_cmsg_u32 {
  39         bool ena;
  40         unsigned int val;
  41 };
  42
  43 struct options {
  44         bool silent_send;
  45         const char *host;
  46         const char *service;
  47         unsigned int size;
  48         unsigned int num_pkt;
  49         struct {
  50                 unsigned int mark;
  51                 unsigned int dontfrag;
  52                 unsigned int tclass;
  53                 unsigned int hlimit;
  54                 unsigned int priority;
  55         } sockopt;
  56         struct {
  57                 unsigned int family;
  58                 unsigned int type;
  59                 unsigned int proto;
  60         } sock;
  61         struct option_cmsg_u32 mark;
  62         struct {
  63                 bool ena;
  64                 unsigned int delay;
  65         } txtime;
  66         struct {
  67                 bool ena;
  68         } ts;
  69         struct {
  70                 struct option_cmsg_u32 dontfrag;
  71                 struct option_cmsg_u32 tclass;
  72                 struct option_cmsg_u32 hlimit;
  73                 struct option_cmsg_u32 exthdr;
  74         } v6;
  75 } opt = {
  76         .size = 13,
  77         .num_pkt = 1,
  78         .sock = {
  79                 .family = AF_UNSPEC,
  80                 .type   = SOCK_DGRAM,
  81                 .proto  = IPPROTO_UDP,
  82         },
  83 };
  84
  85 static struct timespec time_start_real;
  86 static struct timespec time_start_mono;
  87
  88 static void __attribute__((noreturn)) cs_usage(const char *bin)
  89 {
  90         printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);
  91         printf("Options:\n"
  92                "\t\t-s      Silent send() failures\n"
  93                "\t\t-S      send() size\n"
  94                "\t\t-4/-6   Force IPv4 / IPv6 only\n"
  95                "\t\t-p prot Socket protocol\n"
  96                "\t\t        (u = UDP (default); i = ICMP; r = RAW)\n"
  97                "\n"
  98                "\t\t-m val  Set SO_MARK with given value\n"
  99                "\t\t-M val  Set SO_MARK via setsockopt\n"
 100                "\t\t-d val  Set SO_TXTIME with given delay (usec)\n"
 101                "\t\t-t      Enable time stamp reporting\n"
 102                "\t\t-f val  Set don't fragment via cmsg\n"
 103                "\t\t-F val  Set don't fragment via setsockopt\n"
 104                "\t\t-c val  Set TCLASS via cmsg\n"
 105                "\t\t-C val  Set TCLASS via setsockopt\n"
 106                "\t\t-l val  Set HOPLIMIT via cmsg\n"
 107                "\t\t-L val  Set HOPLIMIT via setsockopt\n"
 108                "\t\t-H type Add an IPv6 header option\n"
 109                "\t\t        (h = HOP; d = DST; r = RTDST)"
 110                "");
 111         exit(ERN_HELP);
 112 }
 113
 114 static void cs_parse_args(int argc, char *argv[])
 115 {
 116         int o;
 117
 118         while ((o = getopt(argc, argv, "46sS:p:P:m:M:n:d:tf:F:c:C:l:L:H:")) != -1) {
 119                 switch (o) {
 120                 case 's':
 121                         opt.silent_send = true;
 122                         break;
 123                 case 'S':
 124                         opt.size = atoi(optarg);
 125                         break;
 126                 case '4':
 127                         opt.sock.family = AF_INET;
 128                         break;
 129                 case '6':
 130                         opt.sock.family = AF_INET6;
 131                         break;
 132                 case 'p':
 133                         if (*optarg == 'u' || *optarg == 'U') {
 134                                 opt.sock.proto = IPPROTO_UDP;
 135                         } else if (*optarg == 'i' || *optarg == 'I') {
 136                                 opt.sock.proto = IPPROTO_ICMP;
 137                         } else if (*optarg == 'r') {
 138                                 opt.sock.type = SOCK_RAW;
 139                         } else {
 140                                 printf("Error: unknown protocol: %s\n", optarg);
 141                                 cs_usage(argv[0]);
 142                         }
 143                         break;
 144                 case 'P':
 145                         opt.sockopt.priority = atoi(optarg);
 146                         break;
 147                 case 'm':
 148                         opt.mark.ena = true;
 149                         opt.mark.val = atoi(optarg);
 150                         break;
 151                 case 'M':
 152                         opt.sockopt.mark = atoi(optarg);
 153                         break;
 154                 case 'n':
 155                         opt.num_pkt = atoi(optarg);
 156                         break;
 157                 case 'd':
 158                         opt.txtime.ena = true;
 159                         opt.txtime.delay = atoi(optarg);
 160                         break;
 161                 case 't':
 162                         opt.ts.ena = true;
 163                         break;
 164                 case 'f':
 165                         opt.v6.dontfrag.ena = true;
 166                         opt.v6.dontfrag.val = atoi(optarg);
 167                         break;
 168                 case 'F':
 169                         opt.sockopt.dontfrag = atoi(optarg);
 170                         break;
 171                 case 'c':
 172                         opt.v6.tclass.ena = true;
 173                         opt.v6.tclass.val = atoi(optarg);
 174                         break;
 175                 case 'C':
 176                         opt.sockopt.tclass = atoi(optarg);
 177                         break;
 178                 case 'l':
 179                         opt.v6.hlimit.ena = true;
 180                         opt.v6.hlimit.val = atoi(optarg);
 181                         break;
 182                 case 'L':
 183                         opt.sockopt.hlimit = atoi(optarg);
 184                         break;
 185                 case 'H':
 186                         opt.v6.exthdr.ena = true;
 187                         switch (optarg[0]) {
 188                         case 'h':
 189                                 opt.v6.exthdr.val = IPV6_HOPOPTS;
 190                                 break;
 191                         case 'd':
 192                                 opt.v6.exthdr.val = IPV6_DSTOPTS;
 193                                 break;
 194                         case 'r':
 195                                 opt.v6.exthdr.val = IPV6_RTHDRDSTOPTS;
 196                                 break;
 197                         default:
 198                                 printf("Error: hdr type: %s\n", optarg);
 199                                 break;
 200                         }
 201                         break;
 202                 }
 203         }
 204
 205         if (optind != argc - 2)
 206                 cs_usage(argv[0]);
 207
 208         opt.host = argv[optind];
 209         opt.service = argv[optind + 1];
 210 }
 211
 212 static void memrnd(void *s, size_t n)
 213 {
 214         int *dword = s;
 215         char *byte;
 216
 217         for (; n >= 4; n -= 4)
 218                 *dword++ = rand();
 219         byte = (void *)dword;
 220         while (n--)
 221                 *byte++ = rand();
 222 }
 223
 224 static void
 225 ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,
 226                   int level, int optname, struct option_cmsg_u32 *uopt)
 227 {
 228         struct cmsghdr *cmsg;
 229
 230         if (!uopt->ena)
 231                 return;
 232
 233         cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);
 234         *cmsg_len += CMSG_SPACE(sizeof(__u32));
 235         if (cbuf_sz < *cmsg_len)
 236                 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
 237
 238         cmsg->cmsg_level = level;
 239         cmsg->cmsg_type = optname;
 240         cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
 241         *(__u32 *)CMSG_DATA(cmsg) = uopt->val;
 242 }
 243
 244 static void
 245 cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
 246 {
 247         struct cmsghdr *cmsg;
 248         size_t cmsg_len;
 249
 250         msg->msg_control = cbuf;
 251         cmsg_len = 0;
 252
 253         ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
 254                           SOL_SOCKET, SO_MARK, &opt.mark);
 255         ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
 256                           SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);
 257         ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
 258                           SOL_IPV6, IPV6_TCLASS, &opt.v6.tclass);
 259         ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
 260                           SOL_IPV6, IPV6_HOPLIMIT, &opt.v6.hlimit);
 261
 262         if (opt.txtime.ena) {
 263                 __u64 txtime;
 264
 265                 txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +
 266                          time_start_mono.tv_nsec +
 267                          opt.txtime.delay * 1000;
 268
 269                 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
 270                 cmsg_len += CMSG_SPACE(sizeof(txtime));
 271                 if (cbuf_sz < cmsg_len)
 272                         error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
 273
 274                 cmsg->cmsg_level = SOL_SOCKET;
 275                 cmsg->cmsg_type = SCM_TXTIME;
 276                 cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
 277                 memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));
 278         }
 279         if (opt.ts.ena) {
 280                 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
 281                 cmsg_len += CMSG_SPACE(sizeof(__u32));
 282                 if (cbuf_sz < cmsg_len)
 283                         error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
 284
 285                 cmsg->cmsg_level = SOL_SOCKET;
 286                 cmsg->cmsg_type = SO_TIMESTAMPING;
 287                 cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
 288                 *(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |
 289                                             SOF_TIMESTAMPING_TX_SOFTWARE;
 290         }
 291         if (opt.v6.exthdr.ena) {
 292                 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
 293                 cmsg_len += CMSG_SPACE(8);
 294                 if (cbuf_sz < cmsg_len)
 295                         error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
 296
 297                 cmsg->cmsg_level = SOL_IPV6;
 298                 cmsg->cmsg_type = opt.v6.exthdr.val;
 299                 cmsg->cmsg_len = CMSG_LEN(8);
 300                 *(__u64 *)CMSG_DATA(cmsg) = 0;
 301         }
 302
 303         if (cmsg_len)
 304                 msg->msg_controllen = cmsg_len;
 305         else
 306                 msg->msg_control = NULL;
 307 }
 308
 309 static const char *cs_ts_info2str(unsigned int info)
 310 {
 311         static const char *names[] = {
 312                 [SCM_TSTAMP_SND]        = "SND",
 313                 [SCM_TSTAMP_SCHED]      = "SCHED",
 314                 [SCM_TSTAMP_ACK]        = "ACK",
 315         };
 316
 317         if (info < ARRAY_SIZE(names))
 318                 return names[info];
 319         return "unknown";
 320 }
 321
 322 static unsigned long
 323 cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
 324 {
 325         struct sock_extended_err *see;
 326         struct scm_timestamping *ts;
 327         unsigned long ts_seen = 0;
 328         struct cmsghdr *cmsg;
 329         int i, err;
 330
 331         if (!opt.ts.ena)
 332                 return 0;
 333         msg->msg_control = cbuf;
 334         msg->msg_controllen = cbuf_sz;
 335
 336         while (true) {
 337                 ts = NULL;
 338                 see = NULL;
 339                 memset(cbuf, 0, cbuf_sz);
 340
 341                 err = recvmsg(fd, msg, MSG_ERRQUEUE);
 342                 if (err < 0) {
 343                         if (errno == EAGAIN)
 344                                 break;
 345                         error(ERN_RECVERR, errno, "recvmsg ERRQ");
 346                 }
 347
 348                 for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
 349                      cmsg = CMSG_NXTHDR(msg, cmsg)) {
 350                         if (cmsg->cmsg_level == SOL_SOCKET &&
 351                             cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {
 352                                 if (cmsg->cmsg_len < sizeof(*ts))
 353                                         error(ERN_CMSG_RD, EINVAL, "TS cmsg");
 354
 355                                 ts = (void *)CMSG_DATA(cmsg);
 356                         }
 357                         if ((cmsg->cmsg_level == SOL_IP &&
 358                              cmsg->cmsg_type == IP_RECVERR) ||
 359                             (cmsg->cmsg_level == SOL_IPV6 &&
 360                              cmsg->cmsg_type == IPV6_RECVERR)) {
 361                                 if (cmsg->cmsg_len < sizeof(*see))
 362                                         error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");
 363
 364                                 see = (void *)CMSG_DATA(cmsg);
 365                         }
 366                 }
 367
 368                 if (!ts)
 369                         error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");
 370                 if (!see)
 371                         error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");
 372
 373                 for (i = 0; i < 3; i++) {
 374                         unsigned long long rel_time;
 375
 376                         if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)
 377                                 continue;
 378
 379                         rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *
 380                                 (1000ULL * 1000) +
 381                                 (ts->ts[i].tv_nsec - time_start_real.tv_nsec) /
 382                                 1000;
 383                         printf(" %5s ts%d %lluus\n",
 384                                cs_ts_info2str(see->ee_info),
 385                                i, rel_time);
 386                         ts_seen |= 1 << see->ee_info;
 387                 }
 388         }
 389
 390         return ts_seen;
 391 }
 392
 393 static void ca_set_sockopts(int fd)
 394 {
 395         if (opt.sockopt.mark &&
 396             setsockopt(fd, SOL_SOCKET, SO_MARK,
 397                        &opt.sockopt.mark, sizeof(opt.sockopt.mark)))
 398                 error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");
 399         if (opt.sockopt.dontfrag &&
 400             setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,
 401                        &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))
 402                 error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");
 403         if (opt.sockopt.tclass &&
 404             setsockopt(fd, SOL_IPV6, IPV6_TCLASS,
 405                        &opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))
 406                 error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS");
 407         if (opt.sockopt.hlimit &&
 408             setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS,
 409                        &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))
 410                 error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT");
 411         if (opt.sockopt.priority &&
 412             setsockopt(fd, SOL_SOCKET, SO_PRIORITY,
 413                        &opt.sockopt.priority, sizeof(opt.sockopt.priority)))
 414                 error(ERN_SOCKOPT, errno, "setsockopt SO_PRIORITY");
 415
 416         if (opt.txtime.ena) {
 417                 struct sock_txtime so_txtime = {
 418                         .clockid = CLOCK_MONOTONIC,
 419                 };
 420
 421                 if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
 422                                &so_txtime, sizeof(so_txtime)))
 423                         error(ERN_SOCKOPT, errno, "setsockopt TXTIME");
 424         }
 425         if (opt.ts.ena) {
 426                 __u32 val = SOF_TIMESTAMPING_SOFTWARE |
 427                         SOF_TIMESTAMPING_OPT_TSONLY;
 428
 429                 if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
 430                                &val, sizeof(val)))
 431                         error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");
 432         }
 433 }
 434
 435 int main(int argc, char *argv[])
 436 {
 437         struct addrinfo hints, *ai;
 438         struct iovec iov[1];
 439         unsigned char *buf;
 440         struct msghdr msg;
 441         char cbuf[1024];
 442         int err;
 443         int fd;
 444         int i;
 445
 446         cs_parse_args(argc, argv);
 447
 448         buf = malloc(opt.size);
 449         memrnd(buf, opt.size);
 450
 451         memset(&hints, 0, sizeof(hints));
 452         hints.ai_family = opt.sock.family;
 453
 454         ai = NULL;
 455         err = getaddrinfo(opt.host, opt.service, &hints, &ai);
 456         if (err) {
 457                 fprintf(stderr, "Can't resolve address [%s]:%s\n",
 458                         opt.host, opt.service);
 459                 return ERN_SOCK_CREATE;
 460         }
 461
 462         if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)
 463                 opt.sock.proto = IPPROTO_ICMPV6;
 464
 465         fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);
 466         if (fd < 0) {
 467                 fprintf(stderr, "Can't open socket: %s\n", strerror(errno));
 468                 freeaddrinfo(ai);
 469                 return ERN_RESOLVE;
 470         }
 471
 472         if (opt.sock.proto == IPPROTO_ICMP) {
 473                 buf[0] = ICMP_ECHO;
 474                 buf[1] = 0;
 475         } else if (opt.sock.proto == IPPROTO_ICMPV6) {
 476                 buf[0] = ICMPV6_ECHO_REQUEST;
 477                 buf[1] = 0;
 478         } else if (opt.sock.type == SOCK_RAW) {
 479                 struct udphdr hdr = { 1, 2, htons(opt.size), 0 };
 480                 struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;
 481
 482                 memcpy(buf, &hdr, sizeof(hdr));
 483                 sin6->sin6_port = htons(opt.sock.proto);
 484         }
 485
 486         ca_set_sockopts(fd);
 487
 488         if (clock_gettime(CLOCK_REALTIME, &time_start_real))
 489                 error(ERN_GETTIME, errno, "gettime REALTIME");
 490         if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))
 491                 error(ERN_GETTIME, errno, "gettime MONOTONIC");
 492
 493         iov[0].iov_base = buf;
 494         iov[0].iov_len = opt.size;
 495
 496         memset(&msg, 0, sizeof(msg));
 497         msg.msg_name = ai->ai_addr;
 498         msg.msg_namelen = ai->ai_addrlen;
 499         msg.msg_iov = iov;
 500         msg.msg_iovlen = 1;
 501
 502         cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));
 503
 504         for (i = 0; i < opt.num_pkt; i++) {
 505                 err = sendmsg(fd, &msg, 0);
 506                 if (err < 0) {
 507                         if (!opt.silent_send)
 508                                 fprintf(stderr, "send failed: %s\n", strerror(errno));
 509                         err = ERN_SEND;
 510                         goto err_out;
 511                 } else if (err != (int)opt.size) {
 512                         fprintf(stderr, "short send\n");
 513                         err = ERN_SEND_SHORT;
 514                         goto err_out;
 515                 }
 516         }
 517         err = ERN_SUCCESS;
 518
 519         if (opt.ts.ena) {
 520                 unsigned long seen;
 521                 int i;
 522
 523                 /* Make sure all timestamps have time to loop back */
 524                 for (i = 0; i < 40; i++) {
 525                         seen = cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));
 526                         if (seen & (1 << SCM_TSTAMP_SND))
 527                                 break;
 528                         usleep(opt.txtime.delay / 20);
 529                 }
 530         }
 531
 532 err_out:
 533         close(fd);
 534         freeaddrinfo(ai);
 535         return err;
 536 }