0001
0002 #include <errno.h>
0003 #include <error.h>
0004 #include <netdb.h>
0005 #include <stdbool.h>
0006 #include <stdio.h>
0007 #include <stdlib.h>
0008 #include <string.h>
0009 #include <time.h>
0010 #include <unistd.h>
0011 #include <linux/errqueue.h>
0012 #include <linux/icmp.h>
0013 #include <linux/icmpv6.h>
0014 #include <linux/net_tstamp.h>
0015 #include <linux/types.h>
0016 #include <linux/udp.h>
0017 #include <sys/socket.h>
0018
0019 #include "../kselftest.h"
0020
0021 enum {
0022 ERN_SUCCESS = 0,
0023
0024 ERN_SEND = 1,
0025
0026 ERN_HELP,
0027 ERN_SEND_SHORT,
0028 ERN_SOCK_CREATE,
0029 ERN_RESOLVE,
0030 ERN_CMSG_WR,
0031 ERN_SOCKOPT,
0032 ERN_GETTIME,
0033 ERN_RECVERR,
0034 ERN_CMSG_RD,
0035 ERN_CMSG_RCV,
0036 };
0037
0038 struct option_cmsg_u32 {
0039 bool ena;
0040 unsigned int val;
0041 };
0042
0043 struct options {
0044 bool silent_send;
0045 const char *host;
0046 const char *service;
0047 unsigned int size;
0048 struct {
0049 unsigned int mark;
0050 unsigned int dontfrag;
0051 unsigned int tclass;
0052 unsigned int hlimit;
0053 } sockopt;
0054 struct {
0055 unsigned int family;
0056 unsigned int type;
0057 unsigned int proto;
0058 } sock;
0059 struct option_cmsg_u32 mark;
0060 struct {
0061 bool ena;
0062 unsigned int delay;
0063 } txtime;
0064 struct {
0065 bool ena;
0066 } ts;
0067 struct {
0068 struct option_cmsg_u32 dontfrag;
0069 struct option_cmsg_u32 tclass;
0070 struct option_cmsg_u32 hlimit;
0071 struct option_cmsg_u32 exthdr;
0072 } v6;
0073 } opt = {
0074 .size = 13,
0075 .sock = {
0076 .family = AF_UNSPEC,
0077 .type = SOCK_DGRAM,
0078 .proto = IPPROTO_UDP,
0079 },
0080 };
0081
0082 static struct timespec time_start_real;
0083 static struct timespec time_start_mono;
0084
0085 static void __attribute__((noreturn)) cs_usage(const char *bin)
0086 {
0087 printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);
0088 printf("Options:\n"
0089 "\t\t-s Silent send() failures\n"
0090 "\t\t-S send() size\n"
0091 "\t\t-4/-6 Force IPv4 / IPv6 only\n"
0092 "\t\t-p prot Socket protocol\n"
0093 "\t\t (u = UDP (default); i = ICMP; r = RAW)\n"
0094 "\n"
0095 "\t\t-m val Set SO_MARK with given value\n"
0096 "\t\t-M val Set SO_MARK via setsockopt\n"
0097 "\t\t-d val Set SO_TXTIME with given delay (usec)\n"
0098 "\t\t-t Enable time stamp reporting\n"
0099 "\t\t-f val Set don't fragment via cmsg\n"
0100 "\t\t-F val Set don't fragment via setsockopt\n"
0101 "\t\t-c val Set TCLASS via cmsg\n"
0102 "\t\t-C val Set TCLASS via setsockopt\n"
0103 "\t\t-l val Set HOPLIMIT via cmsg\n"
0104 "\t\t-L val Set HOPLIMIT via setsockopt\n"
0105 "\t\t-H type Add an IPv6 header option\n"
0106 "\t\t (h = HOP; d = DST; r = RTDST)"
0107 "");
0108 exit(ERN_HELP);
0109 }
0110
0111 static void cs_parse_args(int argc, char *argv[])
0112 {
0113 char o;
0114
0115 while ((o = getopt(argc, argv, "46sS:p:m:M:d:tf:F:c:C:l:L:H:")) != -1) {
0116 switch (o) {
0117 case 's':
0118 opt.silent_send = true;
0119 break;
0120 case 'S':
0121 opt.size = atoi(optarg);
0122 break;
0123 case '4':
0124 opt.sock.family = AF_INET;
0125 break;
0126 case '6':
0127 opt.sock.family = AF_INET6;
0128 break;
0129 case 'p':
0130 if (*optarg == 'u' || *optarg == 'U') {
0131 opt.sock.proto = IPPROTO_UDP;
0132 } else if (*optarg == 'i' || *optarg == 'I') {
0133 opt.sock.proto = IPPROTO_ICMP;
0134 } else if (*optarg == 'r') {
0135 opt.sock.type = SOCK_RAW;
0136 } else {
0137 printf("Error: unknown protocol: %s\n", optarg);
0138 cs_usage(argv[0]);
0139 }
0140 break;
0141
0142 case 'm':
0143 opt.mark.ena = true;
0144 opt.mark.val = atoi(optarg);
0145 break;
0146 case 'M':
0147 opt.sockopt.mark = atoi(optarg);
0148 break;
0149 case 'd':
0150 opt.txtime.ena = true;
0151 opt.txtime.delay = atoi(optarg);
0152 break;
0153 case 't':
0154 opt.ts.ena = true;
0155 break;
0156 case 'f':
0157 opt.v6.dontfrag.ena = true;
0158 opt.v6.dontfrag.val = atoi(optarg);
0159 break;
0160 case 'F':
0161 opt.sockopt.dontfrag = atoi(optarg);
0162 break;
0163 case 'c':
0164 opt.v6.tclass.ena = true;
0165 opt.v6.tclass.val = atoi(optarg);
0166 break;
0167 case 'C':
0168 opt.sockopt.tclass = atoi(optarg);
0169 break;
0170 case 'l':
0171 opt.v6.hlimit.ena = true;
0172 opt.v6.hlimit.val = atoi(optarg);
0173 break;
0174 case 'L':
0175 opt.sockopt.hlimit = atoi(optarg);
0176 break;
0177 case 'H':
0178 opt.v6.exthdr.ena = true;
0179 switch (optarg[0]) {
0180 case 'h':
0181 opt.v6.exthdr.val = IPV6_HOPOPTS;
0182 break;
0183 case 'd':
0184 opt.v6.exthdr.val = IPV6_DSTOPTS;
0185 break;
0186 case 'r':
0187 opt.v6.exthdr.val = IPV6_RTHDRDSTOPTS;
0188 break;
0189 default:
0190 printf("Error: hdr type: %s\n", optarg);
0191 break;
0192 }
0193 break;
0194 }
0195 }
0196
0197 if (optind != argc - 2)
0198 cs_usage(argv[0]);
0199
0200 opt.host = argv[optind];
0201 opt.service = argv[optind + 1];
0202 }
0203
0204 static void memrnd(void *s, size_t n)
0205 {
0206 int *dword = s;
0207 char *byte;
0208
0209 for (; n >= 4; n -= 4)
0210 *dword++ = rand();
0211 byte = (void *)dword;
0212 while (n--)
0213 *byte++ = rand();
0214 }
0215
0216 static void
0217 ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,
0218 int level, int optname, struct option_cmsg_u32 *uopt)
0219 {
0220 struct cmsghdr *cmsg;
0221
0222 if (!uopt->ena)
0223 return;
0224
0225 cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);
0226 *cmsg_len += CMSG_SPACE(sizeof(__u32));
0227 if (cbuf_sz < *cmsg_len)
0228 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
0229
0230 cmsg->cmsg_level = level;
0231 cmsg->cmsg_type = optname;
0232 cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
0233 *(__u32 *)CMSG_DATA(cmsg) = uopt->val;
0234 }
0235
0236 static void
0237 cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
0238 {
0239 struct cmsghdr *cmsg;
0240 size_t cmsg_len;
0241
0242 msg->msg_control = cbuf;
0243 cmsg_len = 0;
0244
0245 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
0246 SOL_SOCKET, SO_MARK, &opt.mark);
0247 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
0248 SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);
0249 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
0250 SOL_IPV6, IPV6_TCLASS, &opt.v6.tclass);
0251 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
0252 SOL_IPV6, IPV6_HOPLIMIT, &opt.v6.hlimit);
0253
0254 if (opt.txtime.ena) {
0255 struct sock_txtime so_txtime = {
0256 .clockid = CLOCK_MONOTONIC,
0257 };
0258 __u64 txtime;
0259
0260 if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
0261 &so_txtime, sizeof(so_txtime)))
0262 error(ERN_SOCKOPT, errno, "setsockopt TXTIME");
0263
0264 txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +
0265 time_start_mono.tv_nsec +
0266 opt.txtime.delay * 1000;
0267
0268 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
0269 cmsg_len += CMSG_SPACE(sizeof(txtime));
0270 if (cbuf_sz < cmsg_len)
0271 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
0272
0273 cmsg->cmsg_level = SOL_SOCKET;
0274 cmsg->cmsg_type = SCM_TXTIME;
0275 cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
0276 memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));
0277 }
0278 if (opt.ts.ena) {
0279 __u32 val = SOF_TIMESTAMPING_SOFTWARE |
0280 SOF_TIMESTAMPING_OPT_TSONLY;
0281
0282 if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
0283 &val, sizeof(val)))
0284 error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");
0285
0286 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
0287 cmsg_len += CMSG_SPACE(sizeof(__u32));
0288 if (cbuf_sz < cmsg_len)
0289 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
0290
0291 cmsg->cmsg_level = SOL_SOCKET;
0292 cmsg->cmsg_type = SO_TIMESTAMPING;
0293 cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
0294 *(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |
0295 SOF_TIMESTAMPING_TX_SOFTWARE;
0296 }
0297 if (opt.v6.exthdr.ena) {
0298 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
0299 cmsg_len += CMSG_SPACE(8);
0300 if (cbuf_sz < cmsg_len)
0301 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
0302
0303 cmsg->cmsg_level = SOL_IPV6;
0304 cmsg->cmsg_type = opt.v6.exthdr.val;
0305 cmsg->cmsg_len = CMSG_LEN(8);
0306 *(__u64 *)CMSG_DATA(cmsg) = 0;
0307 }
0308
0309 if (cmsg_len)
0310 msg->msg_controllen = cmsg_len;
0311 else
0312 msg->msg_control = NULL;
0313 }
0314
0315 static const char *cs_ts_info2str(unsigned int info)
0316 {
0317 static const char *names[] = {
0318 [SCM_TSTAMP_SND] = "SND",
0319 [SCM_TSTAMP_SCHED] = "SCHED",
0320 [SCM_TSTAMP_ACK] = "ACK",
0321 };
0322
0323 if (info < ARRAY_SIZE(names))
0324 return names[info];
0325 return "unknown";
0326 }
0327
0328 static void
0329 cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
0330 {
0331 struct sock_extended_err *see;
0332 struct scm_timestamping *ts;
0333 struct cmsghdr *cmsg;
0334 int i, err;
0335
0336 if (!opt.ts.ena)
0337 return;
0338 msg->msg_control = cbuf;
0339 msg->msg_controllen = cbuf_sz;
0340
0341 while (true) {
0342 ts = NULL;
0343 see = NULL;
0344 memset(cbuf, 0, cbuf_sz);
0345
0346 err = recvmsg(fd, msg, MSG_ERRQUEUE);
0347 if (err < 0) {
0348 if (errno == EAGAIN)
0349 break;
0350 error(ERN_RECVERR, errno, "recvmsg ERRQ");
0351 }
0352
0353 for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
0354 cmsg = CMSG_NXTHDR(msg, cmsg)) {
0355 if (cmsg->cmsg_level == SOL_SOCKET &&
0356 cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {
0357 if (cmsg->cmsg_len < sizeof(*ts))
0358 error(ERN_CMSG_RD, EINVAL, "TS cmsg");
0359
0360 ts = (void *)CMSG_DATA(cmsg);
0361 }
0362 if ((cmsg->cmsg_level == SOL_IP &&
0363 cmsg->cmsg_type == IP_RECVERR) ||
0364 (cmsg->cmsg_level == SOL_IPV6 &&
0365 cmsg->cmsg_type == IPV6_RECVERR)) {
0366 if (cmsg->cmsg_len < sizeof(*see))
0367 error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");
0368
0369 see = (void *)CMSG_DATA(cmsg);
0370 }
0371 }
0372
0373 if (!ts)
0374 error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");
0375 if (!see)
0376 error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");
0377
0378 for (i = 0; i < 3; i++) {
0379 unsigned long long rel_time;
0380
0381 if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)
0382 continue;
0383
0384 rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *
0385 (1000ULL * 1000) +
0386 (ts->ts[i].tv_nsec - time_start_real.tv_nsec) /
0387 1000;
0388 printf(" %5s ts%d %lluus\n",
0389 cs_ts_info2str(see->ee_info),
0390 i, rel_time);
0391 }
0392 }
0393 }
0394
0395 static void ca_set_sockopts(int fd)
0396 {
0397 if (opt.sockopt.mark &&
0398 setsockopt(fd, SOL_SOCKET, SO_MARK,
0399 &opt.sockopt.mark, sizeof(opt.sockopt.mark)))
0400 error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");
0401 if (opt.sockopt.dontfrag &&
0402 setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,
0403 &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))
0404 error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");
0405 if (opt.sockopt.tclass &&
0406 setsockopt(fd, SOL_IPV6, IPV6_TCLASS,
0407 &opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))
0408 error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS");
0409 if (opt.sockopt.hlimit &&
0410 setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS,
0411 &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))
0412 error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT");
0413 }
0414
0415 int main(int argc, char *argv[])
0416 {
0417 struct addrinfo hints, *ai;
0418 struct iovec iov[1];
0419 struct msghdr msg;
0420 char cbuf[1024];
0421 char *buf;
0422 int err;
0423 int fd;
0424
0425 cs_parse_args(argc, argv);
0426
0427 buf = malloc(opt.size);
0428 memrnd(buf, opt.size);
0429
0430 memset(&hints, 0, sizeof(hints));
0431 hints.ai_family = opt.sock.family;
0432
0433 ai = NULL;
0434 err = getaddrinfo(opt.host, opt.service, &hints, &ai);
0435 if (err) {
0436 fprintf(stderr, "Can't resolve address [%s]:%s\n",
0437 opt.host, opt.service);
0438 return ERN_SOCK_CREATE;
0439 }
0440
0441 if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)
0442 opt.sock.proto = IPPROTO_ICMPV6;
0443
0444 fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);
0445 if (fd < 0) {
0446 fprintf(stderr, "Can't open socket: %s\n", strerror(errno));
0447 freeaddrinfo(ai);
0448 return ERN_RESOLVE;
0449 }
0450
0451 if (opt.sock.proto == IPPROTO_ICMP) {
0452 buf[0] = ICMP_ECHO;
0453 buf[1] = 0;
0454 } else if (opt.sock.proto == IPPROTO_ICMPV6) {
0455 buf[0] = ICMPV6_ECHO_REQUEST;
0456 buf[1] = 0;
0457 } else if (opt.sock.type == SOCK_RAW) {
0458 struct udphdr hdr = { 1, 2, htons(opt.size), 0 };
0459 struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;
0460
0461 memcpy(buf, &hdr, sizeof(hdr));
0462 sin6->sin6_port = htons(opt.sock.proto);
0463 }
0464
0465 ca_set_sockopts(fd);
0466
0467 if (clock_gettime(CLOCK_REALTIME, &time_start_real))
0468 error(ERN_GETTIME, errno, "gettime REALTIME");
0469 if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))
0470 error(ERN_GETTIME, errno, "gettime MONOTONIC");
0471
0472 iov[0].iov_base = buf;
0473 iov[0].iov_len = opt.size;
0474
0475 memset(&msg, 0, sizeof(msg));
0476 msg.msg_name = ai->ai_addr;
0477 msg.msg_namelen = ai->ai_addrlen;
0478 msg.msg_iov = iov;
0479 msg.msg_iovlen = 1;
0480
0481 cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));
0482
0483 err = sendmsg(fd, &msg, 0);
0484 if (err < 0) {
0485 if (!opt.silent_send)
0486 fprintf(stderr, "send failed: %s\n", strerror(errno));
0487 err = ERN_SEND;
0488 goto err_out;
0489 } else if (err != (int)opt.size) {
0490 fprintf(stderr, "short send\n");
0491 err = ERN_SEND_SHORT;
0492 goto err_out;
0493 } else {
0494 err = ERN_SUCCESS;
0495 }
0496
0497
0498 usleep(opt.txtime.delay);
0499
0500 cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));
0501
0502 err_out:
0503 close(fd);
0504 freeaddrinfo(ai);
0505 return err;
0506 }
