Example #1
0
/* check and handle received messages */
void handle(PtpClock *ptpClock)
{
  int ret;
  ssize_t length;
  Boolean isFromSelf;
  Boolean isEvent;
  Boolean badTime = FALSE;
  TimeInternal time = { 0, 0 };
  
  if(!ptpClock->message_activity)
  {
    ret = netSelect(0, ptpClock);
    if(ret < 0)
    {
      PERROR("failed to poll sockets");
      toState(PTP_FAULTY, ptpClock);
      return;
    }
    else if(!ret)
    {
      DBGV("handle: nothing\n");
      return;
    }
    /* else length > 0 */
  }
  
  DBGV("handle: something\n");

  isEvent = TRUE;
  length = netRecvEvent(ptpClock->msgIbuf,
                        ptpClock->delayedTiming ? NULL : &time,
                        ptpClock);
  if(length < 0)
  {
    PERROR("failed to receive on the event socket");
    toState(PTP_FAULTY, ptpClock);
    return;
  }
  else if(!length)
  {
    isEvent = FALSE;
    length = netRecvGeneral(ptpClock->msgIbuf, ptpClock);
    if(length < 0)
    {
      PERROR("failed to receive on the general socket");
      toState(PTP_FAULTY, ptpClock);
      return;
    }
    else if(!length)
      return;
  }
  
  ptpClock->message_activity = TRUE;
  
  if(!msgPeek(ptpClock->msgIbuf, length))
    return;
  
  if(length < HEADER_LENGTH)
  {
    ERROR("message shorter than header length\n");
    toState(PTP_FAULTY, ptpClock);
    return;
  }
  
  msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);

  if(isEvent && ptpClock->delayedTiming)
  {
    /* query hardware for matching receive time stamp */
    if(!getReceiveTime(&time, ptpClock->msgTmpHeader.sourceUuid, ptpClock->msgTmpHeader.sequenceId, ptpClock))
    {
      /*
       * Incoming packets without hardware time stamp cannot be ignored outright because
       * a master might only be able to time stamp DelayReq packets; ignoring the Sync
       * packets from another, better clock would break the clock selection protocol.
       * Therefore set system time as fallback and decide below what to do.
       */
      DBGV("*** message with no time stamp ***\n");
      getTime(&time, ptpClock);
      badTime = TRUE;
    }
  }
  
  DBGV("%s Receipt of Message\n"
    "   version %d\n"
    "   type %d\n"
    "   uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n"
    "   sequence %d\n"
    "   time %us %dns\n",
    isEvent ? "event" : "control",
    ptpClock->msgTmpHeader.versionPTP,
    ptpClock->msgTmpHeader.control,
    ptpClock->msgTmpHeader.sourceUuid[0], ptpClock->msgTmpHeader.sourceUuid[1],
    ptpClock->msgTmpHeader.sourceUuid[2], ptpClock->msgTmpHeader.sourceUuid[3],
    ptpClock->msgTmpHeader.sourceUuid[4], ptpClock->msgTmpHeader.sourceUuid[5],
    ptpClock->msgTmpHeader.sequenceId,
    time.seconds, time.nanoseconds);
  
  if(ptpClock->msgTmpHeader.versionPTP != VERSION_PTP)
  {
    DBGV("ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP);
    return;
  }
  
  if( memcmp(ptpClock->msgTmpHeader.subdomain, ptpClock->subdomain_name,
    PTP_SUBDOMAIN_NAME_LENGTH) )
  {
    DBGV("ignore message from subdomain %s\n", ptpClock->msgTmpHeader.subdomain);
    return;
  }
  
  isFromSelf = ptpClock->msgTmpHeader.sourceCommunicationTechnology == ptpClock->port_communication_technology
    && ptpClock->msgTmpHeader.sourcePortId == ptpClock->port_id_field
    && !memcmp(ptpClock->msgTmpHeader.sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH);
  
  /* subtract the inbound latency adjustment if it is not a loop back and the
     time stamp seems reasonable */
  if(!isFromSelf && time.seconds > 0)
    subTime(&time, &time, &ptpClock->runTimeOpts.inboundLatency);
  
  switch(ptpClock->msgTmpHeader.control)
  {
  case PTP_SYNC_MESSAGE:
    handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock);
    break;
    
  case PTP_FOLLOWUP_MESSAGE:
    handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
    break;
    
  case PTP_DELAY_REQ_MESSAGE:
    handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock);
    break;
    
  case PTP_DELAY_RESP_MESSAGE:
    handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
    break;
    
  case PTP_MANAGEMENT_MESSAGE:
    handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
    break;
    
   default:
    DBG("handle: unrecognized message\n");
    break;
  }
}
/* check and handle received messages */
void handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  int ret;
  ssize_t length;
  Boolean isFromSelf;
  TimeInternal time = { 0, 0 };
  TimeInternal now =  { 0, 0 };

  if(!ptpClock->message_activity)
  {
    ret = netSelect(0, &ptpClock->netPath);
    if(ret < 0)
    {
      PERROR("failed to poll sockets");
      toState(PTP_FAULTY, rtOpts, ptpClock);
      return;
    }
    else if(!ret)
    {
#if 0
      DBGV("handle: nothing\n");
#endif
      return;
    }
    /* else length > 0 */
  }

  DBGV("handle: something\n");

  length = netRecvEvent(ptpClock->msgIbuf, &time, &ptpClock->netPath);
  getTime(&now);
  DBG("Time is %ds %dns\n", now.seconds, now.nanoseconds);
  if(length < 0)
  {
    PERROR("failed to receive on the event socket");
    toState(PTP_FAULTY, rtOpts, ptpClock);
    return;
  }
  else if(!length)
  {
    length = netRecvGeneral(ptpClock->msgIbuf, &ptpClock->netPath);
    if(length < 0)
    {
      PERROR("failed to receive on the general socket");
      toState(PTP_FAULTY, rtOpts, ptpClock);
      return;
    }
    else if(!length)
      return;
  }

  ptpClock->message_activity = TRUE;

  if(!msgPeek(ptpClock->msgIbuf, length))
    return;

  if(length < HEADER_LENGTH)
  {
    ERROR("message shorter than header length\n");
    toState(PTP_FAULTY, rtOpts, ptpClock);
    return;
  }

  msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);

  DBG("event Receipt of Message\n   type %d\n"
    "   uuid %02x:%02x:%02x:%02x:%02x:%02x\n"
    "   sequence %d\n   time %us %dns\n",
    ptpClock->msgTmpHeader.control,
    ptpClock->msgTmpHeader.sourceUuid[0], ptpClock->msgTmpHeader.sourceUuid[1], ptpClock->msgTmpHeader.sourceUuid[2],
    ptpClock->msgTmpHeader.sourceUuid[3], ptpClock->msgTmpHeader.sourceUuid[4], ptpClock->msgTmpHeader.sourceUuid[5],
    ptpClock->msgTmpHeader.sequenceId,
    time.seconds, time.nanoseconds);

  isFromSelf = ptpClock->msgTmpHeader.sourceCommunicationTechnology == ptpClock->port_communication_technology
    && ptpClock->msgTmpHeader.sourcePortId == ptpClock->port_id_field
    && !memcmp(ptpClock->msgTmpHeader.sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH);

  /* subtract the inbound latency adjustment if it is not a loop back and the
     time stamp seems reasonable */
  if(!isFromSelf && time.seconds > 0)
    subTime(&time, &time, &rtOpts->inboundLatency);

  switch(ptpClock->msgTmpHeader.control)
  {
  case PTP_SYNC_MESSAGE:
    handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock);
    break;

  case PTP_FOLLOWUP_MESSAGE:
    handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
    break;

  case PTP_DELAY_REQ_MESSAGE:
    handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock);
    break;

  case PTP_DELAY_RESP_MESSAGE:
    handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
    break;

  case PTP_MANAGEMENT_MESSAGE:
    handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
    break;

   default:
    DBG("handle: unrecognized message\n");
    break;
  }
}