Add third_party libs

1 files changed, 401 insertions, 0 deletions

diff --git a/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c
new file mode 100644
index 0000000..5826ebc
--- /dev/null
+++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c
@@ -0,0 +1,401 @@
+/* net.c */

+

+#include "utils/lwiplib.h"

+#include "utils/ptpdlib.h"

+#include "lwip/inet.h"

+

+/* Network Buffer Queue Functions. */

+static void netQInit(BufQueue *pQ)

+{

+  pQ->get = 0;

+  pQ->put = 0;

+  pQ->count = 0;

+}

+

+static Integer32 netQPut(BufQueue *pQ, void *pbuf)

+{

+  if(pQ->count >= PBUF_QUEUE_SIZE)

+    return FALSE;

+  pQ->pbuf[pQ->put] = pbuf;

+  pQ->put = (pQ->put + 1) % PBUF_QUEUE_SIZE;

+  pQ->count++;

+  return TRUE;

+}

+

+void *netQGet(BufQueue *pQ)

+{

+  void *pbuf;

+

+  if(!pQ->count)

+    return NULL;

+  pbuf = pQ->pbuf[pQ->get];

+  pQ->get = (pQ->get + 1) % PBUF_QUEUE_SIZE;

+  pQ->count--;

+  return pbuf;

+}

+

+static Integer32 netQCheck(BufQueue *pQ)

+{

+  if(!pQ->count)

+    return FALSE;

+  return TRUE;

+}

+

+/* Processing an incoming message on the Event port. */

+static void eventRecv(void *arg, struct udp_pcb *pcb, struct pbuf *p,

+  struct ip_addr *addr, u16_t port)

+{

+  NetPath *netPath = (NetPath *)arg;

+

+  /* prevent warnings about unused arguments */

+  (void)pcb; (void)addr; (void)port;

+

+  /* Place the incoming message on the Event Port QUEUE. */

+  if(!netQPut(&netPath->eventQ, p))

+    PERROR("Event Queue Full!\n");

+}

+

+/* Processing an incoming message on the Event port. */

+static void generalRecv(void *arg, struct udp_pcb *pcb, struct pbuf *p,

+  struct ip_addr *addr, u16_t port)

+{

+  NetPath *netPath = (NetPath *)arg;

+

+  /* prevent warnings about unused arguments */

+  (void)pcb; (void)addr; (void)port;

+

+  /* Place the incoming message on the Event Port QUEUE. */

+    if(!netQPut(&netPath->generalQ, p))

+    {

+        PERROR("Event Queue Full!\n");

+    }

+}

+

+Boolean lookupSubdomainAddress(Octet *subdomainName, Octet *subdomainAddress)

+{

+  UInteger32 h;

+

+  /* set multicast group address based on subdomainName */

+  if (!memcmp(subdomainName, DEFAULT_PTP_DOMAIN_NAME, PTP_SUBDOMAIN_NAME_LENGTH))

+    memcpy(subdomainAddress, DEFAULT_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH);

+  else if(!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN1_NAME, PTP_SUBDOMAIN_NAME_LENGTH))

+    memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN1_ADDRESS, NET_ADDRESS_LENGTH);

+  else if(!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN2_NAME, PTP_SUBDOMAIN_NAME_LENGTH))

+    memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN2_ADDRESS, NET_ADDRESS_LENGTH);

+  else if(!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN3_NAME, PTP_SUBDOMAIN_NAME_LENGTH))

+    memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN3_ADDRESS, NET_ADDRESS_LENGTH);

+  else

+  {

+    h = crc_algorithm(subdomainName, PTP_SUBDOMAIN_NAME_LENGTH) % 3;

+    switch(h)

+    {

+    case 0:

+      memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN1_ADDRESS, NET_ADDRESS_LENGTH);

+      break;

+    case 1:

+      memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN2_ADDRESS, NET_ADDRESS_LENGTH);

+      break;

+    case 2:

+      memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN3_ADDRESS, NET_ADDRESS_LENGTH);

+      break;

+    default:

+      ERROR("handle out of range for '%s'!\n", subdomainName);

+      return FALSE;

+    }

+  }

+

+  return TRUE;

+}

+

+Boolean netInit(NetPath *netPath, RunTimeOpts *rtOpts, PtpClock *ptpClock)

