protocol.c

最新版IAR FOR ARM（EWARM）5.11中的代码例子

/* protocol.c */

#include "ptpd.h"

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

void handle(RunTimeOpts*,PtpClock*);
void handleSync(MsgHeader*,Octet*,ssize_t,TimeInternal*,Boolean,RunTimeOpts*,PtpClock*);
void handleFollowUp(MsgHeader*,Octet*,ssize_t,Boolean,RunTimeOpts*,PtpClock*);
void handleDelayReq(MsgHeader*,Octet*,ssize_t,TimeInternal*,Boolean,RunTimeOpts*,PtpClock*);
void handleDelayResp(MsgHeader*,Octet*,ssize_t,Boolean,RunTimeOpts*,PtpClock*);
void handleManagement(MsgHeader*,Octet*,ssize_t,Boolean,RunTimeOpts*,PtpClock*);

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

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


/* first time in a non-rtos system, call this function */
void protocol_first(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  DBG("event POWERUP\n");

  toState(PTP_INITIALIZING, rtOpts, ptpClock);

  if(ptpClock->port_state != PTP_INITIALIZING)
    doState(rtOpts, ptpClock);
  else if(!doInit(rtOpts, ptpClock))
    return;

  if(ptpClock->message_activity)
    DBGV("activity\n");
#if 0
  else
    DBGV("no activity\n");
#endif
}

/* forever after, call this function in a non-rtos system */
void protocol_loop(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  if(ptpClock->port_state != PTP_INITIALIZING)
    doState(rtOpts, ptpClock);
  else if(!doInit(rtOpts, ptpClock))
    return;

  if(ptpClock->message_activity)
    DBGV("activity\n");
#if 0
  else
    DBGV("no activity\n");
#endif
}

/* loop forever. doState() has a switch for the actions and events to be
   checked for 'port_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 for the new 'port_state'. */
void protocol(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  DBG("event POWERUP\n");

  toState(PTP_INITIALIZING, rtOpts, ptpClock);

  for(;;)
  {
    if(ptpClock->port_state != PTP_INITIALIZING)
      doState(rtOpts, ptpClock);
    else if(!doInit(rtOpts, ptpClock))
      return;

    if(ptpClock->message_activity)
      DBGV("activity\n");
#if 0
    else
      DBGV("no activity\n");
#endif
  }
}

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, rtOpts, ptpClock);
    return FALSE;
  }

  /* initialize other stuff */
  initData(rtOpts, ptpClock);
  initTimer();
  initClock(rtOpts, 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: %02x:%02x:%02x:%02x:%02x:%02x\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: %x.%x.%x.%x\n",
    ptpClock->subdomain_address[0], ptpClock->subdomain_address[1],
    ptpClock->subdomain_address[2], ptpClock->subdomain_address[3]);
  DBG("event port address: %x %x\n",
    ptpClock->event_port_address[0], ptpClock->event_port_address[1]);
  DBG("general port address: %x %x\n",
    ptpClock->general_port_address[0], ptpClock->general_port_address[1]);

  toState(PTP_LISTENING, rtOpts, ptpClock);
  return TRUE;
}

/* handle actions and events for 'port_state' */
void doState(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  UInteger8 state;

  ptpClock->message_activity = FALSE;

  switch(ptpClock->port_state)
  {
  case PTP_LISTENING:
  case PTP_PASSIVE:
  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, rtOpts, ptpClock);
    }
    break;

  default:
    break;
  }

  switch(ptpClock->port_state)
  {
  case PTP_FAULTY:
    /* imaginary troubleshooting */

    DBG("event FAULT_CLEARED\n");
    toState(PTP_INITIALIZING, rtOpts, ptpClock);
    return;

  case PTP_LISTENING:
  case PTP_PASSIVE:
  case PTP_UNCALIBRATED:
  case PTP_SLAVE:
    handle(rtOpts, ptpClock);

    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 && ptpClock->clock_stratum != 255)
      {
        m1(ptpClock);
        toState(PTP_MASTER, rtOpts, ptpClock);
      }
      else if(ptpClock->port_state != PTP_LISTENING)
        toState(PTP_LISTENING, rtOpts, ptpClock);
    }

    break;

  case PTP_MASTER:
    if(timerExpired(SYNC_INTERVAL_TIMER, ptpClock->itimer))
    {
      DBGV("event SYNC_INTERVAL_TIMEOUT_EXPIRES\n");
      issueSync(rtOpts, ptpClock);
    }

    handle(rtOpts, ptpClock);

    break;

  case PTP_DISABLED:
    handle(rtOpts, ptpClock);
    break;

  default:
    DBG("do unrecognized state\n");
    break;
  }
}

/* perform actions required when leaving 'port_state' and entering 'state' */
void toState(UInteger8 state, RunTimeOpts *rtOpts, 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(rtOpts, 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(rtOpts, 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->waitingForFollow = FALSE;
    ptpClock->delay_req_send_time.seconds = ptpClock->delay_req_receive_time.seconds = 0;
    ptpClock->delay_req_send_time.nanoseconds = ptpClock->delay_req_receive_time.nanoseconds = 0;

    timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);

    ptpClock->port_state = PTP_SLAVE;
    break;

  default:
    DBG("to unrecognized state\n");
    break;
  }

  if(rtOpts->displayStats)
    displayStats(rtOpts, ptpClock);
}

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

void handleSync(MsgHeader *header, Octet *msgIbuf, ssize_t length, TimeInternal *time, Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  MsgSync *sync;
  TimeInternal originTimestamp;

  if(length < SYNC_PACKET_LENGTH)
  {
    ERROR("short sync message\n");
    toState(PTP_FAULTY, rtOpts, ptpClock);
    return;
  }

  switch(ptpClock->port_state)
  {
  case PTP_FAULTY:
  case PTP_INITIALIZING:
  case PTP_DISABLED: