net.c

来自「最新版IAR FOR ARM（EWARM）5.11中的代码例子」· C语言 代码 · 共 585 行 · 第 1/2 页

  if(!lookupSubdomainAddress(rtOpts->subdomainName, addrStr))
    return FALSE;
  
  if(!inet_aton(addrStr, &netAddr))
  {
    ERROR("failed to encode multi-cast address: %s\n", addrStr);
    return FALSE;
  }
  
  netPath->multicastAddr = 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 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;
  }

  /* set socket time-to-live to 1 */
  temp = 1;
  if( setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_TTL, &temp, sizeof(int)) < 0
    || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_TTL, &temp, sizeof(int)) < 0 )
  {
    PERROR("failed to set the multi-cast time-to-live");
    return FALSE;
  }
  
  /* enable loopback */
  temp = 1;
  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;
  }

  /* make timestamps available through recvmsg() */
  temp = 1;
  if( setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMP, &temp, sizeof(int)) < 0
    || setsockopt(netPath->generalSock, SOL_SOCKET, SO_TIMESTAMP, &temp, sizeof(int)) < 0 )
  {
    PERROR("failed to enable receive time stamps");
    return FALSE;
  }


  return TRUE;
}

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

  imr.imr_multiaddr.s_addr = netPath->multicastAddr;
  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));
  
  netPath->multicastAddr = 0;
  netPath->unicastAddr = 0;
  
  if(netPath->eventSock > 0)
    close(netPath->eventSock);
  netPath->eventSock = -1;
  
  if(netPath->generalSock > 0)
    close(netPath->generalSock);
  netPath->generalSock = -1;
    
  return TRUE;
}

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

ssize_t netRecvEvent(Octet *buf, TimeInternal *time, NetPath *netPath)
{
  ssize_t ret;
  struct msghdr msg;
  struct iovec vec[1];
  struct sockaddr_in from_addr;
  union {
      struct cmsghdr cm;
      char control[CMSG_SPACE(sizeof(struct timeval))];
  } cmsg_un;
  struct cmsghdr *cmsg;
  struct timeval *tv;
  
  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, MSG_DONTWAIT);
  if(ret <= 0)
  {
    if(errno == EAGAIN || errno == EINTR)
      return 0;
    
    return ret;
  }
  
  if(msg.msg_flags&MSG_TRUNC)
  {
    ERROR("received truncated message\n");
    return 0;
  }
  
  /* get time stamp of packet */
  if(!time)
  {
    ERROR("null receive time stamp argument\n");
    return 0;
  }
  
  if(msg.msg_flags&MSG_CTRUNC)
  {
    ERROR("received truncated ancillary data\n");
    return 0;
  }
  
  if(msg.msg_controllen < sizeof(cmsg_un.control))
  {
    ERROR("received short ancillary data (%d/%d)\n",
      msg.msg_controllen, (int)sizeof(cmsg_un.control));
    
    return 0;
  }
  
  tv = 0;
  for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg))
  {
    if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMP)
      tv = (struct timeval *)CMSG_DATA(cmsg);
  }
  
  if(tv)
  {
    time->seconds = tv->tv_sec;
    time->nanoseconds = tv->tv_usec*1000;
    DBGV("kernel recv time stamp %us %dns\n", time->seconds, time->nanoseconds);
  }
  else
  {
    /* 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("no recieve time stamp\n");
    return 0;
  }

  return ret;
}

ssize_t netRecvGeneral(Octet *buf, NetPath *netPath)
{
  ssize_t ret;
  struct sockaddr_in addr;
  socklen_t addr_len = sizeof(struct sockaddr_in);
  
  ret = recvfrom(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT, (struct sockaddr *)&addr, &addr_len);
  if(ret <= 0)
  {
    if(errno == EAGAIN || errno == EINTR)
      return 0;
    
    return ret;
  }
  
  return ret;
}

ssize_t netSendEvent(Octet *buf, UInteger16 length, NetPath *netPath)
{
  ssize_t ret;
  struct sockaddr_in addr;
  
  addr.sin_family = AF_INET;
  addr.sin_port = htons(PTP_EVENT_PORT);
  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");
  
  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");
  }
  
  return ret;
}

ssize_t netSendGeneral(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);
  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");
  
  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 general message\n");
  }
  
  return ret;
}