+{

+  int i;

+  struct in_addr netAddr;

+  char addrStr[NET_ADDRESS_LENGTH];

+

+  DBG("netInit\n");

+

+  /* Allocate tx buffer for the event port. */

+  netPath->eventTxBuf = pbuf_alloc(PBUF_TRANSPORT, PACKET_SIZE, PBUF_RAM);

+  if(netPath->eventTxBuf == NULL)

+  {

+    PERROR("Failed to allocate Event Tx Buffer\n");

+    return FALSE;

+  }

+

+  /* Allocate tx buffer for the general port. */

+  netPath->generalTxBuf = pbuf_alloc(PBUF_TRANSPORT, PACKET_SIZE, PBUF_RAM);

+  if(netPath->generalTxBuf == NULL)

+  {

+    PERROR("Failed to allocate Event Tx Buffer\n");

+    pbuf_free(netPath->eventTxBuf);

+    return FALSE;

+  }

+

+  /* Open lwIP raw udp interfaces for the event port. */

+  netPath->eventPcb = udp_new();

+  if(netPath->eventPcb == NULL)

+  {

+    PERROR("Failed to open Event UDP PCB\n");

+    pbuf_free(netPath->eventTxBuf);

+    pbuf_free(netPath->generalTxBuf);

+    return FALSE;

+  }

+

+  /* Open lwIP raw udp interfaces for the general port. */

+  netPath->generalPcb = udp_new();

+  if(netPath->generalPcb == NULL)

+  {

+    PERROR("Failed to open General UDP PCB\n");

+    udp_remove(netPath->eventPcb);

+    pbuf_free(netPath->eventTxBuf);

+    pbuf_free(netPath->generalTxBuf);

+    return FALSE;

+  }

+

+  /* Initialize the buffer queues. */

+  netQInit(&netPath->eventQ);

+  netQInit(&netPath->generalQ);

+

+  /* Configure network (broadcast/unicast) addresses. */

+  netPath->unicastAddr = 0;

+  if(!lookupSubdomainAddress(rtOpts->subdomainName, addrStr))

+  {

+    udp_disconnect(netPath->eventPcb);

+    udp_disconnect(netPath->generalPcb);

+    udp_remove(netPath->eventPcb);

+    udp_remove(netPath->generalPcb);

+    pbuf_free(netPath->eventTxBuf);

+    pbuf_free(netPath->generalTxBuf);

+    return FALSE;

+  }

+  if(!inet_aton(addrStr, &netAddr))

+  {

+    ERROR("failed to encode multi-cast address: %s\n", addrStr);

+    udp_disconnect(netPath->eventPcb);

+    udp_disconnect(netPath->generalPcb);

+    udp_remove(netPath->eventPcb);

+    udp_remove(netPath->generalPcb);

+    pbuf_free(netPath->eventTxBuf);

+    pbuf_free(netPath->generalTxBuf);

+    return FALSE;

+  }

+  netPath->multicastAddr = netAddr.s_addr;

+

+  /* Setup subdomain address string. */

+  for(i = 0; i < SUBDOMAIN_ADDRESS_LENGTH; ++i)

+  {

+    ptpClock->subdomain_address[i] = (netAddr.s_addr >> (i * 8)) & 0xff;

+  }

+

+  /* Establish the appropriate UDP bindings/connections for events. */

+

+  udp_recv(netPath->eventPcb, eventRecv, netPath);

+  udp_bind(netPath->eventPcb, IP_ADDR_ANY, PTP_EVENT_PORT);

+//  udp_connect(netPath->eventPcb, IP_ADDR_ANY, PTP_EVENT_PORT);

+  *(Integer16*)ptpClock->event_port_address = PTP_EVENT_PORT;

+

+  /* Establish the appropriate UDP bindings/connections for general. */

+  udp_recv(netPath->generalPcb, generalRecv, netPath);

+  udp_bind(netPath->generalPcb, IP_ADDR_ANY, PTP_GENERAL_PORT);

+//  udp_connect(netPath->generalPcb, IP_ADDR_ANY, PTP_GENERAL_PORT);

+  *(Integer16*)ptpClock->general_port_address = PTP_GENERAL_PORT;

+

+  /* Return a success code. */

+  return TRUE;

+}

+

+/* shut down the UDP stuff */

+Boolean netShutdown(NetPath *netPath)

+{

+  /* Disconnect and close the Event UDP interface */

+  if(netPath->eventPcb)

+  {

+    udp_disconnect(netPath->eventPcb);

+    udp_remove(netPath->eventPcb);

+  }

+

+  /* Disconnect and close the General UDP interface */

+  if(netPath->generalPcb)

+  {

+    udp_disconnect(netPath->generalPcb);

+    udp_remove(netPath->generalPcb);

+  }

+

+  /* Free up the Event and General Tx PBUFs. */

+  if(netPath->eventTxBuf)

+  {

+    pbuf_free(netPath->eventTxBuf);

+  }

+  if(netPath->generalTxBuf)

+  {

+    pbuf_free(netPath->generalTxBuf);

+  }

+

+  /* Clear the network addresses. */

+  netPath->multicastAddr = 0;

+  netPath->unicastAddr = 0;

+

+  /* Return a success code. */

+  return TRUE;

+}

+

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

+int netSelect(TimeInternal *timeout, NetPath *netPath)

