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