1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33#define _GNU_SOURCE
34
35#include <arpa/inet.h>
36#include <asm/types.h>
37#include <error.h>
38#include <errno.h>
39#include <inttypes.h>
40#include <linux/errqueue.h>
41#include <linux/if_ether.h>
42#include <linux/net_tstamp.h>
43#include <netdb.h>
44#include <net/if.h>
45#include <netinet/in.h>
46#include <netinet/ip.h>
47#include <netinet/udp.h>
48#include <netinet/tcp.h>
49#include <netpacket/packet.h>
50#include <poll.h>
51#include <stdarg.h>
52#include <stdbool.h>
53#include <stdio.h>
54#include <stdlib.h>
55#include <string.h>
56#include <sys/ioctl.h>
57#include <sys/select.h>
58#include <sys/socket.h>
59#include <sys/time.h>
60#include <sys/types.h>
61#include <time.h>
62#include <unistd.h>
63
64
65static int cfg_proto = SOCK_STREAM;
66static int cfg_ipproto = IPPROTO_TCP;
67static int cfg_num_pkts = 4;
68static int do_ipv4 = 1;
69static int do_ipv6 = 1;
70static int cfg_payload_len = 10;
71static bool cfg_show_payload;
72static bool cfg_do_pktinfo;
73static bool cfg_loop_nodata;
74static uint16_t dest_port = 9000;
75
76static struct sockaddr_in daddr;
77static struct sockaddr_in6 daddr6;
78static struct timespec ts_prev;
79
80static void __print_timestamp(const char *name, struct timespec *cur,
81 uint32_t key, int payload_len)
82{
83 if (!(cur->tv_sec | cur->tv_nsec))
84 return;
85
86 fprintf(stderr, " %s: %lu s %lu us (seq=%u, len=%u)",
87 name, cur->tv_sec, cur->tv_nsec / 1000,
88 key, payload_len);
89
90 if ((ts_prev.tv_sec | ts_prev.tv_nsec)) {
91 int64_t cur_ms, prev_ms;
92
93 cur_ms = (long) cur->tv_sec * 1000 * 1000;
94 cur_ms += cur->tv_nsec / 1000;
95
96 prev_ms = (long) ts_prev.tv_sec * 1000 * 1000;
97 prev_ms += ts_prev.tv_nsec / 1000;
98
99 fprintf(stderr, " (%+" PRId64 " us)", cur_ms - prev_ms);
100 }
101
102 ts_prev = *cur;
103 fprintf(stderr, "\n");
104}
105
106static void print_timestamp_usr(void)
107{
108 struct timespec ts;
109 struct timeval tv;
110
111 gettimeofday(&tv, NULL);
112 ts.tv_sec = tv.tv_sec;
113 ts.tv_nsec = tv.tv_usec * 1000;
114
115 __print_timestamp(" USR", &ts, 0, 0);
116}
117
118static void print_timestamp(struct scm_timestamping *tss, int tstype,
119 int tskey, int payload_len)
120{
121 const char *tsname;
122
123 switch (tstype) {
124 case SCM_TSTAMP_SCHED:
125 tsname = " ENQ";
126 break;
127 case SCM_TSTAMP_SND:
128 tsname = " SND";
129 break;
130 case SCM_TSTAMP_ACK:
131 tsname = " ACK";
132 break;
133 default:
134 error(1, 0, "unknown timestamp type: %u",
135 tstype);
136 }
137 __print_timestamp(tsname, &tss->ts[0], tskey, payload_len);
138}
139
140
141static void print_payload(char *data, int len)
142{
143 int i;
144
145 if (!len)
146 return;
147
148 if (len > 70)
149 len = 70;
150
151 fprintf(stderr, "payload: ");
152 for (i = 0; i < len; i++)
153 fprintf(stderr, "%02hhx ", data[i]);
154 fprintf(stderr, "\n");
155}
156
157static void print_pktinfo(int family, int ifindex, void *saddr, void *daddr)
158{
159 char sa[INET6_ADDRSTRLEN], da[INET6_ADDRSTRLEN];
160
161 fprintf(stderr, " pktinfo: ifindex=%u src=%s dst=%s\n",
162 ifindex,
163 saddr ? inet_ntop(family, saddr, sa, sizeof(sa)) : "unknown",
164 daddr ? inet_ntop(family, daddr, da, sizeof(da)) : "unknown");
165}
166
167static void __poll(int fd)
168{
169 struct pollfd pollfd;
170 int ret;
171
172 memset(&pollfd, 0, sizeof(pollfd));
173 pollfd.fd = fd;
174 ret = poll(&pollfd, 1, 100);
175 if (ret != 1)
176 error(1, errno, "poll");
177}
178
179static void __recv_errmsg_cmsg(struct msghdr *msg, int payload_len)
180{
181 struct sock_extended_err *serr = NULL;
182 struct scm_timestamping *tss = NULL;
183 struct cmsghdr *cm;
184 int batch = 0;
185
186 for (cm = CMSG_FIRSTHDR(msg);
187 cm && cm->cmsg_len;
188 cm = CMSG_NXTHDR(msg, cm)) {
189 if (cm->cmsg_level == SOL_SOCKET &&
190 cm->cmsg_type == SCM_TIMESTAMPING) {
191 tss = (void *) CMSG_DATA(cm);
192 } else if ((cm->cmsg_level == SOL_IP &&
193 cm->cmsg_type == IP_RECVERR) ||
194 (cm->cmsg_level == SOL_IPV6 &&
195 cm->cmsg_type == IPV6_RECVERR)) {
196 serr = (void *) CMSG_DATA(cm);
197 if (serr->ee_errno != ENOMSG ||
198 serr->ee_origin != SO_EE_ORIGIN_TIMESTAMPING) {
199 fprintf(stderr, "unknown ip error %d %d\n",
200 serr->ee_errno,
201 serr->ee_origin);
202 serr = NULL;
203 }
204 } else if (cm->cmsg_level == SOL_IP &&
205 cm->cmsg_type == IP_PKTINFO) {
206 struct in_pktinfo *info = (void *) CMSG_DATA(cm);
207 print_pktinfo(AF_INET, info->ipi_ifindex,
208 &info->ipi_spec_dst, &info->ipi_addr);
209 } else if (cm->cmsg_level == SOL_IPV6 &&
210 cm->cmsg_type == IPV6_PKTINFO) {
211 struct in6_pktinfo *info6 = (void *) CMSG_DATA(cm);
212 print_pktinfo(AF_INET6, info6->ipi6_ifindex,
213 NULL, &info6->ipi6_addr);
214 } else
215 fprintf(stderr, "unknown cmsg %d,%d\n",
216 cm->cmsg_level, cm->cmsg_type);
217
218 if (serr && tss) {
219 print_timestamp(tss, serr->ee_info, serr->ee_data,
220 payload_len);
221 serr = NULL;
222 tss = NULL;
223 batch++;
224 }
225 }
226
227 if (batch > 1)
228 fprintf(stderr, "batched %d timestamps\n", batch);
229}
230
231static int recv_errmsg(int fd)
232{
233 static char ctrl[1024 ];
234 static struct msghdr msg;
235 struct iovec entry;
236 static char *data;
237 int ret = 0;
238
239 data = malloc(cfg_payload_len);
240 if (!data)
241 error(1, 0, "malloc");
242
243 memset(&msg, 0, sizeof(msg));
244 memset(&entry, 0, sizeof(entry));
245 memset(ctrl, 0, sizeof(ctrl));
246
247 entry.iov_base = data;
248 entry.iov_len = cfg_payload_len;
249 msg.msg_iov = &entry;
250 msg.msg_iovlen = 1;
251 msg.msg_name = NULL;
252 msg.msg_namelen = 0;
253 msg.msg_control = ctrl;
254 msg.msg_controllen = sizeof(ctrl);
255
256 ret = recvmsg(fd, &msg, MSG_ERRQUEUE);
257 if (ret == -1 && errno != EAGAIN)
258 error(1, errno, "recvmsg");
259
260 if (ret >= 0) {
261 __recv_errmsg_cmsg(&msg, ret);
262 if (cfg_show_payload)
263 print_payload(data, cfg_payload_len);
264 }
265
266 free(data);
267 return ret == -1;
268}
269
270static void do_test(int family, unsigned int opt)
271{
272 char *buf;
273 int fd, i, val = 1, total_len;
274
275 if (family == AF_INET6 && cfg_proto != SOCK_STREAM) {
276
277 fprintf(stderr, "test: skipping datagram over IPv6\n");
278 return;
279 }
280
281 total_len = cfg_payload_len;
282 if (cfg_proto == SOCK_RAW) {
283 total_len += sizeof(struct udphdr);
284 if (cfg_ipproto == IPPROTO_RAW)
285 total_len += sizeof(struct iphdr);
286 }
287
288 buf = malloc(total_len);
289 if (!buf)
290 error(1, 0, "malloc");
291
292 fd = socket(family, cfg_proto, cfg_ipproto);
293 if (fd < 0)
294 error(1, errno, "socket");
295
296 if (cfg_proto == SOCK_STREAM) {
297 if (setsockopt(fd, IPPROTO_TCP, TCP_NODELAY,
298 (char*) &val, sizeof(val)))
299 error(1, 0, "setsockopt no nagle");
300
301 if (family == PF_INET) {
302 if (connect(fd, (void *) &daddr, sizeof(daddr)))
303 error(1, errno, "connect ipv4");
304 } else {
305 if (connect(fd, (void *) &daddr6, sizeof(daddr6)))
306 error(1, errno, "connect ipv6");
307 }
308 }
309
310 if (cfg_do_pktinfo) {
311 if (family == AF_INET6) {
312 if (setsockopt(fd, SOL_IPV6, IPV6_RECVPKTINFO,
313 &val, sizeof(val)))
314 error(1, errno, "setsockopt pktinfo ipv6");
315 } else {
316 if (setsockopt(fd, SOL_IP, IP_PKTINFO,
317 &val, sizeof(val)))
318 error(1, errno, "setsockopt pktinfo ipv4");
319 }
320 }
321
322 opt |= SOF_TIMESTAMPING_SOFTWARE |
323 SOF_TIMESTAMPING_OPT_CMSG |
324 SOF_TIMESTAMPING_OPT_ID;
325 if (cfg_loop_nodata)
326 opt |= SOF_TIMESTAMPING_OPT_TSONLY;
327
328 if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
329 (char *) &opt, sizeof(opt)))
330 error(1, 0, "setsockopt timestamping");
331
332 for (i = 0; i < cfg_num_pkts; i++) {
333 memset(&ts_prev, 0, sizeof(ts_prev));
334 memset(buf, 'a' + i, total_len);
335
336 if (cfg_proto == SOCK_RAW) {
337 struct udphdr *udph;
338 int off = 0;
339
340 if (cfg_ipproto == IPPROTO_RAW) {
341 struct iphdr *iph = (void *) buf;
342
343 memset(iph, 0, sizeof(*iph));
344 iph->ihl = 5;
345 iph->version = 4;
346 iph->ttl = 2;
347 iph->daddr = daddr.sin_addr.s_addr;
348 iph->protocol = IPPROTO_UDP;
349
350
351 off = sizeof(*iph);
352 }
353
354 udph = (void *) buf + off;
355 udph->source = ntohs(9000);
356 udph->dest = ntohs(dest_port);
357 udph->len = ntohs(sizeof(*udph) + cfg_payload_len);
358 udph->check = 0;
359 }
360
361 print_timestamp_usr();
362 if (cfg_proto != SOCK_STREAM) {
363 if (family == PF_INET)
364 val = sendto(fd, buf, total_len, 0, (void *) &daddr, sizeof(daddr));
365 else
366 val = sendto(fd, buf, total_len, 0, (void *) &daddr6, sizeof(daddr6));
367 } else {
368 val = send(fd, buf, cfg_payload_len, 0);
369 }
370 if (val != total_len)
371 error(1, errno, "send");
372
373
374 usleep(50 * 1000);
375
376 __poll(fd);
377
378 while (!recv_errmsg(fd)) {}
379 }
380
381 if (close(fd))
382 error(1, errno, "close");
383
384 free(buf);
385 usleep(400 * 1000);
386}
387
388static void __attribute__((noreturn)) usage(const char *filepath)
389{
390 fprintf(stderr, "\nUsage: %s [options] hostname\n"
391 "\nwhere options are:\n"
392 " -4: only IPv4\n"
393 " -6: only IPv6\n"
394 " -h: show this message\n"
395 " -I: request PKTINFO\n"
396 " -l N: send N bytes at a time\n"
397 " -n: set no-payload option\n"
398 " -r: use raw\n"
399 " -R: use raw (IP_HDRINCL)\n"
400 " -p N: connect to port N\n"
401 " -u: use udp\n"
402 " -x: show payload (up to 70 bytes)\n",
403 filepath);
404 exit(1);
405}
406
407static void parse_opt(int argc, char **argv)
408{
409 int proto_count = 0;
410 char c;
411
412 while ((c = getopt(argc, argv, "46hIl:np:rRux")) != -1) {
413 switch (c) {
414 case '4':
415 do_ipv6 = 0;
416 break;
417 case '6':
418 do_ipv4 = 0;
419 break;
420 case 'I':
421 cfg_do_pktinfo = true;
422 break;
423 case 'n':
424 cfg_loop_nodata = true;
425 break;
426 case 'r':
427 proto_count++;
428 cfg_proto = SOCK_RAW;
429 cfg_ipproto = IPPROTO_UDP;
430 break;
431 case 'R':
432 proto_count++;
433 cfg_proto = SOCK_RAW;
434 cfg_ipproto = IPPROTO_RAW;
435 break;
436 case 'u':
437 proto_count++;
438 cfg_proto = SOCK_DGRAM;
439 cfg_ipproto = IPPROTO_UDP;
440 break;
441 case 'l':
442 cfg_payload_len = strtoul(optarg, NULL, 10);
443 break;
444 case 'p':
445 dest_port = strtoul(optarg, NULL, 10);
446 break;
447 case 'x':
448 cfg_show_payload = true;
449 break;
450 case 'h':
451 default:
452 usage(argv[0]);
453 }
454 }
455
456 if (!cfg_payload_len)
457 error(1, 0, "payload may not be nonzero");
458 if (cfg_proto != SOCK_STREAM && cfg_payload_len > 1472)
459 error(1, 0, "udp packet might exceed expected MTU");
460 if (!do_ipv4 && !do_ipv6)
461 error(1, 0, "pass -4 or -6, not both");
462 if (proto_count > 1)
463 error(1, 0, "pass -r, -R or -u, not multiple");
464
465 if (optind != argc - 1)
466 error(1, 0, "missing required hostname argument");
467}
468
469static void resolve_hostname(const char *hostname)
470{
471 struct addrinfo *addrs, *cur;
472 int have_ipv4 = 0, have_ipv6 = 0;
473
474 if (getaddrinfo(hostname, NULL, NULL, &addrs))
475 error(1, errno, "getaddrinfo");
476
477 cur = addrs;
478 while (cur && !have_ipv4 && !have_ipv6) {
479 if (!have_ipv4 && cur->ai_family == AF_INET) {
480 memcpy(&daddr, cur->ai_addr, sizeof(daddr));
481 daddr.sin_port = htons(dest_port);
482 have_ipv4 = 1;
483 }
484 else if (!have_ipv6 && cur->ai_family == AF_INET6) {
485 memcpy(&daddr6, cur->ai_addr, sizeof(daddr6));
486 daddr6.sin6_port = htons(dest_port);
487 have_ipv6 = 1;
488 }
489 cur = cur->ai_next;
490 }
491 if (addrs)
492 freeaddrinfo(addrs);
493
494 do_ipv4 &= have_ipv4;
495 do_ipv6 &= have_ipv6;
496}
497
498static void do_main(int family)
499{
500 fprintf(stderr, "family: %s\n",
501 family == PF_INET ? "INET" : "INET6");
502
503 fprintf(stderr, "test SND\n");
504 do_test(family, SOF_TIMESTAMPING_TX_SOFTWARE);
505
506 fprintf(stderr, "test ENQ\n");
507 do_test(family, SOF_TIMESTAMPING_TX_SCHED);
508
509 fprintf(stderr, "test ENQ + SND\n");
510 do_test(family, SOF_TIMESTAMPING_TX_SCHED |
511 SOF_TIMESTAMPING_TX_SOFTWARE);
512
513 if (cfg_proto == SOCK_STREAM) {
514 fprintf(stderr, "\ntest ACK\n");
515 do_test(family, SOF_TIMESTAMPING_TX_ACK);
516
517 fprintf(stderr, "\ntest SND + ACK\n");
518 do_test(family, SOF_TIMESTAMPING_TX_SOFTWARE |
519 SOF_TIMESTAMPING_TX_ACK);
520
521 fprintf(stderr, "\ntest ENQ + SND + ACK\n");
522 do_test(family, SOF_TIMESTAMPING_TX_SCHED |
523 SOF_TIMESTAMPING_TX_SOFTWARE |
524 SOF_TIMESTAMPING_TX_ACK);
525 }
526}
527
528const char *sock_names[] = { NULL, "TCP", "UDP", "RAW" };
529
530int main(int argc, char **argv)
531{
532 if (argc == 1)
533 usage(argv[0]);
534
535 parse_opt(argc, argv);
536 resolve_hostname(argv[argc - 1]);
537
538 fprintf(stderr, "protocol: %s\n", sock_names[cfg_proto]);
539 fprintf(stderr, "payload: %u\n", cfg_payload_len);
540 fprintf(stderr, "server port: %u\n", dest_port);
541 fprintf(stderr, "\n");
542
543 if (do_ipv4)
544 do_main(PF_INET);
545 if (do_ipv6)
546 do_main(PF_INET6);
547
548 return 0;
549}
550
