/*-
 * 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.
 */

/**
 * @file   net.c
 * @date   Tue Jul 20 16:17:49 2010
 *
 * @brief  Functions to interact with the network sockets and NIC driver.
 *
 */
// #include <cr_section_macros.h>

#include "../ptpd.h"

#if defined(NXPIMXRT1050)
#include "lwipopts.h"

#include "lwip/netif.h"
#include "lwip/netdb.h"
#include "lwip/sockets.h"
#include "enet_mac_if1.h"     // RT ETHNET driver provides the time function

#if defined(ETH_802_3_PTP_SUPPORT)
#include "ethernet.h"        // for struct eth_hdr
#include "netif/ethernet.h"  // for ethernet_output()
#include "lwip/tcpip.h"           // for tcpip_trycallback()
#include "sav_init.h"        // for call back

#define DEFAULT_PTP_RECVMBOX_SIZE 5  // 5 ptrs to msg pbufs

sys_mbox_t ptpMboxHandle = NULL;  // or void * ptpMboxHandle;
sys_mutex_t lock_ptp_core;

#endif  // ETH_802_3_PTP_SUPPORT
#endif  // NXPIMXRT1050

// jem - These should be in net/if.h (if it existed in lwIP...)
#define IFF_UP 0x01
#define IFF_LOOPBACK 0x08
#define IFF_RUNNING 0x40
#define IFF_MULTICAST 0x1000

/* choose kernel-level nanoseconds or microseconds resolution on the client-side */
#if !defined(SO_TIMESTAMPNS) && !defined(SO_TIMESTAMP) && !defined(SO_BINTIME) && !defined(PTP_ENABLED)
#error kernel-level timestamps not detected
#endif

#define netGetDefaultNetif() (netif_default)

const static Octet ptpDefaultHwAddress[] = {0x01, 0x1b, 0x19, 0x00, 0x00, 0x00};
const static Octet ptpPeerHwAddress[] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e};

// __RAMFUNC(RAM3)
static void netPackPtpData(Octet * buf, enet_ptp_time_data_t *pptpTimeData)
{
	pptpTimeData->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
	pptpTimeData->sequenceId = flip16(*(UInteger16 *) (buf + 30));
	pptpTimeData->version = (*(UInteger4 *) (buf + 1)) & 0x0F;
	memcpy(pptpTimeData->sourcePortId, (buf + 20), 10);
/*
	// For test
	pptpTimeData->timeStamp.second = flip32(*(UInteger32 *) (buf + 36));
	pptpTimeData->timeStamp.nanosecond = flip32(*(UInteger32 *) (buf + 40));
*/
}

/**
 * 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;

	setsockopt(netPath->eventSock, IPPROTO_IP, IP_DROP_MEMBERSHIP, 
		   &imr, sizeof(struct ip_mreq));
	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(NetPath * netPath)
{
	/* Close General Multicast */
	netShutdownMulticastIPv4(netPath, netPath->multicastAddr);
	netPath->multicastAddr = 0;

	/* Close Peer Multicast */
	netShutdownMulticastIPv4(netPath, netPath->peerMulticastAddr);
	netPath->peerMulticastAddr = 0;
	
	return TRUE;
}

/* shut down the UDP stuff */
Boolean 
netShutdown(NetPath * netPath)
{
	netShutdownMulticast(netPath);

	netPath->unicastAddr = 0;

	/* Close sockets */
	if (netPath->eventSock > 0)
		close(netPath->eventSock);
	netPath->eventSock = -1;

	if (netPath->generalSock > 0)
		close(netPath->generalSock);
	netPath->generalSock = -1;

	return TRUE;
}


Boolean
chooseMcastGroup(RunTimeOpts * rtOpts, struct in_addr *netAddr)
{

	char *addrStr;

#ifdef PTP_EXPERIMENTAL
	switch(rtOpts->mcast_group_Number){
	case 0:
		addrStr = DEFAULT_PTP_DOMAIN_ADDRESS;
		break;

	case 1:
		addrStr = ALTERNATE_PTP_DOMAIN1_ADDRESS;
		break;
	case 2:
		addrStr = ALTERNATE_PTP_DOMAIN2_ADDRESS;
		break;
	case 3:
		addrStr = ALTERNATE_PTP_DOMAIN3_ADDRESS;
		break;

	default:
		ERROR("Unk group %d\n", rtOpts->mcast_group_Number);
		exit(3);
		break;
	}
#else
	addrStr = DEFAULT_PTP_DOMAIN_ADDRESS;
#endif

	if (!inet_aton(addrStr, netAddr)) {
		ERROR("failed to encode multicast address: %s\n", addrStr);
		return FALSE;
	}
	return TRUE;
}


 /* Find the local network interface */
