protocol.c

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

    DBGV("handleSync: disreguard\n");
    return;

  case PTP_UNCALIBRATED:
  case PTP_SLAVE:
    if(isFromSelf)
    {
      DBG("handleSync: ignore from self\n");
      return;
    }

    if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
      return;

    DBGV("handleSync: looking for uuid %02x:%02x:%02x:%02x:%02x:%02x\n",
      ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2],
      ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]);

    if( header->sequenceId > ptpClock->parent_last_sync_sequence_number
      && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && header->sourcePortId == ptpClock->parent_port_id
      && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
    {
      /* addForeign() takes care of msgUnpackSync() */
      ptpClock->record_update = TRUE;
      sync = addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock);

      if(sync->syncInterval != ptpClock->sync_interval)
      {
        DBGV("message's sync interval is %d, but clock's is %d\n", sync->syncInterval, ptpClock->sync_interval);
        /* spec recommends handling a sync interval discrepancy as a fault */
      }

      ptpClock->sync_receive_time.seconds = time->seconds;
      ptpClock->sync_receive_time.nanoseconds = time->nanoseconds;

      if(!getFlag(header->flags, PTP_ASSIST))
      {
        ptpClock->waitingForFollow = FALSE;

        toInternalTime(&originTimestamp, &sync->originTimestamp, &ptpClock->halfEpoch);
        updateOffset(&originTimestamp, &ptpClock->sync_receive_time,
          &ptpClock->ofm_filt, rtOpts, ptpClock);
        updateClock(rtOpts, ptpClock);
      }
      else
      {
        ptpClock->waitingForFollow = TRUE;
      }

      s1(header, sync, ptpClock);

      if(!(--ptpClock->R))
      {
        issueDelayReq(rtOpts, ptpClock);

        ptpClock->Q = 0;
        ptpClock->R = getRand(&ptpClock->random_seed)%(PTP_DELAY_REQ_INTERVAL - 2) + 2;
        DBG("Q = %d, R = %d\n", ptpClock->Q, ptpClock->R);
      }

      DBGV("SYNC_RECEIPT_TIMER reset\n");
      timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
    }
    else
    {
      DBGV("handleSync: unwanted\n");
    }

  case PTP_MASTER:
  default:
    if( header->sourceCommunicationTechnology == ptpClock->clock_communication_technology
      || header->sourceCommunicationTechnology == PTP_DEFAULT
      || ptpClock->clock_communication_technology == PTP_DEFAULT )
    {
      if(!isFromSelf)
      {
        ptpClock->record_update = TRUE;
        addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock);
      }
      else if(ptpClock->port_state == PTP_MASTER && ptpClock->clock_followup_capable)
      {
        addTime(time, time, &rtOpts->outboundLatency);
        issueFollowup(time, rtOpts, ptpClock);
      }
    }
    break;
  }
}

void handleFollowUp(MsgHeader *header, Octet *msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  MsgFollowUp *follow;
  TimeInternal preciseOriginTimestamp;

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

  switch(ptpClock->port_state)
  {
  case PTP_SLAVE:
    if(isFromSelf)
    {
      DBG("handleFollowUp: ignore from self\n");
      return;
    }

    if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
      return;

    DBGV("handleFollowUp: looking for uuid %02x:%02x:%02x:%02x:%02x:%02x\n",
      ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2],
      ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]);

    follow = &ptpClock->msgTmp.follow;
    msgUnpackFollowUp(ptpClock->msgIbuf, follow);

    if( ptpClock->waitingForFollow
      && follow->associatedSequenceId == ptpClock->parent_last_sync_sequence_number
      && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && header->sourcePortId == ptpClock->parent_port_id
      && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
    {
      ptpClock->waitingForFollow = FALSE;

      toInternalTime(&preciseOriginTimestamp, &follow->preciseOriginTimestamp, &ptpClock->halfEpoch);
      updateOffset(&preciseOriginTimestamp, &ptpClock->sync_receive_time,
        &ptpClock->ofm_filt, rtOpts, ptpClock);
      updateClock(rtOpts, ptpClock);
    }
    else
    {
      DBGV("handleFollowUp: unwanted\n");
    }
    break;

  default:
    DBGV("handleFollowUp: disreguard\n");
    return;
  }
}

