537 lines
13 KiB
C
537 lines
13 KiB
C
// SPDX-License-Identifier: GPL-2.0-or-later
|
|
#include <errno.h>
|
|
#include <error.h>
|
|
#include <netdb.h>
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <unistd.h>
|
|
#include <linux/errqueue.h>
|
|
#include <linux/icmp.h>
|
|
#include <linux/icmpv6.h>
|
|
#include <linux/net_tstamp.h>
|
|
#include <linux/types.h>
|
|
#include <linux/udp.h>
|
|
#include <sys/socket.h>
|
|
|
|
#include "../kselftest.h"
|
|
|
|
enum {
|
|
ERN_SUCCESS = 0,
|
|
/* Well defined errors, callers may depend on these */
|
|
ERN_SEND = 1,
|
|
/* Informational, can reorder */
|
|
ERN_HELP,
|
|
ERN_SEND_SHORT,
|
|
ERN_SOCK_CREATE,
|
|
ERN_RESOLVE,
|
|
ERN_CMSG_WR,
|
|
ERN_SOCKOPT,
|
|
ERN_GETTIME,
|
|
ERN_RECVERR,
|
|
ERN_CMSG_RD,
|
|
ERN_CMSG_RCV,
|
|
};
|
|
|
|
struct option_cmsg_u32 {
|
|
bool ena;
|
|
unsigned int val;
|
|
};
|
|
|
|
struct options {
|
|
bool silent_send;
|
|
const char *host;
|
|
const char *service;
|
|
unsigned int size;
|
|
unsigned int num_pkt;
|
|
struct {
|
|
unsigned int mark;
|
|
unsigned int dontfrag;
|
|
unsigned int tclass;
|
|
unsigned int hlimit;
|
|
unsigned int priority;
|
|
} sockopt;
|
|
struct {
|
|
unsigned int family;
|
|
unsigned int type;
|
|
unsigned int proto;
|
|
} sock;
|
|
struct option_cmsg_u32 mark;
|
|
struct {
|
|
bool ena;
|
|
unsigned int delay;
|
|
} txtime;
|
|
struct {
|
|
bool ena;
|
|
} ts;
|
|
struct {
|
|
struct option_cmsg_u32 dontfrag;
|
|
struct option_cmsg_u32 tclass;
|
|
struct option_cmsg_u32 hlimit;
|
|
struct option_cmsg_u32 exthdr;
|
|
} v6;
|
|
} opt = {
|
|
.size = 13,
|
|
.num_pkt = 1,
|
|
.sock = {
|
|
.family = AF_UNSPEC,
|
|
.type = SOCK_DGRAM,
|
|
.proto = IPPROTO_UDP,
|
|
},
|
|
};
|
|
|
|
static struct timespec time_start_real;
|
|
static struct timespec time_start_mono;
|
|
|
|
static void __attribute__((noreturn)) cs_usage(const char *bin)
|
|
{
|
|
printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);
|
|
printf("Options:\n"
|
|
"\t\t-s Silent send() failures\n"
|
|
"\t\t-S send() size\n"
|
|
"\t\t-4/-6 Force IPv4 / IPv6 only\n"
|
|
"\t\t-p prot Socket protocol\n"
|
|
"\t\t (u = UDP (default); i = ICMP; r = RAW)\n"
|
|
"\n"
|
|
"\t\t-m val Set SO_MARK with given value\n"
|
|
"\t\t-M val Set SO_MARK via setsockopt\n"
|
|
"\t\t-d val Set SO_TXTIME with given delay (usec)\n"
|
|
"\t\t-t Enable time stamp reporting\n"
|
|
"\t\t-f val Set don't fragment via cmsg\n"
|
|
"\t\t-F val Set don't fragment via setsockopt\n"
|
|
"\t\t-c val Set TCLASS via cmsg\n"
|
|
"\t\t-C val Set TCLASS via setsockopt\n"
|
|
"\t\t-l val Set HOPLIMIT via cmsg\n"
|
|
"\t\t-L val Set HOPLIMIT via setsockopt\n"
|
|
"\t\t-H type Add an IPv6 header option\n"
|
|
"\t\t (h = HOP; d = DST; r = RTDST)"
|
|
"");
|
|
exit(ERN_HELP);
|
|
}
|
|
|
|
static void cs_parse_args(int argc, char *argv[])
|
|
{
|
|
int o;
|
|
|
|
while ((o = getopt(argc, argv, "46sS:p:P:m:M:n:d:tf:F:c:C:l:L:H:")) != -1) {
|
|
switch (o) {
|
|
case 's':
|
|
opt.silent_send = true;
|
|
break;
|
|
case 'S':
|
|
opt.size = atoi(optarg);
|
|
break;
|
|
case '4':
|
|
opt.sock.family = AF_INET;
|
|
break;
|
|
case '6':
|
|
opt.sock.family = AF_INET6;
|
|
break;
|
|
case 'p':
|
|
if (*optarg == 'u' || *optarg == 'U') {
|
|
opt.sock.proto = IPPROTO_UDP;
|
|
} else if (*optarg == 'i' || *optarg == 'I') {
|
|
opt.sock.proto = IPPROTO_ICMP;
|
|
} else if (*optarg == 'r') {
|
|
opt.sock.type = SOCK_RAW;
|
|
} else {
|
|
printf("Error: unknown protocol: %s\n", optarg);
|
|
cs_usage(argv[0]);
|
|
}
|
|
break;
|
|
case 'P':
|
|
opt.sockopt.priority = atoi(optarg);
|
|
break;
|
|
case 'm':
|
|
opt.mark.ena = true;
|
|
opt.mark.val = atoi(optarg);
|
|
break;
|
|
case 'M':
|
|
opt.sockopt.mark = atoi(optarg);
|
|
break;
|
|
case 'n':
|
|
opt.num_pkt = atoi(optarg);
|
|
break;
|
|
case 'd':
|
|
opt.txtime.ena = true;
|
|
opt.txtime.delay = atoi(optarg);
|
|
break;
|
|
case 't':
|
|
opt.ts.ena = true;
|
|
break;
|
|
case 'f':
|
|
opt.v6.dontfrag.ena = true;
|
|
opt.v6.dontfrag.val = atoi(optarg);
|
|
break;
|
|
case 'F':
|
|
opt.sockopt.dontfrag = atoi(optarg);
|
|
break;
|
|
case 'c':
|
|
opt.v6.tclass.ena = true;
|
|
opt.v6.tclass.val = atoi(optarg);
|
|
break;
|
|
case 'C':
|
|
opt.sockopt.tclass = atoi(optarg);
|
|
break;
|
|
case 'l':
|
|
opt.v6.hlimit.ena = true;
|
|
opt.v6.hlimit.val = atoi(optarg);
|
|
break;
|
|
case 'L':
|
|
opt.sockopt.hlimit = atoi(optarg);
|
|
break;
|
|
case 'H':
|
|
opt.v6.exthdr.ena = true;
|
|
switch (optarg[0]) {
|
|
case 'h':
|
|
opt.v6.exthdr.val = IPV6_HOPOPTS;
|
|
break;
|
|
case 'd':
|
|
opt.v6.exthdr.val = IPV6_DSTOPTS;
|
|
break;
|
|
case 'r':
|
|
opt.v6.exthdr.val = IPV6_RTHDRDSTOPTS;
|
|
break;
|
|
default:
|
|
printf("Error: hdr type: %s\n", optarg);
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (optind != argc - 2)
|
|
cs_usage(argv[0]);
|
|
|
|
opt.host = argv[optind];
|
|
opt.service = argv[optind + 1];
|
|
}
|
|
|
|
static void memrnd(void *s, size_t n)
|
|
{
|
|
int *dword = s;
|
|
char *byte;
|
|
|
|
for (; n >= 4; n -= 4)
|
|
*dword++ = rand();
|
|
byte = (void *)dword;
|
|
while (n--)
|
|
*byte++ = rand();
|
|
}
|
|
|
|
static void
|
|
ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,
|
|
int level, int optname, struct option_cmsg_u32 *uopt)
|
|
{
|
|
struct cmsghdr *cmsg;
|
|
|
|
if (!uopt->ena)
|
|
return;
|
|
|
|
cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);
|
|
*cmsg_len += CMSG_SPACE(sizeof(__u32));
|
|
if (cbuf_sz < *cmsg_len)
|
|
error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
|
|
|
|
cmsg->cmsg_level = level;
|
|
cmsg->cmsg_type = optname;
|
|
cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
|
|
*(__u32 *)CMSG_DATA(cmsg) = uopt->val;
|
|
}
|
|
|
|
static void
|
|
cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
|
|
{
|
|
struct cmsghdr *cmsg;
|
|
size_t cmsg_len;
|
|
|
|
msg->msg_control = cbuf;
|
|
cmsg_len = 0;
|
|
|
|
ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
|
|
SOL_SOCKET, SO_MARK, &opt.mark);
|
|
ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
|
|
SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);
|
|
ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
|
|
SOL_IPV6, IPV6_TCLASS, &opt.v6.tclass);
|
|
ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
|
|
SOL_IPV6, IPV6_HOPLIMIT, &opt.v6.hlimit);
|
|
|
|
if (opt.txtime.ena) {
|
|
__u64 txtime;
|
|
|
|
txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +
|
|
time_start_mono.tv_nsec +
|
|
opt.txtime.delay * 1000;
|
|
|
|
cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
|
|
cmsg_len += CMSG_SPACE(sizeof(txtime));
|
|
if (cbuf_sz < cmsg_len)
|
|
error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
|
|
|
|
cmsg->cmsg_level = SOL_SOCKET;
|
|
cmsg->cmsg_type = SCM_TXTIME;
|
|
cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
|
|
memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));
|
|
}
|
|
if (opt.ts.ena) {
|
|
cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
|
|
cmsg_len += CMSG_SPACE(sizeof(__u32));
|
|
if (cbuf_sz < cmsg_len)
|
|
error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
|
|
|
|
cmsg->cmsg_level = SOL_SOCKET;
|
|
cmsg->cmsg_type = SO_TIMESTAMPING;
|
|
cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
|
|
*(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |
|
|
SOF_TIMESTAMPING_TX_SOFTWARE;
|
|
}
|
|
if (opt.v6.exthdr.ena) {
|
|
cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
|
|
cmsg_len += CMSG_SPACE(8);
|
|
if (cbuf_sz < cmsg_len)
|
|
error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
|
|
|
|
cmsg->cmsg_level = SOL_IPV6;
|
|
cmsg->cmsg_type = opt.v6.exthdr.val;
|
|
cmsg->cmsg_len = CMSG_LEN(8);
|
|
*(__u64 *)CMSG_DATA(cmsg) = 0;
|
|
}
|
|
|
|
if (cmsg_len)
|
|
msg->msg_controllen = cmsg_len;
|
|
else
|
|
msg->msg_control = NULL;
|
|
}
|
|
|
|
static const char *cs_ts_info2str(unsigned int info)
|
|
{
|
|
static const char *names[] = {
|
|
[SCM_TSTAMP_SND] = "SND",
|
|
[SCM_TSTAMP_SCHED] = "SCHED",
|
|
[SCM_TSTAMP_ACK] = "ACK",
|
|
};
|
|
|
|
if (info < ARRAY_SIZE(names))
|
|
return names[info];
|
|
return "unknown";
|
|
}
|
|
|
|
static unsigned long
|
|
cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
|
|
{
|
|
struct sock_extended_err *see;
|
|
struct scm_timestamping *ts;
|
|
unsigned long ts_seen = 0;
|
|
struct cmsghdr *cmsg;
|
|
int i, err;
|
|
|
|
if (!opt.ts.ena)
|
|
return 0;
|
|
msg->msg_control = cbuf;
|
|
msg->msg_controllen = cbuf_sz;
|
|
|
|
while (true) {
|
|
ts = NULL;
|
|
see = NULL;
|
|
memset(cbuf, 0, cbuf_sz);
|
|
|
|
err = recvmsg(fd, msg, MSG_ERRQUEUE);
|
|
if (err < 0) {
|
|
if (errno == EAGAIN)
|
|
break;
|
|
error(ERN_RECVERR, errno, "recvmsg ERRQ");
|
|
}
|
|
|
|
for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
|
|
cmsg = CMSG_NXTHDR(msg, cmsg)) {
|
|
if (cmsg->cmsg_level == SOL_SOCKET &&
|
|
cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {
|
|
if (cmsg->cmsg_len < sizeof(*ts))
|
|
error(ERN_CMSG_RD, EINVAL, "TS cmsg");
|
|
|
|
ts = (void *)CMSG_DATA(cmsg);
|
|
}
|
|
if ((cmsg->cmsg_level == SOL_IP &&
|
|
cmsg->cmsg_type == IP_RECVERR) ||
|
|
(cmsg->cmsg_level == SOL_IPV6 &&
|
|
cmsg->cmsg_type == IPV6_RECVERR)) {
|
|
if (cmsg->cmsg_len < sizeof(*see))
|
|
error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");
|
|
|
|
see = (void *)CMSG_DATA(cmsg);
|
|
}
|
|
}
|
|
|
|
if (!ts)
|
|
error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");
|
|
if (!see)
|
|
error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
unsigned long long rel_time;
|
|
|
|
if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)
|
|
continue;
|
|
|
|
rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *
|
|
(1000ULL * 1000) +
|
|
(ts->ts[i].tv_nsec - time_start_real.tv_nsec) /
|
|
1000;
|
|
printf(" %5s ts%d %lluus\n",
|
|
cs_ts_info2str(see->ee_info),
|
|
i, rel_time);
|
|
ts_seen |= 1 << see->ee_info;
|
|
}
|
|
}
|
|
|
|
return ts_seen;
|
|
}
|
|
|
|
static void ca_set_sockopts(int fd)
|
|
{
|
|
if (opt.sockopt.mark &&
|
|
setsockopt(fd, SOL_SOCKET, SO_MARK,
|
|
&opt.sockopt.mark, sizeof(opt.sockopt.mark)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");
|
|
if (opt.sockopt.dontfrag &&
|
|
setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,
|
|
&opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");
|
|
if (opt.sockopt.tclass &&
|
|
setsockopt(fd, SOL_IPV6, IPV6_TCLASS,
|
|
&opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS");
|
|
if (opt.sockopt.hlimit &&
|
|
setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS,
|
|
&opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT");
|
|
if (opt.sockopt.priority &&
|
|
setsockopt(fd, SOL_SOCKET, SO_PRIORITY,
|
|
&opt.sockopt.priority, sizeof(opt.sockopt.priority)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt SO_PRIORITY");
|
|
|
|
if (opt.txtime.ena) {
|
|
struct sock_txtime so_txtime = {
|
|
.clockid = CLOCK_MONOTONIC,
|
|
};
|
|
|
|
if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
|
|
&so_txtime, sizeof(so_txtime)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt TXTIME");
|
|
}
|
|
if (opt.ts.ena) {
|
|
__u32 val = SOF_TIMESTAMPING_SOFTWARE |
|
|
SOF_TIMESTAMPING_OPT_TSONLY;
|
|
|
|
if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
|
|
&val, sizeof(val)))
|
|
error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");
|
|
}
|
|
}
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
struct addrinfo hints, *ai;
|
|
struct iovec iov[1];
|
|
unsigned char *buf;
|
|
struct msghdr msg;
|
|
char cbuf[1024];
|
|
int err;
|
|
int fd;
|
|
int i;
|
|
|
|
cs_parse_args(argc, argv);
|
|
|
|
buf = malloc(opt.size);
|
|
memrnd(buf, opt.size);
|
|
|
|
memset(&hints, 0, sizeof(hints));
|
|
hints.ai_family = opt.sock.family;
|
|
|
|
ai = NULL;
|
|
err = getaddrinfo(opt.host, opt.service, &hints, &ai);
|
|
if (err) {
|
|
fprintf(stderr, "Can't resolve address [%s]:%s\n",
|
|
opt.host, opt.service);
|
|
return ERN_SOCK_CREATE;
|
|
}
|
|
|
|
if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)
|
|
opt.sock.proto = IPPROTO_ICMPV6;
|
|
|
|
fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);
|
|
if (fd < 0) {
|
|
fprintf(stderr, "Can't open socket: %s\n", strerror(errno));
|
|
freeaddrinfo(ai);
|
|
return ERN_RESOLVE;
|
|
}
|
|
|
|
if (opt.sock.proto == IPPROTO_ICMP) {
|
|
buf[0] = ICMP_ECHO;
|
|
buf[1] = 0;
|
|
} else if (opt.sock.proto == IPPROTO_ICMPV6) {
|
|
buf[0] = ICMPV6_ECHO_REQUEST;
|
|
buf[1] = 0;
|
|
} else if (opt.sock.type == SOCK_RAW) {
|
|
struct udphdr hdr = { 1, 2, htons(opt.size), 0 };
|
|
struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;
|
|
|
|
memcpy(buf, &hdr, sizeof(hdr));
|
|
sin6->sin6_port = htons(opt.sock.proto);
|
|
}
|
|
|
|
ca_set_sockopts(fd);
|
|
|
|
if (clock_gettime(CLOCK_REALTIME, &time_start_real))
|
|
error(ERN_GETTIME, errno, "gettime REALTIME");
|
|
if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))
|
|
error(ERN_GETTIME, errno, "gettime MONOTONIC");
|
|
|
|
iov[0].iov_base = buf;
|
|
iov[0].iov_len = opt.size;
|
|
|
|
memset(&msg, 0, sizeof(msg));
|
|
msg.msg_name = ai->ai_addr;
|
|
msg.msg_namelen = ai->ai_addrlen;
|
|
msg.msg_iov = iov;
|
|
msg.msg_iovlen = 1;
|
|
|
|
cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));
|
|
|
|
for (i = 0; i < opt.num_pkt; i++) {
|
|
err = sendmsg(fd, &msg, 0);
|
|
if (err < 0) {
|
|
if (!opt.silent_send)
|
|
fprintf(stderr, "send failed: %s\n", strerror(errno));
|
|
err = ERN_SEND;
|
|
goto err_out;
|
|
} else if (err != (int)opt.size) {
|
|
fprintf(stderr, "short send\n");
|
|
err = ERN_SEND_SHORT;
|
|
goto err_out;
|
|
}
|
|
}
|
|
err = ERN_SUCCESS;
|
|
|
|
if (opt.ts.ena) {
|
|
unsigned long seen;
|
|
int i;
|
|
|
|
/* Make sure all timestamps have time to loop back */
|
|
for (i = 0; i < 40; i++) {
|
|
seen = cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));
|
|
if (seen & (1 << SCM_TSTAMP_SND))
|
|
break;
|
|
usleep(opt.txtime.delay / 20);
|
|
}
|
|
}
|
|
|
|
err_out:
|
|
close(fd);
|
|
freeaddrinfo(ai);
|
|
return err;
|
|
}
|