UInteger32 
findIface(Octet * ifaceName, UInteger8 * communicationTechnology,
    Octet * uuid, NetPath * netPath)
{

#if defined(NXPIMXRT1050)
    struct netif * iface;
    (void) communicationTechnology;
    (void) netPath;
    iface = netGetDefaultNetif();
	
    memcpy(uuid, iface->hwaddr, iface->hwaddr_len);
    memcpy(ifaceName, iface->name, sizeof (iface->name));

#if !LWIP_IPV6
    return iface->ip_addr.addr;
#else
    return 0;
#endif



#else /* usually *BSD */

	struct ifaddrs *if_list, *ifv4, *ifh;

	if (getifaddrs(&if_list) < 0) {
		PERROR("getifaddrs() failed");
		return FALSE;
	}
	/* find an IPv4, multicast, UP interface, right name(if supplied) */
	for (ifv4 = if_list; ifv4 != NULL; ifv4 = ifv4->ifa_next) {
		if ((ifv4->ifa_flags & IFF_UP) == 0)
			continue;
		if ((ifv4->ifa_flags & IFF_RUNNING) == 0)
			continue;
		if ((ifv4->ifa_flags & IFF_LOOPBACK))
			continue;
		if ((ifv4->ifa_flags & IFF_MULTICAST) == 0)
			continue;
                /* must have IPv4 address */
		if (ifv4->ifa_addr->sa_family != AF_INET)
			continue;
		if (ifaceName[0] && strncmp(ifv4->ifa_name, ifaceName, 
					    IF_NAMESIZE) != 0)
			continue;
		break;
	}

	if (ifv4 == NULL) {
		if (ifaceName[0]) {
			ERROR("interface \"%s\" does not exist,"
			      "or is not appropriate\n", ifaceName);
			return FALSE;
		}
		ERROR("no suitable interfaces found!");
		return FALSE;
	}
	/* find the AF_LINK info associated with the chosen interface */
	for (ifh = if_list; ifh != NULL; ifh = ifh->ifa_next) {
		if (ifh->ifa_addr->sa_family != AF_LINK)
			continue;
		if (strncmp(ifv4->ifa_name, ifh->ifa_name, IF_NAMESIZE) == 0)
			break;
	}

	if (ifh == NULL) {
		ERROR("could not get hardware address for interface \"%s\"\n", 
		      ifv4->ifa_name);
		return FALSE;
	}
	/* check that the interface TYPE is OK */
	if (((struct sockaddr_dl *)ifh->ifa_addr)->sdl_type != IFT_ETHER) {
		ERROR("\"%s\" is not an ethernet interface!\n", ifh->ifa_name);
		return FALSE;
	}
//	DBG("==> %s %s %s\n", ifv4->ifa_name,
//	    inet_ntoa(((struct sockaddr_in *)ifv4->ifa_addr)->sin_addr),
//	    ether_ntoa((struct ether_addr *)
//		       LLADDR((struct sockaddr_dl *)ifh->ifa_addr))
//	    );

	*communicationTechnology = PTP_ETHER;
	memcpy(ifaceName, ifh->ifa_name, IFACE_NAME_LENGTH);
	memcpy(uuid, LLADDR((struct sockaddr_dl *)ifh->ifa_addr), 
	       PTP_UUID_LENGTH);

	return ((struct sockaddr_in *)ifv4->ifa_addr)->sin_addr.s_addr;

#endif
}


