protocol.c

IEEE 1588-2002编程源码

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

void handleSync(MsgHeader *header, MsgSync *sync, TimeRepresentation *time, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  switch(ptpClock->port_state) {
  case PTP_FAULTY:
  case PTP_INITIALIZING:
  case PTP_DISABLED:
    DBGV("handleSync: disreguard\n");
    return;
    
  case PTP_UNCALIBRATED:
  case PTP_SLAVE:
    if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
      return;
    
    DBGV("handleSync: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\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->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && header->sourcePortId == ptpClock->parent_port_id
      && header->sequenceId > ptpClock->parent_last_sync_sequence_number
      && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH))
    {
      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_receipt_time.seconds = time->seconds;
      ptpClock->sync_receipt_time.nanoseconds = time->nanoseconds;
      
      if(!getFlag(header->flags, PTP_ASSIST))
      {
        ptpClock->waitingForFollow = FALSE;
        
        updateOffset(&sync->originTimestamp, &ptpClock->sync_receipt_time,
          &ptpClock->ofm_filt, 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);
      }
    }
  default:
    DBGV("SYNC_RECEIPT_TIMER reset\n");
    timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
    break;
  }
}

void handleFollowUp(MsgHeader *header, MsgFollowUp *follow, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  switch(ptpClock->port_state) {
  case PTP_FAULTY:
  case PTP_INITIALIZING:
  case PTP_DISABLED:
  default:
    DBGV("handleFollowUp: disreguard\n");
    return;
    
  case PTP_SLAVE:
    if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
      return;
    
    DBGV("handleFollowUp: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\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->sourceCommunicationTechnology == ptpClock->parent_communication_technology
       /*&& header->sourcePortId == ptpClock->parent_port_id*/
	&& follow->associatedSequenceId == ptpClock->parent_last_sync_sequence_number
	&& !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH)
       && ptpClock->waitingForFollow)
    {
      ptpClock->waitingForFollow = FALSE;
      
      updateOffset(&follow->preciseOriginTimestamp, &ptpClock->sync_receipt_time,
        &ptpClock->ofm_filt, ptpClock);
      updateClock(rtOpts, ptpClock);
    }
    else
    {
      DBG("handleFollowUp: unwanted\n");
    }
    break;
  }
}

void handleDelayResp(MsgHeader *header, MsgDelayResp *resp, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  switch(ptpClock->port_state) {
  case PTP_FAULTY:
  case PTP_INITIALIZING:
  case PTP_DISABLED:
  default:
    DBGV("handleDelayResp: disreguard\n");
    return;
    
  case PTP_SLAVE:
    if(header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && header->sourcePortId == ptpClock->parent_port_id
      && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH)
      && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
      && 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))
    {
      ptpClock->sentDelayReq = FALSE;
      updateDelay(&ptpClock->delay_req_sending_time, &resp->delayReceiptTimestamp,
        &ptpClock->owd_filt, rtOpts, ptpClock);
    }
    else
    {
      DBGV("handleDelayResp: unwanted\n");
    }
    break;
  }
}

void handleManagement(MsgHeader *header, MsgManagement *manage, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  UInteger8 state;
  
  if(ptpClock->port_state == PTP_INITIALIZING)
    return;
  
  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)
         && manage->targetUuid[0] == '\0') )
  {
    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, ptpClock);
      break;
    }
  }
}

void issueSync(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  ++ptpClock->last_sync_event_sequence_number;
  
  msgPackSync(ptpClock->msgObuf, FALSE, ptpClock);
  
  if( netSendEvent(0, ptpClock->msgObuf, SYNC_PACKET_LENGTH,
      &ptpClock->sync_receipt_time, &ptpClock->netPath) )
  {
    addTime(&ptpClock->sync_receipt_time, &ptpClock->sync_receipt_time, &rtOpts->outboundLatency);
  }
  else
    toState(PTP_FAULTY, ptpClock);
  
  DBGV("event Send %d Message at %lus %ldns\n",
     PTP_SYNC_MESSAGE,
     ptpClock->sync_receipt_time.seconds,
     ptpClock->sync_receipt_time.nanoseconds);
  
  if(!ptpClock->clock_followup_capable)
    return;
  
  ++ptpClock->last_general_event_sequence_number;
  
  msgPackFollowUp(ptpClock->msgObuf, ptpClock->last_sync_event_sequence_number,
    &ptpClock->sync_receipt_time, ptpClock);
  
  if( !netSendGeneral(0, ptpClock->msgObuf, FOLLOW_UP_PACKET_LENGTH, &ptpClock->netPath) )
    toState(PTP_FAULTY, ptpClock);
  
  DBGV("event Send %d Message\n", PTP_FOLLOWUP_MESSAGE);
}

