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