Gerhard Englederselftests/net: timestamping: Fix bind_phc check678dfd54 years agoHistory
551 lines · 15.6 KB · CRawDownload
// SPDX-License-Identifier: GPL-2.0-only/* * This program demonstrates how the various time stamping features in * the Linux kernel work. It emulates the behavior of a PTP * implementation in stand-alone master mode by sending PTPv1 Sync * multicasts once every second. It looks for similar packets, but * beyond that doesn't actually implement PTP. * * Outgoing packets are time stamped with SO_TIMESTAMPING with or * without hardware support. * * Incoming packets are time stamped with SO_TIMESTAMPING with or * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and * SO_TIMESTAMP[NS]. * * Copyright (C) 2009 Intel Corporation. * Author: Patrick Ohly <patrick.ohly@intel.com> */ #include <stdio.h>#include <stdlib.h>#include <errno.h>#include <string.h> #include <sys/time.h>#include <sys/socket.h>#include <sys/select.h>#include <sys/ioctl.h>#include <arpa/inet.h>#include <net/if.h> #include <asm/types.h>#include <linux/net_tstamp.h>#include <linux/errqueue.h>#include <linux/sockios.h> #ifndef SO_TIMESTAMPING# define SO_TIMESTAMPING         37# define SCM_TIMESTAMPING        SO_TIMESTAMPING#endif #ifndef SO_TIMESTAMPNS# define SO_TIMESTAMPNS 35#endif static void usage(const char *error){	if (error)		printf("invalid option: %s\n", error);	printf("timestamping <interface> [bind_phc_index] [option]*\n\n"	       "Options:\n"	       "  IP_MULTICAST_LOOP - looping outgoing multicasts\n"	       "  SO_TIMESTAMP - normal software time stamping, ms resolution\n"	       "  SO_TIMESTAMPNS - more accurate software time stamping\n"	       "  SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"	       "  SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"	       "  SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"	       "  SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"	       "  SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"	       "  SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"	       "  SOF_TIMESTAMPING_BIND_PHC - request to bind a PHC of PTP vclock\n"	       "  SIOCGSTAMP - check last socket time stamp\n"	       "  SIOCGSTAMPNS - more accurate socket time stamp\n"	       "  PTPV2 - use PTPv2 messages\n");	exit(1);} static void bail(const char *error){	printf("%s: %s\n", error, strerror(errno));	exit(1);} static const unsigned char sync[] = {	0x00, 0x01, 0x00, 0x01,	0x5f, 0x44, 0x46, 0x4c,	0x54, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x01, 0x01, 	/* fake uuid */	0x00, 0x01,	0x02, 0x03, 0x04, 0x05, 	0x00, 0x01, 0x00, 0x37,	0x00, 0x00, 0x00, 0x08,	0x00, 0x00, 0x00, 0x00,	0x49, 0x05, 0xcd, 0x01,	0x29, 0xb1, 0x8d, 0xb0,	0x00, 0x00, 0x00, 0x00,	0x00, 0x01, 	/* fake uuid */	0x00, 0x01,	0x02, 0x03, 0x04, 0x05, 	0x00, 0x00, 0x00, 0x37,	0x00, 0x00, 0x00, 0x04,	0x44, 0x46, 0x4c, 0x54,	0x00, 0x00, 0xf0, 0x60,	0x00, 0x01, 0x00, 0x00,	0x00, 0x00, 0x00, 0x01,	0x00, 0x00, 0xf0, 0x60,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x04,	0x44, 0x46, 0x4c, 0x54,	0x00, 0x01, 	/* fake uuid */	0x00, 0x01,	0x02, 0x03, 0x04, 0x05, 	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00}; static const unsigned char sync_v2[] = {	0x00, 0x02, 0x00, 0x2C,	0x00, 0x00, 0x02, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0xFF,	0xFE, 0x00, 0x00, 0x00,	0x00, 0x01, 0x00, 0x01,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,}; static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2){	size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);	const void *sync_p = ptpv2 ? sync_v2 : sync;	struct timeval now;	int res; 	res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);	gettimeofday(&now, 0);	if (res < 0)		printf("%s: %s\n", "send", strerror(errno));	else		printf("%ld.%06ld: sent %d bytes\n",		       (long)now.tv_sec, (long)now.tv_usec,		       res);} static void printpacket(struct msghdr *msg, int res,			char *data,			int sock, int recvmsg_flags,			int siocgstamp, int siocgstampns, int ptpv2){	struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;	size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);	const void *sync_p = ptpv2 ? sync_v2 : sync;	struct cmsghdr *cmsg;	struct timeval tv;	struct timespec ts;	struct timeval now; 	gettimeofday(&now, 0); 	printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",	       (long)now.tv_sec, (long)now.tv_usec,	       (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",	       res,	       inet_ntoa(from_addr->sin_addr),	       msg->msg_controllen);	for (cmsg = CMSG_FIRSTHDR(msg);	     cmsg;	     cmsg = CMSG_NXTHDR(msg, cmsg)) {		printf("   cmsg len %zu: ", cmsg->cmsg_len);		switch (cmsg->cmsg_level) {		case SOL_SOCKET:			printf("SOL_SOCKET ");			switch (cmsg->cmsg_type) {			case SO_TIMESTAMP: {				struct timeval *stamp =					(struct timeval *)CMSG_DATA(cmsg);				printf("SO_TIMESTAMP %ld.%06ld",				       (long)stamp->tv_sec,				       (long)stamp->tv_usec);				break;			}			case SO_TIMESTAMPNS: {				struct timespec *stamp =					(struct timespec *)CMSG_DATA(cmsg);				printf("SO_TIMESTAMPNS %ld.%09ld",				       (long)stamp->tv_sec,				       (long)stamp->tv_nsec);				break;			}			case SO_TIMESTAMPING: {				struct timespec *stamp =					(struct timespec *)CMSG_DATA(cmsg);				printf("SO_TIMESTAMPING ");				printf("SW %ld.%09ld ",				       (long)stamp->tv_sec,				       (long)stamp->tv_nsec);				stamp++;				/* skip deprecated HW transformed */				stamp++;				printf("HW raw %ld.%09ld",				       (long)stamp->tv_sec,				       (long)stamp->tv_nsec);				break;			}			default:				printf("type %d", cmsg->cmsg_type);				break;			}			break;		case IPPROTO_IP:			printf("IPPROTO_IP ");			switch (cmsg->cmsg_type) {			case IP_RECVERR: {				struct sock_extended_err *err =					(struct sock_extended_err *)CMSG_DATA(cmsg);				printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",					strerror(err->ee_errno),					err->ee_origin,#ifdef SO_EE_ORIGIN_TIMESTAMPING					err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?					"bounced packet" : "unexpected origin"#else					"probably SO_EE_ORIGIN_TIMESTAMPING"#endif					);				if (res < sync_len)					printf(" => truncated data?!");				else if (!memcmp(sync_p, data + res - sync_len, sync_len))					printf(" => GOT OUR DATA BACK (HURRAY!)");				break;			}			case IP_PKTINFO: {				struct in_pktinfo *pktinfo =					(struct in_pktinfo *)CMSG_DATA(cmsg);				printf("IP_PKTINFO interface index %u",					pktinfo->ipi_ifindex);				break;			}			default:				printf("type %d", cmsg->cmsg_type);				break;			}			break;		default:			printf("level %d type %d",				cmsg->cmsg_level,				cmsg->cmsg_type);			break;		}		printf("\n");	} 	if (siocgstamp) {		if (ioctl(sock, SIOCGSTAMP, &tv))			printf("   %s: %s\n", "SIOCGSTAMP", strerror(errno));		else			printf("SIOCGSTAMP %ld.%06ld\n",			       (long)tv.tv_sec,			       (long)tv.tv_usec);	}	if (siocgstampns) {		if (ioctl(sock, SIOCGSTAMPNS, &ts))			printf("   %s: %s\n", "SIOCGSTAMPNS", strerror(errno));		else			printf("SIOCGSTAMPNS %ld.%09ld\n",			       (long)ts.tv_sec,			       (long)ts.tv_nsec);	}} static void recvpacket(int sock, int recvmsg_flags,		       int siocgstamp, int siocgstampns, int ptpv2){	char data[256];	struct msghdr msg;	struct iovec entry;	struct sockaddr_in from_addr;	struct {		struct cmsghdr cm;		char control[512];	} control;	int res; 	memset(&msg, 0, sizeof(msg));	msg.msg_iov = &entry;	msg.msg_iovlen = 1;	entry.iov_base = data;	entry.iov_len = sizeof(data);	msg.msg_name = (caddr_t)&from_addr;	msg.msg_namelen = sizeof(from_addr);	msg.msg_control = &control;	msg.msg_controllen = sizeof(control); 	res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);	if (res < 0) {		printf("%s %s: %s\n",		       "recvmsg",		       (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",		       strerror(errno));	} else {		printpacket(&msg, res, data,			    sock, recvmsg_flags,			    siocgstamp, siocgstampns, ptpv2);	}} int main(int argc, char **argv){	int so_timestamp = 0;	int so_timestampns = 0;	int siocgstamp = 0;	int siocgstampns = 0;	int ip_multicast_loop = 0;	int ptpv2 = 0;	char *interface;	int i;	int enabled = 1;	int sock;	struct ifreq device;	struct ifreq hwtstamp;	struct hwtstamp_config hwconfig, hwconfig_requested;	struct so_timestamping so_timestamping_get = { 0, 0 };	struct so_timestamping so_timestamping = { 0, 0 };	struct sockaddr_in addr;	struct ip_mreq imr;	struct in_addr iaddr;	int val;	socklen_t len;	struct timeval next;	size_t if_len; 	if (argc < 2)		usage(0);	interface = argv[1];	if_len = strlen(interface);	if (if_len >= IFNAMSIZ) {		printf("interface name exceeds IFNAMSIZ\n");		exit(1);	} 	if (argc >= 3 && sscanf(argv[2], "%d", &so_timestamping.bind_phc) == 1)		val = 3;	else		val = 2; 	for (i = val; i < argc; i++) {		if (!strcasecmp(argv[i], "SO_TIMESTAMP"))			so_timestamp = 1;		else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))			so_timestampns = 1;		else if (!strcasecmp(argv[i], "SIOCGSTAMP"))			siocgstamp = 1;		else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))			siocgstampns = 1;		else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))			ip_multicast_loop = 1;		else if (!strcasecmp(argv[i], "PTPV2"))			ptpv2 = 1;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_TX_HARDWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_TX_SOFTWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_RX_HARDWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_RX_SOFTWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_SOFTWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))			so_timestamping.flags |= SOF_TIMESTAMPING_RAW_HARDWARE;		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_BIND_PHC"))			so_timestamping.flags |= SOF_TIMESTAMPING_BIND_PHC;		else			usage(argv[i]);	} 	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);	if (sock < 0)		bail("socket"); 	memset(&device, 0, sizeof(device));	memcpy(device.ifr_name, interface, if_len + 1);	if (ioctl(sock, SIOCGIFADDR, &device) < 0)		bail("getting interface IP address"); 	memset(&hwtstamp, 0, sizeof(hwtstamp));	memcpy(hwtstamp.ifr_name, interface, if_len + 1);	hwtstamp.ifr_data = (void *)&hwconfig;	memset(&hwconfig, 0, sizeof(hwconfig));	hwconfig.tx_type =		(so_timestamping.flags & SOF_TIMESTAMPING_TX_HARDWARE) ?		HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;	hwconfig.rx_filter =		(so_timestamping.flags & SOF_TIMESTAMPING_RX_HARDWARE) ?		ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :		HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;	hwconfig_requested = hwconfig;	if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {		if ((errno == EINVAL || errno == ENOTSUP) &&		    hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&		    hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)			printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");		else			bail("SIOCSHWTSTAMP");	}	printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",	       hwconfig_requested.tx_type, hwconfig.tx_type,	       hwconfig_requested.rx_filter, hwconfig.rx_filter); 	/* bind to PTP port */	addr.sin_family = AF_INET;	addr.sin_addr.s_addr = htonl(INADDR_ANY);	addr.sin_port = htons(319 /* PTP event port */);	if (bind(sock,		 (struct sockaddr *)&addr,		 sizeof(struct sockaddr_in)) < 0)		bail("bind"); 	if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface, if_len))		bail("bind device"); 	/* set multicast group for outgoing packets */	inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */	addr.sin_addr = iaddr;	imr.imr_multiaddr.s_addr = iaddr.s_addr;	imr.imr_interface.s_addr =		((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;	if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,		       &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)		bail("set multicast"); 	/* join multicast group, loop our own packet */	if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,		       &imr, sizeof(struct ip_mreq)) < 0)		bail("join multicast group"); 	if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,		       &ip_multicast_loop, sizeof(enabled)) < 0) {		bail("loop multicast");	} 	/* set socket options for time stamping */	if (so_timestamp &&		setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,			   &enabled, sizeof(enabled)) < 0)		bail("setsockopt SO_TIMESTAMP"); 	if (so_timestampns &&		setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,			   &enabled, sizeof(enabled)) < 0)		bail("setsockopt SO_TIMESTAMPNS"); 	if (so_timestamping.flags &&	    setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping,		       sizeof(so_timestamping)) < 0)		bail("setsockopt SO_TIMESTAMPING"); 	/* request IP_PKTINFO for debugging purposes */	if (setsockopt(sock, SOL_IP, IP_PKTINFO,		       &enabled, sizeof(enabled)) < 0)		printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno)); 	/* verify socket options */	len = sizeof(val);	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)		printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));	else		printf("SO_TIMESTAMP %d\n", val); 	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)		printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",		       strerror(errno));	else		printf("SO_TIMESTAMPNS %d\n", val); 	len = sizeof(so_timestamping_get);	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_get,		       &len) < 0) {		printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",		       strerror(errno));	} else {		printf("SO_TIMESTAMPING flags %d, bind phc %d\n",		       so_timestamping_get.flags, so_timestamping_get.bind_phc);		if (so_timestamping_get.flags != so_timestamping.flags ||		    so_timestamping_get.bind_phc != so_timestamping.bind_phc)			printf("   not expected, flags %d, bind phc %d\n",			       so_timestamping.flags, so_timestamping.bind_phc);	} 	/* send packets forever every five seconds */	gettimeofday(&next, 0);	next.tv_sec = (next.tv_sec + 1) / 5 * 5;	next.tv_usec = 0;	while (1) {

Lines 1–500 of 551.