void issueDelayReq(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  ptpClock->sentDelayReq = TRUE;
  ptpClock->sentDelayReqSequenceId = ++ptpClock->last_sync_event_sequence_number;

  msgPackDelayReq(ptpClock->msgObuf, FALSE, ptpClock);
  
  if( netSendEvent(0, ptpClock->msgObuf, DELAY_REQ_PACKET_LENGTH,
      &ptpClock->delay_req_sending_time, &ptpClock->netPath) )
  {
    addTime(&ptpClock->delay_req_sending_time, &ptpClock->delay_req_sending_time, &rtOpts->outboundLatency);
  }
  else
    toState(PTP_FAULTY, ptpClock);
  
  DBGV("event Send %d Message at %lus %ldns\n",
     PTP_DELAY_REQ_MESSAGE,
     ptpClock->delay_req_sending_time.seconds,
     ptpClock->delay_req_sending_time.nanoseconds);
}

void issueDelayResp(TimeRepresentation *time, MsgHeader *header, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  ++ptpClock->last_general_event_sequence_number;
    
  msgPackDelayResp(ptpClock->msgObuf, header, time, ptpClock);
  
  if( !netSendGeneral(0, ptpClock->msgObuf, DELAY_RESP_PACKET_LENGTH, &ptpClock->netPath) )
    toState(PTP_FAULTY, ptpClock);
    
  DBGV("event Send %d Message\n", PTP_DELAY_RESP_MESSAGE);
}

void issueManagement(MsgHeader *header, MsgManagement *manage, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  UInteger16 len;
  
  ++ptpClock->last_general_event_sequence_number;
  
  if(!(len = msgPackManagement(ptpClock->msgObuf, header, manage, ptpClock)))
    return;
  ;
  if( !netSendGeneral(0, ptpClock->msgObuf, len, &ptpClock->netPath) )
    toState(PTP_FAULTY, ptpClock);
  
  DBG("sent management message\n");
}

void initData(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
  DBG("initData\n");
  
  if(rtOpts->slaveOnly)
    rtOpts->clockStratum = 255;
  
  if(rtOpts->resetClock)
    ptpClock->need_reset = TRUE;
  
  /* Port configuration data set */
  ptpClock->last_sync_event_sequence_number = 0;
  ptpClock->last_general_event_sequence_number = 0;
  ptpClock->port_id_field = 1;
  ptpClock->burst_enabled = BURST_ENABLED;
  
  /* Default data set */
  ptpClock->clock_communication_technology = ptpClock->port_communication_technology;
  memcpy(ptpClock->clock_uuid_field, ptpClock->port_uuid_field, PTP_UUID_LENGTH);
  ptpClock->clock_port_id_field = ptpClock->port_id_field;
  ptpClock->clock_stratum = rtOpts->clockStratum;
  memcpy(ptpClock->clock_identifier, rtOpts->clockIdentifier, PTP_CODE_STRING_LENGTH);
  ptpClock->sync_interval = rtOpts->syncInterval;
  
  ptpClock->clock_variance = rtOpts->clockVariance;  /* see spec 7.7 */
  ptpClock->clock_followup_capable = CLOCK_FOLLOWUP;
  ptpClock->preferred = rtOpts->clockPreferred;
  ptpClock->initializable = INITIALIZABLE;
  ptpClock->external_timing = EXTERNAL_TIMING;
  ptpClock->is_boundary_clock = BOUNDARY_CLOCK;
  memcpy(ptpClock->subdomain_name, rtOpts->subdomainName, PTP_SUBDOMAIN_NAME_LENGTH);
  ptpClock->number_ports = NUMBER_PORTS;
  ptpClock->number_foreign_records = 0;
  ptpClock->max_foreign_records = rtOpts->max_foreign_records;
  
  /* Global time properties data set */
  ptpClock->current_utc_offset = rtOpts->currentUtcOffset;
  ptpClock->epoch_number = rtOpts->epochNumber;
  
  /* other stuff */
  ptpClock->random_seed = *(UInteger32*)(ptpClock->port_uuid_field+2);
}

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 %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\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;
}