+{

+  /* Check the packet queues.  If there is data, return TRUE. */

+  if(netQCheck(&netPath->eventQ) || netQCheck(&netPath->generalQ))

+    return 1;

+  return 0;

+}

+

+/* Pop a message off of the event queue and copy it to the passed in buffer. */

+size_t netRecvEvent(Octet *buf, TimeInternal *time, NetPath *netPath)

+{

+  int i, j;

+  int iPacketSize;

+  struct pbuf *p, *pcopy;

+

+  /* Attempt to get a buffer from the Q.  If none is available,

+   * return length of 0. */

+

+  p = netQGet(&netPath->eventQ);

+  if(p == NULL)

+    return 0;

+  pcopy = p;

+

+

+  /* Here, p points to a valid PBUF structure.  Verify that we have

+   * enough space to store the contents. */

+  if(p->tot_len > PACKET_SIZE)

+  {

+    ERROR("received truncated message\n");

+    return 0;

+  }

+

+  /* Copy the PBUF payload into the buffer. */

+  j = 0;

+  iPacketSize = p->tot_len;

+  for(i = 0; i < iPacketSize; i++)

+  {

+    buf[i] = ((u8_t *)pcopy->payload)[j++];

+    if(j == pcopy->len)

+    {

+      pcopy = pcopy->next;

+      j = 0;

+    }

+  }

+

+  /* Get the timestamp information. */

+  time->seconds = p->time_s;

+  time->nanoseconds = p->time_ns;

+

+  /* Free up the pbuf (chain). */

+  pbuf_free(p);

+

+  /* Return the length of data copied. */

+  return iPacketSize;

+}

+

+/* Pop a message off of the general queue and copy to the passed in buffer. */

+size_t netRecvGeneral(Octet *buf, NetPath *netPath)

+{

+  int i, j;

+  int iPacketSize;

+  struct pbuf *p, *pcopy;

+

+

+  /* Attempt to get a buffer from the Q.  If none is available,

+   * return length of 0. */

+  p = netQGet(&netPath->generalQ);

+  if(p == NULL)

+    return 0;

+  pcopy = p;

+

+  /* Here, p points to a valid PBUF structure.  Verify that we have

+   * enough space to store the contents. */

+  if(p->tot_len > PACKET_SIZE)

+  {

+    ERROR("received truncated message\n");

+    return 0;

+  }

+

+  /* Copy the PBUF payload into the buffer. */

+  j = 0;

+  iPacketSize = p->tot_len;

+  for(i = 0; i < iPacketSize; i++)

+  {

+    buf[i] = ((u8_t *)pcopy->payload)[j++];

+    if(j == pcopy->len)

+    {

+      pcopy = pcopy->next;

+      j = 0;

+    }

+  }

+

+  /* Free up the pbuf (chain). */

+  pbuf_free(p);

+

+  /* Return the length of data copied. */

+  return iPacketSize;

+}

+

+/* Transmit a packet on the Event Port. */

+size_t netSendEvent(Octet *buf, UInteger16 length, NetPath *netPath)

+{

+  int i, j;

+  struct pbuf *pcopy;

+

+  /* Reallocate the tx pbuf based on the current size. */

+  pbuf_realloc(netPath->eventTxBuf, length);

+  pcopy = netPath->eventTxBuf;

+

+  /* Copy the incoming data into the pbuf payload. */

+  j = 0;

+  for(i = 0; i < length; i++)

+  {

+    ((u8_t *)pcopy->payload)[j++] = buf[i];

+    if(j == pcopy->len)

+    {

+        pcopy = pcopy->next;

+        j = 0;

+    }

+  }

+

+  /* send the buffer. */

+  udp_sendto(netPath->eventPcb, netPath->eventTxBuf,

+    (void *)&netPath->multicastAddr, PTP_EVENT_PORT);

+

+  return(length);

+}

+

+/* Transmit a packet on the General Port. */

+size_t netSendGeneral(Octet *buf, UInteger16 length, NetPath *netPath)

+{

+  int i, j;

+  struct pbuf *pcopy;

+

+  /* Reallocate the tx pbuf based on the current size. */

+  pbuf_realloc(netPath->generalTxBuf, length);

+  pcopy = netPath->generalTxBuf;

+

+  /* Copy the incoming data into the pbuf payload. */

+  j = 0;

+  for(i = 0; i < length; i++)

+  {

+    ((u8_t *)pcopy->payload)[j++] = buf[i];

+    if(j == pcopy->len)

+    {

+      pcopy = pcopy->next;

+      j = 0;

+    }

+  }

+

+  /* send the buffer. */

+  udp_sendto(netPath->eventPcb, netPath->generalTxBuf,

+    (void *)&netPath->multicastAddr, PTP_GENERAL_PORT);

+

+  return(length);

+}