0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020 #include <stdio.h>
0021 #include <stdlib.h>
0022 #include <errno.h>
0023 #include <string.h>
0024
0025 #include <sys/time.h>
0026 #include <sys/socket.h>
0027 #include <sys/select.h>
0028 #include <sys/ioctl.h>
0029 #include <arpa/inet.h>
0030 #include <net/if.h>
0031
0032 #include <asm/types.h>
0033 #include <linux/net_tstamp.h>
0034 #include <linux/errqueue.h>
0035 #include <linux/sockios.h>
0036
0037 #ifndef SO_TIMESTAMPING
0038 # define SO_TIMESTAMPING 37
0039 # define SCM_TIMESTAMPING SO_TIMESTAMPING
0040 #endif
0041
0042 #ifndef SO_TIMESTAMPNS
0043 # define SO_TIMESTAMPNS 35
0044 #endif
0045
0046 static void usage(const char *error)
0047 {
0048 if (error)
0049 printf("invalid option: %s\n", error);
0050 printf("timestamping <interface> [bind_phc_index] [option]*\n\n"
0051 "Options:\n"
0052 " IP_MULTICAST_LOOP - looping outgoing multicasts\n"
0053 " SO_TIMESTAMP - normal software time stamping, ms resolution\n"
0054 " SO_TIMESTAMPNS - more accurate software time stamping\n"
0055 " SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
0056 " SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
0057 " SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
0058 " SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
0059 " SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
0060 " SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
0061 " SOF_TIMESTAMPING_BIND_PHC - request to bind a PHC of PTP vclock\n"
0062 " SIOCGSTAMP - check last socket time stamp\n"
0063 " SIOCGSTAMPNS - more accurate socket time stamp\n"
0064 " PTPV2 - use PTPv2 messages\n");
0065 exit(1);
0066 }
0067
0068 static void bail(const char *error)
0069 {
0070 printf("%s: %s\n", error, strerror(errno));
0071 exit(1);
0072 }
0073
0074 static const unsigned char sync[] = {
0075 0x00, 0x01, 0x00, 0x01,
0076 0x5f, 0x44, 0x46, 0x4c,
0077 0x54, 0x00, 0x00, 0x00,
0078 0x00, 0x00, 0x00, 0x00,
0079 0x00, 0x00, 0x00, 0x00,
0080 0x01, 0x01,
0081
0082
0083 0x00, 0x01,
0084 0x02, 0x03, 0x04, 0x05,
0085
0086 0x00, 0x01, 0x00, 0x37,
0087 0x00, 0x00, 0x00, 0x08,
0088 0x00, 0x00, 0x00, 0x00,
0089 0x49, 0x05, 0xcd, 0x01,
0090 0x29, 0xb1, 0x8d, 0xb0,
0091 0x00, 0x00, 0x00, 0x00,
0092 0x00, 0x01,
0093
0094
0095 0x00, 0x01,
0096 0x02, 0x03, 0x04, 0x05,
0097
0098 0x00, 0x00, 0x00, 0x37,
0099 0x00, 0x00, 0x00, 0x04,
0100 0x44, 0x46, 0x4c, 0x54,
0101 0x00, 0x00, 0xf0, 0x60,
0102 0x00, 0x01, 0x00, 0x00,
0103 0x00, 0x00, 0x00, 0x01,
0104 0x00, 0x00, 0xf0, 0x60,
0105 0x00, 0x00, 0x00, 0x00,
0106 0x00, 0x00, 0x00, 0x04,
0107 0x44, 0x46, 0x4c, 0x54,
0108 0x00, 0x01,
0109
0110
0111 0x00, 0x01,
0112 0x02, 0x03, 0x04, 0x05,
0113
0114 0x00, 0x00, 0x00, 0x00,
0115 0x00, 0x00, 0x00, 0x00,
0116 0x00, 0x00, 0x00, 0x00,
0117 0x00, 0x00, 0x00, 0x00
0118 };
0119
0120 static const unsigned char sync_v2[] = {
0121 0x00, 0x02, 0x00, 0x2C,
0122 0x00, 0x00, 0x02, 0x00,
0123 0x00, 0x00, 0x00, 0x00,
0124 0x00, 0x00, 0x00, 0x00,
0125 0x00, 0x00, 0x00, 0x00,
0126 0x00, 0x00, 0x00, 0xFF,
0127 0xFE, 0x00, 0x00, 0x00,
0128 0x00, 0x01, 0x00, 0x01,
0129 0x00, 0x00, 0x00, 0x00,
0130 0x00, 0x00, 0x00, 0x00,
0131 0x00, 0x00, 0x00, 0x00,
0132 };
0133
0134 static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
0135 {
0136 size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
0137 const void *sync_p = ptpv2 ? sync_v2 : sync;
0138 struct timeval now;
0139 int res;
0140
0141 res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);
0142 gettimeofday(&now, 0);
0143 if (res < 0)
0144 printf("%s: %s\n", "send", strerror(errno));
0145 else
0146 printf("%ld.%06ld: sent %d bytes\n",
0147 (long)now.tv_sec, (long)now.tv_usec,
0148 res);
0149 }
0150
0151 static void printpacket(struct msghdr *msg, int res,
0152 char *data,
0153 int sock, int recvmsg_flags,
0154 int siocgstamp, int siocgstampns, int ptpv2)
0155 {
0156 struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
0157 size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
0158 const void *sync_p = ptpv2 ? sync_v2 : sync;
0159 struct cmsghdr *cmsg;
0160 struct timeval tv;
0161 struct timespec ts;
0162 struct timeval now;
0163
0164 gettimeofday(&now, 0);
0165
0166 printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
0167 (long)now.tv_sec, (long)now.tv_usec,
0168 (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
0169 res,
0170 inet_ntoa(from_addr->sin_addr),
0171 msg->msg_controllen);
0172 for (cmsg = CMSG_FIRSTHDR(msg);
0173 cmsg;
0174 cmsg = CMSG_NXTHDR(msg, cmsg)) {
0175 printf(" cmsg len %zu: ", cmsg->cmsg_len);
0176 switch (cmsg->cmsg_level) {
0177 case SOL_SOCKET:
0178 printf("SOL_SOCKET ");
0179 switch (cmsg->cmsg_type) {
0180 case SO_TIMESTAMP: {
0181 struct timeval *stamp =
0182 (struct timeval *)CMSG_DATA(cmsg);
0183 printf("SO_TIMESTAMP %ld.%06ld",
0184 (long)stamp->tv_sec,
0185 (long)stamp->tv_usec);
0186 break;
0187 }
0188 case SO_TIMESTAMPNS: {
0189 struct timespec *stamp =
0190 (struct timespec *)CMSG_DATA(cmsg);
0191 printf("SO_TIMESTAMPNS %ld.%09ld",
0192 (long)stamp->tv_sec,
0193 (long)stamp->tv_nsec);
0194 break;
0195 }
0196 case SO_TIMESTAMPING: {
0197 struct timespec *stamp =
0198 (struct timespec *)CMSG_DATA(cmsg);
0199 printf("SO_TIMESTAMPING ");
0200 printf("SW %ld.%09ld ",
0201 (long)stamp->tv_sec,
0202 (long)stamp->tv_nsec);
0203 stamp++;
0204
0205 stamp++;
0206 printf("HW raw %ld.%09ld",
0207 (long)stamp->tv_sec,
0208 (long)stamp->tv_nsec);
0209 break;
0210 }
0211 default:
0212 printf("type %d", cmsg->cmsg_type);
0213 break;
0214 }
0215 break;
0216 case IPPROTO_IP:
0217 printf("IPPROTO_IP ");
0218 switch (cmsg->cmsg_type) {
0219 case IP_RECVERR: {
0220 struct sock_extended_err *err =
0221 (struct sock_extended_err *)CMSG_DATA(cmsg);
0222 printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
0223 strerror(err->ee_errno),
0224 err->ee_origin,
0225 #ifdef SO_EE_ORIGIN_TIMESTAMPING
0226 err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
0227 "bounced packet" : "unexpected origin"
0228 #else
0229 "probably SO_EE_ORIGIN_TIMESTAMPING"
0230 #endif
0231 );
0232 if (res < sync_len)
0233 printf(" => truncated data?!");
0234 else if (!memcmp(sync_p, data + res - sync_len, sync_len))
0235 printf(" => GOT OUR DATA BACK (HURRAY!)");
0236 break;
0237 }
0238 case IP_PKTINFO: {
0239 struct in_pktinfo *pktinfo =
0240 (struct in_pktinfo *)CMSG_DATA(cmsg);
0241 printf("IP_PKTINFO interface index %u",
0242 pktinfo->ipi_ifindex);
0243 break;
0244 }
0245 default:
0246 printf("type %d", cmsg->cmsg_type);
0247 break;
0248 }
0249 break;
0250 default:
0251 printf("level %d type %d",
0252 cmsg->cmsg_level,
0253 cmsg->cmsg_type);
0254 break;
0255 }
0256 printf("\n");
0257 }
0258
0259 if (siocgstamp) {
0260 if (ioctl(sock, SIOCGSTAMP, &tv))
0261 printf(" %s: %s\n", "SIOCGSTAMP", strerror(errno));
0262 else
0263 printf("SIOCGSTAMP %ld.%06ld\n",
0264 (long)tv.tv_sec,
0265 (long)tv.tv_usec);
0266 }
0267 if (siocgstampns) {
0268 if (ioctl(sock, SIOCGSTAMPNS, &ts))
0269 printf(" %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
0270 else
0271 printf("SIOCGSTAMPNS %ld.%09ld\n",
0272 (long)ts.tv_sec,
0273 (long)ts.tv_nsec);
0274 }
0275 }
0276
0277 static void recvpacket(int sock, int recvmsg_flags,
0278 int siocgstamp, int siocgstampns, int ptpv2)
0279 {
0280 char data[256];
0281 struct msghdr msg;
0282 struct iovec entry;
0283 struct sockaddr_in from_addr;
0284 struct {
0285 struct cmsghdr cm;
0286 char control[512];
0287 } control;
0288 int res;
0289
0290 memset(&msg, 0, sizeof(msg));
0291 msg.msg_iov = &entry;
0292 msg.msg_iovlen = 1;
0293 entry.iov_base = data;
0294 entry.iov_len = sizeof(data);
0295 msg.msg_name = (caddr_t)&from_addr;
0296 msg.msg_namelen = sizeof(from_addr);
0297 msg.msg_control = &control;
0298 msg.msg_controllen = sizeof(control);
0299
0300 res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
0301 if (res < 0) {
0302 printf("%s %s: %s\n",
0303 "recvmsg",
0304 (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
0305 strerror(errno));
0306 } else {
0307 printpacket(&msg, res, data,
0308 sock, recvmsg_flags,
0309 siocgstamp, siocgstampns, ptpv2);
0310 }
0311 }
0312
0313 int main(int argc, char **argv)
0314 {
0315 int so_timestamp = 0;
0316 int so_timestampns = 0;
0317 int siocgstamp = 0;
0318 int siocgstampns = 0;
0319 int ip_multicast_loop = 0;
0320 int ptpv2 = 0;
0321 char *interface;
0322 int i;
0323 int enabled = 1;
0324 int sock;
0325 struct ifreq device;
0326 struct ifreq hwtstamp;
0327 struct hwtstamp_config hwconfig, hwconfig_requested;
0328 struct so_timestamping so_timestamping_get = { 0, 0 };
0329 struct so_timestamping so_timestamping = { 0, 0 };
0330 struct sockaddr_in addr;
0331 struct ip_mreq imr;
0332 struct in_addr iaddr;
0333 int val;
0334 socklen_t len;
0335 struct timeval next;
0336 size_t if_len;
0337
0338 if (argc < 2)
0339 usage(0);
0340 interface = argv[1];
0341 if_len = strlen(interface);
0342 if (if_len >= IFNAMSIZ) {
0343 printf("interface name exceeds IFNAMSIZ\n");
0344 exit(1);
0345 }
0346
0347 if (argc >= 3 && sscanf(argv[2], "%d", &so_timestamping.bind_phc) == 1)
0348 val = 3;
0349 else
0350 val = 2;
0351
0352 for (i = val; i < argc; i++) {
0353 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
0354 so_timestamp = 1;
0355 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
0356 so_timestampns = 1;
0357 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
0358 siocgstamp = 1;
0359 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
0360 siocgstampns = 1;
0361 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
0362 ip_multicast_loop = 1;
0363 else if (!strcasecmp(argv[i], "PTPV2"))
0364 ptpv2 = 1;
0365 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
0366 so_timestamping.flags |= SOF_TIMESTAMPING_TX_HARDWARE;
0367 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
0368 so_timestamping.flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
0369 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
0370 so_timestamping.flags |= SOF_TIMESTAMPING_RX_HARDWARE;
0371 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
0372 so_timestamping.flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
0373 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
0374 so_timestamping.flags |= SOF_TIMESTAMPING_SOFTWARE;
0375 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
0376 so_timestamping.flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
0377 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_BIND_PHC"))
0378 so_timestamping.flags |= SOF_TIMESTAMPING_BIND_PHC;
0379 else
0380 usage(argv[i]);
0381 }
0382
0383 sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
0384 if (sock < 0)
0385 bail("socket");
0386
0387 memset(&device, 0, sizeof(device));
0388 memcpy(device.ifr_name, interface, if_len + 1);
0389 if (ioctl(sock, SIOCGIFADDR, &device) < 0)
0390 bail("getting interface IP address");
0391
0392 memset(&hwtstamp, 0, sizeof(hwtstamp));
0393 memcpy(hwtstamp.ifr_name, interface, if_len + 1);
0394 hwtstamp.ifr_data = (void *)&hwconfig;
0395 memset(&hwconfig, 0, sizeof(hwconfig));
0396 hwconfig.tx_type =
0397 (so_timestamping.flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
0398 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
0399 hwconfig.rx_filter =
0400 (so_timestamping.flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
0401 ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
0402 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
0403 hwconfig_requested = hwconfig;
0404 if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
0405 if ((errno == EINVAL || errno == ENOTSUP) &&
0406 hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
0407 hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
0408 printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
0409 else
0410 bail("SIOCSHWTSTAMP");
0411 }
0412 printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
0413 hwconfig_requested.tx_type, hwconfig.tx_type,
0414 hwconfig_requested.rx_filter, hwconfig.rx_filter);
0415
0416
0417 addr.sin_family = AF_INET;
0418 addr.sin_addr.s_addr = htonl(INADDR_ANY);
0419 addr.sin_port = htons(319 );
0420 if (bind(sock,
0421 (struct sockaddr *)&addr,
0422 sizeof(struct sockaddr_in)) < 0)
0423 bail("bind");
0424
0425 if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface, if_len))
0426 bail("bind device");
0427
0428
0429 inet_aton("224.0.1.130", &iaddr);
0430 addr.sin_addr = iaddr;
0431 imr.imr_multiaddr.s_addr = iaddr.s_addr;
0432 imr.imr_interface.s_addr =
0433 ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
0434 if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
0435 &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
0436 bail("set multicast");
0437
0438
0439 if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
0440 &imr, sizeof(struct ip_mreq)) < 0)
0441 bail("join multicast group");
0442
0443 if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
0444 &ip_multicast_loop, sizeof(enabled)) < 0) {
0445 bail("loop multicast");
0446 }
0447
0448
0449 if (so_timestamp &&
0450 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
0451 &enabled, sizeof(enabled)) < 0)
0452 bail("setsockopt SO_TIMESTAMP");
0453
0454 if (so_timestampns &&
0455 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
0456 &enabled, sizeof(enabled)) < 0)
0457 bail("setsockopt SO_TIMESTAMPNS");
0458
0459 if (so_timestamping.flags &&
0460 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping,
0461 sizeof(so_timestamping)) < 0)
0462 bail("setsockopt SO_TIMESTAMPING");
0463
0464
0465 if (setsockopt(sock, SOL_IP, IP_PKTINFO,
0466 &enabled, sizeof(enabled)) < 0)
0467 printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
0468
0469
0470 len = sizeof(val);
0471 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
0472 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
0473 else
0474 printf("SO_TIMESTAMP %d\n", val);
0475
0476 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
0477 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
0478 strerror(errno));
0479 else
0480 printf("SO_TIMESTAMPNS %d\n", val);
0481
0482 len = sizeof(so_timestamping_get);
0483 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_get,
0484 &len) < 0) {
0485 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
0486 strerror(errno));
0487 } else {
0488 printf("SO_TIMESTAMPING flags %d, bind phc %d\n",
0489 so_timestamping_get.flags, so_timestamping_get.bind_phc);
0490 if (so_timestamping_get.flags != so_timestamping.flags ||
0491 so_timestamping_get.bind_phc != so_timestamping.bind_phc)
0492 printf(" not expected, flags %d, bind phc %d\n",
0493 so_timestamping.flags, so_timestamping.bind_phc);
0494 }
0495
0496
0497 gettimeofday(&next, 0);
0498 next.tv_sec = (next.tv_sec + 1) / 5 * 5;
0499 next.tv_usec = 0;
0500 while (1) {
0501 struct timeval now;
0502 struct timeval delta;
0503 long delta_us;
0504 int res;
0505 fd_set readfs, errorfs;
0506
0507 gettimeofday(&now, 0);
0508 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
0509 (long)(next.tv_usec - now.tv_usec);
0510 if (delta_us > 0) {
0511
0512 delta.tv_sec = delta_us / 1000000;
0513 delta.tv_usec = delta_us % 1000000;
0514
0515 FD_ZERO(&readfs);
0516 FD_ZERO(&errorfs);
0517 FD_SET(sock, &readfs);
0518 FD_SET(sock, &errorfs);
0519 printf("%ld.%06ld: select %ldus\n",
0520 (long)now.tv_sec, (long)now.tv_usec,
0521 delta_us);
0522 res = select(sock + 1, &readfs, 0, &errorfs, &delta);
0523 gettimeofday(&now, 0);
0524 printf("%ld.%06ld: select returned: %d, %s\n",
0525 (long)now.tv_sec, (long)now.tv_usec,
0526 res,
0527 res < 0 ? strerror(errno) : "success");
0528 if (res > 0) {
0529 if (FD_ISSET(sock, &readfs))
0530 printf("ready for reading\n");
0531 if (FD_ISSET(sock, &errorfs))
0532 printf("has error\n");
0533 recvpacket(sock, 0,
0534 siocgstamp,
0535 siocgstampns, ptpv2);
0536 recvpacket(sock, MSG_ERRQUEUE,
0537 siocgstamp,
0538 siocgstampns, ptpv2);
0539 }
0540 } else {
0541
0542 sendpacket(sock,
0543 (struct sockaddr *)&addr,
0544 sizeof(addr), ptpv2);
0545 next.tv_sec += 5;
0546 continue;
0547 }
0548 }
0549
0550 return 0;
0551 }