void handleDelayReq(MsgHeader *header, Octet *msgIbuf, ssize_t length, TimeInternal *time, Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  if(length < DELAY_REQ_PACKET_LENGTH)
  {
    ERROR("short delay request message\n");
    toState(PTP_FAULTY, rtOpts, ptpClock);
    return;
  }

  switch(ptpClock->port_state)
  {
  case PTP_MASTER:
    if(isFromSelf)
    {
      DBG("handleDelayReq: ignore from self\n");
      return;
    }

    if( header->sourceCommunicationTechnology == ptpClock->clock_communication_technology
      || header->sourceCommunicationTechnology == PTP_DEFAULT
      || ptpClock->clock_communication_technology == PTP_DEFAULT )
    {
      issueDelayResp(time, &ptpClock->msgTmpHeader, rtOpts, ptpClock);
    }

    break;

  case PTP_SLAVE:
    if(isFromSelf)
    {
      DBG("handleDelayReq: self\n");

      ptpClock->delay_req_send_time.seconds = time->seconds;
      ptpClock->delay_req_send_time.nanoseconds = time->nanoseconds;

      addTime(&ptpClock->delay_req_send_time, &ptpClock->delay_req_send_time, &rtOpts->outboundLatency);

      if(ptpClock->delay_req_receive_time.seconds)
      {
        updateDelay(&ptpClock->delay_req_send_time, &ptpClock->delay_req_receive_time,
          &ptpClock->owd_filt, rtOpts, ptpClock);

        ptpClock->delay_req_receive_time.seconds = ptpClock->delay_req_receive_time.seconds = 0;
        ptpClock->delay_req_receive_time.nanoseconds = ptpClock->delay_req_receive_time.nanoseconds = 0;
      }
    }
    break;

  default:
    DBGV("handleDelayReq: disreguard\n");
    return;
  }
}

