/*- * Copyright (c) 2016-2017, 2023 Wind River Systems, Inc, * Copyright (c) 2014 Wojciech Owczarek, * George V. Neville-Neil * Copyright (c) 2012-2013 George V. Neville-Neil, * Wojciech Owczarek. * Copyright (c) 2011-2012 George V. Neville-Neil, * Steven Kreuzer, * Martin Burnicki, * Jan Breuer, * Gael Mace, * Alexandre Van Kempen, * Inaqui Delgado, * Rick Ratzel, * National Instruments. * Copyright (c) 2009-2010 George V. Neville-Neil, * Steven Kreuzer, * Martin Burnicki, * Jan Breuer, * Gael Mace, * Alexandre Van Kempen * * Copyright (c) 2005-2008 Kendall Correll, Aidan Williams * * All Rights Reserved * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are * met: * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /* modification history -------------------- 17oct23,rjq Fix Y2038 issue. 25jan17,xxy Add support for TSN Clocks and Timers (F6548) 15aug16,xxy Add PTP features.(F6696) 13jun16,xxy Add PTP one-step hardware Tx timeStamp 02feb16,h_l Init version. */ /** * @file net.c * @date Tue Jul 20 16:17:49 2010 * * @brief Functions to interact with the network sockets and NIC driver. * * */ #include "../ptpd.h" #include #include /* for MUX_PASSTHRU memory validation */ #include /** * shutdown the IPv4 multicast for specific address * * @param netPath * @param multicastAddr * * @return TRUE if successful */ static Boolean netShutdownMulticastIPv4(NetPath *netPath, Integer32 multicastAddr) { struct ip_mreq imr; /* Close General Multicast */ imr.imr_multiaddr.s_addr = multicastAddr; imr.imr_interface.s_addr = netPath->interfaceAddr.s_addr; (void)setsockopt(netPath->eventSock, IPPROTO_IP, IP_DROP_MEMBERSHIP, &imr, sizeof(struct ip_mreq)); (void)setsockopt(netPath->generalSock, IPPROTO_IP, IP_DROP_MEMBERSHIP, &imr, sizeof(struct ip_mreq)); return TRUE; } /** * shutdown the multicast (both General and Peer) * * @param netPath * * @return TRUE if successful */ static Boolean netShutdownMulticast(RunTimeOpts *rtOpts, NetPath *netPath) { DBG("netShutdownMulticast %s %d \n", netPath->ethName, netPath->ethIndex); /* Close General Multicast */ (void)netShutdownMulticastIPv4(netPath, netPath->multicastAddr); netPath->multicastAddr = 0; /* Close Peer Multicast */ (void)netShutdownMulticastIPv4(netPath, netPath->peerMulticastAddr); netPath->peerMulticastAddr = 0; return TRUE; } /* * For future use: Check if IPv4 address is multiast - * If last 4 bits of an address are 0xE (1110), it's multicast */ /* static Boolean isIpMulticast(struct in_addr in) { if((ntohl(in.s_addr) >> 28) == 0x0E ) return TRUE; return FALSE; } */ /* shut down the UDP stuff */ Boolean netShutdown(RunTimeOpts *rtOpts, NetPath *netPath) { /*netShutdownMulticast(rtOpts,netPath);*/ DBG("netShutdown %s %d\n", netPath->ethName, netPath->ethIndex); netPath->unicastAddr = 0; /* Close sockets */ if (netPath->eventSock >= 0) { (void)close(netPath->eventSock); } netPath->eventSock = -1; if (netPath->generalSock >= 0) { (void)close(netPath->generalSock); } netPath->generalSock = -1; freeIpv4AccessList(&netPath->timingAcl); freeIpv4AccessList(&netPath->managementAcl); return TRUE; } /* Check if interface ifaceName exists. Return 1 on success, 0 when interface doesn't exists, -1 on failure. */ static int interfaceExists(char *ifaceName) { int ret; struct ifaddrs *ifaddr, *ifa; if (!strlen(ifaceName)) { DBG("interfaceExists called for an empty interface!"); return 0; } if (getifaddrs(&ifaddr) == -1) { PERROR("Could not get interface list"); ret = -1; goto end; } for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { if (!strcmp(ifaceName, ifa->ifa_name)) { ret = 1; goto end; } } ret = 0; DBG("Interface not found: %s\n", ifaceName); end: freeifaddrs(ifaddr); return ret; } static int getInterfaceFlags(char *ifaceName, unsigned int *flags) { int ret; struct ifaddrs *ifaddr, *ifa; if (!strlen(ifaceName)) { DBG("interfaceExists called for an empty interface!"); return 0; } if (getifaddrs(&ifaddr) == -1) { PERROR("Could not get interface list"); ret = -1; goto end; } for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { if (!strcmp(ifaceName, ifa->ifa_name)) { *flags = ifa->ifa_flags; ret = 1; goto end; } } ret = 0; DBG("getInterfaceFlags Interface not found: %s\n", ifaceName); end: freeifaddrs(ifaddr); return ret; } /* Try getting addr address of family family from interface ifaceName. Return 1 on success, 0 when no suitable address available, -1 on failure. */ static int getInterfaceAddress(char *ifaceName, int family, struct sockaddr *addr) { int ret; struct ifaddrs *ifaddr, *ifa; if (getifaddrs(&ifaddr) == -1) { PERROR("Could not get interface list"); ret = -1; goto end; } for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { if (!strcmp(ifaceName, ifa->ifa_name) && ifa->ifa_addr->sa_family == family) { memcpy(addr, ifa->ifa_addr, sizeof(struct sockaddr)); ret = 1; goto end; } } ret = 0; DBG("getInterfaceAddress Interface not found: %s\n", ifaceName); end: freeifaddrs(ifaddr); return ret; } /* Try getting hwAddrSize bytes of ifaceName hardware address, and place them in hwAddr. Return 1 on success, 0 when no suitable hw address available, -1 on failure. */ static int getHwAddress (char *ifaceName, unsigned char *hwAddr, int hwAddrSize) { void *pCookie; char ethName[16]; int ethIndex; if (!strlen(ifaceName)) { return 0; } memset(ethName, 0, 16); strncpy(ethName, ifaceName, 3); ethIndex = atoi(ifaceName + 3); pCookie = (void *)endFindByName(ethName, ethIndex); (void)muxIoctl(pCookie, EIOCGADDR, hwAddr); return 1; } /** * check the hardware time stamp feature support or not * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * * Return 1 if hw timestamp supported, else return 0. */ Boolean netGetHwTimeStampCap (void *ethCookie) { END_CAPABILITIES hwCaps; int supported; int res; bzero ((char *) &hwCaps, sizeof (END_CAPABILITIES)); res = muxIoctl(ethCookie, EIOCGIFCAP, (void *)&hwCaps); if (res < 0) { return 0; } supported = hwCaps.cap_available & IFCAP_IEEE1588; return (supported) ? 1 : 0 ; } /** * check the hardware time stamp feature support one step or not * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * * Return 1 if hw timestamp supported, else return 0. */ Boolean netGetHwOneStepCap (void *ethCookie) { END_CAPABILITIES hwCaps; int supported; int res; bzero ((char *) &hwCaps, sizeof (END_CAPABILITIES)); res = muxIoctl(ethCookie, EIOCGIFCAP, (void *)&hwCaps); if (res < 0) { return PTP_FALSE; } supported = hwCaps.cap_available & IFCAP_IEEE1588_ONE_STEP; return (supported) ? PTP_TRUE : PTP_FALSE ; } /** * enable or disable the hardware time stamp feature * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param enable, 1 enable, 0 disable * * Return 0 on success, -1 on failure. */ int netEnableHwTimeStamp (void *ethCookie, UINT8 enable) { END_CAPABILITIES hwCaps; int enabled; int res; bzero ((char *) &hwCaps, sizeof (END_CAPABILITIES)); res = muxIoctl(ethCookie, EIOCGIFCAP, (void *)&hwCaps); if (res < 0) { return PTP_ERROR; } enabled = hwCaps.cap_enabled & IFCAP_IEEE1588; /* It has alread enabled or disabled */ if (((enabled) && (enable)) || ((!enabled) && (!enable))) { return PTP_OK; } /* enable or disable as needed */ if (enable) { hwCaps.cap_enabled |= IFCAP_IEEE1588; } else { hwCaps.cap_enabled &= ~IFCAP_IEEE1588; } res = muxIoctl(ethCookie, EIOCSIFCAP, (void *)&hwCaps); if (res < 0) { return PTP_ERROR; } else { return PTP_OK; } } /** * adjust the frequency * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param adj, the delta time to be adjust * * Return 0 on success, -1 on failure. */ int netAdjustFreq (void *ethCookie, INT32 adj) { return muxIoctl(ethCookie, EIOCS1588FREQADJ, (void *)adj); } /** * set the system time * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be set * * Return 0 on success, -1 on failure. */ int netSetSysTime (void *ethCookie, struct timespec *ts) { return muxIoctl(ethCookie, EIOCS1588SYSTIM, (void *)ts); } /** * Add the system time * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be set * * Return 0 on success, -1 on failure. */ int netSetSysTimeAdd (void *ethCookie, INT64 * counts) { return muxIoctl(ethCookie, EIOCS1588TIMEADD, (void *)counts); } /** * get the system time * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be get * * Return 0 on success, -1 on failure. */ int netGetSysTime (void *ethCookie, struct timespec *ts) { return muxIoctl(ethCookie, EIOCG1588SYSTIM, (void *)ts); } /** * get the system time interval * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be get * * Return 0 on success, -1 on failure. */ int netGetSysIntervalTime (void *ethCookie, IEEE1588_TIMER_SYNC *sync) { return muxIoctl(ethCookie, EIOCG1588SYSTIMEINTERVAL, (void *)sync); } /** * get the system time interval * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be get * * Return 0 on success, -1 on failure. */ int netSetSysIntervalTime (void *ethCookie, IEEE1588_TIMER_SYNC *sync) { return muxIoctl(ethCookie, EIOCS1588SYSTIMEINTERVAL, (void *)sync); } /** * get the system time sync delay * * @param ifaceName, the interface name, such as "gei" * @param unit, the interface unit number, such as 0, 1 * @param ts, the system time to be get * * Return 0 on success, -1 on failure. */ int netGetSysSyncDelayTime (void *ethCookie, long *delay) { return muxIoctl(ethCookie, EIOCG1588SYSTIMEDELAY, (void *)delay); } static Boolean getInterfaceInfo(char *ifaceName, InterfaceInfo *ifaceInfo) { int res; res = interfaceExists(ifaceName); if (res == -1) { return FALSE; } else if (res == 0) { (void)printf("Interface %s does not exist.\n", ifaceName); return FALSE; } res = getInterfaceAddress(ifaceName, ifaceInfo->addressFamily, &ifaceInfo->afAddress); if (res == -1) { return FALSE; } ifaceInfo->hasAfAddress = res; res = getHwAddress(ifaceName, (unsigned char *)ifaceInfo->hwAddress, 6); if (res == -1) { return FALSE; } ifaceInfo->hasHwAddress = res; res = getInterfaceFlags(ifaceName, &ifaceInfo->flags); if (res == -1) { return FALSE; } return TRUE; } /** * Init the multcast for specific IPv4 address * * @param netPath * @param multicastAddr * * @return TRUE if successful */ static Boolean netInitMulticastIPv4(NetPath *netPath, Integer32 multicastAddr) { struct ip_mreq imr; /* multicast send only on specified interface */ imr.imr_multiaddr.s_addr = multicastAddr; imr.imr_interface.s_addr = netPath->interfaceAddr.s_addr; if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_IF, &netPath->interfaceAddr, sizeof(struct in_addr)) < 0 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_IF, &netPath->interfaceAddr, sizeof(struct in_addr)) < 0) { PERROR("error while setting outgoig multicast interface " "(IP_MULTICAST_IF)"); return FALSE; } /* join multicast group (for receiving) on specified interface */ if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imr, sizeof(struct ip_mreq)) < 0 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imr, sizeof(struct ip_mreq)) < 0) { PERROR("failed to join the multicast group"); return FALSE; } return TRUE; } /** * Init the multcast (both General and Peer) * * @param netPath * @param rtOpts * * @return TRUE if successful */ static Boolean netInitMulticast(NetPath *netPath, RunTimeOpts *rtOpts) { struct in_addr netAddr; char addrStr[NET_ADDRESS_LENGTH + 1]; /* do not join multicast in unicast mode */ if (rtOpts->ip_mode == IPMODE_UNICAST) { return TRUE; } /* Init General multicast IP address */ strncpy(addrStr, DEFAULT_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH); #ifdef VXWORKS_OS if (inet_aton(addrStr, &netAddr)) { #else if (!inet_aton(addrStr, &netAddr)) #endif (void)printf("failed to encode multicast address: %s\n", addrStr); return FALSE; } /* this allows for leaving groups only if joined */ netPath->joinedMulticast = TRUE; netPath->multicastAddr = netAddr.s_addr; if (!netInitMulticastIPv4(netPath, netPath->multicastAddr)) { return FALSE; } /* End of General multicast Ip address init */ /* Init Peer multicast IP address */ strncpy(addrStr, PEER_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH); #ifdef VXWORKS_OS if (inet_aton(addrStr, &netAddr)) { #else if (!inet_aton(addrStr, &netAddr)) { #endif (void)printf("failed to encode multicast address: %s\n", addrStr); return FALSE; } netPath->peerMulticastAddr = netAddr.s_addr; if (!netInitMulticastIPv4(netPath, netPath->peerMulticastAddr)) { return FALSE; } /* End of Peer multicast Ip address init */ return TRUE; } static Boolean netSetMulticastTTL(int sockfd, int ttl) { #if defined(__OpenBSD__) || defined(__sun) uint8_t temp = (uint8_t) ttl; #else int temp = ttl; #endif if (setsockopt(sockfd, IPPROTO_IP, IP_MULTICAST_TTL, &temp, sizeof(temp)) < 0) { PERROR("Failed to set socket multicast time-to-live"); return FALSE; } return TRUE; } static Boolean netSetMulticastLoopback(NetPath *netPath, Boolean value) { int temp = value ? 1 : 0; temp = 0; DBG("Going to set multicast loopback with %d \n", temp); if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_LOOP, &temp, sizeof(temp)) < 0) { PERROR("Failed to set multicast loopback"); return FALSE; } return TRUE; } static Boolean getTxTimestamp(RunTimeOpts *rtOpts, TimeInternal *timeStamp) { int i = 0; int val = 0; struct timespec ts; if (rtOpts->hardTimeStamp == PTP_FALSE) { (void)clock_gettime(CLOCK_REALTIME, (struct timespec *)&ts); } else { for (i = 0; i < 10; i++) { (void)taskDelay(1 + i); val = muxIoctl(rtOpts->ethCookie, EIOCG1588TXTIMSTAMP, (void *)&ts); if (val == 0) { if ((rtOpts->debug_level > PTP_DEBUG_LEVEL_2) && (i > 0)) (void)printf("getTxTimestamp after retry %d time %s %lds %09ldns\n", i, rtOpts->primaryIfaceName, ts.tv_sec, ts.tv_nsec); break; } else { if (rtOpts->debug_level > PTP_DEBUG_LEVEL_2) (void)printf("getTxTimestamp fail for %d time %s\n", i + 1, rtOpts->primaryIfaceName); } } } if (i < 10) { timeStamp->nanoseconds = ts.tv_nsec; timeStamp->seconds = (int64_t)ts.tv_sec; return TRUE; } else { return FALSE; } } /** * Initialize timestamping of packets * * @param netPath * * @return TRUE if successful */ static Boolean netInitTimestamping(NetPath *netPath, RunTimeOpts *rtOpts) { int val; Boolean result = TRUE; if (rtOpts->hardTimeStamp) { val = (SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE); } else { val = (SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE); } if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPING, &val, sizeof(int)) < 0) { (void)printf("netInitTimestamping: failed to enable SO_TIMESTAMPING"); result = FALSE; } return result; } Boolean hostLookup(const char *hostname, Integer32 *addr) { if (hostname[0]) { /* Attempt a DNS lookup first. */ struct hostent *host; #ifdef VXWORKS_OS host = gethostbyname(hostname); #else host = gethostbyname2(hostname, AF_INET); #endif if (host != NULL) { if (host->h_length != 4) { PERROR("unicast host resolved to non ipv4" "address"); hostentFree(host); return FALSE; } *addr = *(uint32_t *)host->h_addr_list[0]; hostentFree(host); return TRUE; } else { struct in_addr netAddr; /* Maybe it's a dotted quad. */ #ifdef VXWORKS_OS if (inet_aton(hostname, &netAddr)) #else if (!inet_aton(hostname, &netAddr)) #endif { (void)printf("failed to encode unicast address: %s\n", hostname); return FALSE; } else { *addr = netAddr.s_addr; return TRUE; } } } return FALSE; } /** * Init all network transports * * @param netPath * @param rtOpts * @param ptpClock * * @return TRUE if successful */ Boolean netInit(NetPath *netPath, RunTimeOpts *rtOpts, PtpClock *ptpClock) { int temp; struct sockaddr_in addr; DBG("netInit\n"); netPath->generalSock = -1; netPath->eventSock = -1; if (rtOpts->transport == IEEE_802_3) { char mcastaddr[ETHER_ADDR_LEN]; struct packet_mreq { unsigned int mr_ifindex; unsigned char mr_type; unsigned char mr_alen; unsigned char mr_address[8]; } mreq; /* open socket */ if ((netPath->eventSock = socket (AF_PACKET, SOCK_DGRAM, htons (0x88F7))) < 0) { PERROR("failed to initialize PACKET socket"); return FALSE; } /* bind to specific device */ if (setsockopt (netPath->eventSock, SOL_SOCKET, SO_BINDTODEVICE, rtOpts->primaryIfaceName, IFNAMSIZ) < 0) { PERROR("failed to call SO_BINDTODEVICE on the interface"); return FALSE; } /* join multicast */ mreq.mr_ifindex = if_nametoindex (rtOpts->primaryIfaceName); mreq.mr_type = 0; /* IP_PACKET_MR_MULTICAST */ mreq.mr_alen = ETHER_ADDR_LEN; mreq.mr_address[0] = ptpClock->netPath.layer2addr.sll_addr[0]; mreq.mr_address[1] = ptpClock->netPath.layer2addr.sll_addr[1]; mreq.mr_address[2] = ptpClock->netPath.layer2addr.sll_addr[2]; mreq.mr_address[3] = ptpClock->netPath.layer2addr.sll_addr[3]; mreq.mr_address[4] = ptpClock->netPath.layer2addr.sll_addr[4]; mreq.mr_address[5] = ptpClock->netPath.layer2addr.sll_addr[5]; if (setsockopt (netPath->eventSock, 263, /* IP_SOL_PACKET */ 1, /* IP_PACKET_ADD_MEMBERSHIP */ &mreq, sizeof(mreq)) < 0) { PERROR("failed to call PACKET_ADD_MEMBERSHIP on the interface"); return FALSE; } /* make timestamps available through recvmsg() */ if (!netInitTimestamping(netPath, rtOpts)) { PERROR("Failed to enable packet time stamping\n"); return FALSE; } netPath->headerOffset = PACKET_BEGIN_ETHER; } else { netPath->headerOffset = PACKET_BEGIN_UDP; /* open sockets */ if ((netPath->eventSock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 || (netPath->generalSock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) { PERROR("failed to initialize sockets"); return FALSE; } } netPath->interfaceInfo.addressFamily = AF_INET; /* the if is here only to get rid of an unused result warning. */ if ( getInterfaceInfo(rtOpts->ifaceName, &netPath->interfaceInfo) != 1) { return FALSE; } /* No HW address, we'll use the protocol address to form interfaceID -> clockID */ if ( !netPath->interfaceInfo.hasHwAddress && netPath->interfaceInfo.hasAfAddress ) { uint32_t addr = ((struct sockaddr_in *) & (netPath->interfaceInfo.afAddress))->sin_addr.s_addr; memcpy(netPath->interfaceID, &addr, 2); memcpy(netPath->interfaceID + 4, &addr + 2, 2); /* Initialise interfaceID with hardware address */ } else { memcpy(&netPath->interfaceID, &netPath->interfaceInfo.hwAddress, sizeof(netPath->interfaceID) <= sizeof(netPath->interfaceInfo.hwAddress) ? sizeof(netPath->interfaceID) : sizeof(netPath->interfaceInfo.hwAddress) ); } DBG("Listening on IP: %s\n", inet_ntoa( ((struct sockaddr_in *) & (netPath->interfaceInfo.afAddress))->sin_addr)); if (rtOpts->transport != IEEE_802_3) { /* save interface address for IGMP refresh */ { struct sockaddr_in *sin = (struct sockaddr_in *) & (netPath->interfaceInfo.afAddress); netPath->interfaceAddr = sin->sin_addr; } DBG("Local IP address used : %s \n", inet_ntoa(netPath->interfaceAddr)); temp = 1; /* allow address reuse */ if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_REUSEADDR, &temp, sizeof(int)) < 0 || setsockopt(netPath->generalSock, SOL_SOCKET, SO_REUSEADDR, &temp, sizeof(int)) < 0) { DBG("failed to set socket reuse\n"); } /* bind sockets */ /* * need INADDR_ANY to allow receipt of multicast and uni-cast * messages */ addr.sin_addr.s_addr = htonl(INADDR_ANY); addr.sin_family = AF_INET; addr.sin_port = htons(PTP_EVENT_PORT); if (bind(netPath->eventSock, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) { PERROR("failed to bind event socket"); return FALSE; } addr.sin_port = htons(PTP_GENERAL_PORT); if (bind(netPath->generalSock, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) { PERROR("failed to bind general socket"); return FALSE; } /* Set socket dscp */ if (rtOpts->dscpValue) { if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_TOS, &rtOpts->dscpValue, sizeof(int)) < 0 || setsockopt(netPath->generalSock, IPPROTO_IP, IP_TOS, &rtOpts->dscpValue, sizeof(int)) < 0) { PERROR("Failed to set socket DSCP bits"); return FALSE; } } /* send a uni-cast address if specified (useful for testing) */ if (!hostLookup(rtOpts->unicastAddress, &netPath->unicastAddr)) { netPath->unicastAddr = 0; } if (rtOpts->ip_mode != IPMODE_UNICAST) { /* init UDP Multicast on both Default and Peer addresses */ if (!netInitMulticast(netPath, rtOpts)) { return FALSE; } /* set socket time-to-live */ if (!netSetMulticastTTL(netPath->eventSock, rtOpts->ttl) || !netSetMulticastTTL(netPath->generalSock, rtOpts->ttl)) { return FALSE; } /* start tracking TTL */ netPath->ttlEvent = rtOpts->ttl; netPath->ttlGeneral = rtOpts->ttl; } /* Reset the failure indicator when (re)starting network */ netPath->txTimestampFailure = FALSE; /* for SO_TIMESTAMPING we're receiving transmitted packets via ERRQUEUE */ temp = 0; /* make timestamps available through recvmsg() */ if (!netInitTimestamping(netPath, rtOpts)) { (void)printf("Failed to enable packet time stamping\n"); return FALSE; } /* If we failed to initialise SO_TIMESTAMPING, enable mcast loopback */ if (netPath->txTimestampFailure) { temp = 1; } if (!netSetMulticastLoopback(netPath, temp)) { return FALSE; } } /* Compile ACLs */ if (rtOpts->timingAclEnabled) { freeIpv4AccessList(&netPath->timingAcl); netPath->timingAcl = createIpv4AccessList(rtOpts->timingAclPermitText, rtOpts->timingAclDenyText, rtOpts->timingAclOrder); } if (rtOpts->managementAclEnabled) { freeIpv4AccessList(&netPath->managementAcl); netPath->managementAcl = createIpv4AccessList(rtOpts->managementAclPermitText, rtOpts->managementAclDenyText, rtOpts->managementAclOrder); } return TRUE; } /*Check if data has been received*/ int netSelect(RunTimeOpts *rtOpts, TimeInternal *timeout, NetPath *netPath, fd_set *readfds) { int ret, nfds; struct timeval tv, *tv_ptr; if (timeout) { if (isTimeInternalNegative(timeout)) { (void)printf("Negative timeout attempted for select()\n"); return -1; } tv.tv_sec = timeout->seconds; tv.tv_usec = timeout->nanoseconds / 1000; tv_ptr = &tv; } else { tv_ptr = NULL; /**************************************************************************** * netSelect() only wakes up every whole second will cause all slaves send delay * request at the samesecond which can cause problems for the master (because it * cannot get RX timestamp if packets come in to frequent). Changed to * 100 ms and now it works. By Markus. */ tv.tv_sec = 0; /*added by Hui*/ tv.tv_usec = 100000; tv_ptr = &tv; } FD_ZERO(readfds); nfds = 0; FD_SET(netPath->eventSock, readfds); if (netPath->generalSock >= 0) { FD_SET(netPath->generalSock, readfds); } nfds = netPath->eventSock; if (netPath->eventSock < netPath->generalSock) { nfds = netPath->generalSock; } nfds++; ret = select(nfds, readfds, 0, 0, tv_ptr); if (ret < 0) { if (errno == EAGAIN || errno == EINTR) { return 0; } } return ret; } /** * store received data from network to "buf" , get and store the * SO_TIMESTAMP value in "time" for an event message * * @note Should this function be merged with netRecvGeneral(), below? * Jan Breuer: I think that netRecvGeneral should be * simplified. Timestamp returned by this function is never * used. According to this, netInitTimestamping can be also simplified * to initialize timestamping only on eventSock. * * @param buf * @param time * @param netPath * * @return */ ssize_t netRecvEvent(Octet *buf, TimeInternal *time, NetPath *netPath, int flags) { ssize_t ret = 0; struct msghdr msg; struct iovec vec[1]; struct sockaddr_in from_addr; union { struct cmsghdr cm; char control[256]; } cmsg_un; struct cmsghdr *cmsg; TimeInternal *tv; Boolean timestampValid = FALSE; vec[0].iov_base = buf; vec[0].iov_len = PACKET_SIZE; memset(&msg, 0, sizeof(msg)); memset(&from_addr, 0, sizeof(from_addr)); memset(buf, 0, PACKET_SIZE); memset(&cmsg_un, 0, sizeof(cmsg_un)); msg.msg_name = (caddr_t)&from_addr; msg.msg_namelen = sizeof(from_addr); msg.msg_iov = vec; msg.msg_iovlen = 1; msg.msg_control = cmsg_un.control; msg.msg_controllen = sizeof(cmsg_un.control); msg.msg_flags = 0; ret = recvmsg(netPath->eventSock, &msg, flags | MSG_DONTWAIT); if (ret <= 0) { if (errno == EAGAIN || errno == EINTR) { return 0; } return ret; }; if (msg.msg_flags & MSG_TRUNC) { (void)printf("received truncated message\n"); return 0; } /* get time stamp of packet */ if (!time) { (void)printf("null receive time stamp argument\n"); return 0; } if (msg.msg_flags & MSG_CTRUNC) { (void)printf("received truncated ancillary data\n"); return 0; } netPath->lastRecvAddr = from_addr.sin_addr.s_addr; netPath->receivedPackets++; if (msg.msg_controllen <= 0) { (void)printf("received short ancillary data (%ld/%ld)\n", (long)msg.msg_controllen, (long)sizeof(cmsg_un.control)); return 0; } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == SOL_SOCKET) { if (cmsg->cmsg_type == SO_TIMESTAMP) { tv = (struct TimeInternal *)CMSG_DATA(cmsg); time->seconds = tv->seconds; time->nanoseconds = tv->nanoseconds; timestampValid = TRUE; } } } if (!timestampValid) { /* * do not try to get by with recording the time here, better * to fail because the time recorded could be well after the * message receive, which would put a big spike in the * offset signal sent to the clock servo */ DBG("netRecvEvent: no receive time stamp\n"); return 0; } return ret; } /** * * store received data from network to "buf" get and store the * SO_TIMESTAMP value in "time" for a general message * * @param buf * @param time * @param netPath * * @return */ ssize_t netRecvGeneral(Octet *buf, NetPath *netPath) { ssize_t ret = 0; struct sockaddr_in from_addr; socklen_t from_addr_len = sizeof(from_addr); ret = recvfrom(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT, (struct sockaddr *)&from_addr, &from_addr_len); netPath->lastRecvAddr = from_addr.sin_addr.s_addr; return ret; } ssize_t netSendEvent(Octet *buf, UInteger16 length, PtpClock *ptpClock , RunTimeOpts *rtOpts, Integer32 alt_dst, TimeInternal *tim) { ssize_t ret; NetPath *netPath = &ptpClock->netPath; if (rtOpts->transport == IEEE_802_3 ) { ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&netPath->layer2addr, sizeof(netPath->layer2addr)); if (rtOpts->debug_level > PTP_DEBUG_LEVEL_2) (void)printf("netPath->txTimestampFailure %d \n", netPath->txTimestampFailure); if (ptpClock->twoStepFlag == TWO_STEP_FLAG) { if (!getTxTimestamp(rtOpts, tim)) { (void)printf("Get Tx timestamp fail\n"); if (tim) { clearTime(tim); } netPath->txTimestampFailure = TRUE; } else { netPath->txTimestampFailure = FALSE; } } if (ret <= 0) { DBG("Error sending ether multicast event message\n"); } else { netPath->sentPackets++; } } else { struct sockaddr_in addr; addr.sin_family = AF_INET; addr.sin_port = htons(PTP_EVENT_PORT); if (netPath->unicastAddr || alt_dst ) { if (netPath->unicastAddr) { addr.sin_addr.s_addr = netPath->unicastAddr; } else { addr.sin_addr.s_addr = alt_dst; } /* * This function is used for PTP only anyway... * If we're sending to a unicast address, set the UNICAST flag. */ *(char *)(buf + 6) |= PTP_UNICAST; ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending unicast event message\n"); } else { netPath->sentPackets++; } if (!getTxTimestamp(rtOpts, tim)) { netPath->txTimestampFailure = TRUE; if (tim) { clearTime(tim); } } else { netPath->txTimestampFailure = FALSE; } if (netPath->txTimestampFailure) { /* We've had a TX timestamp receipt timeout - falling back to packet looping */ addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr; ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error looping back unicast event message\n"); } } } else { addr.sin_addr.s_addr = netPath->multicastAddr; /* Is TTL OK? */ if (netPath->ttlEvent != rtOpts->ttl) { /* Try restoring TTL */ /* set socket time-to-live */ if (netSetMulticastTTL(netPath->eventSock, rtOpts->ttl)) { netPath->ttlEvent = rtOpts->ttl; } } ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending multicast event message\n"); } else { netPath->sentPackets++; } if (!getTxTimestamp(rtOpts, tim)) { (void)printf("try to get hard timestamp fail\n"); if (tim) { clearTime(tim); } netPath->txTimestampFailure = TRUE; /* Try re-enabling MULTICAST_LOOP */ (void)netSetMulticastLoopback(netPath, TRUE); } else { netPath->txTimestampFailure = FALSE; } } } return ret; } ssize_t netSendGeneral(Octet *buf, UInteger16 length, NetPath *netPath, RunTimeOpts *rtOpts, Integer32 alt_dst) { ssize_t ret; if (rtOpts->transport == IEEE_802_3) { ret = sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&netPath->layer2addr, sizeof(netPath->layer2addr)); if (ret <= 0) { DBG("Error sending ether multicast general message\n"); } else { netPath->sentPackets++; } } else { struct sockaddr_in addr; addr.sin_family = AF_INET; addr.sin_port = htons(PTP_GENERAL_PORT); if (netPath->unicastAddr || alt_dst ) { if (netPath->unicastAddr) { addr.sin_addr.s_addr = netPath->unicastAddr; } else { addr.sin_addr.s_addr = alt_dst; } /* * This function is used for PTP only anyway... * If we're sending to a unicast address, set the UNICAST flag. */ *(char *)(buf + 6) |= PTP_UNICAST; ret = sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending unicast general message\n"); } else { netPath->sentPackets++; } } else { addr.sin_addr.s_addr = netPath->multicastAddr; /* Is TTL OK? */ if (netPath->ttlGeneral != rtOpts->ttl) { /* Try restoring TTL */ if (netSetMulticastTTL(netPath->generalSock, rtOpts->ttl)) { netPath->ttlGeneral = rtOpts->ttl; } } ret = sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending multicast general message\n"); } else { netPath->sentPackets++; } } } return ret; } ssize_t netSendPeerGeneral(Octet *buf, UInteger16 length, NetPath *netPath, RunTimeOpts *rtOpts) { ssize_t ret; struct sockaddr_in addr; addr.sin_family = AF_INET; addr.sin_port = htons(PTP_GENERAL_PORT); if (rtOpts->transport == IEEE_802_3) { ret = sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&netPath->layer2addr, sizeof(netPath->layer2addr)); if (ret <= 0) { DBG("Error sending peer general message\n"); } else { netPath->sentPackets++; } } else { if (netPath->unicastAddr) { addr.sin_addr.s_addr = netPath->unicastAddr; ret = sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending unicast peer general message\n"); } } else { addr.sin_addr.s_addr = netPath->peerMulticastAddr; /* is TTL already 1 ? */ if (netPath->ttlGeneral != 1) { /* Try setting TTL to 1 */ if (netSetMulticastTTL(netPath->generalSock, 1)) { netPath->ttlGeneral = 1; } } ret = sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending multicast peer general message\n"); } } } if (ret > 0) { netPath->sentPackets++; } return ret; } ssize_t netSendPeerEvent(Octet *buf, UInteger16 length, PtpClock *ptpClock , RunTimeOpts *rtOpts, TimeInternal *tim) { ssize_t ret; NetPath *netPath = &ptpClock->netPath; if (rtOpts->transport == IEEE_802_3 ) { ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&netPath->layer2addr, sizeof(netPath->layer2addr)); if (rtOpts->debug_level > PTP_DEBUG_LEVEL_2) (void)printf("netPath->txTimestampFailure %d \n", netPath->txTimestampFailure); if (ptpClock->twoStepFlag == TWO_STEP_FLAG) { if (!getTxTimestamp(rtOpts, tim)) { (void)printf("Peer Get Tx timestamp fail\n"); if (tim) { clearTime(tim); } netPath->txTimestampFailure = TRUE; } else { netPath->txTimestampFailure = FALSE; } } if (ret <= 0) { DBG("Error sending ether multicast event message\n"); } else { netPath->sentPackets++; } } else { struct sockaddr_in addr; addr.sin_family = AF_INET; addr.sin_port = htons(PTP_EVENT_PORT); if (netPath->unicastAddr) { addr.sin_addr.s_addr = netPath->unicastAddr; ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending unicast peer event message\n"); } else { netPath->sentPackets++; } if (!getTxTimestamp(rtOpts, tim)) { netPath->txTimestampFailure = TRUE; if (tim) { clearTime(tim); } } else { netPath->txTimestampFailure = FALSE; } if (netPath->txTimestampFailure) { /* We've had a TX timestamp receipt timeout - falling back to packet looping */ addr.sin_addr.s_addr = netPath->interfaceAddr.s_addr; ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error looping back unicast event message\n"); } } } else { addr.sin_addr.s_addr = netPath->peerMulticastAddr; /* is TTL already 1 ? */ if (netPath->ttlEvent != 1) { /* Try setting TTL to 1 */ if (netSetMulticastTTL(netPath->eventSock, 1)) { netPath->ttlEvent = 1; } } ret = sendto(netPath->eventSock, buf, length, 0x400/*IP_MSG_TX_TIMESTAMP*/, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret <= 0) { DBG("Error sending multicast peer event message\n"); } else { netPath->sentPackets++; } if (!getTxTimestamp(rtOpts, tim)) { if (tim) { clearTime(tim); } netPath->txTimestampFailure = TRUE; /* Try re-enabling MULTICAST_LOOP */ (void)netSetMulticastLoopback(netPath, TRUE); } else { netPath->txTimestampFailure = FALSE; } } } if (ret > 0) { netPath->sentPackets++; } return ret; } /* * refresh IGMP on a timeout */ /* * @return TRUE if successful */ Boolean netRefreshIGMP(NetPath *netPath, RunTimeOpts *rtOpts, PtpClock *ptpClock) { DBG("netRefreshIGMP\n"); if (netPath->joinedMulticast) { (void)netShutdownMulticast(rtOpts, netPath); } /* suspend process 100 milliseconds, to make sure the kernel sends the IGMP_leave properly */ (void)usleep(100 * 1000); if (!netInitMulticast(netPath, rtOpts)) { return FALSE; } return TRUE; }