#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)
// Used by netInit_802_3_ptp()
uint8_t xx_hwAddr[6];
static Boolean getInterfaceInfo(char* ifaceName, InterfaceInfo* ifaceInfo)
{
	int i;

	const uint8_t *hwAddr = NULL;
    (void) ifaceName;  // Not used. This is one of the rtOpts and is not needed for the IMXRT1050 port.
//    struct netif * iface;

	ifaceInfo->ifIndex = 0;
    ifaceInfo->hasAfAddress = 1;
    ifaceInfo->hasHwAddress = 1;

//    iface = netGetDefaultNetif();   // jem - the NXP port #defines this as netif_default at the top of the file. Replace?
    hwAddr = netGetHwAddr();
    ifaceInfo->afAddress.sa_family = AF_INET;

#if 0	// FIXME: NickP: this was yielding a warning, has this routine been left in a final state?
    struct sockaddr_in *inet_sock_p = (struct sockaddr_in*) &(ifaceInfo->afAddress);  // Cast to IPv4 type socket
#endif
//    inet_sock_p->sin_addr.s_addr = iface->ip_addr.addr; // Copy address. As not true socket, port number is not set.


    memcpy(ifaceInfo->hwAddress, hwAddr, 6);  // len should be 6
//    memcpy(ifaceInfo->hwAddress, iface->hwaddr, iface->hwaddr_len);  // len should be 6

    ifaceInfo->flags = (IFF_UP | IFF_RUNNING | IFF_MULTICAST);  // TODO jem - may need this to reflect if the interface is actually up and running.
    for(i=0; i<=5; i++)
    	xx_hwAddr[i] = hwAddr[i];

//    jem - I know the interface name should be passed to this function, but this shows where it is in FreeRTOS if you need it.
//    memcpy(ifaceName, iface->name, sizeof (iface->name));

    return TRUE;
}
#endif


/**
 * 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, 
		       &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0
	    || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_IF, 
			  &imr.imr_interface.s_addr, sizeof(struct in_addr)) 
	    < 0) {
		PERROR("failed to enable multi-cast on the interface");
		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 multi-cast group");
		return FALSE;
	}
	return TRUE;
}

/**
 * Init the multcast (both General and Peer)
 * 
 * @param netPath 
 * @param rtOpts 
 * 
 * @return TRUE if successful
 */
Boolean
netInitMulticast(NetPath * netPath,  RunTimeOpts * rtOpts)
{
	struct in_addr netAddr;
	char addrStr[NET_ADDRESS_LENGTH];
	
	/* Init General multicast IP address */
	if(!chooseMcastGroup(rtOpts, &netAddr)){
		return FALSE;
	}
	netPath->multicastAddr = netAddr.s_addr;
	if(!netInitMulticastIPv4(netPath, netPath->multicastAddr)) {
		return FALSE;
	}
	/* End of General multicast Ip address init */


	/* Init Peer multicast IP address */
	memcpy(addrStr, PEER_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);

	if (!inet_aton(addrStr, &netAddr)) {
		ERROR("failed to encode multi-cast 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;
}



#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)

// Used by netSend_802_3_PtpEthMsg_Callback()
// __RAMFUNC(RAM3)
static Boolean getTxTimestamp(Octet *buf, NetPath* netPath,TimeInternal* timeStamp)
{

    // jem - Use NXP enet driver to fetch the tx timestamp
    // This code is copied from the NXP port based on v2.2.2, originally in netSendEvent()

    enet_ptp_time_data_t ptpTimeData;

    netPath->txTimestampFailure = FALSE;
    getTime(timeStamp); /* Read the current time used for timestamp in case reading from driver fails. */

    netPackPtpData(buf, &ptpTimeData);

    if(!enet_get_txframe_time(&ptpTimeData)){
        /* return the timestamp gotten from driver */
        timeStamp->nanoseconds = ptpTimeData.timeStamp.nanosecond;
        timeStamp->seconds = (Integer32)ptpTimeData.timeStamp.second;
    } else {
        /* suspend process 1 millisecond to make sure current frame was sent by enet */
        milliSleep(1);
        /* Try to read timestamp again */
        if(!enet_get_txframe_time(&ptpTimeData)){
        /* return the timestamp gotten from driver */
            timeStamp->nanoseconds = ptpTimeData.timeStamp.nanosecond;
            timeStamp->seconds = (Integer32)ptpTimeData.timeStamp.second;
        }
        else
        {
            WARNING("getTxTimestamp failure - reverting to local time value\n");
            return FALSE;
        }
    }

    return TRUE;
}
#endif


#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)
/**
 * Initialize timestamping of packets
 *
 * @param netPath 
 * 
 * @return TRUE if successful
 */
Boolean 
netInitTimestamping(NetPath * netPath)
{
//	int val = 1;
	Boolean result = TRUE;
	
#if defined(SO_TIMESTAMPNS) /* Linux, Apple */
	DBG("netInitTimestamping: trying to use SO_TIMESTAMPNS\n");
	
	if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0
	    || setsockopt(netPath->generalSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0) {
		PERROR("netInitTimestamping: failed to enable SO_TIMESTAMPNS");
		result = FALSE;
	}
#elif defined(SO_BINTIME) /* FreeBSD */
	DBG("netInitTimestamping: trying to use SO_BINTIME\n");
		
	if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_BINTIME, &val, sizeof(int)) < 0
	    || setsockopt(netPath->generalSock, SOL_SOCKET, SO_BINTIME, &val, sizeof(int)) < 0) {
		PERROR("netInitTimestamping: failed to enable SO_BINTIME");
		result = FALSE;
	}
#elif defined(NXPIMXRT1050)
	result = TRUE;
#else

	result = FALSE;
#endif
			
/* fallback method */
#if defined(SO_TIMESTAMP) /* Linux, Apple, FreeBSD */
	if (!result) {
		DBG("netInitTimestamping: trying to use SO_TIMESTAMP\n");
		
		if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMP, &val, sizeof(int)) < 0
		    || setsockopt(netPath->generalSock, SOL_SOCKET, SO_TIMESTAMP, &val, sizeof(int)) < 0) {
			PERROR("netInitTimestamping: failed to enable SO_TIMESTAMP");
			result = FALSE;
		}
		result = TRUE;
	}
