protocol.c

IEEE 1588-2002编程源码

/* protocol.c */

#include "ptpd.h"

Boolean doInit(RunTimeOpts*,PtpClock*);
void doState(RunTimeOpts*,PtpClock*);
void toState(UInteger8,PtpClock*);

void handle(RunTimeOpts*,PtpClock*);
void handleSync(MsgHeader*,MsgSync*,TimeRepresentation*,RunTimeOpts*,PtpClock*);
void handleFollowUp(MsgHeader*,MsgFollowUp*,RunTimeOpts*,PtpClock*);
void handleDelayResp(MsgHeader*,MsgDelayResp*,RunTimeOpts*,PtpClock*);
void handleManagement(MsgHeader*,MsgManagement*,RunTimeOpts*,PtpClock*);

void issueSync(RunTimeOpts*,PtpClock*);
void issueDelayReq(RunTimeOpts*,PtpClock*);
void issueDelayResp(TimeRepresentation*,MsgHeader*,RunTimeOpts*,PtpClock*);
void issueManagement(MsgHeader*,MsgManagement*,RunTimeOpts*,PtpClock*);

void initData(RunTimeOpts*,PtpClock*);

MsgSync * addForeign(Octet*,MsgHeader*,PtpClock*);

/* loop until port_state is null, each case has the actions and events to
   be checked at that state. the actions and events may or may not
   change port_state by calling toState(), but once they are done we
   loop around again and perform the actions required at port_state. the 
   STATE_CHANGE_EVENT happens in any state except the initializing state. */
void protocol(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  TimeRepresentation t;
  Integer16 interval;
  
  DBG("event POWERUP\n");
  
  if(rtOpts->resetClock)
    ptpClock->need_reset = TRUE;
  
  toState(PTP_INITIALIZING, ptpClock);
  
  for(;;)
  {
    if(ptpClock->port_state != PTP_INITIALIZING)
      doState(rtOpts, ptpClock);
    else if(!doInit(rtOpts, ptpClock))
      return;
    
    if(!ptpClock->message_activity)
    {
      DBGV("no activity\n");
      interval = PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval);
      t.seconds = interval;
      t.nanoseconds = 0;
      while(!nanoSleep(&t));
      
      timerUpdate(interval, ptpClock->itimer);
    }
    else
    {
      DBGV("activity\n");
    }
  }
}

Boolean doInit(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  DBG("manufacturerIdentity: %s\n", MANUFACTURER_ID);
  
  /* initialize networking */
  netShutdown(&ptpClock->netPath);
  if(!netInit(&ptpClock->netPath, rtOpts, ptpClock))
  {
    ERROR("failed to initialize network\n");
    toState(PTP_FAULTY, ptpClock);
    return FALSE;
  }
  
  /* initialize other stuff */
  initData(rtOpts, ptpClock);
  initClock(ptpClock);
  M1(ptpClock);
  msgPackHeader(ptpClock->msgObuf, ptpClock);
  
  DBG("sync message interval: %d\n", PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval));
  DBG("clock identifier: %s\n", ptpClock->clock_identifier);
  DBG("256*log2(clock variance): %d\n", ptpClock->clock_variance);
  DBG("clock stratum: %d\n", ptpClock->clock_stratum);
  DBG("clock preferred?: %s\n", ptpClock->preferred?"yes":"no");
  DBG("bound interface name: %s\n", rtOpts->ifaceName);
  DBG("communication technology: %d\n", ptpClock->port_communication_technology);
  DBG("uuid: %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
    ptpClock->port_uuid_field[0], ptpClock->port_uuid_field[1], ptpClock->port_uuid_field[2],
    ptpClock->port_uuid_field[3], ptpClock->port_uuid_field[4], ptpClock->port_uuid_field[5]);
  DBG("PTP subdomain name: %s\n", ptpClock->subdomain_name);
  DBG("subdomain address: %hhx.%hhx.%hhx.%hhx\n",
    ptpClock->subdomain_address[0], ptpClock->subdomain_address[1],
    ptpClock->subdomain_address[2], ptpClock->subdomain_address[3]);
  DBG("event port address: %hhx %hhx\n",
    ptpClock->event_port_address[0], ptpClock->event_port_address[1]);
  DBG("general port address: %hhx %hhx\n",
    ptpClock->general_port_address[0], ptpClock->general_port_address[1]);
  
  toState(PTP_LISTENING, ptpClock);
  return TRUE;
}

