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