Commit | Line | Data |
---|---|---|
cb9eff09 PO |
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 <patrick.ohly@intel.com> | |
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 | ||
f28f9e43 RD |
44 | #include <asm/types.h> |
45 | #include <linux/net_tstamp.h> | |
46 | #include <linux/errqueue.h> | |
cb9eff09 PO |
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 | ||
f28f9e43 | 167 | printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n", |
cb9eff09 PO |
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)) { | |
f28f9e43 | 176 | printf(" cmsg len %zu: ", cmsg->cmsg_len); |
cb9eff09 PO |
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, | |
228 | #ifdef SO_EE_ORIGIN_TIMESTAMPING | |
229 | err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ? | |
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 | ||
372 | sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); | |
fe9a2302 | 373 | if (sock < 0) |
cb9eff09 PO |
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; | |
b607bd90 | 384 | memset(&hwconfig, 0, sizeof(hwconfig)); |
cb9eff09 PO |
385 | hwconfig.tx_type = |
386 | (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ? | |
387 | HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF; | |
388 | hwconfig.rx_filter = | |
389 | (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ? | |
390 | HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE; | |
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 | } |