#endif

	return result;
}
#endif


/**
 * start all of the UDP stuff 
 * must specify 'subdomainName', and optionally 'ifaceName', 
 * if not then pass ifaceName == "" 
 * on socket options, see the 'socket(7)' and 'ip' man pages 
 *
 * @param netPath 
 * @param rtOpts 
 * @param ptpClock 
 * 
 * @return TRUE if successful
 */
Boolean 
netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock)
{
	int temp;
	struct in_addr interfaceAddr, netAddr;
	struct sockaddr_in addr;

//	DBG("netInit\n");

	/* 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 initalize sockets");
		return FALSE;
	}
	/* find a network interface */
	if (!(interfaceAddr.s_addr = 
	      findIface(rtOpts->ifaceName, 
			&ptpClock->port_communication_technology,
			ptpClock->port_uuid_field, netPath)))
		return FALSE;

	/* save interface address for IGMP refresh */
	netPath->interfaceAddr = interfaceAddr;
	
//	DBG("Local IP address used : %s \n", inet_ntoa(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");
		return FALSE;
	}
	/* bind sockets */
	/*
	 * need INADDR_ANY to allow receipt of multi-cast and uni-cast
	 * messages
	 */
	addr.sin_family = AF_INET;
	addr.sin_addr.s_addr = htonl(INADDR_ANY);
	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;
	}

#if 1
	/* send a uni-cast address if specified (useful for testing) */
	if (rtOpts->unicastAddress[0]) {
		/* Attempt a DNS lookup first. */
		struct hostent *host;
		host = gethostbyname(rtOpts->unicastAddress);
                if (host != NULL) {
			if (host->h_length != 4) {
				PERROR("unicast host resolved to non ipv4"
				       "address");
				return FALSE;
			}
			netPath->unicastAddr = 
				*(uint32_t *)host->h_addr_list[0];
		} else {
			/* Maybe it's a dotted quad. */
			if (!inet_aton(rtOpts->unicastAddress, &netAddr)) {
				ERROR("failed to encode uni-cast address: %s\n",
				      rtOpts->unicastAddress);
				return FALSE;
				//netPath->unicastAddr = netAddr.s_addr;
			}
                }
	} else {
                netPath->unicastAddr = 0;
	}
#endif

	/* init UDP Multicast on both Default and Pear addresses */
	if (!netInitMulticast(netPath, rtOpts)) {
		return FALSE;
	}

	/* set socket time-to-live to 1 */

	if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_TTL, 
		       &rtOpts->ttl, sizeof(int)) < 0
	    || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_TTL, 
			  &rtOpts->ttl, sizeof(int)) < 0) {
		PERROR("failed to set the multi-cast time-to-live");
		return FALSE;
	}

	/* enable loopback */
	temp = 1;

	DBG("Going to set IP_MULTICAST_LOOP with %d \n", temp);

	if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_LOOP, 
		       &temp, sizeof(int)) < 0
	    || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_LOOP, 
			  &temp, sizeof(int)) < 0) {
		PERROR("failed to enable multi-cast loopback");
		return FALSE;
	}

