1892 lines
46 KiB
C
1892 lines
46 KiB
C
/*-
|
|
* 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 <endCommon.h>
|
|
#include <muxLib.h> /* for MUX_PASSTHRU memory validation */
|
|
|
|
#include <ipcom_netif.h>
|
|
|
|
|
|
/**
|
|
* 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;
|
|
}
|