net.c

IEEE 1588-2002编程源码

/* net.c */

#include "../ptpd.h"

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

/* depends on linux specific ioctls (see 'netdevice' man page) */
UInteger32 findIface(Octet *ifaceName, UInteger8 *communicationTechnology,
  Octet *uuid, Boolean multicast, NetPath *netPath)
{
  struct ifconf data;
  struct ifreq device[IFCONF_LENGTH];
  UInteger8 i = 0;
  int flags;
  
  data.ifc_len = sizeof(device);
  data.ifc_req = device;
  
  memset(data.ifc_buf,0,data.ifc_len);
  
  flags = IFF_UP|IFF_RUNNING|(multicast?IFF_MULTICAST:0);
  
  /* look for an interface if none specified */
  if(ifaceName[0] == '\0')
  {
    /* no iface specified */
    /* get list of network interfaces*/
    if(ioctl(netPath->eventSock, SIOCGIFCONF, &data) < 0)
    {
      PERROR("failed to get network interface list");
      return FALSE;
    }
    
    if(data.ifc_len == sizeof(device))
    {
      DBG("device list may exceed allocated space\n");
    }
    
    /* search through interfaces */
    for(i=0; i < data.ifc_len/sizeof(device[0]); ++i)
    {
      DBGV("%d %s %s\n",i,device[i].ifr_name,inet_ntoa(((struct sockaddr_in *)&device[i].ifr_addr)->sin_addr));
      
      if(ioctl(netPath->eventSock, SIOCGIFFLAGS, &device[i]) < 0)
      {
        DBGV("failed to get device flags\n");
      }
      else if(memcmp("eth",device[i].ifr_name,3))  /* probably a bad way to do this */
      {
        DBGV("unsupported communication technology\n");
      }
      else if( ((device[i].ifr_flags)&flags) == flags )
      {
        /* found one */
        *communicationTechnology = PTP_ETHER;
        memcpy(ifaceName, device[i].ifr_name, IFACE_NAME_LENGTH);
        break;
      }
    }
    
    if(ifaceName[0] == '\0')
    {
      ERROR("failed to find an ethernet interface up, running, and multicast enabled\n");
      return FALSE;
    }
  }
  else
  {
    /* iface specified */
    *communicationTechnology = PTP_ETHER;
    memcpy(device[i].ifr_name, ifaceName, IFACE_NAME_LENGTH);
  }

  /* get mac address for use as the uuid */
  if(ioctl(netPath->eventSock, SIOCGIFHWADDR, &device[i]) < 0)
  {
    PERROR("failed to get mac address");
    return FALSE;
  }
  memcpy(uuid, device[i].ifr_hwaddr.sa_data, PTP_UUID_LENGTH);
  
  /* get ip address */
  if(ioctl(netPath->eventSock, SIOCGIFADDR, &device[i]) < 0)
  {
    PERROR("failed to get ip address");
    return FALSE;
  }
  
  return ((struct sockaddr_in *)&device[i].ifr_addr)->sin_addr.s_addr;
}

/* start all of the UDP stuff */
/* must specify 'subdomainName', optionally 'ifaceName', if not then pass ifaceName == "" */
/* returns other args */
/* on socket options, see the 'socket(7)' and 'ip' man pages */
Boolean netInit(NetPath *netPath, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  int temp, i;
  struct in_addr interfaceAddr, netAddr;
  struct sockaddr_in addr;
  struct ip_mreq imr;
  char addrStr[NET_ADDRESS_LENGTH];
  char *s;
  
  /* 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;
  }

  /* set non-blocking reads */
  fcntl(netPath->generalSock, F_SETFL, O_NONBLOCK);
  fcntl(netPath->eventSock, F_SETFL, O_NONBLOCK);
  
  /* find a network interface */
  if( !(interfaceAddr.s_addr = findIface(rtOpts->ifaceName, &ptpClock->port_communication_technology,
    ptpClock->port_uuid_field, !rtOpts->directAddress, netPath)) )
    return FALSE;
  
  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 unicast 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;
  }
  
  /* set general and port address */
  *(Integer16*)ptpClock->event_port_address = PTP_EVENT_PORT;
  *(Integer16*)ptpClock->general_port_address = PTP_GENERAL_PORT;
  
  /* set socket to a unicast address if specified (useful for testing) */
  if(rtOpts->directAddress[0])
  {
    if(!inet_aton(rtOpts->directAddress, &netAddr))
      return FALSE;
    
    netPath->bcastAddr = netAddr.s_addr;
    
    s = rtOpts->directAddress;
    for(i = 0; i < SUBDOMAIN_ADDRESS_LENGTH; ++i)
    {
      ptpClock->subdomain_address[i] = strtol(s, &s, 0);
      
      if(!s)
        break;
      
      ++s;
    }
    
    return TRUE;
  }
  
  /* resolve PTP subdomain */
  if(!lookupSubdomainAddress(rtOpts->subdomainName, addrStr))
    return FALSE;
  
  inet_aton(addrStr, &netAddr);
  netPath->bcastAddr = netAddr.s_addr;
  
  s = addrStr;
  for(i = 0; i < SUBDOMAIN_ADDRESS_LENGTH; ++i)
  {
    ptpClock->subdomain_address[i] = strtol(s, &s, 0);
    
    if(!s)
      break;
    
    ++s;
  }
  
  /* multicast send only on specified interface */
  imr.imr_multiaddr.s_addr = netAddr.s_addr;
  imr.imr_interface.s_addr = 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 set multicast send 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 multicast group for receiving");
    return FALSE;
  }

  /*set socket time to life to 1 */
  temp = 1;
  setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_TTL, &temp, sizeof(int));
  setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_TTL, &temp, sizeof(int));
  
  #ifdef PTPD_DBGV
  temp = 1;  /* allow loopback */
  #else
  temp = 0;  /* or not */
  #endif
  setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_LOOP, &temp, sizeof(int));
  setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_LOOP, &temp, sizeof(int));

  return TRUE;
}

