1*053f45beSAndroid Build Coastguard Worker // SPDX-License-Identifier: GPL-2.0-only
2*053f45beSAndroid Build Coastguard Worker /*
3*053f45beSAndroid Build Coastguard Worker * This program demonstrates how the various time stamping features in
4*053f45beSAndroid Build Coastguard Worker * the Linux kernel work. It emulates the behavior of a PTP
5*053f45beSAndroid Build Coastguard Worker * implementation in stand-alone master mode by sending PTPv1 Sync
6*053f45beSAndroid Build Coastguard Worker * multicasts once every second. It looks for similar packets, but
7*053f45beSAndroid Build Coastguard Worker * beyond that doesn't actually implement PTP.
8*053f45beSAndroid Build Coastguard Worker *
9*053f45beSAndroid Build Coastguard Worker * Outgoing packets are time stamped with SO_TIMESTAMPING with or
10*053f45beSAndroid Build Coastguard Worker * without hardware support.
11*053f45beSAndroid Build Coastguard Worker *
12*053f45beSAndroid Build Coastguard Worker * Incoming packets are time stamped with SO_TIMESTAMPING with or
13*053f45beSAndroid Build Coastguard Worker * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and
14*053f45beSAndroid Build Coastguard Worker * SO_TIMESTAMP[NS].
15*053f45beSAndroid Build Coastguard Worker *
16*053f45beSAndroid Build Coastguard Worker * Copyright (C) 2009 Intel Corporation.
17*053f45beSAndroid Build Coastguard Worker * Author: Patrick Ohly <[email protected]>
18*053f45beSAndroid Build Coastguard Worker */
19*053f45beSAndroid Build Coastguard Worker
20*053f45beSAndroid Build Coastguard Worker #include <stdio.h>
21*053f45beSAndroid Build Coastguard Worker #include <stdlib.h>
22*053f45beSAndroid Build Coastguard Worker #include <errno.h>
23*053f45beSAndroid Build Coastguard Worker #include <string.h>
24*053f45beSAndroid Build Coastguard Worker
25*053f45beSAndroid Build Coastguard Worker #include <sys/time.h>
26*053f45beSAndroid Build Coastguard Worker #include <sys/socket.h>
27*053f45beSAndroid Build Coastguard Worker #include <sys/select.h>
28*053f45beSAndroid Build Coastguard Worker #include <sys/ioctl.h>
29*053f45beSAndroid Build Coastguard Worker #include <arpa/inet.h>
30*053f45beSAndroid Build Coastguard Worker #include <net/if.h>
31*053f45beSAndroid Build Coastguard Worker
32*053f45beSAndroid Build Coastguard Worker #include <asm/types.h>
33*053f45beSAndroid Build Coastguard Worker #include <linux/net_tstamp.h>
34*053f45beSAndroid Build Coastguard Worker #include <linux/errqueue.h>
35*053f45beSAndroid Build Coastguard Worker #include <linux/sockios.h>
36*053f45beSAndroid Build Coastguard Worker
37*053f45beSAndroid Build Coastguard Worker #ifndef SO_TIMESTAMPING
38*053f45beSAndroid Build Coastguard Worker # define SO_TIMESTAMPING 37
39*053f45beSAndroid Build Coastguard Worker # define SCM_TIMESTAMPING SO_TIMESTAMPING
40*053f45beSAndroid Build Coastguard Worker #endif
41*053f45beSAndroid Build Coastguard Worker
42*053f45beSAndroid Build Coastguard Worker #ifndef SO_TIMESTAMPNS
43*053f45beSAndroid Build Coastguard Worker # define SO_TIMESTAMPNS 35
44*053f45beSAndroid Build Coastguard Worker #endif
45*053f45beSAndroid Build Coastguard Worker
usage(const char * error)46*053f45beSAndroid Build Coastguard Worker static void usage(const char *error)
47*053f45beSAndroid Build Coastguard Worker {
48*053f45beSAndroid Build Coastguard Worker if (error)
49*053f45beSAndroid Build Coastguard Worker printf("invalid option: %s\n", error);
50*053f45beSAndroid Build Coastguard Worker printf("timestamping <interface> [bind_phc_index] [option]*\n\n"
51*053f45beSAndroid Build Coastguard Worker "Options:\n"
52*053f45beSAndroid Build Coastguard Worker " IP_MULTICAST_LOOP - looping outgoing multicasts\n"
53*053f45beSAndroid Build Coastguard Worker " SO_TIMESTAMP - normal software time stamping, ms resolution\n"
54*053f45beSAndroid Build Coastguard Worker " SO_TIMESTAMPNS - more accurate software time stamping\n"
55*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
56*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
57*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
58*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
59*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
60*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
61*053f45beSAndroid Build Coastguard Worker " SOF_TIMESTAMPING_BIND_PHC - request to bind a PHC of PTP vclock\n"
62*053f45beSAndroid Build Coastguard Worker " SIOCGSTAMP - check last socket time stamp\n"
63*053f45beSAndroid Build Coastguard Worker " SIOCGSTAMPNS - more accurate socket time stamp\n"
64*053f45beSAndroid Build Coastguard Worker " PTPV2 - use PTPv2 messages\n");
65*053f45beSAndroid Build Coastguard Worker exit(1);
66*053f45beSAndroid Build Coastguard Worker }
67*053f45beSAndroid Build Coastguard Worker
bail(const char * error)68*053f45beSAndroid Build Coastguard Worker static void bail(const char *error)
69*053f45beSAndroid Build Coastguard Worker {
70*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", error, strerror(errno));
71*053f45beSAndroid Build Coastguard Worker exit(1);
72*053f45beSAndroid Build Coastguard Worker }
73*053f45beSAndroid Build Coastguard Worker
74*053f45beSAndroid Build Coastguard Worker static const unsigned char sync[] = {
75*053f45beSAndroid Build Coastguard Worker 0x00, 0x01, 0x00, 0x01,
76*053f45beSAndroid Build Coastguard Worker 0x5f, 0x44, 0x46, 0x4c,
77*053f45beSAndroid Build Coastguard Worker 0x54, 0x00, 0x00, 0x00,
78*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
79*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
80*053f45beSAndroid Build Coastguard Worker 0x01, 0x01,
81*053f45beSAndroid Build Coastguard Worker
82*053f45beSAndroid Build Coastguard Worker /* fake uuid */
83*053f45beSAndroid Build Coastguard Worker 0x00, 0x01,
84*053f45beSAndroid Build Coastguard Worker 0x02, 0x03, 0x04, 0x05,
85*053f45beSAndroid Build Coastguard Worker
86*053f45beSAndroid Build Coastguard Worker 0x00, 0x01, 0x00, 0x37,
87*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x08,
88*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
89*053f45beSAndroid Build Coastguard Worker 0x49, 0x05, 0xcd, 0x01,
90*053f45beSAndroid Build Coastguard Worker 0x29, 0xb1, 0x8d, 0xb0,
91*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
92*053f45beSAndroid Build Coastguard Worker 0x00, 0x01,
93*053f45beSAndroid Build Coastguard Worker
94*053f45beSAndroid Build Coastguard Worker /* fake uuid */
95*053f45beSAndroid Build Coastguard Worker 0x00, 0x01,
96*053f45beSAndroid Build Coastguard Worker 0x02, 0x03, 0x04, 0x05,
97*053f45beSAndroid Build Coastguard Worker
98*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x37,
99*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x04,
100*053f45beSAndroid Build Coastguard Worker 0x44, 0x46, 0x4c, 0x54,
101*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0xf0, 0x60,
102*053f45beSAndroid Build Coastguard Worker 0x00, 0x01, 0x00, 0x00,
103*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x01,
104*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0xf0, 0x60,
105*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
106*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x04,
107*053f45beSAndroid Build Coastguard Worker 0x44, 0x46, 0x4c, 0x54,
108*053f45beSAndroid Build Coastguard Worker 0x00, 0x01,
109*053f45beSAndroid Build Coastguard Worker
110*053f45beSAndroid Build Coastguard Worker /* fake uuid */
111*053f45beSAndroid Build Coastguard Worker 0x00, 0x01,
112*053f45beSAndroid Build Coastguard Worker 0x02, 0x03, 0x04, 0x05,
113*053f45beSAndroid Build Coastguard Worker
114*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
115*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
116*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
117*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00
118*053f45beSAndroid Build Coastguard Worker };
119*053f45beSAndroid Build Coastguard Worker
120*053f45beSAndroid Build Coastguard Worker static const unsigned char sync_v2[] = {
121*053f45beSAndroid Build Coastguard Worker 0x00, 0x02, 0x00, 0x2C,
122*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x02, 0x00,
123*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
124*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
125*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
126*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0xFF,
127*053f45beSAndroid Build Coastguard Worker 0xFE, 0x00, 0x00, 0x00,
128*053f45beSAndroid Build Coastguard Worker 0x00, 0x01, 0x00, 0x01,
129*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
130*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
131*053f45beSAndroid Build Coastguard Worker 0x00, 0x00, 0x00, 0x00,
132*053f45beSAndroid Build Coastguard Worker };
133*053f45beSAndroid Build Coastguard Worker
sendpacket(int sock,struct sockaddr * addr,socklen_t addr_len,int ptpv2)134*053f45beSAndroid Build Coastguard Worker static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
135*053f45beSAndroid Build Coastguard Worker {
136*053f45beSAndroid Build Coastguard Worker size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
137*053f45beSAndroid Build Coastguard Worker const void *sync_p = ptpv2 ? sync_v2 : sync;
138*053f45beSAndroid Build Coastguard Worker struct timeval now;
139*053f45beSAndroid Build Coastguard Worker int res;
140*053f45beSAndroid Build Coastguard Worker
141*053f45beSAndroid Build Coastguard Worker res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);
142*053f45beSAndroid Build Coastguard Worker gettimeofday(&now, 0);
143*053f45beSAndroid Build Coastguard Worker if (res < 0)
144*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", "send", strerror(errno));
145*053f45beSAndroid Build Coastguard Worker else
146*053f45beSAndroid Build Coastguard Worker printf("%ld.%06ld: sent %d bytes\n",
147*053f45beSAndroid Build Coastguard Worker (long)now.tv_sec, (long)now.tv_usec,
148*053f45beSAndroid Build Coastguard Worker res);
149*053f45beSAndroid Build Coastguard Worker }
150*053f45beSAndroid Build Coastguard Worker
printpacket(struct msghdr * msg,int res,char * data,int sock,int recvmsg_flags,int siocgstamp,int siocgstampns,int ptpv2)151*053f45beSAndroid Build Coastguard Worker static void printpacket(struct msghdr *msg, int res,
152*053f45beSAndroid Build Coastguard Worker char *data,
153*053f45beSAndroid Build Coastguard Worker int sock, int recvmsg_flags,
154*053f45beSAndroid Build Coastguard Worker int siocgstamp, int siocgstampns, int ptpv2)
155*053f45beSAndroid Build Coastguard Worker {
156*053f45beSAndroid Build Coastguard Worker struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
157*053f45beSAndroid Build Coastguard Worker size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
158*053f45beSAndroid Build Coastguard Worker const void *sync_p = ptpv2 ? sync_v2 : sync;
159*053f45beSAndroid Build Coastguard Worker struct cmsghdr *cmsg;
160*053f45beSAndroid Build Coastguard Worker struct timeval tv;
161*053f45beSAndroid Build Coastguard Worker struct timespec ts;
162*053f45beSAndroid Build Coastguard Worker struct timeval now;
163*053f45beSAndroid Build Coastguard Worker
164*053f45beSAndroid Build Coastguard Worker gettimeofday(&now, 0);
165*053f45beSAndroid Build Coastguard Worker
166*053f45beSAndroid Build Coastguard Worker printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
167*053f45beSAndroid Build Coastguard Worker (long)now.tv_sec, (long)now.tv_usec,
168*053f45beSAndroid Build Coastguard Worker (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
169*053f45beSAndroid Build Coastguard Worker res,
170*053f45beSAndroid Build Coastguard Worker inet_ntoa(from_addr->sin_addr),
171*053f45beSAndroid Build Coastguard Worker msg->msg_controllen);
172*053f45beSAndroid Build Coastguard Worker for (cmsg = CMSG_FIRSTHDR(msg);
173*053f45beSAndroid Build Coastguard Worker cmsg;
174*053f45beSAndroid Build Coastguard Worker cmsg = CMSG_NXTHDR(msg, cmsg)) {
175*053f45beSAndroid Build Coastguard Worker printf(" cmsg len %zu: ", cmsg->cmsg_len);
176*053f45beSAndroid Build Coastguard Worker switch (cmsg->cmsg_level) {
177*053f45beSAndroid Build Coastguard Worker case SOL_SOCKET:
178*053f45beSAndroid Build Coastguard Worker printf("SOL_SOCKET ");
179*053f45beSAndroid Build Coastguard Worker switch (cmsg->cmsg_type) {
180*053f45beSAndroid Build Coastguard Worker case SO_TIMESTAMP: {
181*053f45beSAndroid Build Coastguard Worker struct timeval *stamp =
182*053f45beSAndroid Build Coastguard Worker (struct timeval *)CMSG_DATA(cmsg);
183*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMP %ld.%06ld",
184*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_sec,
185*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_usec);
186*053f45beSAndroid Build Coastguard Worker break;
187*053f45beSAndroid Build Coastguard Worker }
188*053f45beSAndroid Build Coastguard Worker case SO_TIMESTAMPNS: {
189*053f45beSAndroid Build Coastguard Worker struct timespec *stamp =
190*053f45beSAndroid Build Coastguard Worker (struct timespec *)CMSG_DATA(cmsg);
191*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMPNS %ld.%09ld",
192*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_sec,
193*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_nsec);
194*053f45beSAndroid Build Coastguard Worker break;
195*053f45beSAndroid Build Coastguard Worker }
196*053f45beSAndroid Build Coastguard Worker case SO_TIMESTAMPING: {
197*053f45beSAndroid Build Coastguard Worker struct timespec *stamp =
198*053f45beSAndroid Build Coastguard Worker (struct timespec *)CMSG_DATA(cmsg);
199*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMPING ");
200*053f45beSAndroid Build Coastguard Worker printf("SW %ld.%09ld ",
201*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_sec,
202*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_nsec);
203*053f45beSAndroid Build Coastguard Worker stamp++;
204*053f45beSAndroid Build Coastguard Worker /* skip deprecated HW transformed */
205*053f45beSAndroid Build Coastguard Worker stamp++;
206*053f45beSAndroid Build Coastguard Worker printf("HW raw %ld.%09ld",
207*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_sec,
208*053f45beSAndroid Build Coastguard Worker (long)stamp->tv_nsec);
209*053f45beSAndroid Build Coastguard Worker break;
210*053f45beSAndroid Build Coastguard Worker }
211*053f45beSAndroid Build Coastguard Worker default:
212*053f45beSAndroid Build Coastguard Worker printf("type %d", cmsg->cmsg_type);
213*053f45beSAndroid Build Coastguard Worker break;
214*053f45beSAndroid Build Coastguard Worker }
215*053f45beSAndroid Build Coastguard Worker break;
216*053f45beSAndroid Build Coastguard Worker case IPPROTO_IP:
217*053f45beSAndroid Build Coastguard Worker printf("IPPROTO_IP ");
218*053f45beSAndroid Build Coastguard Worker switch (cmsg->cmsg_type) {
219*053f45beSAndroid Build Coastguard Worker case IP_RECVERR: {
220*053f45beSAndroid Build Coastguard Worker struct sock_extended_err *err =
221*053f45beSAndroid Build Coastguard Worker (struct sock_extended_err *)CMSG_DATA(cmsg);
222*053f45beSAndroid Build Coastguard Worker printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
223*053f45beSAndroid Build Coastguard Worker strerror(err->ee_errno),
224*053f45beSAndroid Build Coastguard Worker err->ee_origin,
225*053f45beSAndroid Build Coastguard Worker #ifdef SO_EE_ORIGIN_TIMESTAMPING
226*053f45beSAndroid Build Coastguard Worker err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
227*053f45beSAndroid Build Coastguard Worker "bounced packet" : "unexpected origin"
228*053f45beSAndroid Build Coastguard Worker #else
229*053f45beSAndroid Build Coastguard Worker "probably SO_EE_ORIGIN_TIMESTAMPING"
230*053f45beSAndroid Build Coastguard Worker #endif
231*053f45beSAndroid Build Coastguard Worker );
232*053f45beSAndroid Build Coastguard Worker if (res < sync_len)
233*053f45beSAndroid Build Coastguard Worker printf(" => truncated data?!");
234*053f45beSAndroid Build Coastguard Worker else if (!memcmp(sync_p, data + res - sync_len, sync_len))
235*053f45beSAndroid Build Coastguard Worker printf(" => GOT OUR DATA BACK (HURRAY!)");
236*053f45beSAndroid Build Coastguard Worker break;
237*053f45beSAndroid Build Coastguard Worker }
238*053f45beSAndroid Build Coastguard Worker case IP_PKTINFO: {
239*053f45beSAndroid Build Coastguard Worker struct in_pktinfo *pktinfo =
240*053f45beSAndroid Build Coastguard Worker (struct in_pktinfo *)CMSG_DATA(cmsg);
241*053f45beSAndroid Build Coastguard Worker printf("IP_PKTINFO interface index %u",
242*053f45beSAndroid Build Coastguard Worker pktinfo->ipi_ifindex);
243*053f45beSAndroid Build Coastguard Worker break;
244*053f45beSAndroid Build Coastguard Worker }
245*053f45beSAndroid Build Coastguard Worker default:
246*053f45beSAndroid Build Coastguard Worker printf("type %d", cmsg->cmsg_type);
247*053f45beSAndroid Build Coastguard Worker break;
248*053f45beSAndroid Build Coastguard Worker }
249*053f45beSAndroid Build Coastguard Worker break;
250*053f45beSAndroid Build Coastguard Worker default:
251*053f45beSAndroid Build Coastguard Worker printf("level %d type %d",
252*053f45beSAndroid Build Coastguard Worker cmsg->cmsg_level,
253*053f45beSAndroid Build Coastguard Worker cmsg->cmsg_type);
254*053f45beSAndroid Build Coastguard Worker break;
255*053f45beSAndroid Build Coastguard Worker }
256*053f45beSAndroid Build Coastguard Worker printf("\n");
257*053f45beSAndroid Build Coastguard Worker }
258*053f45beSAndroid Build Coastguard Worker
259*053f45beSAndroid Build Coastguard Worker if (siocgstamp) {
260*053f45beSAndroid Build Coastguard Worker if (ioctl(sock, SIOCGSTAMP, &tv))
261*053f45beSAndroid Build Coastguard Worker printf(" %s: %s\n", "SIOCGSTAMP", strerror(errno));
262*053f45beSAndroid Build Coastguard Worker else
263*053f45beSAndroid Build Coastguard Worker printf("SIOCGSTAMP %ld.%06ld\n",
264*053f45beSAndroid Build Coastguard Worker (long)tv.tv_sec,
265*053f45beSAndroid Build Coastguard Worker (long)tv.tv_usec);
266*053f45beSAndroid Build Coastguard Worker }
267*053f45beSAndroid Build Coastguard Worker if (siocgstampns) {
268*053f45beSAndroid Build Coastguard Worker if (ioctl(sock, SIOCGSTAMPNS, &ts))
269*053f45beSAndroid Build Coastguard Worker printf(" %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
270*053f45beSAndroid Build Coastguard Worker else
271*053f45beSAndroid Build Coastguard Worker printf("SIOCGSTAMPNS %ld.%09ld\n",
272*053f45beSAndroid Build Coastguard Worker (long)ts.tv_sec,
273*053f45beSAndroid Build Coastguard Worker (long)ts.tv_nsec);
274*053f45beSAndroid Build Coastguard Worker }
275*053f45beSAndroid Build Coastguard Worker }
276*053f45beSAndroid Build Coastguard Worker
recvpacket(int sock,int recvmsg_flags,int siocgstamp,int siocgstampns,int ptpv2)277*053f45beSAndroid Build Coastguard Worker static void recvpacket(int sock, int recvmsg_flags,
278*053f45beSAndroid Build Coastguard Worker int siocgstamp, int siocgstampns, int ptpv2)
279*053f45beSAndroid Build Coastguard Worker {
280*053f45beSAndroid Build Coastguard Worker char data[256];
281*053f45beSAndroid Build Coastguard Worker struct msghdr msg;
282*053f45beSAndroid Build Coastguard Worker struct iovec entry;
283*053f45beSAndroid Build Coastguard Worker struct sockaddr_in from_addr;
284*053f45beSAndroid Build Coastguard Worker struct {
285*053f45beSAndroid Build Coastguard Worker struct cmsghdr cm;
286*053f45beSAndroid Build Coastguard Worker char control[512];
287*053f45beSAndroid Build Coastguard Worker } control;
288*053f45beSAndroid Build Coastguard Worker int res;
289*053f45beSAndroid Build Coastguard Worker
290*053f45beSAndroid Build Coastguard Worker memset(&msg, 0, sizeof(msg));
291*053f45beSAndroid Build Coastguard Worker msg.msg_iov = &entry;
292*053f45beSAndroid Build Coastguard Worker msg.msg_iovlen = 1;
293*053f45beSAndroid Build Coastguard Worker entry.iov_base = data;
294*053f45beSAndroid Build Coastguard Worker entry.iov_len = sizeof(data);
295*053f45beSAndroid Build Coastguard Worker msg.msg_name = (caddr_t)&from_addr;
296*053f45beSAndroid Build Coastguard Worker msg.msg_namelen = sizeof(from_addr);
297*053f45beSAndroid Build Coastguard Worker msg.msg_control = &control;
298*053f45beSAndroid Build Coastguard Worker msg.msg_controllen = sizeof(control);
299*053f45beSAndroid Build Coastguard Worker
300*053f45beSAndroid Build Coastguard Worker res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
301*053f45beSAndroid Build Coastguard Worker if (res < 0) {
302*053f45beSAndroid Build Coastguard Worker printf("%s %s: %s\n",
303*053f45beSAndroid Build Coastguard Worker "recvmsg",
304*053f45beSAndroid Build Coastguard Worker (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
305*053f45beSAndroid Build Coastguard Worker strerror(errno));
306*053f45beSAndroid Build Coastguard Worker } else {
307*053f45beSAndroid Build Coastguard Worker printpacket(&msg, res, data,
308*053f45beSAndroid Build Coastguard Worker sock, recvmsg_flags,
309*053f45beSAndroid Build Coastguard Worker siocgstamp, siocgstampns, ptpv2);
310*053f45beSAndroid Build Coastguard Worker }
311*053f45beSAndroid Build Coastguard Worker }
312*053f45beSAndroid Build Coastguard Worker
main(int argc,char ** argv)313*053f45beSAndroid Build Coastguard Worker int main(int argc, char **argv)
314*053f45beSAndroid Build Coastguard Worker {
315*053f45beSAndroid Build Coastguard Worker int so_timestamp = 0;
316*053f45beSAndroid Build Coastguard Worker int so_timestampns = 0;
317*053f45beSAndroid Build Coastguard Worker int siocgstamp = 0;
318*053f45beSAndroid Build Coastguard Worker int siocgstampns = 0;
319*053f45beSAndroid Build Coastguard Worker int ip_multicast_loop = 0;
320*053f45beSAndroid Build Coastguard Worker int ptpv2 = 0;
321*053f45beSAndroid Build Coastguard Worker char *interface;
322*053f45beSAndroid Build Coastguard Worker int i;
323*053f45beSAndroid Build Coastguard Worker int enabled = 1;
324*053f45beSAndroid Build Coastguard Worker int sock;
325*053f45beSAndroid Build Coastguard Worker struct ifreq device;
326*053f45beSAndroid Build Coastguard Worker struct ifreq hwtstamp;
327*053f45beSAndroid Build Coastguard Worker struct hwtstamp_config hwconfig, hwconfig_requested;
328*053f45beSAndroid Build Coastguard Worker struct so_timestamping so_timestamping_get = { 0, 0 };
329*053f45beSAndroid Build Coastguard Worker struct so_timestamping so_timestamping = { 0, 0 };
330*053f45beSAndroid Build Coastguard Worker struct sockaddr_in addr;
331*053f45beSAndroid Build Coastguard Worker struct ip_mreq imr;
332*053f45beSAndroid Build Coastguard Worker struct in_addr iaddr;
333*053f45beSAndroid Build Coastguard Worker int val;
334*053f45beSAndroid Build Coastguard Worker socklen_t len;
335*053f45beSAndroid Build Coastguard Worker struct timeval next;
336*053f45beSAndroid Build Coastguard Worker size_t if_len;
337*053f45beSAndroid Build Coastguard Worker
338*053f45beSAndroid Build Coastguard Worker if (argc < 2)
339*053f45beSAndroid Build Coastguard Worker usage(0);
340*053f45beSAndroid Build Coastguard Worker interface = argv[1];
341*053f45beSAndroid Build Coastguard Worker if_len = strlen(interface);
342*053f45beSAndroid Build Coastguard Worker if (if_len >= IFNAMSIZ) {
343*053f45beSAndroid Build Coastguard Worker printf("interface name exceeds IFNAMSIZ\n");
344*053f45beSAndroid Build Coastguard Worker exit(1);
345*053f45beSAndroid Build Coastguard Worker }
346*053f45beSAndroid Build Coastguard Worker
347*053f45beSAndroid Build Coastguard Worker if (argc >= 3 && sscanf(argv[2], "%d", &so_timestamping.bind_phc) == 1)
348*053f45beSAndroid Build Coastguard Worker val = 3;
349*053f45beSAndroid Build Coastguard Worker else
350*053f45beSAndroid Build Coastguard Worker val = 2;
351*053f45beSAndroid Build Coastguard Worker
352*053f45beSAndroid Build Coastguard Worker for (i = val; i < argc; i++) {
353*053f45beSAndroid Build Coastguard Worker if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
354*053f45beSAndroid Build Coastguard Worker so_timestamp = 1;
355*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
356*053f45beSAndroid Build Coastguard Worker so_timestampns = 1;
357*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
358*053f45beSAndroid Build Coastguard Worker siocgstamp = 1;
359*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
360*053f45beSAndroid Build Coastguard Worker siocgstampns = 1;
361*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
362*053f45beSAndroid Build Coastguard Worker ip_multicast_loop = 1;
363*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "PTPV2"))
364*053f45beSAndroid Build Coastguard Worker ptpv2 = 1;
365*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
366*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_TX_HARDWARE;
367*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
368*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
369*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
370*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_RX_HARDWARE;
371*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
372*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
373*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
374*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_SOFTWARE;
375*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
376*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
377*053f45beSAndroid Build Coastguard Worker else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_BIND_PHC"))
378*053f45beSAndroid Build Coastguard Worker so_timestamping.flags |= SOF_TIMESTAMPING_BIND_PHC;
379*053f45beSAndroid Build Coastguard Worker else
380*053f45beSAndroid Build Coastguard Worker usage(argv[i]);
381*053f45beSAndroid Build Coastguard Worker }
382*053f45beSAndroid Build Coastguard Worker
383*053f45beSAndroid Build Coastguard Worker sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
384*053f45beSAndroid Build Coastguard Worker if (sock < 0)
385*053f45beSAndroid Build Coastguard Worker bail("socket");
386*053f45beSAndroid Build Coastguard Worker
387*053f45beSAndroid Build Coastguard Worker memset(&device, 0, sizeof(device));
388*053f45beSAndroid Build Coastguard Worker memcpy(device.ifr_name, interface, if_len + 1);
389*053f45beSAndroid Build Coastguard Worker if (ioctl(sock, SIOCGIFADDR, &device) < 0)
390*053f45beSAndroid Build Coastguard Worker bail("getting interface IP address");
391*053f45beSAndroid Build Coastguard Worker
392*053f45beSAndroid Build Coastguard Worker memset(&hwtstamp, 0, sizeof(hwtstamp));
393*053f45beSAndroid Build Coastguard Worker memcpy(hwtstamp.ifr_name, interface, if_len + 1);
394*053f45beSAndroid Build Coastguard Worker hwtstamp.ifr_data = (void *)&hwconfig;
395*053f45beSAndroid Build Coastguard Worker memset(&hwconfig, 0, sizeof(hwconfig));
396*053f45beSAndroid Build Coastguard Worker hwconfig.tx_type =
397*053f45beSAndroid Build Coastguard Worker (so_timestamping.flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
398*053f45beSAndroid Build Coastguard Worker HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
399*053f45beSAndroid Build Coastguard Worker hwconfig.rx_filter =
400*053f45beSAndroid Build Coastguard Worker (so_timestamping.flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
401*053f45beSAndroid Build Coastguard Worker ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
402*053f45beSAndroid Build Coastguard Worker HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
403*053f45beSAndroid Build Coastguard Worker hwconfig_requested = hwconfig;
404*053f45beSAndroid Build Coastguard Worker if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
405*053f45beSAndroid Build Coastguard Worker if ((errno == EINVAL || errno == ENOTSUP) &&
406*053f45beSAndroid Build Coastguard Worker hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
407*053f45beSAndroid Build Coastguard Worker hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
408*053f45beSAndroid Build Coastguard Worker printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
409*053f45beSAndroid Build Coastguard Worker else
410*053f45beSAndroid Build Coastguard Worker bail("SIOCSHWTSTAMP");
411*053f45beSAndroid Build Coastguard Worker }
412*053f45beSAndroid Build Coastguard Worker printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
413*053f45beSAndroid Build Coastguard Worker hwconfig_requested.tx_type, hwconfig.tx_type,
414*053f45beSAndroid Build Coastguard Worker hwconfig_requested.rx_filter, hwconfig.rx_filter);
415*053f45beSAndroid Build Coastguard Worker
416*053f45beSAndroid Build Coastguard Worker /* bind to PTP port */
417*053f45beSAndroid Build Coastguard Worker addr.sin_family = AF_INET;
418*053f45beSAndroid Build Coastguard Worker addr.sin_addr.s_addr = htonl(INADDR_ANY);
419*053f45beSAndroid Build Coastguard Worker addr.sin_port = htons(319 /* PTP event port */);
420*053f45beSAndroid Build Coastguard Worker if (bind(sock,
421*053f45beSAndroid Build Coastguard Worker (struct sockaddr *)&addr,
422*053f45beSAndroid Build Coastguard Worker sizeof(struct sockaddr_in)) < 0)
423*053f45beSAndroid Build Coastguard Worker bail("bind");
424*053f45beSAndroid Build Coastguard Worker
425*053f45beSAndroid Build Coastguard Worker if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface, if_len))
426*053f45beSAndroid Build Coastguard Worker bail("bind device");
427*053f45beSAndroid Build Coastguard Worker
428*053f45beSAndroid Build Coastguard Worker /* set multicast group for outgoing packets */
429*053f45beSAndroid Build Coastguard Worker inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
430*053f45beSAndroid Build Coastguard Worker addr.sin_addr = iaddr;
431*053f45beSAndroid Build Coastguard Worker imr.imr_multiaddr.s_addr = iaddr.s_addr;
432*053f45beSAndroid Build Coastguard Worker imr.imr_interface.s_addr =
433*053f45beSAndroid Build Coastguard Worker ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
434*053f45beSAndroid Build Coastguard Worker if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
435*053f45beSAndroid Build Coastguard Worker &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
436*053f45beSAndroid Build Coastguard Worker bail("set multicast");
437*053f45beSAndroid Build Coastguard Worker
438*053f45beSAndroid Build Coastguard Worker /* join multicast group, loop our own packet */
439*053f45beSAndroid Build Coastguard Worker if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
440*053f45beSAndroid Build Coastguard Worker &imr, sizeof(struct ip_mreq)) < 0)
441*053f45beSAndroid Build Coastguard Worker bail("join multicast group");
442*053f45beSAndroid Build Coastguard Worker
443*053f45beSAndroid Build Coastguard Worker if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
444*053f45beSAndroid Build Coastguard Worker &ip_multicast_loop, sizeof(enabled)) < 0) {
445*053f45beSAndroid Build Coastguard Worker bail("loop multicast");
446*053f45beSAndroid Build Coastguard Worker }
447*053f45beSAndroid Build Coastguard Worker
448*053f45beSAndroid Build Coastguard Worker /* set socket options for time stamping */
449*053f45beSAndroid Build Coastguard Worker if (so_timestamp &&
450*053f45beSAndroid Build Coastguard Worker setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
451*053f45beSAndroid Build Coastguard Worker &enabled, sizeof(enabled)) < 0)
452*053f45beSAndroid Build Coastguard Worker bail("setsockopt SO_TIMESTAMP");
453*053f45beSAndroid Build Coastguard Worker
454*053f45beSAndroid Build Coastguard Worker if (so_timestampns &&
455*053f45beSAndroid Build Coastguard Worker setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
456*053f45beSAndroid Build Coastguard Worker &enabled, sizeof(enabled)) < 0)
457*053f45beSAndroid Build Coastguard Worker bail("setsockopt SO_TIMESTAMPNS");
458*053f45beSAndroid Build Coastguard Worker
459*053f45beSAndroid Build Coastguard Worker if (so_timestamping.flags &&
460*053f45beSAndroid Build Coastguard Worker setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping,
461*053f45beSAndroid Build Coastguard Worker sizeof(so_timestamping)) < 0)
462*053f45beSAndroid Build Coastguard Worker bail("setsockopt SO_TIMESTAMPING");
463*053f45beSAndroid Build Coastguard Worker
464*053f45beSAndroid Build Coastguard Worker /* request IP_PKTINFO for debugging purposes */
465*053f45beSAndroid Build Coastguard Worker if (setsockopt(sock, SOL_IP, IP_PKTINFO,
466*053f45beSAndroid Build Coastguard Worker &enabled, sizeof(enabled)) < 0)
467*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
468*053f45beSAndroid Build Coastguard Worker
469*053f45beSAndroid Build Coastguard Worker /* verify socket options */
470*053f45beSAndroid Build Coastguard Worker len = sizeof(val);
471*053f45beSAndroid Build Coastguard Worker if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
472*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
473*053f45beSAndroid Build Coastguard Worker else
474*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMP %d\n", val);
475*053f45beSAndroid Build Coastguard Worker
476*053f45beSAndroid Build Coastguard Worker if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
477*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
478*053f45beSAndroid Build Coastguard Worker strerror(errno));
479*053f45beSAndroid Build Coastguard Worker else
480*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMPNS %d\n", val);
481*053f45beSAndroid Build Coastguard Worker
482*053f45beSAndroid Build Coastguard Worker len = sizeof(so_timestamping_get);
483*053f45beSAndroid Build Coastguard Worker if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_get,
484*053f45beSAndroid Build Coastguard Worker &len) < 0) {
485*053f45beSAndroid Build Coastguard Worker printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
486*053f45beSAndroid Build Coastguard Worker strerror(errno));
487*053f45beSAndroid Build Coastguard Worker } else {
488*053f45beSAndroid Build Coastguard Worker printf("SO_TIMESTAMPING flags %d, bind phc %d\n",
489*053f45beSAndroid Build Coastguard Worker so_timestamping_get.flags, so_timestamping_get.bind_phc);
490*053f45beSAndroid Build Coastguard Worker if (so_timestamping_get.flags != so_timestamping.flags ||
491*053f45beSAndroid Build Coastguard Worker so_timestamping_get.bind_phc != so_timestamping.bind_phc)
492*053f45beSAndroid Build Coastguard Worker printf(" not expected, flags %d, bind phc %d\n",
493*053f45beSAndroid Build Coastguard Worker so_timestamping.flags, so_timestamping.bind_phc);
494*053f45beSAndroid Build Coastguard Worker }
495*053f45beSAndroid Build Coastguard Worker
496*053f45beSAndroid Build Coastguard Worker /* send packets forever every five seconds */
497*053f45beSAndroid Build Coastguard Worker gettimeofday(&next, 0);
498*053f45beSAndroid Build Coastguard Worker next.tv_sec = (next.tv_sec + 1) / 5 * 5;
499*053f45beSAndroid Build Coastguard Worker next.tv_usec = 0;
500*053f45beSAndroid Build Coastguard Worker while (1) {
501*053f45beSAndroid Build Coastguard Worker struct timeval now;
502*053f45beSAndroid Build Coastguard Worker struct timeval delta;
503*053f45beSAndroid Build Coastguard Worker long delta_us;
504*053f45beSAndroid Build Coastguard Worker int res;
505*053f45beSAndroid Build Coastguard Worker fd_set readfs, errorfs;
506*053f45beSAndroid Build Coastguard Worker
507*053f45beSAndroid Build Coastguard Worker gettimeofday(&now, 0);
508*053f45beSAndroid Build Coastguard Worker delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
509*053f45beSAndroid Build Coastguard Worker (long)(next.tv_usec - now.tv_usec);
510*053f45beSAndroid Build Coastguard Worker if (delta_us > 0) {
511*053f45beSAndroid Build Coastguard Worker /* continue waiting for timeout or data */
512*053f45beSAndroid Build Coastguard Worker delta.tv_sec = delta_us / 1000000;
513*053f45beSAndroid Build Coastguard Worker delta.tv_usec = delta_us % 1000000;
514*053f45beSAndroid Build Coastguard Worker
515*053f45beSAndroid Build Coastguard Worker FD_ZERO(&readfs);
516*053f45beSAndroid Build Coastguard Worker FD_ZERO(&errorfs);
517*053f45beSAndroid Build Coastguard Worker FD_SET(sock, &readfs);
518*053f45beSAndroid Build Coastguard Worker FD_SET(sock, &errorfs);
519*053f45beSAndroid Build Coastguard Worker printf("%ld.%06ld: select %ldus\n",
520*053f45beSAndroid Build Coastguard Worker (long)now.tv_sec, (long)now.tv_usec,
521*053f45beSAndroid Build Coastguard Worker delta_us);
522*053f45beSAndroid Build Coastguard Worker res = select(sock + 1, &readfs, 0, &errorfs, &delta);
523*053f45beSAndroid Build Coastguard Worker gettimeofday(&now, 0);
524*053f45beSAndroid Build Coastguard Worker printf("%ld.%06ld: select returned: %d, %s\n",
525*053f45beSAndroid Build Coastguard Worker (long)now.tv_sec, (long)now.tv_usec,
526*053f45beSAndroid Build Coastguard Worker res,
527*053f45beSAndroid Build Coastguard Worker res < 0 ? strerror(errno) : "success");
528*053f45beSAndroid Build Coastguard Worker if (res > 0) {
529*053f45beSAndroid Build Coastguard Worker if (FD_ISSET(sock, &readfs))
530*053f45beSAndroid Build Coastguard Worker printf("ready for reading\n");
531*053f45beSAndroid Build Coastguard Worker if (FD_ISSET(sock, &errorfs))
532*053f45beSAndroid Build Coastguard Worker printf("has error\n");
533*053f45beSAndroid Build Coastguard Worker recvpacket(sock, 0,
534*053f45beSAndroid Build Coastguard Worker siocgstamp,
535*053f45beSAndroid Build Coastguard Worker siocgstampns, ptpv2);
536*053f45beSAndroid Build Coastguard Worker recvpacket(sock, MSG_ERRQUEUE,
537*053f45beSAndroid Build Coastguard Worker siocgstamp,
538*053f45beSAndroid Build Coastguard Worker siocgstampns, ptpv2);
539*053f45beSAndroid Build Coastguard Worker }
540*053f45beSAndroid Build Coastguard Worker } else {
541*053f45beSAndroid Build Coastguard Worker /* write one packet */
542*053f45beSAndroid Build Coastguard Worker sendpacket(sock,
543*053f45beSAndroid Build Coastguard Worker (struct sockaddr *)&addr,
544*053f45beSAndroid Build Coastguard Worker sizeof(addr), ptpv2);
545*053f45beSAndroid Build Coastguard Worker next.tv_sec += 5;
546*053f45beSAndroid Build Coastguard Worker continue;
547*053f45beSAndroid Build Coastguard Worker }
548*053f45beSAndroid Build Coastguard Worker }
549*053f45beSAndroid Build Coastguard Worker
550*053f45beSAndroid Build Coastguard Worker return 0;
551*053f45beSAndroid Build Coastguard Worker }
552