void handleDelayResp(MsgHeader *header, Octet *msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  MsgDelayResp *resp;

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

  switch(ptpClock->port_state)
  {
  case PTP_SLAVE:
    if(isFromSelf)
    {
      DBG("handleDelayResp: ignore from self\n");
      return;
    }

    resp = &ptpClock->msgTmp.resp;
    msgUnpackDelayResp(ptpClock->msgIbuf, resp);

    if( ptpClock->sentDelayReq
      && resp->requestingSourceSequenceId == ptpClock->sentDelayReqSequenceId
      && resp->requestingSourceCommunicationTechnology == ptpClock->port_communication_technology
      && resp->requestingSourcePortId == ptpClock->port_id_field
      && !memcmp(resp->requestingSourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH)
      && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && header->sourcePortId == ptpClock->parent_port_id
      && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
    {
      ptpClock->sentDelayReq = FALSE;

      toInternalTime(&ptpClock->delay_req_receive_time, &resp->delayReceiptTimestamp, &ptpClock->halfEpoch);

      if(ptpClock->delay_req_send_time.seconds)
      {
        updateDelay(&ptpClock->delay_req_send_time, &ptpClock->delay_req_receive_time,
          &ptpClock->owd_filt, rtOpts, ptpClock);

        ptpClock->delay_req_receive_time.seconds = ptpClock->delay_req_receive_time.seconds = 0;
        ptpClock->delay_req_receive_time.nanoseconds = ptpClock->delay_req_receive_time.nanoseconds = 0;
      }
    }
    else
    {
      DBGV("handleDelayResp: unwanted\n");
    }
    break;

  default:
    DBGV("handleDelayResp: disreguard\n");
    return;
  }
}

void handleManagement(MsgHeader *header, Octet *msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  MsgManagement *manage;

  UInteger8 state;

  if(ptpClock->port_state == PTP_INITIALIZING)
    return;

  manage = &ptpClock->msgTmp.manage;
  msgUnpackManagement(ptpClock->msgIbuf, manage);

  if( (manage->targetCommunicationTechnology == ptpClock->clock_communication_technology
    && !memcmp(manage->targetUuid, ptpClock->clock_uuid_field, PTP_UUID_LENGTH))
    || ((manage->targetCommunicationTechnology == PTP_DEFAULT
    || manage->targetCommunicationTechnology == ptpClock->clock_communication_technology)
    && !sum(manage->targetUuid, PTP_UUID_LENGTH)) )
  {
    switch(manage->managementMessageKey)
    {
    case PTP_MM_OBTAIN_IDENTITY:
    case PTP_MM_GET_DEFAULT_DATA_SET:
    case PTP_MM_GET_CURRENT_DATA_SET:
    case PTP_MM_GET_PARENT_DATA_SET:
    case PTP_MM_GET_PORT_DATA_SET:
    case PTP_MM_GET_GLOBAL_TIME_DATA_SET:
    case PTP_MM_GET_FOREIGN_DATA_SET:
      issueManagement(header, manage, rtOpts, ptpClock);
      break;

    default:
      ptpClock->record_update = TRUE;
      state = msgUnloadManagement(ptpClock->msgIbuf, manage, ptpClock, rtOpts);
      if(state != ptpClock->port_state)
        toState(state, rtOpts, ptpClock);
      break;
    }
  }
  else
  {
    DBG("handleManagement: unwanted\n");
  }
}

/* pack and send various messages */
void issueSync(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  TimeInternal internalTime;
  TimeRepresentation originTimestamp;

  ++ptpClock->last_sync_event_sequence_number;
  ptpClock->grandmaster_sequence_number = ptpClock->last_sync_event_sequence_number;

  getTime(&internalTime);
  fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
  msgPackSync(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock);

  if(!netSendEvent(ptpClock->msgObuf, SYNC_PACKET_LENGTH, &ptpClock->netPath))
    toState(PTP_FAULTY, rtOpts, ptpClock);
  else
    DBGV("sent sync message\n");
}

void issueFollowup(TimeInternal *time, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  TimeRepresentation preciseOriginTimestamp;

  ++ptpClock->last_general_event_sequence_number;

  fromInternalTime(time, &preciseOriginTimestamp, ptpClock->halfEpoch);
  msgPackFollowUp(ptpClock->msgObuf, ptpClock->last_sync_event_sequence_number, &preciseOriginTimestamp, ptpClock);

  if(!netSendGeneral(ptpClock->msgObuf, FOLLOW_UP_PACKET_LENGTH, &ptpClock->netPath))
    toState(PTP_FAULTY, rtOpts, ptpClock);
  else
    DBGV("sent followup message\n");
}

void issueDelayReq(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  TimeInternal internalTime;
  TimeRepresentation originTimestamp;

  ptpClock->sentDelayReq = TRUE;
  ptpClock->sentDelayReqSequenceId = ++ptpClock->last_sync_event_sequence_number;

  getTime(&internalTime);
  fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
  msgPackDelayReq(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock);

  if(!netSendEvent(ptpClock->msgObuf, DELAY_REQ_PACKET_LENGTH, &ptpClock->netPath))
    toState(PTP_FAULTY, rtOpts, ptpClock);
  else
    DBGV("sent delay request message\n");
}

void issueDelayResp(TimeInternal *time, MsgHeader *header, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  TimeRepresentation delayReceiptTimestamp;

  ++ptpClock->last_general_event_sequence_number;

  fromInternalTime(time, &delayReceiptTimestamp, ptpClock->halfEpoch);
  msgPackDelayResp(ptpClock->msgObuf, header, &delayReceiptTimestamp, ptpClock);

  if(!netSendGeneral(ptpClock->msgObuf, DELAY_RESP_PACKET_LENGTH, &ptpClock->netPath))
    toState(PTP_FAULTY, rtOpts, ptpClock);
  else
    DBGV("sent delay response message\n");
}

void issueManagement(MsgHeader *header, MsgManagement *manage, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  UInteger16 length;

  ++ptpClock->last_general_event_sequence_number;

  if(!(length = msgPackManagementResponse(ptpClock->msgObuf, header, manage, ptpClock)))
    return;

  if(!netSendGeneral(ptpClock->msgObuf, length, &ptpClock->netPath))
    toState(PTP_FAULTY, rtOpts, ptpClock);
  else
    DBGV("sent management message\n");
}

/* add or update an entry in the foreign master data set */
MsgSync * addForeign(Octet *buf, MsgHeader *header, PtpClock *ptpClock)
{
  int i, j;
  Boolean found = FALSE;

  DBGV("updateForeign\n");

  j = ptpClock->foreign_record_best;
  for(i = 0; i < ptpClock->number_foreign_records; ++i)
  {
    if(header->sourceCommunicationTechnology == ptpClock->foreign[j].foreign_master_communication_technology
      && header->sourcePortId == ptpClock->foreign[j].foreign_master_port_id
      && !memcmp(header->sourceUuid, ptpClock->foreign[j].foreign_master_uuid, PTP_UUID_LENGTH))
    {
      ++ptpClock->foreign[j].foreign_master_syncs;
      found = TRUE;
      DBGV("updateForeign: update record %d\n", j);
      break;
    }

    j = (j + 1)%ptpClock->number_foreign_records;
  }

  if(!found)
  {
    if(ptpClock->number_foreign_records < ptpClock->max_foreign_records)
      ++ptpClock->number_foreign_records;

    j = ptpClock->foreign_record_i;

    ptpClock->foreign[j].foreign_master_communication_technology =
      header->sourceCommunicationTechnology;
    ptpClock->foreign[j].foreign_master_port_id =
      header->sourcePortId;
    memcpy(ptpClock->foreign[j].foreign_master_uuid,
      header->sourceUuid, PTP_UUID_LENGTH);

    DBG("updateForeign: new record (%d,%d) %d %d %02x:%02x:%02x:%02x:%02x:%02x\n",
      ptpClock->foreign_record_i, ptpClock->number_foreign_records,
      ptpClock->foreign[j].foreign_master_communication_technology,
      ptpClock->foreign[j].foreign_master_port_id,
      ptpClock->foreign[j].foreign_master_uuid[0], ptpClock->foreign[j].foreign_master_uuid[1],
      ptpClock->foreign[j].foreign_master_uuid[2], ptpClock->foreign[j].foreign_master_uuid[3],
      ptpClock->foreign[j].foreign_master_uuid[4], ptpClock->foreign[j].foreign_master_uuid[5]);

    ptpClock->foreign_record_i = (ptpClock->foreign_record_i + 1)%ptpClock->max_foreign_records;
  }

  msgUnpackHeader(buf, &ptpClock->foreign[j].header);
  msgUnpackSync(buf, &ptpClock->foreign[j].sync);

  return &ptpClock->foreign[j].sync;
}