/* port specific state actions */
void doState(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  UInteger8 state;
  
  ptpClock->message_activity = FALSE;
  
  switch(ptpClock->port_state)
  {
  case PTP_LISTENING:
  case PTP_SLAVE:
  case PTP_MASTER:
    if(ptpClock->record_update)
    {
      ptpClock->record_update = FALSE;
      state = bmc(ptpClock->foreign, rtOpts, ptpClock);
      if(state != ptpClock->port_state)
        toState(state, ptpClock);
    }
    break;
    
  default:
    break;
  }
  
  switch(ptpClock->port_state)
  {
  case PTP_FAULTY:
    /* imaginary troubleshooting */
    
    DBG("event FAULT_CLEARED\n");
    toState(PTP_INITIALIZING, ptpClock);
    return;
    
  case PTP_LISTENING:
  case PTP_PASSIVE:
  case PTP_UNCALIBRATED:
  case PTP_SLAVE:
    handle(rtOpts, ptpClock);
    
    if(ptpClock->need_reset)
    {
      if(ptpClock->one_way_delay.seconds || ptpClock->one_way_delay.nanoseconds)
      {
        ptpClock->need_reset = FALSE;
        getTime(&ptpClock->timeTmp.seconds, &ptpClock->timeTmp.nanoseconds);
        subTime(&ptpClock->timeTmp, &ptpClock->timeTmp, &ptpClock->offset_from_master);
        if(!rtOpts->noAdjust)
        {
          setTime(ptpClock->timeTmp.seconds, ptpClock->timeTmp.nanoseconds);
          initClock(ptpClock);
        }
        return;
      }
    }
    
    if(timerExpired(SYNC_RECEIPT_TIMER, ptpClock->itimer))
    {
      DBG("event SYNC_RECEIPT_TIMEOUT_EXPIRES\n");
      ptpClock->number_foreign_records = 0;
      ptpClock->foreign_record_i = 0;
      if(!rtOpts->slaveOnly)
      {
        M1(ptpClock);
        toState(PTP_MASTER, ptpClock);
      }
      else if(ptpClock->port_state != PTP_LISTENING)
        toState(PTP_LISTENING, ptpClock);
    }
    
    break;
    
  case PTP_MASTER:
    handle(rtOpts, ptpClock);
    
    if(timerExpired(SYNC_INTERVAL_TIMER, ptpClock->itimer))
    {
      DBGV("event SYNC_INTERVAL_TIMEOUT_EXPIRES\n");
      issueSync(rtOpts, ptpClock);
    }
    
    break;
    
  case PTP_DISABLED:
    handle(rtOpts, ptpClock);
    break;
    
  default:
    DBG("do unrecognized state\n");
    break;
  }
  
  if(rtOpts->displayStats)
    displayStats(ptpClock);
}