#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)
	/* make timestamps available through recvmsg() */
	if (!netInitTimestamping(netPath)) {
		ERROR("failed to enable receive time stamps");
		return FALSE;
	}
#endif

	return TRUE;
}



#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)
/*
 * Check the incoming ptp mailbox for a message.
 * Will return true if there is one or more waiting.
 *
 * Similar to select() as it returns the number of ptp type ethernet messages waiting to be processed.
 * If block param is non zero will suspend thread until a msg is available.
 */
int netSelectPtpEthMessage(unsigned int block)
{
    uint32_t *msgBuff = NULL;

    if (block > 0)
    {
        block = portMAX_DELAY;
    }

    BaseType_t res = xQueuePeek(ptpMboxHandle, &msgBuff, block); // Would rather not use a FreeRTOS api directly but
                                                                                 // lwIP does not provide a sys_arch function for this.
    if (res == pdPASS)
    {
        return 1;
    }

    if (res == errQUEUE_EMPTY)
    {
        return 0;
    }

    return -1;
}
#endif // NXPIMXRT1050 && ETH_802_3_PTP_SUPPORT




/*Check if data have been received*/
int 
netSelect(TimeInternal * timeout, NetPath * netPath)
{
	int ret, nfds;
	fd_set readfds;
	struct timeval tv, *tv_ptr;

	if (timeout < 0)
		return FALSE;

	FD_ZERO(&readfds);
	FD_SET(netPath->eventSock, &readfds);
	FD_SET(netPath->generalSock, &readfds);

	if (timeout) {
		tv.tv_sec = timeout->seconds;
		tv.tv_usec = timeout->nanoseconds / 1000;
		tv_ptr = &tv;
	} else
		tv_ptr = 0;

	if (netPath->eventSock > netPath->generalSock)
		nfds = netPath->eventSock;
	else
		nfds = netPath->generalSock;

	ret = select(nfds + 1, &readfds, 0, 0, tv_ptr) > 0;

	if (ret < 0) {
		if (errno == EAGAIN || errno == EINTR)
			return 0;
	}
	return ret;
}



#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)
/*
 * Set the passed param to the handle for the incoming ptp frames mailbox.
 */
err_t netGetPtpMailboxHandle(sys_mbox_t *handle)
{
    if (ptpMboxHandle == NULL)
    {
        return ERR_VAL;
    }
    else
    {
        *handle = ptpMboxHandle;
        return ERR_OK;
    }
}

/*
 * Initialise objects that are used for IEEE 802.3 Ethernet frame transmission and reception
 */
err_t netInit_802_3_Ptp(NetPath * netPath, RunTimeOpts * rtOpts)
{
//    const static Octet ptpDestHwAddress[] = {0x01, 0x1b, 0x19, 0x00, 0x00, 0x00};
    err_t ret = ERR_OK;

    (void) getInterfaceInfo(rtOpts->ifaceName, &netPath->interfaceInfo);

	// If no HW address then 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);
    }
    else
    {
        // Initialise interfaceID with our hardware address
        memcpy(&netPath->interfaceID, &netPath->interfaceInfo.hwAddress,
			   sizeof(netPath->interfaceID) <= sizeof(netPath->interfaceInfo.hwAddress) ?
               sizeof(netPath->interfaceID) : sizeof(netPath->interfaceInfo.hwAddress));
	}

    memcpy(&netPath->etherDest, ptpDefaultHwAddress, 6);

    if (ptpMboxHandle == NULL)
    {
        // Create the mailbox that contains pointers to incoming ethernet messages.
        ret = sys_mbox_new(&ptpMboxHandle, DEFAULT_PTP_RECVMBOX_SIZE);
        if (ret != ERR_OK)
        {
            ERROR("Failed to create PTP message mailbox\n");
            return ret;
        }
    }

    // Create and take the semaphore that's used to signal when Ethernet frame transmit callback is complete.
    ret = sys_mutex_new(&lock_ptp_core);
    if(ret != ERR_OK)
    {
        ERROR("Failed to create lock_ptp_core\n");

    }
    return ret;
}

