mirror of
https://gitee.com/Vancouver2017/luban-lite-t3e-pro.git
synced 2025-12-14 18:38:55 +00:00
426 lines
10 KiB
C
426 lines
10 KiB
C
/* net.c */
|
|
|
|
#include "../ptpd.h"
|
|
|
|
/* Initialize network queue. */
|
|
static void netQInit(BufQueue *queue)
|
|
{
|
|
queue->head = 0;
|
|
queue->tail = 0;
|
|
//sys_mutex_new(&queue->mutex);
|
|
}
|
|
|
|
/* Put data to the network queue. */
|
|
static bool netQPut(BufQueue *queue, void *pbuf)
|
|
{
|
|
bool retval = FALSE;
|
|
|
|
//sys_mutex_lock(&queue->mutex);
|
|
|
|
// Is there room on the queue for the buffer?
|
|
if (((queue->head + 1) & PBUF_QUEUE_MASK) != queue->tail)
|
|
{
|
|
// Place the buffer in the queue.
|
|
queue->head = (queue->head + 1) & PBUF_QUEUE_MASK;
|
|
queue->pbuf[queue->head] = pbuf;
|
|
retval = TRUE;
|
|
}
|
|
|
|
//sys_mutex_unlock(&queue->mutex);
|
|
|
|
return retval;
|
|
}
|
|
|
|
/* Get data from the network queue. */
|
|
static void *netQGet(BufQueue *queue)
|
|
{
|
|
void *pbuf = NULL;
|
|
|
|
//sys_mutex_lock(&queue->mutex);
|
|
|
|
// Is there a buffer on the queue?
|
|
if (queue->tail != queue->head)
|
|
{
|
|
// Get the buffer from the queue.
|
|
queue->tail = (queue->tail + 1) & PBUF_QUEUE_MASK;
|
|
pbuf = queue->pbuf[queue->tail];
|
|
}
|
|
|
|
//sys_mutex_unlock(&queue->mutex);
|
|
|
|
return pbuf;
|
|
}
|
|
|
|
/* Free any remaining pbufs in the queue. */
|
|
static void netQEmpty(BufQueue *queue)
|
|
{
|
|
//sys_mutex_lock(&queue->mutex);
|
|
|
|
// Free each remaining buffer in the queue.
|
|
while (queue->tail != queue->head)
|
|
{
|
|
// Get the buffer from the queue.
|
|
queue->tail = (queue->tail + 1) & PBUF_QUEUE_MASK;
|
|
pbuf_free(queue->pbuf[queue->tail]);
|
|
}
|
|
|
|
//sys_mutex_unlock(&queue->mutex);
|
|
}
|
|
|
|
/* Check if something is in the queue */
|
|
static bool netQCheck(BufQueue *queue)
|
|
{
|
|
bool retval = FALSE;
|
|
|
|
//sys_mutex_lock(&queue->mutex);
|
|
|
|
if (queue->tail != queue->head) retval = TRUE;
|
|
|
|
//sys_mutex_unlock(&queue->mutex);
|
|
|
|
return retval;
|
|
}
|
|
|
|
/* Shut down the UDP and network stuff */
|
|
bool netShutdown(NetPath *netPath)
|
|
{
|
|
ip4_addr_t multicastAaddr;
|
|
|
|
DBG("netShutdown\n");
|
|
|
|
/* leave multicast group */
|
|
multicastAaddr.addr = netPath->multicastAddr;
|
|
igmp_leavegroup(IP_ADDR_ANY, &multicastAaddr);
|
|
|
|
/* Disconnect and close the Event UDP interface */
|
|
if (netPath->eventPcb)
|
|
{
|
|
udp_disconnect(netPath->eventPcb);
|
|
udp_remove(netPath->eventPcb);
|
|
netPath->eventPcb = NULL;
|
|
}
|
|
|
|
/* Disconnect and close the General UDP interface */
|
|
if (netPath->generalPcb)
|
|
{
|
|
udp_disconnect(netPath->generalPcb);
|
|
udp_remove(netPath->generalPcb);
|
|
netPath->generalPcb = NULL;
|
|
}
|
|
|
|
/* Clear the network addresses. */
|
|
netPath->multicastAddr = 0;
|
|
netPath->unicastAddr = 0;
|
|
|
|
/* Return a success code. */
|
|
return TRUE;
|
|
}
|
|
|
|
/* Find interface to be used. uuid should be filled with MAC address of the interface.
|
|
Will return the IPv4 address of the interface. */
|
|
static int32_t findIface(const octet_t *ifaceName, octet_t *uuid, NetPath *netPath)
|
|
{
|
|
struct netif *iface;
|
|
|
|
iface = netif_default;
|
|
memcpy(uuid, iface->hwaddr, iface->hwaddr_len);
|
|
|
|
return iface->ip_addr.addr;
|
|
}
|
|
|
|
/* Process an incoming message on the Event port. */
|
|
static void netRecvEventCallback(void *arg, struct udp_pcb *pcb, struct pbuf *p,
|
|
struct ip_addr *addr, u16_t port)
|
|
{
|
|
NetPath *netPath = (NetPath *) arg;
|
|
|
|
/* Place the incoming message on the Event Port QUEUE. */
|
|
if (!netQPut(&netPath->eventQ, p))
|
|
{
|
|
pbuf_free(p);
|
|
ERROR("netRecvEventCallback: queue full\n");
|
|
return;
|
|
}
|
|
|
|
/* Alert the PTP thread there is now something to do. */
|
|
ptpd_alert();
|
|
}
|
|
|
|
/* Process an incoming message on the General port. */
|
|
static void netRecvGeneralCallback(void *arg, struct udp_pcb *pcb, struct pbuf *p,
|
|
struct ip_addr *addr, u16_t port)
|
|
{
|
|
NetPath *netPath = (NetPath *) arg;
|
|
|
|
/* Place the incoming message on the Event Port QUEUE. */
|
|
if (!netQPut(&netPath->generalQ, p))
|
|
{
|
|
pbuf_free(p);
|
|
ERROR("netRecvGeneralCallback: queue full\n");
|
|
return;
|
|
}
|
|
|
|
/* Alert the PTP thread there is now something to do. */
|
|
ptpd_alert();
|
|
}
|
|
|
|
/* Start all of the UDP stuff */
|
|
bool netInit(NetPath *netPath, PtpClock *ptpClock)
|
|
{
|
|
struct in_addr netAddr;
|
|
ip4_addr_t interfaceAddr;
|
|
char addrStr[NET_ADDRESS_LENGTH];
|
|
|
|
DBG("netInit\n");
|
|
|
|
/* Initialize the buffer queues. */
|
|
netQInit(&netPath->eventQ);
|
|
netQInit(&netPath->generalQ);
|
|
|
|
/* Find a network interface */
|
|
interfaceAddr.addr = findIface(ptpClock->rtOpts->ifaceName, ptpClock->portUuidField, netPath);
|
|
if (!(interfaceAddr.addr))
|
|
{
|
|
ERROR("netInit: Failed to find interface address\n");
|
|
goto fail01;
|
|
}
|
|
|
|
/* Open lwIP raw udp interfaces for the event port. */
|
|
netPath->eventPcb = udp_new();
|
|
if (NULL == netPath->eventPcb)
|
|
{
|
|
ERROR("netInit: Failed to open Event UDP PCB\n");
|
|
goto fail02;
|
|
}
|
|
|
|
/* Open lwIP raw udp interfaces for the general port. */
|
|
netPath->generalPcb = udp_new();
|
|
if (NULL == netPath->generalPcb)
|
|
{
|
|
ERROR("netInit: Failed to open General UDP PCB\n");
|
|
goto fail03;
|
|
}
|
|
|
|
/* Configure network (broadcast/unicast) addresses. */
|
|
netPath->unicastAddr = 0; /* disable unicast */
|
|
|
|
/* Init General multicast IP address */
|
|
memcpy(addrStr, DEFAULT_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);
|
|
if (!inet_aton(addrStr, &netAddr))
|
|
{
|
|
ERROR("netInit: failed to encode multi-cast address: %s\n", addrStr);
|
|
goto fail04;
|
|
}
|
|
netPath->multicastAddr = netAddr.s_addr;
|
|
|
|
/* Join multicast group (for receiving) on specified interface */
|
|
igmp_joingroup(&interfaceAddr, (struct ip_addr *)&netAddr);
|
|
|
|
/* Init Peer multicast IP address */
|
|
memcpy(addrStr, PEER_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);
|
|
if (!inet_aton(addrStr, &netAddr))
|
|
{
|
|
ERROR("netInit: failed to encode peer multi-cast address: %s\n", addrStr);
|
|
goto fail04;
|
|
}
|
|
netPath->peerMulticastAddr = netAddr.s_addr;
|
|
|
|
/* Join peer multicast group (for receiving) on specified interface */
|
|
igmp_joingroup(&interfaceAddr, (struct ip_addr *) &netAddr);
|
|
|
|
/* Multicast send only on specified interface. */
|
|
netPath->eventPcb->mcast_ip4.addr = netPath->multicastAddr;
|
|
netPath->generalPcb->mcast_ip4.addr = netPath->multicastAddr;
|
|
|
|
/* Establish the appropriate UDP bindings/connections for events. */
|
|
udp_recv(netPath->eventPcb, netRecvEventCallback, netPath);
|
|
udp_bind(netPath->eventPcb, IP_ADDR_ANY, PTP_EVENT_PORT);
|
|
/* udp_connect(netPath->eventPcb, &netAddr, PTP_EVENT_PORT); */
|
|
|
|
/* Establish the appropriate UDP bindings/connections for general. */
|
|
udp_recv(netPath->generalPcb, netRecvGeneralCallback, netPath);
|
|
udp_bind(netPath->generalPcb, IP_ADDR_ANY, PTP_GENERAL_PORT);
|
|
/* udp_connect(netPath->generalPcb, &netAddr, PTP_GENERAL_PORT); */
|
|
|
|
/* Return a success code. */
|
|
return TRUE;
|
|
|
|
fail04:
|
|
udp_remove(netPath->generalPcb);
|
|
fail03:
|
|
udp_remove(netPath->eventPcb);
|
|
fail02:
|
|
fail01:
|
|
return FALSE;
|
|
}
|
|
|
|
/* Wait for a packet to come in on either port. For now, there is no wait.
|
|
* Simply check to see if a packet is available on either port and return 1,
|
|
* otherwise return 0. */
|
|
int32_t netSelect(NetPath *netPath, const TimeInternal *timeout)
|
|
{
|
|
/* Check the packet queues. If there is data, return TRUE. */
|
|
if (netQCheck(&netPath->eventQ) || netQCheck(&netPath->generalQ)) return 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Delete all waiting packets in event queue. */
|
|
void netEmptyEventQ(NetPath *netPath)
|
|
{
|
|
netQEmpty(&netPath->eventQ);
|
|
}
|
|
|
|
static ssize_t netRecv(octet_t *buf, TimeInternal *time, BufQueue *msgQueue)
|
|
{
|
|
int i;
|
|
int j;
|
|
u16_t length;
|
|
struct pbuf *p;
|
|
struct pbuf *pcopy;
|
|
|
|
/* Get the next buffer from the queue. */
|
|
if ((p = (struct pbuf*) netQGet(msgQueue)) == NULL)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
/* Verify that we have enough space to store the contents. */
|
|
if (p->tot_len > PACKET_SIZE)
|
|
{
|
|
ERROR("netRecv: received truncated message\n");
|
|
pbuf_free(p);
|
|
return 0;
|
|
}
|
|
|
|
/* Verify there is contents to copy. */
|
|
if (p->tot_len == 0)
|
|
{
|
|
ERROR("netRecv: received empty packet\n");
|
|
pbuf_free(p);
|
|
return 0;
|
|
}
|
|
|
|
if (time != NULL)
|
|
{
|
|
#if LWIP_PTP
|
|
time->seconds = p->second;
|
|
time->nanoseconds = p->nanosecond;
|
|
#else
|
|
getTime(time);
|
|
#endif
|
|
}
|
|
|
|
/* Get the length of the buffer to copy. */
|
|
length = p->tot_len;
|
|
|
|
/* Copy the pbuf payload into the buffer. */
|
|
pcopy = p;
|
|
j = 0;
|
|
for (i = 0; i < length; i++)
|
|
{
|
|
// Copy the next byte in the payload.
|
|
buf[i] = ((u8_t *)pcopy->payload)[j++];
|
|
|
|
// Skip to the next buffer in the payload?
|
|
if (j == pcopy->len)
|
|
{
|
|
// Move to the next buffer.
|
|
pcopy = pcopy->next;
|
|
j = 0;
|
|
}
|
|
}
|
|
|
|
/* Free up the pbuf (chain). */
|
|
pbuf_free(p);
|
|
|
|
return length;
|
|
}
|
|
|
|
ssize_t netRecvEvent(NetPath *netPath, octet_t *buf, TimeInternal *time)
|
|
{
|
|
return netRecv(buf, time, &netPath->eventQ);
|
|
}
|
|
|
|
ssize_t netRecvGeneral(NetPath *netPath, octet_t *buf, TimeInternal *time)
|
|
{
|
|
return netRecv(buf, time, &netPath->generalQ);
|
|
}
|
|
|
|
static ssize_t netSend(const octet_t *buf, int16_t length, TimeInternal *time, const int32_t * addr, struct udp_pcb * pcb)
|
|
{
|
|
err_t result;
|
|
struct pbuf * p;
|
|
|
|
/* Allocate the tx pbuf based on the current size. */
|
|
p = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_RAM);
|
|
if (NULL == p)
|
|
{
|
|
ERROR("netSend: Failed to allocate Tx Buffer\n");
|
|
goto fail01;
|
|
}
|
|
|
|
/* Copy the incoming data into the pbuf payload. */
|
|
result = pbuf_take(p, buf, length);
|
|
if (ERR_OK != result)
|
|
{
|
|
ERROR("netSend: Failed to copy data to Pbuf (%d)\n", result);
|
|
goto fail02;
|
|
}
|
|
|
|
/* send the buffer. */
|
|
result = udp_sendto(pcb, p, (void *)addr, pcb->local_port);
|
|
if (ERR_OK != result)
|
|
{
|
|
ERROR("netSend: Failed to send data (%d)\n", result);
|
|
goto fail02;
|
|
}
|
|
|
|
if (time != NULL)
|
|
{
|
|
#if LWIP_PTP
|
|
time->seconds = p->second;
|
|
time->nanoseconds = p->nanosecond;
|
|
#else
|
|
/* TODO: use of loopback mode */
|
|
/*
|
|
time->seconds = 0;
|
|
time->nanoseconds = 0;
|
|
*/
|
|
getTime(time);
|
|
#endif
|
|
DBGV("netSend: %d sec %d nsec\n", time->seconds, time->nanoseconds);
|
|
} else {
|
|
DBGV("netSend\n");
|
|
}
|
|
|
|
|
|
fail02:
|
|
pbuf_free(p);
|
|
|
|
fail01:
|
|
return length;
|
|
|
|
/* return (0 == result) ? length : 0; */
|
|
}
|
|
|
|
ssize_t netSendEvent(NetPath *netPath, const octet_t *buf, int16_t length, TimeInternal *time)
|
|
{
|
|
return netSend(buf, length, time, &netPath->multicastAddr, netPath->eventPcb);
|
|
}
|
|
|
|
ssize_t netSendGeneral(NetPath *netPath, const octet_t *buf, int16_t length)
|
|
{
|
|
return netSend(buf, length, NULL, &netPath->multicastAddr, netPath->generalPcb);
|
|
}
|
|
|
|
ssize_t netSendPeerGeneral(NetPath *netPath, const octet_t *buf, int16_t length)
|
|
{
|
|
return netSend(buf, length, NULL, &netPath->peerMulticastAddr, netPath->generalPcb);
|
|
}
|
|
|
|
ssize_t netSendPeerEvent(NetPath *netPath, const octet_t *buf, int16_t length, TimeInternal* time)
|
|
{
|
|
return netSend(buf, length, time, &netPath->peerMulticastAddr, netPath->eventPcb);
|
|
}
|