/* perform actions required when leaving 'port_state' and entering 'state' */
void toState(UInteger8 state, PtpClock *ptpClock)
{
  ptpClock->message_activity = TRUE;
  
  /* leaving state tasks */
  switch(ptpClock->port_state)
  {
  case PTP_MASTER:
    timerStop(SYNC_INTERVAL_TIMER, ptpClock->itimer);
    timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
    break;
    
  case PTP_SLAVE:
    initClock(ptpClock);
    break;
    
  default:
    break;
  }
  
  /* entering state tasks */
  switch(state)
  {
  case PTP_INITIALIZING:
    DBG("state PTP_INITIALIZING\n");
    timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer);
    
    ptpClock->port_state = PTP_INITIALIZING;
    break;
    
  case PTP_FAULTY:
    DBG("state PTP_FAULTY\n");
    timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer);
    
    ptpClock->port_state = PTP_FAULTY;
    break;
    
  case PTP_DISABLED:
    DBG("state change to PTP_DISABLED\n");
    timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer);
    
    ptpClock->port_state = PTP_DISABLED;
    break;
    
  case PTP_LISTENING:
    DBG("state PTP_LISTENING\n");
    
    timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
    
    ptpClock->port_state = PTP_LISTENING;
    break;
    
  case PTP_MASTER:
    DBG("state PTP_MASTER\n");
    
    if(ptpClock->port_state != PTP_PRE_MASTER)
      timerStart(SYNC_INTERVAL_TIMER, PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
    timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer);
    
    ptpClock->port_state = PTP_MASTER;
    break;
    
  case PTP_PASSIVE:
    DBG("state PTP_PASSIVE\n");
    ptpClock->port_state = PTP_PASSIVE;
    break;
    
  case PTP_UNCALIBRATED:
    DBG("state PTP_UNCALIBRATED\n");
    ptpClock->port_state = PTP_UNCALIBRATED;
    break;
    
  case PTP_SLAVE:
    DBG("state PTP_PTP_SLAVE\n");
    
    initClock(ptpClock);
    
    /* R is chosen to allow a few syncs before we first get a one-way delay estimate */
    /* this is to allow the offset filter to fill for an accurate initial clock reset */
    ptpClock->Q = 0;
    ptpClock->R = getRand(&ptpClock->random_seed)%4 + 4;
    DBG("Q = %d, R = %d\n", ptpClock->Q, ptpClock->R);
    
    ptpClock->port_state = PTP_SLAVE;
    break;
    
  default:
    DBG("to unrecognized state\n");
    break;
  }
}

void handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  MsgSync *sync;
  TimeRepresentation time;
  
  if(netRecvEvent(0, ptpClock->msgIbuf, &time, &ptpClock->netPath))
  {
    subTime(&time, &time, &rtOpts->inboundLatency);
  }
  else if(!netRecvGeneral(0, ptpClock->msgIbuf, &ptpClock->netPath))
    return;
    
  ptpClock->message_activity = TRUE;
  
  if(!msgPeek(ptpClock->msgIbuf))
    return;
  
  msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
  
  DBGV("event Receipt of Message\n   type %d\n"
    "   uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n   time %lus %ldns\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],
    time.seconds, time.nanoseconds);
  
  if(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))
  {
    DBGV("handleMessage: ignoring message from self\n");
    return;
  }
  
  switch(ptpClock->msgTmpHeader.control) {
  case PTP_SYNC_MESSAGE:
    /* addForeign() takes care of msgUnpackSync() */
    sync = addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock);
    ptpClock->record_update = TRUE;
    handleSync(&ptpClock->msgTmpHeader, sync, &time, rtOpts, ptpClock);
    break;
    
  case PTP_DELAY_REQ_MESSAGE:
    if(ptpClock->port_state == PTP_MASTER)
      issueDelayResp(&time, &ptpClock->msgTmpHeader, rtOpts, ptpClock);
    break;
    
  case PTP_FOLLOWUP_MESSAGE:
    msgUnpackFollowUp(ptpClock->msgIbuf, &ptpClock->msgTmp.follow);
    handleFollowUp(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.follow, rtOpts, ptpClock);
    break;
    
  case PTP_DELAY_RESP_MESSAGE:
    msgUnpackDelayResp(ptpClock->msgIbuf, &ptpClock->msgTmp.resp);
    handleDelayResp(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.resp, rtOpts, ptpClock);
    break;
    
  case PTP_MANAGEMENT_MESSAGE:
    msgUnpackManagement(ptpClock->msgIbuf, &ptpClock->msgTmp.manage);
    handleManagement(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.manage, rtOpts, ptpClock);