/* shut down the UDP stuff */
Boolean netShutdown(NetPath *netPath)
{
  struct ip_mreq imr;

  imr.imr_multiaddr.s_addr = netPath->bcastAddr;
  imr.imr_interface.s_addr = htonl(INADDR_ANY);

  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));
  
  if(netPath->eventSock > 0)
    close(netPath->eventSock);
  netPath->eventSock = -1;
  
  if(netPath->generalSock > 0)
    close(netPath->generalSock);
  netPath->generalSock = -1;
    
  return TRUE;
}

Boolean netRecvEvent(Octet *address, Octet *buf, TimeRepresentation * t, NetPath *netPath)
{
  struct timeval time;
  struct sockaddr_in addr;
  socklen_t addr_len = sizeof(struct sockaddr_in);
  
  if(recvfrom(netPath->eventSock, buf, PACKET_SIZE, 0, (struct sockaddr *)&addr, &addr_len) <= 0)
    return FALSE;
  
  /* get time stamp of packet */
  if(!t)
    return FALSE;
  
  if(!ioctl(netPath->eventSock, SIOCGSTAMP, &time))
  {
    t->seconds = time.tv_sec;
    t->nanoseconds = time.tv_usec*1000;
    DBGV("kernel recv time stamp %lus %ldns\n", t->seconds, t->nanoseconds);
  }
  else
  {
    /* do not try to get by with recording the time here, better to fail
       because the time recorded could be well after message receipt,
       which would put a big spike in the offset signal sent to the
       clock servo */
    DBG("error getting recieve time\n");
    return FALSE;
  }

  /* save address */
  if(address)
    memcpy(address, inet_ntoa(addr.sin_addr), NET_ADDRESS_LENGTH);
  
  return TRUE;
}

Boolean netRecvGeneral(Octet *address, Octet *buf, NetPath *netPath)
{
  struct sockaddr_in addr;
  socklen_t addr_len = sizeof(struct sockaddr_in);
  
  if(recvfrom(netPath->generalSock, buf, PACKET_SIZE, 0, (struct sockaddr *)&addr, &addr_len) <= 0)
    return FALSE;
  
  /* save address */
  if(address)
    memcpy(address, inet_ntoa(addr.sin_addr), NET_ADDRESS_LENGTH);
    
  return TRUE;
}

Boolean netSendEvent(Octet *address, Octet *buf, UInteger16 length, TimeRepresentation * t, NetPath *netPath)
{
  struct timeval time;
  struct sockaddr_in addr;
  
  addr.sin_family = AF_INET;
  addr.sin_port = htons(PTP_EVENT_PORT);
  if(address)
    inet_aton(address, &addr.sin_addr);
  else
    addr.sin_addr.s_addr = netPath->bcastAddr;
  
  if(sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0)
  {
    DBGV("error sending event message\n");
    return FALSE;
  }
  
  /* time stamp */
  if(!t)
    return FALSE;
#ifdef SIOCGSENDSTAMP
  if(!ioctl(netPath->eventSock, SIOCGSENDSTAMP, &time))
  {
    t->seconds = time.tv_sec;
    t->nanoseconds = time.tv_usec*1000;
    DBGV("kernel send time stamp %lus %ldns\n", t->seconds, t->nanoseconds);
  }
  else
#endif
  if(!gettimeofday(&time, NULL))
  {
    t->seconds = time.tv_sec;
    t->nanoseconds = time.tv_usec*1000;
    DBGV("user-space send time stamp %lus %ldns\n", t->seconds, t->nanoseconds);
  }
  else
  {
    DBG("error getting send time\n");
    return FALSE;
  }

  return TRUE;
}

Boolean netSendGeneral(Octet *address, Octet *buf, UInteger16 length, NetPath *netPath)
{
  struct sockaddr_in addr;
  
  addr.sin_family = AF_INET;
  addr.sin_port = htons(PTP_GENERAL_PORT);
  if(address)
    inet_aton(address, &addr.sin_addr);
  else
    addr.sin_addr.s_addr = netPath->bcastAddr;
  
  if(sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0)
  {
    DBG("error sending general message\n");
    return FALSE;
  }
  
  return TRUE;
}