/*
 * Get IEEE 802.3 ptp Ethernet frame and its timestamp
 */
// volatile TimeInternal xxTime;
// volatile ssize_t xx_test;
// __RAMFUNC(RAM3)
ssize_t netRecvPtpEthFrame(Octet * buf, TimeInternal * time)
{
	ssize_t ret;
	TimeInternal tempTime;
#if defined(SO_TIMESTAMPING)
	enet_ptp_time_data_t ptpTimeData;
#endif

	if (!buf)
    {
        ERROR("null receive buf argument\n");
        return 0;
	}

	if (!time)
    {
        ERROR("null receive time stamp argument\n");
        return 0;
	}

    // Read the current time used for time stamp in case reading from driver failed or software time stamping in use.
//    getTime(time);
    getTime(&tempTime);

    struct pbuf *pbuf_p = NULL;  // the mailbox msg is a pointer to a pbuf that contains the rx frame.

    // Return immediately if nothing waiting.
    if (sys_arch_mbox_tryfetch(&ptpMboxHandle, (void **)(&pbuf_p)) == SYS_MBOX_EMPTY)
    {
        return 0;
    }

    // If here then we have the ptr to the frame.
    // copy ptp message into our buffer and free the pbuf memory.
	sys_mutex_lock(&lock_ptp_core);
    // Copy the received Ethernet message minus its header.
    memcpy(buf, pbuf_p->payload + sizeof(struct eth_hdr), pbuf_p->len - sizeof(struct eth_hdr));
    ret = pbuf_p->len - sizeof(struct eth_hdr);
    pbuf_free(pbuf_p);
	sys_mutex_unlock(&lock_ptp_core);

#if defined(SO_TIMESTAMPING)
    // Get hardware time stamp of packet from enet driver
    // Only event type messages are timestamped (sync, delay req, pdelay request & pdelay response)

    netPackPtpData(buf, &ptpTimeData);

/* For debug
    if(ptpTimeData.messageType == SYNC)
    {
//    	xxTime.nanoseconds = ptpTimeData.timeStamp.nanosecond;
//    	xxTime.seconds = (Integer32)ptpTimeData.timeStamp.second;
    	xx_test = 0;
    }

    else if(ptpTimeData.messageType == FOLLOW_UP)
    {
//    	xxTime.nanoseconds = ptpTimeData.timeStamp.nanosecond;
//    	xxTime.seconds = (Integer32)ptpTimeData.timeStamp.second;
    	xx_test = 8;
    }

    else if(ptpTimeData.messageType == ANNOUNCE)
    {
//    	xxTime.nanoseconds = ptpTimeData.timeStamp.nanosecond;
//    	xxTime.seconds = (Integer32)ptpTimeData.timeStamp.second;
    	xx_test = 11;
    }

    else if(ptpTimeData.messageType == PDELAY_REQ)
    {
//    	xxTime.nanoseconds = ptpTimeData.timeStamp.nanosecond;
//    	xxTime.seconds = (Integer32)ptpTimeData.timeStamp.second;
    	xx_test = 2;
    }
*/

    if (ptpTimeData.messageType == SYNC ||
        ptpTimeData.messageType == DELAY_REQ ||
        ptpTimeData.messageType == PDELAY_REQ ||
        ptpTimeData.messageType == PDELAY_RESP)
    {

        if(!enet_get_rxframe_time(&ptpTimeData))
        {
            time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
            time->seconds = (Integer32)ptpTimeData.timeStamp.second;
        }
        else
        {
            *time = tempTime;
            WARNING("Failed to get rx frame timestamp - using local ptp time\n");
        }
    }
#else
    *time = tempTime;
#endif

    // Just for test
//    xxTime.nanoseconds = time->nanoseconds;
//    xxTime.seconds = time->seconds;
    return ret;
}
#endif // NXPIMXRT1050 && ETH_802_3_PTP_SUPPORT




/** 
 * 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
 */

