1 /*-
2 * Copyright (c) 2014 Wojciech Owczarek,
3 * George V. Neville-Neil
4 * Copyright (c) 2012-2013 George V. Neville-Neil,
5 * Wojciech Owczarek.
6 * Copyright (c) 2011-2012 George V. Neville-Neil,
7 * Steven Kreuzer,
8 * Martin Burnicki,
9 * Jan Breuer,
10 * Gael Mace,
11 * Alexandre Van Kempen,
12 * Inaqui Delgado,
13 * Rick Ratzel,
14 * National Instruments.
15 * Copyright (c) 2009-2010 George V. Neville-Neil,
16 * Steven Kreuzer,
17 * Martin Burnicki,
18 * Jan Breuer,
19 * Gael Mace,
20 * Alexandre Van Kempen
21 *
22 * Copyright (c) 2005-2008 Kendall Correll, Aidan Williams
23 *
24 * All Rights Reserved
25 *
26 * Redistribution and use in source and binary forms, with or without
27 * modification, are permitted provided that the following conditions are
28 * met:
29 * 1. Redistributions of source code must retain the above copyright notice,
30 * this list of conditions and the following disclaimer.
31 * 2. Redistributions in binary form must reproduce the above copyright
32 * notice, this list of conditions and the following disclaimer in the
33 * documentation and/or other materials provided with the distribution.
34 *
35 * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR
36 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
37 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
38 * DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
39 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
42 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
43 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
44 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
45 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
46 */
47
48 /**
49 * @file net.c
50 * @date Tue Jul 20 16:17:49 2010
51 *
52 * @brief Functions to interact with the network sockets and NIC driver.
53 *
54 *
55 */
56
57 #ifdef __sun
58 /*
59 * These are needed to enable control messages on sendmsg().
60 * I'll save the commentary on why the bend-over-backwards old way of doing
61 * sendmsg() is still around.
62 *
63 * STRICTLY SPEAKING this program should be linked against libxnet instead
64 * of libsocket because of these interfaces, but we'll cross that bridge
65 * when we get to it.
66 */
67 #define _XOPEN_SOURCE 500
68 #define __EXTENSIONS__
69 #endif
70
71 #include "../ptpd.h"
72
73 #ifdef PTPD_PCAP
74 #ifdef HAVE_PCAP_PCAP_H
75 #include <pcap/pcap.h>
76 #else /* !HAVE_PCAP_PCAP_H */
77 /* Cases like RHEL5 and others where only pcap.h exists */
78 #ifdef HAVE_PCAP_H
79 #include <pcap.h>
80 #endif /* HAVE_PCAP_H */
81 #endif
82 #define PCAP_TIMEOUT 1 /* expressed in milliseconds */
83 #endif
84
85 #if defined PTPD_SNMP
86 #include <net-snmp/net-snmp-config.h>
87 #include <net-snmp/net-snmp-includes.h>
88 #include <net-snmp/agent/net-snmp-agent-includes.h>
89 #endif
90
91 /* choose kernel-level nanoseconds or microseconds resolution on the client-side */
92 #if !defined(SO_TIMESTAMPING) && !defined(SO_TIMESTAMPNS) && !defined(SO_TIMESTAMP) && !defined(SO_BINTIME)
93 #error No kernel-level support for packet timestamping detected!
94 #endif
95
96 #ifdef SO_TIMESTAMPING
97 #include <linux/net_tstamp.h>
98 #include <linux/sockios.h>
99 #include <linux/ethtool.h>
100 #endif /* SO_TIMESTAMPING */
101
102 /**
103 * shutdown the IPv4 multicast for specific address
104 *
105 * @param netPath
106 * @param multicastAddr
107 *
108 * @return TRUE if successful
109 */
110 static Boolean
111 netShutdownMulticastIPv4(NetPath * netPath, Integer32 multicastAddr)
112 {
113 struct ip_mreq imr;
114
115 /* Close General Multicast */
116 imr.imr_multiaddr.s_addr = multicastAddr;
117 imr.imr_interface.s_addr = netPath->interfaceAddr.s_addr;
118
119 setsockopt(netPath->eventSock, IPPROTO_IP, IP_DROP_MEMBERSHIP,
120 &imr, sizeof(struct ip_mreq));
121 setsockopt(netPath->generalSock, IPPROTO_IP, IP_DROP_MEMBERSHIP,
122 &imr, sizeof(struct ip_mreq));
123
124 return TRUE;
125 }
126
127 /**
128 * shutdown the multicast (both General and Peer)
129 *
130 * @param netPath
131 *
132 * @return TRUE if successful
133 */
134 static Boolean
135 netShutdownMulticast(NetPath * netPath)
136 {
137 /* Close General Multicast */
138 netShutdownMulticastIPv4(netPath, netPath->multicastAddr);
139 netPath->multicastAddr = 0;
140
141 /* Close Peer Multicast */
142 netShutdownMulticastIPv4(netPath, netPath->peerMulticastAddr);
143 netPath->peerMulticastAddr = 0;
144
145 return TRUE;
146 }
147
148 /*
149 * For future use: Check if IPv4 address is multiast -
150 * If last 4 bits of an address are 0xE (1110), it's multicast
151 */
152 /*
153 static Boolean
154 isIpMulticast(struct in_addr in)
155 {
156 if((ntohl(in.s_addr) >> 28) == 0x0E )
157 return TRUE;
158 return FALSE;
159 }
160 */
161
162 /* shut down the UDP stuff */
163 Boolean
164 netShutdown(NetPath * netPath)
165 {
166 netShutdownMulticast(netPath);
167
168 netPath->unicastAddr = 0;
169
170 /* Close sockets */
171 if (netPath->eventSock >= 0)
172 close(netPath->eventSock);
173 netPath->eventSock = -1;
174
175 if (netPath->generalSock >= 0)
176 close(netPath->generalSock);
177 netPath->generalSock = -1;
178
179 #ifdef PTPD_PCAP
180 if (netPath->pcapEvent != NULL) {
181 pcap_close(netPath->pcapEvent);
182 netPath->pcapEventSock = -1;
183 }
184 if (netPath->pcapGeneral != NULL) {
185 pcap_close(netPath->pcapGeneral);
186 netPath->pcapGeneralSock = -1;
187 }
188 #endif
189
190 freeIpv4AccessList(&netPath->timingAcl);
191 freeIpv4AccessList(&netPath->managementAcl);
192
193 return TRUE;
194 }
195
196 /* Check if interface ifaceName exists. Return 1 on success, 0 when interface doesn't exists, -1 on failure.
197 */
198
199 static int
200 interfaceExists(char* ifaceName)
201 {
202
203 int ret;
204 struct ifaddrs *ifaddr, *ifa;
205
206 if(!strlen(ifaceName)) {
207 DBG("interfaceExists called for an empty interface!");
208 return 0;
209 }
210
211 if(getifaddrs(&ifaddr) == -1) {
212 PERROR("Could not get interface list");
213 ret = -1;
214 goto end;
215
216 }
217
218 for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
219
220 if(!strcmp(ifaceName, ifa->ifa_name)) {
221 ret = 1;
222 goto end;
223 }
224
225 }
226
227 ret = 0;
228 DBG("Interface not found: %s\n", ifaceName);
229
230 end:
231 freeifaddrs(ifaddr);
232 return ret;
233 }
234
235 static int
236 getInterfaceFlags(char* ifaceName, unsigned int* flags)
237 {
238
239 int ret;
240 struct ifaddrs *ifaddr, *ifa;
241
242 if(!strlen(ifaceName)) {
243 DBG("interfaceExists called for an empty interface!");
244 return 0;
245 }
246
247 if(getifaddrs(&ifaddr) == -1) {
248 PERROR("Could not get interface list");
249 ret = -1;
250 goto end;
251
252 }
253
254 for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
255
256 if(!strcmp(ifaceName, ifa->ifa_name)) {
257 *flags = ifa->ifa_flags;
258 ret = 1;
259 goto end;
260 }
261
262 }
263
264 ret = 0;
265 DBG("Interface not found: %s\n", ifaceName);
266
267 end:
268 freeifaddrs(ifaddr);
269 return ret;
270 }
271
272
273 /* Try getting addr address of family family from interface ifaceName.
274 Return 1 on success, 0 when no suitable address available, -1 on failure.
275 */
276 static int
277 getInterfaceAddress(char* ifaceName, int family, struct in_addr* addr) {
278
279 int ret;
280 struct ifaddrs *ifaddr, *ifa;
281
282 if(getifaddrs(&ifaddr) == -1) {
283 PERROR("Could not get interface list");
284 ret = -1;
285 goto end;
286
287 }
288
289 for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
290
291 if(!strcmp(ifaceName, ifa->ifa_name) && ifa->ifa_addr->sa_family == family) {
292
293 *addr = ((struct sockaddr_in*)ifa->ifa_addr)->sin_addr;
294 ret = 1;
295 goto end;
296
297 }
298
299 }
300
301 ret = 0;
302 DBG("Interface not found: %s\n", ifaceName);
303
304 end:
305
306 freeifaddrs(ifaddr);
307 return ret;
308 }
309
310
311 /* Try getting hwAddrSize bytes of ifaceName hardware address,
312 and place them in hwAddr. Return 1 on success, 0 when no suitable
313 hw address available, -1 on failure.
314 */
315 static int
316 getHwAddress (char* ifaceName, unsigned char* hwAddr, int hwAddrSize)
317 {
318
319 int ret;
320 if(!strlen(ifaceName))
321 return 0;
322
323 #if defined(AF_LINK) && !defined(__sun)
324 /* BSD* - AF_LINK gives us access to the hw address via struct sockaddr_dl */
325
326 struct ifaddrs *ifaddr, *ifa;
327
328 if(getifaddrs(&ifaddr) == -1) {
329 PERROR("Could not get interface list");
330 ret = -1;
331 goto end;
332
333 }
334
335 for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
336
337 if(!strcmp(ifaceName, ifa->ifa_name) && ifa->ifa_addr->sa_family == AF_LINK) {
338
339 struct sockaddr_dl* sdl = (struct sockaddr_dl *)ifa->ifa_addr;
340 if(sdl->sdl_type == IFT_ETHER || sdl->sdl_type == IFT_L2VLAN) {
341
342 memcpy(hwAddr, LLADDR(sdl),
343 hwAddrSize <= sizeof(sdl->sdl_data) ?
344 hwAddrSize : sizeof(sdl->sdl_data));
345 ret = 1;
346 goto end;
347 } else {
348 DBGV("Unsupported hardware address family on %s\n", ifaceName);
349 ret = 0;
350 goto end;
351 }
352 }
353
354 }
355
356 ret = 0;
357 DBG("Interface not found: %s\n", ifaceName);
358
359 end:
360
361 freeifaddrs(ifaddr);
362 return ret;
363
364 #else
365 /* Linux and Solarish systems have SIOCGIFHWADDR/SIOCGLIFHWADDR. Use it. */
366
367 int sockfd;
368 struct ifreq ifr;
369
370 sockfd = socket(AF_INET, SOCK_DGRAM, 0);
371
372 if(sockfd < 0) {
373 PERROR("Could not open test socket");
374 return -1;
375 }
376
377 memset(&ifr, 0, sizeof (ifr));
378
379 strncpy(ifr.ifr_name, ifaceName, IFACE_NAME_LENGTH);
380
381 if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) < 0) {
382 DBGV("failed to request hardware address for %s", ifaceName);
383 ret = -1;
384 goto end;
385 }
386
387
388 #ifdef __sun
389 int af = ifr.ifr_addr.sa_family;
390 #else
391 int af = ifr.ifr_hwaddr.sa_family;
392 #endif
393
394 if ( af == ARPHRD_ETHER
395 || af == ARPHRD_IEEE802
396 #ifdef ARPHRD_INFINIBAND
397 || af == ARPHRD_INFINIBAND
398 #endif
399 ) {
400 #ifdef __sun
401 memcpy(hwAddr, ifr.ifr_addr.sa_data, hwAddrSize);
402 #else
403 memcpy(hwAddr, ifr.ifr_hwaddr.sa_data, hwAddrSize);
404 #endif
405 ret = 1;
406 } else {
407 DBGV("Unsupported hardware address family on %s\n", ifaceName);
408 ret = 0;
409 }
410 end:
411 close(sockfd);
412 return ret;
413
414 #endif /* AF_LINK */
415
416 }
417
418 static Boolean getInterfaceInfo(char* ifaceName, InterfaceInfo* ifaceInfo)
419 {
420
421 int res;
422
423 res = interfaceExists(ifaceName);
424
425 if (res == -1) {
426
427 return FALSE;
428
429 } else if (res == 0) {
430
431 ERROR("Interface %s does not exist.\n", ifaceName);
432 return FALSE;
433 }
434
435 res = getInterfaceAddress(ifaceName, ifaceInfo->addressFamily, &ifaceInfo->afAddress);
436
437 if (res == -1) {
438
439 return FALSE;
440
441 }
442
443 ifaceInfo->hasAfAddress = res;
444
445 res = getHwAddress(ifaceName, (unsigned char*)ifaceInfo->hwAddress, 6);
446
447 if (res == -1) {
448
449 return FALSE;
450
451 }
452
453 ifaceInfo->hasHwAddress = res;
454
455 res = getInterfaceFlags(ifaceName, &ifaceInfo->flags);
456
457 if (res == -1) {
458
459 return FALSE;
460
461 }
462
463 return TRUE;
464
465 }
466
467 Boolean
468 testInterface(char * ifaceName, RunTimeOpts* rtOpts)
469 {
470
471 InterfaceInfo info;
472
473 info.addressFamily = AF_INET;
474
475 if(getInterfaceInfo(ifaceName, &info) != 1)
476 return FALSE;
477
478 switch(rtOpts->transport) {
479
480 case UDP_IPV4:
481 if(!info.hasAfAddress) {
482 ERROR("Interface %s has no IPv4 address set\n", ifaceName);
483 return FALSE;
484 }
485 break;
486
487 case IEEE_802_3:
488 if(!info.hasHwAddress) {
489 ERROR("Interface %s has no supported hardware address - possibly not an Ethernet interface\n", ifaceName);
490 return FALSE;
491 }
492 break;
493
494 default:
495 ERROR("Unsupported transport: %d\n", rtOpts->transport);
496 return FALSE;
497
498 }
499
500 if(!(info.flags & IFF_UP) || !(info.flags & IFF_RUNNING))
501 WARNING("Interface %s seems to be down. PTPd will not operate correctly until it's up.\n", ifaceName);
502
503 if(info.flags & IFF_LOOPBACK)
504 WARNING("Interface %s is a loopback interface.\n", ifaceName);
505
506 if(!(info.flags & IFF_MULTICAST)
507 && rtOpts->transport==UDP_IPV4
508 && rtOpts->ip_mode != IPMODE_UNICAST) {
509 WARNING("Interface %s is not multicast capable.\n", ifaceName);
510 }
511
512 return TRUE;
513
514 }
515
516 /**
517 * Init the multcast for specific IPv4 address
518 *
519 * @param netPath
520 * @param multicastAddr
521 *
522 * @return TRUE if successful
523 */
524 static Boolean
525 netInitMulticastIPv4(NetPath * netPath, Integer32 multicastAddr)
526 {
527 struct ip_mreq imr;
528
529 /* multicast send only on specified interface */
530 imr.imr_multiaddr.s_addr = multicastAddr;
531 imr.imr_interface.s_addr = netPath->interfaceAddr.s_addr;
532 if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_IF,
533 &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0
534 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_IF,
535 &imr.imr_interface.s_addr, sizeof(struct in_addr))
536 < 0) {
537 PERROR("failed to enable multi-cast on the interface");
538 return FALSE;
539 }
540 /* join multicast group (for receiving) on specified interface */
541 if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
542 &imr, sizeof(struct ip_mreq)) < 0
543 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
544 &imr, sizeof(struct ip_mreq)) < 0) {
545 PERROR("failed to join the multi-cast group");
546 return FALSE;
547 }
548 return TRUE;
549 }
550
551 /**
552 * Init the multcast (both General and Peer)
553 *
554 * @param netPath
555 * @param rtOpts
556 *
557 * @return TRUE if successful
558 */
559 static Boolean
560 netInitMulticast(NetPath * netPath, RunTimeOpts * rtOpts)
561 {
562 struct in_addr netAddr;
563 char addrStr[NET_ADDRESS_LENGTH+1];
564
565
566 /* Init General multicast IP address */
567 strncpy(addrStr, DEFAULT_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);
568 if (!inet_aton(addrStr, &netAddr)) {
569 ERROR("failed to encode multicast address: %s\n", addrStr);
570 return FALSE;
571 }
572
573 netPath->multicastAddr = netAddr.s_addr;
574 if(!netInitMulticastIPv4(netPath, netPath->multicastAddr)) {
575 return FALSE;
576 }
577
578 /* End of General multicast Ip address init */
579
580
581 /* Init Peer multicast IP address */
582 strncpy(addrStr, PEER_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);
583 if (!inet_aton(addrStr, &netAddr)) {
584 ERROR("failed to encode multi-cast address: %s\n", addrStr);
585 return FALSE;
586 }
587 netPath->peerMulticastAddr = netAddr.s_addr;
588 if(!netInitMulticastIPv4(netPath, netPath->peerMulticastAddr)) {
589 return FALSE;
590 }
591 /* End of Peer multicast Ip address init */
592
593 return TRUE;
594 }
595
596 static Boolean
597 netSetMulticastTTL(int sockfd, int ttl) {
598
599 #if defined(__OpenBSD__) || defined(__sun)
600 uint8_t temp = (uint8_t) ttl;
601 #else
602 int temp = ttl;
603 #endif
604
605 if (setsockopt(sockfd, IPPROTO_IP, IP_MULTICAST_TTL,
606 &temp, sizeof(temp)) < 0) {
607 PERROR("Failed to set socket multicast time-to-live");
608 return FALSE;
609 }
610 return TRUE;
611 }
612
613 static Boolean
614 netSetMulticastLoopback(NetPath * netPath, Boolean value) {
615 #if defined(__OpenBSD__) || defined(__sun)
616 uint8_t temp = value ? 1 : 0;
617 #else
618 int temp = value ? 1 : 0;
619 #endif
620 DBG("Going to set multicast loopback with %d \n", temp);
621
622 if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_LOOP,
623 &temp, sizeof(temp)) < 0) {
624 PERROR("Failed to set multicast loopback");
625 return FALSE;
626 }
627
628 return TRUE;
629 }
630
631 #if defined(SO_TIMESTAMPING) && defined(SO_TIMESTAMPNS)
632 static Boolean
633 getTxTimestamp(NetPath* netPath,TimeInternal* timeStamp) {
634 extern PtpClock *G_ptpClock;
635 ssize_t length;
636 fd_set tmpSet;
637 struct timeval timeOut = {0,0};
638 int val = 1;
639 if(netPath->txTimestampFailure)
640 goto end;
641
642 FD_ZERO(&tmpSet);
643 FD_SET(netPath->eventSock, &tmpSet);
644
645 if(select(netPath->eventSock + 1, &tmpSet, NULL, NULL, &timeOut) > 0) {
646 if (FD_ISSET(netPath->eventSock, &tmpSet)) {
647 length = netRecvEvent(G_ptpClock->msgIbuf, timeStamp,
648 netPath, MSG_ERRQUEUE);
649
650 if (length > 0) {
651 DBG("Grabbed sent msg via errqueue: %d bytes, at %d.%d\n", length, timeStamp->seconds, timeStamp->nanoseconds);
652 return TRUE;
653 } else if (length < 0) {
654 DBG("Failed to poll error queue for SO_TIMESTAMPING transmit time");
655 G_ptpClock->counters.messageRecvErrors++;
656 goto end;
657 } else if (length == 0) {
658 DBG("Received no data from TX error queue");
659 goto end;
660 }
661 }
662 } else {
663 DBG("SO_TIMESTAMPING - t timeout on TX timestamp - will use loop from now on\n");
664 return FALSE;
665 }
666 end:
667 if(setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0) {
668 DBG("gettxtimestamp: failed to revert to SO_TIMESTAMPNS");
669 }
670
671 return FALSE;
672 }
673 #endif /* SO_TIMESTAMPING */
674
675
676 /**
677 * Initialize timestamping of packets
678 *
679 * @param netPath
680 *
681 * @return TRUE if successful
682 */
683 static Boolean
684 netInitTimestamping(NetPath * netPath, RunTimeOpts * rtOpts)
685 {
686
687 int val = 1;
688 Boolean result = TRUE;
689 #if defined(SO_TIMESTAMPING) && defined(SO_TIMESTAMPNS)/* Linux - current API */
690 DBG("netInitTimestamping: trying to use SO_TIMESTAMPING\n");
691 val = SOF_TIMESTAMPING_TX_SOFTWARE |
692 SOF_TIMESTAMPING_RX_SOFTWARE |
693 SOF_TIMESTAMPING_SOFTWARE;
694
695 /* unless compiled with PTPD_EXPERIMENTAL, check if we support the desired tstamp capabilities */
696 #ifndef PTPD_EXPERIMENTAL
697 #ifdef ETHTOOL_GET_TS_INFO
698
699 struct ethtool_ts_info tsInfo;
700 struct ifreq ifRequest;
701 int res;
702
703 memset(&tsInfo, 0, sizeof(tsInfo));
704 memset(&ifRequest, 0, sizeof(ifRequest));
705 tsInfo.cmd = ETHTOOL_GET_TS_INFO;
706 strncpy( ifRequest.ifr_name, rtOpts->ifaceName, IFNAMSIZ - 1);
707 ifRequest.ifr_data = (char *) &tsInfo;
708 res = ioctl(netPath->eventSock, SIOCETHTOOL, &ifRequest);
709
710 if (res < 0) {
711 PERROR("Could not retrieve ethtool timestamping capabilities for %s - reverting to SO_TIMESTAMPNS",
712 rtOpts->ifaceName);
713 val = 1;
714 netPath->txTimestampFailure = FALSE;
715 } else if((tsInfo.so_timestamping & val) != val) {
716 DBGV("Required SO_TIMESTAMPING flags not supported - reverting to SO_TIMESTAMPNS\n");
717 val = 1;
718 netPath->txTimestampFailure = TRUE;
719 }
720 #else
721 netPath->txTimestampFailure = FALSE;
722 val = 1;
723 #endif /* ETHTOOL_GET_TS_INFO */
724 #endif /* PTPD_EXPERIMENTAL */
725
726 if((val==1 && (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0)) ||
727 (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPING, &val, sizeof(int)) < 0)) {
728 PERROR("netInitTimestamping: failed to enable SO_TIMESTAMP%s",(val==1)?"NS":"ING");
729 result = FALSE;
730 } else {
731 DBG("SO_TIMESTAMP%s initialised\n",(val==1)?"NS":"ING");
732 }
733 #elif defined(SO_TIMESTAMPNS) /* Linux, Apple */
734 DBG("netInitTimestamping: trying to use SO_TIMESTAMPNS\n");
735
736 if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0) {
737 PERROR("netInitTimestamping: failed to enable SO_TIMESTAMPNS");
738 result = FALSE;
739 }
740 #elif defined(SO_BINTIME) /* FreeBSD */
741 DBG("netInitTimestamping: trying to use SO_BINTIME\n");
742
743 if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_BINTIME, &val, sizeof(int)) < 0) {
744 PERROR("netInitTimestamping: failed to enable SO_BINTIME");
745 result = FALSE;
746 }
747 #else
748 result = FALSE;
749 #endif
750
751 /* fallback method */
752 #if defined(SO_TIMESTAMP) /* Linux, Apple, FreeBSD */
753 if (!result) {
754 DBG("netInitTimestamping: trying to use SO_TIMESTAMP\n");
755
756 if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMP, &val, sizeof(int)) < 0) {
757 PERROR("netInitTimestamping: failed to enable SO_TIMESTAMP");
758 result = FALSE;
759 }
760 result = TRUE;
761 }
762 #endif
763
764 return result;
765 }
766
767 Boolean
768 hostLookup(const char* hostname, Integer32* addr)
769 {
770 if (hostname[0]) {
771 /* Attempt a DNS lookup first. */
772 struct hostent *host;
773 #ifdef __sun
774 host = getipnodebyname(hostname, AF_INET, AI_DEFAULT, &errno);
775 #else
776 host = gethostbyname2(hostname, AF_INET);
777 #endif
778 if (host != NULL) {
779 if (host->h_length != 4) {
780 PERROR("unicast host resolved to non ipv4"
781 "address");
782 return FALSE;
783 }
784 *addr =
785 *(uint32_t *)host->h_addr_list[0];
786 return TRUE;
787 } else {
788 struct in_addr netAddr;
789 /* Maybe it's a dotted quad. */
790 if (!inet_aton(hostname, &netAddr)) {
791 ERROR("failed to encode unicast address: %s\n",
792 hostname);
793 return FALSE;
794 *addr = netAddr.s_addr;
795 return TRUE;
796 }
797 }
798 }
799
800 return FALSE;
801
802 }
803
804
805 /**
806 * Init all network transports
807 *
808 * @param netPath
809 * @param rtOpts
810 * @param ptpClock
811 *
812 * @return TRUE if successful
813 */
814 Boolean
815 netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock)
816 {
817 int temp;
818 struct sockaddr_in addr;
819
820 #ifdef PTPD_PCAP
821 struct bpf_program program;
822 char errbuf[PCAP_ERRBUF_SIZE];
823 #endif
824
825 DBG("netInit\n");
826
827 #ifdef PTPD_PCAP
828 netPath->pcapEvent = NULL;
829 netPath->pcapGeneral = NULL;
830 netPath->pcapEventSock = -1;
831 netPath->pcapGeneralSock = -1;
832 #endif
833 netPath->generalSock = -1;
834 netPath->eventSock = -1;
835
836 #ifdef PTPD_PCAP
837 if (rtOpts->transport == IEEE_802_3) {
838 netPath->headerOffset = PACKET_BEGIN_ETHER;
839 #ifdef HAVE_STRUCT_ETHER_ADDR_OCTET
840 memcpy(netPath->etherDest.octet, ether_aton(PTP_ETHER_DST), ETHER_ADDR_LEN);
841 memcpy(netPath->peerEtherDest.octet, ether_aton(PTP_ETHER_PEER), ETHER_ADDR_LEN);
842 #else
843 memcpy(netPath->etherDest.ether_addr_octet, ether_aton(PTP_ETHER_DST), ETHER_ADDR_LEN);
844 memcpy(netPath->peerEtherDest.ether_addr_octet, ether_aton(PTP_ETHER_PEER), ETHER_ADDR_LEN);
845 #endif /* HAVE_STRUCT_ETHER_ADDR_OCTET */
846 } else
847 #endif
848 netPath->headerOffset = PACKET_BEGIN_UDP;
849
850 /* open sockets */
851 if ((netPath->eventSock = socket(PF_INET, SOCK_DGRAM,
852 IPPROTO_UDP)) < 0
853 || (netPath->generalSock = socket(PF_INET, SOCK_DGRAM,
854 IPPROTO_UDP)) < 0) {
855 PERROR("failed to initialize sockets");
856 return FALSE;
857 }
858
859 if(!testInterface(rtOpts->ifaceName, rtOpts))
860 return FALSE;
861
862 netPath->interfaceInfo.addressFamily = AF_INET;
863
864 /* the if is here only to get rid of an unused result warning. */
865 if( getInterfaceInfo(rtOpts->ifaceName, &netPath->interfaceInfo)!= 1)
866 return FALSE;
867
868 /* No HW address, we'll use the protocol address to form interfaceID -> clockID */
869 if( !netPath->interfaceInfo.hasHwAddress && netPath->interfaceInfo.hasAfAddress ) {
870 uint32_t addr = netPath->interfaceInfo.afAddress.s_addr;
871 memcpy(netPath->interfaceID, &addr, 2);
872 memcpy(netPath->interfaceID + 4, &addr + 2, 2);
873 /* Initialise interfaceID with hardware address */
874 } else {
875 memcpy(&netPath->interfaceID, &netPath->interfaceInfo.hwAddress,
876 sizeof(netPath->interfaceID) <= sizeof(netPath->interfaceInfo.hwAddress) ?
877 sizeof(netPath->interfaceID) : sizeof(netPath->interfaceInfo.hwAddress)
878 );
879 }
880
881 DBG("Listening on IP: %s\n",inet_ntoa(netPath->interfaceInfo.afAddress));
882
883 #ifdef PTPD_PCAP
884 if (rtOpts->pcap == TRUE) {
885 int promisc = (rtOpts->transport == IEEE_802_3 ) ? 1 : 0;
886 if ((netPath->pcapEvent = pcap_open_live(rtOpts->ifaceName,
887 PACKET_SIZE, promisc,
888 PCAP_TIMEOUT,
889 errbuf)) == NULL) {
890 PERROR("failed to open event pcap");
891 return FALSE;
892 }
893 if (pcap_compile(netPath->pcapEvent, &program,
894 ( rtOpts->transport == IEEE_802_3 ) ?
895 "ether proto 0x88f7":
896 ( rtOpts->ip_mode != IPMODE_MULTICAST ) ?
897 "udp port 319" :
898 "host (224.0.1.129 or 224.0.0.107) and udp port 319" ,
899 1, 0) < 0) {
900 PERROR("failed to compile pcap event filter");
901 pcap_perror(netPath->pcapEvent, "ptpd2");
902 return FALSE;
903 }
904 if (pcap_setfilter(netPath->pcapEvent, &program) < 0) {
905 PERROR("failed to set pcap event filter");
906 return FALSE;
907 }
908 pcap_freecode(&program);
909 if ((netPath->pcapEventSock =
910 pcap_get_selectable_fd(netPath->pcapEvent)) < 0) {
911 PERROR("failed to get pcap event fd");
912 return FALSE;
913 }
914 if ((netPath->pcapGeneral = pcap_open_live(rtOpts->ifaceName,
915 PACKET_SIZE, promisc,
916 PCAP_TIMEOUT,
917 errbuf)) == NULL) {
918 PERROR("failed to open general pcap");
919 return FALSE;
920 }
921 if (rtOpts->transport != IEEE_802_3) {
922 if (pcap_compile(netPath->pcapGeneral, &program,
923 ( rtOpts->ip_mode != IPMODE_MULTICAST ) ?
924 "udp port 320" :
925 "host (224.0.1.129 or 224.0.0.107) and udp port 320" ,
926 1, 0) < 0) {
927 PERROR("failed to compile pcap general filter");
928 pcap_perror(netPath->pcapGeneral, "ptpd2");
929 return FALSE;
930 }
931 if (pcap_setfilter(netPath->pcapGeneral, &program) < 0) {
932 PERROR("failed to set pcap general filter");
933 return FALSE;
934 }
935 pcap_freecode(&program);
936 if ((netPath->pcapGeneralSock =
937 pcap_get_selectable_fd(netPath->pcapGeneral)) < 0) {
938 PERROR("failed to get pcap general fd");
939 return FALSE;
940 }
941 }
942 }
943 #endif
944
945 #ifdef PTPD_PCAP
946 if(rtOpts->transport == IEEE_802_3) {
947 close(netPath->eventSock);
948 netPath->eventSock = -1;
949 close(netPath->generalSock);
950 netPath->generalSock = -1;
951 /* TX timestamp is not generated for PCAP mode and Ethernet transport */
952 #ifdef SO_TIMESTAMPING
953 netPath->txTimestampFailure = TRUE;
954 #endif /* SO_TIMESTAMPING */
955 } else {
956 #endif
957 /* save interface address for IGMP refresh */
958 netPath->interfaceAddr = netPath->interfaceInfo.afAddress;
959
960 DBG("Local IP address used : %s \n", inet_ntoa(netPath->interfaceInfo.afAddress));
961
962 temp = 1; /* allow address reuse */
963 if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_REUSEADDR,
964 &temp, sizeof(int)) < 0
965 || setsockopt(netPath->generalSock, SOL_SOCKET, SO_REUSEADDR,
966 &temp, sizeof(int)) < 0) {
967 DBG("failed to set socket reuse\n");
968 }
969 /* bind sockets */
970 /*
971 * need INADDR_ANY to allow receipt of multi-cast and uni-cast
972 * messages
973 */
974
975 /* why??? */
976 if (rtOpts->pidAsClockId) {
977 if (inet_pton(AF_INET, DEFAULT_PTP_DOMAIN_ADDRESS, &addr.sin_addr) < 0) {
978 PERROR("failed to convert address");
979 return FALSE;
980 }
981 } else
982 addr.sin_addr.s_addr = htonl(INADDR_ANY);
983
984 addr.sin_family = AF_INET;
985 addr.sin_port = htons(PTP_EVENT_PORT);
986 if (bind(netPath->eventSock, (struct sockaddr *)&addr,
987 sizeof(struct sockaddr_in)) < 0) {
988 PERROR("failed to bind event socket");
989 return FALSE;
990 }
991 addr.sin_port = htons(PTP_GENERAL_PORT);
992 if (bind(netPath->generalSock, (struct sockaddr *)&addr,
993 sizeof(struct sockaddr_in)) < 0) {
994 PERROR("failed to bind general socket");
995 return FALSE;
996 }
997
998 #ifdef USE_BINDTODEVICE
999 #ifdef linux
1000 /*
1001 * The following code makes sure that the data is only
1002 * received on the specified interface. Without this option,
1003 * it's possible to receive PTP from another interface, and
1004 * confuse the protocol. Calling bind() with the IP address
1005 * of the device instead of INADDR_ANY does not work.
1006 *
1007 * More info:
1008 * http://developerweb.net/viewtopic.php?id=6471
1009 * http://stackoverflow.com/questions/1207746/problems-with-so-bindtodevice-linux-socket-option
1010 */
1011
1012 if ( rtOpts->ip_mode != IPMODE_HYBRID )
1013 if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_BINDTODEVICE,
1014 rtOpts->ifaceName, strlen(rtOpts->ifaceName)) < 0
1015 || setsockopt(netPath->generalSock, SOL_SOCKET, SO_BINDTODEVICE,
1016 rtOpts->ifaceName, strlen(rtOpts->ifaceName)) < 0){
1017 PERROR("failed to call SO_BINDTODEVICE on the interface");
1018 return FALSE;
1019 }
1020 #endif
1021 #endif
1022
1023 /* Set socket dscp */
1024 if(rtOpts->dscpValue) {
1025
1026 if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_TOS,
1027 &rtOpts->dscpValue, sizeof(int)) < 0
1028 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_TOS,
1029 &rtOpts->dscpValue, sizeof(int)) < 0) {
1030 PERROR("Failed to set socket DSCP bits");
1031 return FALSE;
1032 }
1033 }
1034
1035 /* send a uni-cast address if specified (useful for testing) */
1036 if(!hostLookup(rtOpts->unicastAddress, &netPath->unicastAddr)) {
1037 netPath->unicastAddr = 0;
1038 }
1039
1040
1041 if(rtOpts->ip_mode != IPMODE_UNICAST) {
1042
1043 /* init UDP Multicast on both Default and Peer addresses */
1044 if (!netInitMulticast(netPath, rtOpts))
1045 return FALSE;
1046
1047 /* set socket time-to-live */
1048 if(!netSetMulticastTTL(netPath->eventSock,rtOpts->ttl) ||
1049 !netSetMulticastTTL(netPath->generalSock,rtOpts->ttl))
1050 return FALSE;
1051
1052 /* start tracking TTL */
1053 netPath->ttlEvent = rtOpts->ttl;
1054 netPath->ttlGeneral = rtOpts->ttl;
1055 }
1056
1057 #ifdef SO_TIMESTAMPING
1058 /* Reset the failure indicator when (re)starting network */
1059 netPath->txTimestampFailure = FALSE;
1060 /* for SO_TIMESTAMPING we're receiving transmitted packets via ERRQUEUE */
1061 temp = 0;
1062 #else
1063 /* enable loopback */
1064 temp = 1;
1065 #endif
1066
1067 /* make timestamps available through recvmsg() */
1068 if (!netInitTimestamping(netPath,rtOpts)) {
1069 ERROR("Failed to enable packet time stamping\n");
1070 return FALSE;
1071 }
1072
1073 #ifdef SO_TIMESTAMPING
1074 /* If we failed to initialise SO_TIMESTAMPING, enable mcast loopback */
1075 if(netPath->txTimestampFailure)
1076 temp = 1;
1077 #endif
1078
1079 if(!netSetMulticastLoopback(netPath, temp)) {
1080 return FALSE;
1081 }
1082
1083 #ifdef PTPD_PCAP
1084 }
1085 #endif
1086
1087 /* Compile ACLs */
1088 if(rtOpts->timingAclEnabled) {
1089 freeIpv4AccessList(&netPath->timingAcl);
1090 netPath->timingAcl=createIpv4AccessList(rtOpts->timingAclPermitText,
1091 rtOpts->timingAclDenyText, rtOpts->timingAclOrder);
1092 }
1093 if(rtOpts->managementAclEnabled) {
1094 freeIpv4AccessList(&netPath->managementAcl);
1095 netPath->managementAcl=createIpv4AccessList(rtOpts->managementAclPermitText,
1096 rtOpts->managementAclDenyText, rtOpts->managementAclOrder);
1097 }
1098
1099 return TRUE;
1100 }
1101
1102 /*Check if data has been received*/
1103 int
1104 netSelect(TimeInternal * timeout, NetPath * netPath, fd_set *readfds)
1105 {
1106 int ret, nfds;
1107 struct timeval tv, *tv_ptr;
1108
1109
1110 #if defined PTPD_SNMP
1111 extern RunTimeOpts rtOpts;
1112 struct timeval snmp_timer_wait = { 0, 0}; // initialise to avoid unused warnings when SNMP disabled
1113 int snmpblock = 0;
1114 #endif
1115
1116 if (timeout) {
1117 if(isTimeInternalNegative(timeout)) {
1118 ERROR("Negative timeout attempted for select()\n");
1119 return -1;
1120 }
1121 tv.tv_sec = timeout->seconds;
1122 tv.tv_usec = timeout->nanoseconds / 1000;
1123 tv_ptr = &tv;
1124 } else {
1125 tv_ptr = NULL;
1126 }
1127
1128 FD_ZERO(readfds);
1129 nfds = 0;
1130 #ifdef PTPD_PCAP
1131 if (netPath->pcapEventSock >= 0) {
1132 FD_SET(netPath->pcapEventSock, readfds);
1133 if (netPath->pcapGeneralSock >= 0)
1134 FD_SET(netPath->pcapGeneralSock, readfds);
1135
1136 nfds = netPath->pcapEventSock;
1137 if (netPath->pcapEventSock < netPath->pcapGeneralSock)
1138 nfds = netPath->pcapGeneralSock;
1139
1140 } else if (netPath->eventSock >= 0) {
1141 #endif
1142 FD_SET(netPath->eventSock, readfds);
1143 if (netPath->generalSock >= 0)
1144 FD_SET(netPath->generalSock, readfds);
1145
1146 nfds = netPath->eventSock;
1147 if (netPath->eventSock < netPath->generalSock)
1148 nfds = netPath->generalSock;
1149 #ifdef PTPD_PCAP
1150 }
1151 #endif
1152 nfds++;
1153
1154 #if defined PTPD_SNMP
1155 if (rtOpts.snmp_enabled) {
1156 snmpblock = 1;
1157 if (tv_ptr) {
1158 snmpblock = 0;
1159 memcpy(&snmp_timer_wait, tv_ptr, sizeof(struct timeval));
1160 }
1161 snmp_select_info(&nfds, readfds, &snmp_timer_wait, &snmpblock);
1162 if (snmpblock == 0)
1163 tv_ptr = &snmp_timer_wait;
1164 }
1165 #endif
1166
1167 ret = select(nfds, readfds, 0, 0, tv_ptr);
1168
1169 if (ret < 0) {
1170 if (errno == EAGAIN || errno == EINTR)
1171 return 0;
1172 }
1173 #if defined PTPD_SNMP
1174 if (rtOpts.snmp_enabled) {
1175 /* Maybe we have received SNMP related data */
1176 if (ret > 0) {
1177 snmp_read(readfds);
1178 } else if (ret == 0) {
1179 snmp_timeout();
1180 run_alarms();
1181 }
1182 netsnmp_check_outstanding_agent_requests();
1183 }
1184 #endif
1185 return ret;
1186 }
1187
1188 /**
1189 * store received data from network to "buf" , get and store the
1190 * SO_TIMESTAMP value in "time" for an event message
1191 *
1192 * @note Should this function be merged with netRecvGeneral(), below?
1193 * Jan Breuer: I think that netRecvGeneral should be
1194 * simplified. Timestamp returned by this function is never
1195 * used. According to this, netInitTimestamping can be also simplified
1196 * to initialize timestamping only on eventSock.
1197 *
1198 * @param buf
1199 * @param time
1200 * @param netPath
1201 *
1202 * @return
1203 */
1204
1205 ssize_t
1206 netRecvEvent(Octet * buf, TimeInternal * time, NetPath * netPath, int flags)
1207 {
1208 ssize_t ret = 0;
1209 struct msghdr msg;
1210 struct iovec vec[1];
1211 struct sockaddr_in from_addr;
1212
1213 #ifdef PTPD_PCAP
1214 struct pcap_pkthdr *pkt_header;
1215 const u_char *pkt_data;
1216 #endif
1217
1218 union {
1219 struct cmsghdr cm;
1220 char control[256];
1221 } cmsg_un;
1222
1223 struct cmsghdr *cmsg;
1224
1225 #if defined(SO_TIMESTAMPNS) || defined(SO_TIMESTAMPING)
1226 struct timespec * ts;
1227 #elif defined(SO_BINTIME)
1228 struct bintime * bt;
1229 struct timespec ts;
1230 #endif
1231
1232 #if defined(SO_TIMESTAMP)
1233 struct timeval * tv;
1234 #endif
1235 Boolean timestampValid = FALSE;
1236
1237 #ifdef PTPD_PCAP
1238 if (netPath->pcapEvent == NULL) { /* Using sockets */
1239 #endif
1240 vec[0].iov_base = buf;
1241 vec[0].iov_len = PACKET_SIZE;
1242
1243 memset(&msg, 0, sizeof(msg));
1244 memset(&from_addr, 0, sizeof(from_addr));
1245 memset(buf, 0, PACKET_SIZE);
1246 memset(&cmsg_un, 0, sizeof(cmsg_un));
1247
1248 msg.msg_name = (caddr_t)&from_addr;
1249 msg.msg_namelen = sizeof(from_addr);
1250 msg.msg_iov = vec;
1251 msg.msg_iovlen = 1;
1252 msg.msg_control = cmsg_un.control;
1253 msg.msg_controllen = sizeof(cmsg_un.control);
1254 msg.msg_flags = 0;
1255
1256 ret = recvmsg(netPath->eventSock, &msg, flags | MSG_DONTWAIT);
1257 if (ret <= 0) {
1258 if (errno == EAGAIN || errno == EINTR)
1259 return 0;
1260
1261 return ret;
1262 };
1263 if (msg.msg_flags & MSG_TRUNC) {
1264 ERROR("received truncated message\n");
1265 return 0;
1266 }
1267 /* get time stamp of packet */
1268 if (!time) {
1269 ERROR("null receive time stamp argument\n");
1270 return 0;
1271 }
1272 if (msg.msg_flags & MSG_CTRUNC) {
1273 ERROR("received truncated ancillary data\n");
1274 return 0;
1275 }
1276
1277 #if defined(HAVE_DECL_MSG_ERRQUEUE) && HAVE_DECL_MSG_ERRQUEUE
1278 if(!(flags & MSG_ERRQUEUE))
1279 #endif
1280 netPath->lastRecvAddr = from_addr.sin_addr.s_addr;
1281 netPath->receivedPackets++;
1282
1283 if (msg.msg_controllen <= 0) {
1284 ERROR("received short ancillary data (%ld/%ld)\n",
1285 (long)msg.msg_controllen, (long)sizeof(cmsg_un.control));
1286
1287 return 0;
1288 }
1289
1290 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
1291 cmsg = CMSG_NXTHDR(&msg, cmsg)) {
1292 if (cmsg->cmsg_level == SOL_SOCKET) {
1293 #if defined(SO_TIMESTAMPING) && defined(SO_TIMESTAMPNS)
1294 if(cmsg->cmsg_type == SO_TIMESTAMPING ||
1295 cmsg->cmsg_type == SO_TIMESTAMPNS) {
1296 ts = (struct timespec *)CMSG_DATA(cmsg);
1297 time->seconds = ts->tv_sec;
1298 time->nanoseconds = ts->tv_nsec;
1299 timestampValid = TRUE;
1300 DBG("rcvevent: SO_TIMESTAMP%s %s time stamp: %us %dns\n", netPath->txTimestampFailure ?
1301 "NS" : "ING",
1302 (flags & MSG_ERRQUEUE) ? "(TX)" : "(RX)" , time->seconds, time->nanoseconds);
1303 break;
1304 }
1305 #elif defined(SO_TIMESTAMPNS)
1306 if(cmsg->cmsg_type == SCM_TIMESTAMPNS) {
1307 ts = (struct timespec *)CMSG_DATA(cmsg);
1308 time->seconds = ts->tv_sec;
1309 time->nanoseconds = ts->tv_nsec;
1310 timestampValid = TRUE;
1311 DBGV("kernel NANO recv time stamp %us %dns\n",
1312 time->seconds, time->nanoseconds);
1313 break;
1314 }
1315 #elif defined(SO_BINTIME)
1316 if(cmsg->cmsg_type == SCM_BINTIME) {
1317 bt = (struct bintime *)CMSG_DATA(cmsg);
1318 bintime2timespec(bt, &ts);
1319 time->seconds = ts.tv_sec;
1320 time->nanoseconds = ts.tv_nsec;
1321 timestampValid = TRUE;
1322 DBGV("kernel NANO recv time stamp %us %dns\n",
1323 time->seconds, time->nanoseconds);
1324 break;
1325 }
1326 #endif
1327
1328 #if defined(SO_TIMESTAMP)
1329 if(cmsg->cmsg_type == SCM_TIMESTAMP) {
1330 tv = (struct timeval *)CMSG_DATA(cmsg);
1331 time->seconds = tv->tv_sec;
1332 time->nanoseconds = tv->tv_usec * 1000;
1333 timestampValid = TRUE;
1334 DBGV("kernel MICRO recv time stamp %us %dns\n",
1335 time->seconds, time->nanoseconds);
1336 }
1337 #endif
1338 }
1339 }
1340
1341 if (!timestampValid) {
1342 /*
1343 * do not try to get by with recording the time here, better
1344 * to fail because the time recorded could be well after the
1345 * message receive, which would put a big spike in the
1346 * offset signal sent to the clock servo
1347 */
1348 DBG("netRecvEvent: no receive time stamp\n");
1349 return 0;
1350 }
1351 #ifdef PTPD_PCAP
1352 }
1353 #endif
1354
1355 #ifdef PTPD_PCAP
1356 else { /* Using PCAP */
1357 /* Discard packet on socket */
1358 if (netPath->eventSock >= 0) {
1359 recv(netPath->eventSock, buf, PACKET_SIZE, MSG_DONTWAIT);
1360 }
1361
1362 if ((ret = pcap_next_ex(netPath->pcapEvent, &pkt_header,
1363 &pkt_data)) < 1) {
1364 if (ret < 0)
1365 DBGV("netRecvEvent: pcap_next_ex failed %s\n",
1366 pcap_geterr(netPath->pcapEvent));
1367 return 0;
1368 }
1369
1370 /* Make sure this is IP (could dot1q get here?) */
1371 if( ntohs(*(u_short *)(pkt_data + 12)) != ETHERTYPE_IP)
1372 DBGV("PCAP payload received is not Ethernet: 0x%04x\n",
1373 ntohs(*(u_short *)(pkt_data + 12)));
1374 /* Retrieve source IP from the payload - 14 eth + 12 IP */
1375 netPath->lastRecvAddr = *(Integer32 *)(pkt_data + 26);
1376
1377 netPath->receivedPackets++;
1378 /* XXX Total cheat */
1379 memcpy(buf, pkt_data + netPath->headerOffset,
1380 pkt_header->caplen - netPath->headerOffset);
1381 time->seconds = pkt_header->ts.tv_sec;
1382 time->nanoseconds = pkt_header->ts.tv_usec * 1000;
1383 timestampValid = TRUE;
1384 DBGV("netRecvEvent: kernel PCAP recv time stamp %us %dns\n",
1385 time->seconds, time->nanoseconds);
1386 fflush(NULL);
1387 ret = pkt_header->caplen - netPath->headerOffset;
1388 }
1389 #endif
1390 return ret;
1391 }
1392
1393
1394
1395 /**
1396 *
1397 * store received data from network to "buf" get and store the
1398 * SO_TIMESTAMP value in "time" for a general message
1399 *
1400 * @param buf
1401 * @param time
1402 * @param netPath
1403 *
1404 * @return
1405 */
1406
1407 ssize_t
1408 netRecvGeneral(Octet * buf, NetPath * netPath)
1409 {
1410 ssize_t ret = 0;
1411 struct sockaddr_in from_addr;
1412
1413 #ifdef PTPD_PCAP
1414 struct pcap_pkthdr *pkt_header;
1415 const u_char *pkt_data;
1416 #endif
1417 socklen_t from_addr_len = sizeof(from_addr);
1418
1419 #ifdef PTPD_PCAP
1420 if (netPath->pcapGeneral == NULL) {
1421 #endif
1422 ret=recvfrom(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT, (struct sockaddr*)&from_addr, &from_addr_len);
1423 netPath->lastRecvAddr = from_addr.sin_addr.s_addr;
1424 return ret;
1425 #ifdef PTPD_PCAP
1426 }
1427 #endif
1428
1429 #ifdef PTPD_PCAP
1430 else { /* Using PCAP */
1431 /* Discard packet on socket */
1432 if (netPath->generalSock >= 0)
1433 recv(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT);
1434
1435
1436 if (( ret = pcap_next_ex(netPath->pcapGeneral, &pkt_header,
1437 &pkt_data)) < 1) {
1438 if (ret < 0)
1439 DBGV("netRecvGeneral: pcap_next_ex failed %d %s\n",
1440 ret, pcap_geterr(netPath->pcapGeneral));
1441 return 0;
1442 }
1443 /* Make sure this is IP (could dot1q get here?) */
1444 if( ntohs(*(u_short *)(pkt_data + 12)) != ETHERTYPE_IP)
1445 DBGV("PCAP payload received is not Ethernet: 0x%04x\n",
1446 ntohs(*(u_short *)(pkt_data + 12)));
1447 /* Retrieve source IP from the payload - 14 eth + 12 IP src*/
1448 netPath->lastRecvAddr = *(Integer32 *)(pkt_data + 26);
1449
1450 netPath->receivedPackets++;
1451 /* XXX Total cheat */
1452 memcpy(buf, pkt_data + netPath->headerOffset,
1453 pkt_header->caplen - netPath->headerOffset);
1454 fflush(NULL);
1455 ret = pkt_header->caplen - netPath->headerOffset;
1456 }
1457 #endif
1458 return ret;
1459 }
1460
1461
1462 #ifdef PTPD_PCAP
1463 ssize_t
1464 netSendPcapEther(Octet * buf, UInteger16 length,
1465 struct ether_addr * dst, struct ether_addr * src,
1466 pcap_t * pcap) {
1467 Octet ether[ETHER_HDR_LEN + PACKET_SIZE];
1468 #ifdef HAVE_STRUCT_ETHER_ADDR_OCTET
1469 memcpy(ether, dst->octet, ETHER_ADDR_LEN);
1470 memcpy(ether + ETHER_ADDR_LEN, src->octet, ETHER_ADDR_LEN);
1471 #else
1472 memcpy(ether, dst->ether_addr_octet, ETHER_ADDR_LEN);
1473 memcpy(ether + ETHER_ADDR_LEN, src->ether_addr_octet, ETHER_ADDR_LEN);
1474 #endif /* HAVE_STRUCT_ETHER_ADDR_OCTET */
1475 *((short *)ðer[2 * ETHER_ADDR_LEN]) = htons(PTP_ETHER_TYPE);
1476 memcpy(ether + ETHER_HDR_LEN, buf, length);
1477
1478 return pcap_inject(pcap, ether, ETHER_HDR_LEN + length);
1479 }
1480 #endif
1481
1482 //
1483 // alt_dst: alternative destination.
1484 // if filled, send to this unicast dest;
1485 // if zero, do the normal operation (send to unicast with -u, or send to the multcast group)
1486 //
1487 ///
1488 /// TODO: merge these 2 functions into one
1489 ///
1490 ssize_t
1491 netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
1492 RunTimeOpts *rtOpts, Integer32 alt_dst, TimeInternal * tim)
1493 {
1494 ssize_t ret;
1495 struct sockaddr_in addr;
1496
1497 addr.sin_family = AF_INET;
1498 addr.sin_port = htons(PTP_EVENT_PORT);
1499
1500 #ifdef PTPD_PCAP
1501 /* In PCAP Ethernet mode, we use pcapEvent for receiving all messages
1502 * and pcapGeneral for sending all messages
1503 */
1504 if ((netPath->pcapGeneral != NULL) && (rtOpts->transport == IEEE_802_3 )) {
1505 ret = netSendPcapEther(buf, length,
1506 &netPath->etherDest,
1507 (struct ether_addr *)netPath->interfaceID,
1508 netPath->pcapGeneral);
1509
1510 if (ret <= 0)
1511 DBG("Error sending ether multicast event message\n");
1512 else
1513 netPath->sentPackets++;
1514 } else {
1515 #endif
1516 if (netPath->unicastAddr || alt_dst ) {
1517 if (netPath->unicastAddr) {
1518 addr.sin_addr.s_addr = netPath->unicastAddr;
1519 } else {
1520 addr.sin_addr.s_addr = alt_dst;
1521 }
1522
1523 /*
1524 * This function is used for PTP only anyway...
1525 * If we're sending to a unicast address, set the UNICAST flag.
1526 */
1527 *(char *)(buf + 6) |= PTP_UNICAST;
1528
1529 ret = sendto(netPath->eventSock, buf, length, 0,
1530 (struct sockaddr *)&addr,
1531 sizeof(struct sockaddr_in));
1532 if (ret <= 0)
1533 DBG("Error sending unicast event message\n");
1534 else
1535 netPath->sentPackets++;
1536 #ifndef SO_TIMESTAMPING
1537 /*
1538 * Need to forcibly loop back the packet since
1539 * we are not using multicast.
1540 */
1541
1542 addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr;
1543 ret = sendto(netPath->eventSock, buf, length, 0,
1544 (struct sockaddr *)&addr,
1545 sizeof(struct sockaddr_in));
1546 if (ret <= 0)
1547 DBG("Error looping back unicast event message\n");
1548 #else
1549 if(!netPath->txTimestampFailure) {
1550 if(!getTxTimestamp(netPath, tim)) {
1551 netPath->txTimestampFailure = TRUE;
1552 if (tim) {
1553 clearTime(tim);
1554 }
1555 }
1556 }
1557
1558 if(netPath->txTimestampFailure)
1559 {
1560 /* We've had a TX timestamp receipt timeout - falling back to packet looping */
1561 addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr;
1562 ret = sendto(netPath->eventSock, buf, length, 0,
1563 (struct sockaddr *)&addr,
1564 sizeof(struct sockaddr_in));
1565 if (ret <= 0)
1566 DBG("Error looping back unicast event message\n");
1567 }
1568 #endif /* SO_TIMESTAMPING */
1569 } else {
1570 addr.sin_addr.s_addr = netPath->multicastAddr;
1571 /* Is TTL OK? */
1572 if(netPath->ttlEvent != rtOpts->ttl) {
1573 /* Try restoring TTL */
1574 /* set socket time-to-live */
1575 if (netSetMulticastTTL(netPath->eventSock,rtOpts->ttl)) {
1576 netPath->ttlEvent = rtOpts->ttl;
1577 }
1578 }
1579 ret = sendto(netPath->eventSock, buf, length, 0,
1580 (struct sockaddr *)&addr,
1581 sizeof(struct sockaddr_in));
1582 if (ret <= 0)
1583 DBG("Error sending multicast event message\n");
1584 else
1585 netPath->sentPackets++;
1586 #ifdef SO_TIMESTAMPING
1587 if(!netPath->txTimestampFailure) {
1588 if(!getTxTimestamp(netPath, tim)) {
1589 if (tim) {
1590 clearTime(tim);
1591 }
1592
1593 netPath->txTimestampFailure = TRUE;
1594
1595 /* Try re-enabling MULTICAST_LOOP */
1596 netSetMulticastLoopback(netPath, TRUE);
1597 }
1598 }
1599 #endif /* SO_TIMESTAMPING */
1600 }
1601
1602 #ifdef PTPD_PCAP
1603 }
1604 #endif
1605 return ret;
1606 }
1607
1608 ssize_t
1609 netSendGeneral(Octet * buf, UInteger16 length, NetPath * netPath,
1610 RunTimeOpts *rtOpts, Integer32 alt_dst)
1611 {
1612 ssize_t ret;
1613 struct sockaddr_in addr;
1614
1615 addr.sin_family = AF_INET;
1616 addr.sin_port = htons(PTP_GENERAL_PORT);
1617
1618 #ifdef PTPD_PCAP
1619 if ((netPath->pcapGeneral != NULL) && (rtOpts->transport == IEEE_802_3)) {
1620 ret = netSendPcapEther(buf, length,
1621 &netPath->etherDest,
1622 (struct ether_addr *)netPath->interfaceID,
1623 netPath->pcapGeneral);
1624
1625 if (ret <= 0)
1626 DBG("Error sending ether multicast general message\n");
1627 else
1628 netPath->sentPackets++;
1629 } else {
1630 #endif
1631 if(netPath->unicastAddr || alt_dst ){
1632 if (netPath->unicastAddr) {
1633 addr.sin_addr.s_addr = netPath->unicastAddr;
1634 } else {
1635 addr.sin_addr.s_addr = alt_dst;
1636 }
1637
1638 /*
1639 * This function is used for PTP only anyway...
1640 * If we're sending to a unicast address, set the UNICAST flag.
1641 */
1642 *(char *)(buf + 6) |= PTP_UNICAST;
1643
1644 ret = sendto(netPath->generalSock, buf, length, 0,
1645 (struct sockaddr *)&addr,
1646 sizeof(struct sockaddr_in));
1647 if (ret <= 0)
1648 DBG("Error sending unicast general message\n");
1649 else
1650 netPath->sentPackets++;
1651 } else {
1652 addr.sin_addr.s_addr = netPath->multicastAddr;
1653
1654 /* Is TTL OK? */
1655 if(netPath->ttlGeneral != rtOpts->ttl) {
1656 /* Try restoring TTL */
1657 if (netSetMulticastTTL(netPath->generalSock,rtOpts->ttl)) {
1658 netPath->ttlGeneral = rtOpts->ttl;
1659 }
1660 }
1661
1662 ret = sendto(netPath->generalSock, buf, length, 0,
1663 (struct sockaddr *)&addr,
1664 sizeof(struct sockaddr_in));
1665 if (ret <= 0)
1666 DBG("Error sending multicast general message\n");
1667 else
1668 netPath->sentPackets++;
1669 }
1670
1671 #ifdef PTPD_PCAP
1672 }
1673 #endif
1674 return ret;
1675 }
1676
1677 ssize_t
1678 netSendPeerGeneral(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts *rtOpts)
1679 {
1680
1681 ssize_t ret;
1682 struct sockaddr_in addr;
1683
1684 addr.sin_family = AF_INET;
1685 addr.sin_port = htons(PTP_GENERAL_PORT);
1686
1687 #ifdef PTPD_PCAP
1688 if ((netPath->pcapGeneral != NULL) && (rtOpts->transport == IEEE_802_3)) {
1689 ret = netSendPcapEther(buf, length,
1690 &netPath->peerEtherDest,
1691 (struct ether_addr *)netPath->interfaceID,
1692 netPath->pcapGeneral);
1693
1694 if (ret <= 0)
1695 DBG("error sending ether multi-cast general message\n");
1696 } else if (netPath->unicastAddr)
1697 #else
1698 if (netPath->unicastAddr)
1699 #endif
1700 {
1701 addr.sin_addr.s_addr = netPath->unicastAddr;
1702
1703 ret = sendto(netPath->generalSock, buf, length, 0,
1704 (struct sockaddr *)&addr,
1705 sizeof(struct sockaddr_in));
1706 if (ret <= 0)
1707 DBG("Error sending unicast peer general message\n");
1708
1709 } else {
1710 addr.sin_addr.s_addr = netPath->peerMulticastAddr;
1711
1712 /* is TTL already 1 ? */
1713 if(netPath->ttlGeneral != 1) {
1714 /* Try setting TTL to 1 */
1715 if (netSetMulticastTTL(netPath->generalSock,1)) {
1716 netPath->ttlGeneral = 1;
1717 }
1718 }
1719 ret = sendto(netPath->generalSock, buf, length, 0,
1720 (struct sockaddr *)&addr,
1721 sizeof(struct sockaddr_in));
1722 if (ret <= 0)
1723 DBG("Error sending multicast peer general message\n");
1724 }
1725
1726 if (ret > 0)
1727 netPath->sentPackets++;
1728
1729 return ret;
1730
1731 }
1732
1733 ssize_t
1734 netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts *rtOpts, TimeInternal * tim)
1735 {
1736 ssize_t ret;
1737 struct sockaddr_in addr;
1738
1739 addr.sin_family = AF_INET;
1740 addr.sin_port = htons(PTP_EVENT_PORT);
1741
1742 #ifdef PTPD_PCAP
1743 if ((netPath->pcapGeneral != NULL) && (rtOpts->transport == IEEE_802_3)) {
1744 ret = netSendPcapEther(buf, length,
1745 &netPath->peerEtherDest,
1746 (struct ether_addr *)netPath->interfaceID,
1747 netPath->pcapGeneral);
1748
1749 if (ret <= 0)
1750 DBG("error sending ether multi-cast general message\n");
1751 } else if (netPath->unicastAddr)
1752 #else
1753 if (netPath->unicastAddr)
1754 #endif
1755 {
1756 addr.sin_addr.s_addr = netPath->unicastAddr;
1757
1758 ret = sendto(netPath->eventSock, buf, length, 0,
1759 (struct sockaddr *)&addr,
1760 sizeof(struct sockaddr_in));
1761 if (ret <= 0)
1762 DBG("Error sending unicast peer event message\n");
1763 else
1764 netPath->sentPackets++;
1765
1766 #ifndef SO_TIMESTAMPING
1767 /*
1768 * Need to forcibly loop back the packet since
1769 * we are not using multicast.
1770 */
1771 addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr;
1772
1773 ret = sendto(netPath->eventSock, buf, length, 0,
1774 (struct sockaddr *)&addr,
1775 sizeof(struct sockaddr_in));
1776 if (ret <= 0)
1777 DBG("Error looping back unicast peer event message\n");
1778 #else
1779 if(!netPath->txTimestampFailure) {
1780 if(!getTxTimestamp(netPath, tim)) {
1781 netPath->txTimestampFailure = TRUE;
1782 if (tim) {
1783 clearTime(tim);
1784 }
1785 }
1786 }
1787
1788 if(netPath->txTimestampFailure) {
1789 /* We've had a TX timestamp receipt timeout - falling back to packet looping */
1790 addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr;
1791 ret = sendto(netPath->eventSock, buf, length, 0,
1792 (struct sockaddr *)&addr,
1793 sizeof(struct sockaddr_in));
1794 if (ret <= 0)
1795 DBG("Error looping back unicast event message\n");
1796 }
1797 #endif /* SO_TIMESTAMPING */
1798
1799 } else {
1800 addr.sin_addr.s_addr = netPath->peerMulticastAddr;
1801
1802 /* is TTL already 1 ? */
1803 if(netPath->ttlEvent != 1) {
1804 /* Try setting TTL to 1 */
1805 if (netSetMulticastTTL(netPath->eventSock,1)) {
1806 netPath->ttlEvent = 1;
1807 }
1808 }
1809 ret = sendto(netPath->eventSock, buf, length, 0,
1810 (struct sockaddr *)&addr,
1811 sizeof(struct sockaddr_in));
1812 if (ret <= 0)
1813 DBG("Error sending multicast peer event message\n");
1814 else
1815 netPath->sentPackets++;
1816 #ifdef SO_TIMESTAMPING
1817 if(!netPath->txTimestampFailure) {
1818 if(!getTxTimestamp(netPath, tim)) {
1819 if (tim) {
1820 clearTime(tim);
1821 }
1822
1823 netPath->txTimestampFailure = TRUE;
1824
1825 /* Try re-enabling MULTICAST_LOOP */
1826 netSetMulticastLoopback(netPath, TRUE);
1827 }
1828 }
1829 #endif /* SO_TIMESTAMPING */
1830 }
1831
1832 if (ret > 0)
1833 netPath->sentPackets++;
1834
1835 return ret;
1836 }
1837
1838
1839
1840 /*
1841 * refresh IGMP on a timeout
1842 */
1843 /*
1844 * @return TRUE if successful
1845 */
1846 Boolean
1847 netRefreshIGMP(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock)
1848 {
1849 DBG("netRefreshIGMP\n");
1850
1851 netShutdownMulticast(netPath);
1852
1853 /* suspend process 100 milliseconds, to make sure the kernel sends the IGMP_leave properly */
1854 usleep(100*1000);
1855
1856 if (!netInitMulticast(netPath, rtOpts)) {
1857 return FALSE;
1858 }
1859 return TRUE;
1860 }