// __RAMFUNC(RAM3)
ssize_t netRecvEvent(Octet * buf, TimeInternal * time, NetPath * netPath)
{
	ssize_t ret;
	enet_ptp_time_data_t ptpTimeData;
//	volatile int xx;  //For test
    
	getTime(time); /* Read the current time used for timestame in case of reading from driver failed. */
	ret = recv(netPath->eventSock, buf, PACKET_SIZE, MSG_DONTWAIT);
	if (ret <= 0) {
		if (errno == EAGAIN || errno == EINTR)
			return 0;

		return ret;
	}

	/* get time stamp of packet */
	if (!time) {
		ERROR("null receive time stamp argument\n");
		return 0;
	}

	netPackPtpData(buf, &ptpTimeData);

	if(!enet_get_rxframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
		time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
		time->seconds = (Integer32)ptpTimeData.timeStamp.second;
	}
//	else
//		xx = 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 
 */

// __RAMFUNC(RAM3)
ssize_t netRecvGeneral(Octet * buf, TimeInternal * time, NetPath * netPath)
{
	ssize_t ret;
//	enet_ptp_time_data_t ptpTimeData;

	/* return the timestamp gotten from driver */
	//getTime(time);
	ret = recv(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT);
	if (ret <= 0) {
		if (errno == EAGAIN || errno == EINTR)
			return 0;

		return ret;
	}

#if 0
	/* get time stamp of packet */
	if (!time) {
		ERROR("null receive time stamp argument\n");
		return 0;
	}

	netPackPtpData(buf, &ptpTimeData);

	if(!enet_get_rxframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
		time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
		time->seconds = (Integer32)ptpTimeData.timeStamp.second;
	}
#endif
	return ret;
}



#if defined(NXPIMXRT1050) && defined(ETH_802_3_PTP_SUPPORT)

static void ethernet_ptp_send_callback(UInteger16 ptp_frame_length, unsigned char *data)
{
    enet1_send_frame(data, ptp_frame_length);
}


ssize_t netSend_802_3_PtpEthMsg(Octet messageID, Octet * buf, UInteger16 length, NetPath * netPath,
	                            const RunTimeOpts *rtOpts, TimeInternal * tim)
{
	ssize_t ret = 0; // ERR_OK | ERR_MEM

	struct pbuf p;
	struct eth_hdr ethhdr;

	u16_t eth_type_be = lwip_htons(ETHTYPE_PTP);

    // Make a lwIP pbuf and put ptr to the data to send in it.
    p.next = NULL;
    p.payload = buf;
    p.tot_len = length;
    p.len = length;
    p.type_internal = PBUF_RAM;

    // the following two are not used, but put sensible values in anyway.
    p.flags = PBUF_FLAG_PUSH | PBUF_FLAG_LLMCAST;    // send immediately, link level multicast.
    p.ref = 1; // one reference to structure (this one!)

    if (pbuf_header(&p, SIZEOF_ETH_HDR) != 0) {
    	return ERR_BUF;
    }

    ethhdr.type = eth_type_be;
    memcpy(&ethhdr.src,  netPath->interfaceID, ETH_HWADDR_LEN);
	if((messageID == PDELAY_REQ) || (messageID == PDELAY_RESP) || (messageID == PDELAY_RESP_FOLLOW_UP))
	{
		 memcpy(&ethhdr.dest, ptpPeerHwAddress, ETH_HWADDR_LEN);
	}
	else
	{
		memcpy(&ethhdr.dest, ptpDefaultHwAddress, ETH_HWADDR_LEN);
	}

	memcpy(p.payload, &ethhdr, SIZEOF_ETH_HDR);

	ret = sav_callback_with_block((uint8_t)SAV_TRANSMIT_CALLBACK, (sav_callback_fn)ethernet_ptp_send_callback, p.len, p.payload, 0);  // Do not block. (block version does not work from an interrupt anyway)

    if (ret == ERR_OK)
    {
        if (tim != NULL) //  general messages don't need tx timestamp recording.
        {

            if(getTxTimestamp(buf, netPath, tim) == FALSE)
            {
            	ret = ERR_MEM;

                netPath->txTimestampFailure = TRUE;
	        }
	    }
	}

    return ret;  // set by the callback function.
}


#endif // NXPIMXRT1050 && ETH_802_3_PTP_SUPPORT




//
// alt_dst: alternative destination.
//   if filled, send to this unicast dest;
//   if zero, do the normal operation (send to unicast with -u, or send to the multcast group)
//
///
/// TODO: merge these 2 functions into one
///
// __RAMFUNC(RAM3)
ssize_t netSendEvent(Octet * buf, UInteger16 length, TimeInternal * time, NetPath * netPath, Integer32 alt_dst)
{
	ssize_t ret;
	struct sockaddr_in addr;
	enet_ptp_time_data_t ptpTimeData;

	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;
		}

		ret = sendto(netPath->eventSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending uni-cast event message\n");
		/* 
		 * Need to forcibly loop back the packet since
		 * we are not using multicast. 
		 */
		addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

		ret = sendto(netPath->eventSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error looping back uni-cast event message\n");
		
	} else {
		addr.sin_addr.s_addr = netPath->multicastAddr;

		ret = sendto(netPath->eventSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending multi-cast event message\n");
	}

	getTime(time); /* Read the current time used for timestame in case of reading from driver failed. */

	netPackPtpData(buf, &ptpTimeData);

	if(!enet_get_txframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
		time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
		time->seconds = (Integer32)ptpTimeData.timeStamp.second;
	} else {
		/* suspend process 1milliseconds, to make sure current frame was sent by enet */
		milliSleep(1);
		/* Try to read timestamp again */
		if(!enet_get_txframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
			time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
			time->seconds = (Integer32)ptpTimeData.timeStamp.second;
		}
	}

	return ret;
}

// __RAMFUNC(RAM3)
ssize_t netSendGeneral(Octet * buf, UInteger16 length, NetPath * netPath, Integer32 alt_dst)
{
	ssize_t ret;
	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;
		}


		ret = sendto(netPath->generalSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending uni-cast general message\n");
	} else {
		addr.sin_addr.s_addr = netPath->multicastAddr;

		ret = sendto(netPath->generalSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending multi-cast general message\n");
	}
	return ret;
}

// __RAMFUNC(RAM3)
ssize_t netSendPeerGeneral(Octet * buf, UInteger16 length, NetPath * netPath)
{
	ssize_t ret;
	struct sockaddr_in addr;

	addr.sin_family = AF_INET;
	addr.sin_port = htons(PTP_GENERAL_PORT);

	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 uni-cast general message\n");

	} else {
		addr.sin_addr.s_addr = netPath->peerMulticastAddr;

		ret = sendto(netPath->generalSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending multi-cast general message\n");
	}
	return ret;

}

//__RAMFUNC(RAM3)
ssize_t netSendPeerEvent(Octet * buf, UInteger16 length, TimeInternal * time, NetPath * netPath)
{
	ssize_t ret;
	struct sockaddr_in addr;
	enet_ptp_time_data_t ptpTimeData;	

	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, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending uni-cast event message\n");

		/* 
		 * Need to forcibly loop back the packet since
		 * we are not using multicast. 
		 */
		addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
		
		ret = sendto(netPath->eventSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error looping back uni-cast event message\n");			
	} else {
		addr.sin_addr.s_addr = netPath->peerMulticastAddr;

		ret = sendto(netPath->eventSock, buf, length, 0, 
			     (struct sockaddr *)&addr, 
			     sizeof(struct sockaddr_in));
		if (ret <= 0)
			DBG("error sending multi-cast event message\n");
	}

	getTime(time); /* Read the current time used for timestame in case of reading from driver failed. */

	netPackPtpData(buf, &ptpTimeData);

	if(!enet_get_txframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
		time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
		time->seconds = (Integer32)ptpTimeData.timeStamp.second;
	} else {
		/* suspend process 1milliseconds, to make sure current frame was sent by enet */
		milliSleep(1);
		/* Try to read timestamp again */
		if(!enet_get_txframe_time(&ptpTimeData)){
		/* return the timestamp gotten from driver */
			time->nanoseconds = ptpTimeData.timeStamp.nanosecond;
			time->seconds = (Integer32)ptpTimeData.timeStamp.second;
		}
	}

	
	return ret;
}



/*
 * refresh IGMP on a timeout
 */
/*
 * @return TRUE if successful
 */
Boolean
netRefreshIGMP(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock)
{
//	DBG("netRefreshIGMP\n");
	
	netShutdownMulticast(netPath);
	
	/* suspend process 100 milliseconds, to make sure the kernel sends the IGMP_leave properly */
	milliSleep(100);

	if (!netInitMulticast(netPath, rtOpts)) {
		return FALSE;
	}
	
//	INFO("refreshed IGMP multicast memberships\n");
	return TRUE;
}
