msg.c

来自「最新版IAR FOR ARM（EWARM）5.11中的代码例子」· C语言 代码 · 共 656 行 · 第 1/3 页

  case PTP_MM_UPDATE_DEFAULT_DATA_SET:
    if(!rtOpts->slaveOnly)
      ptpClock->clock_stratum = *(UInteger8*)(buf + 63);
    memcpy(ptpClock->clock_identifier, buf + 64, 4);
    ptpClock->clock_variance = flip16(*(Integer16*)(buf + 70));
    ptpClock->preferred = *(UInteger8*)(buf + 75);
    rtOpts->syncInterval = *(UInteger8*)(buf + 79);
    memcpy(rtOpts->subdomainName, buf + 80, 16);
    break;

  case PTP_MM_UPDATE_GLOBAL_TIME_PROPERTIES:
    ptpClock->current_utc_offset = flip16(*(Integer16*)(buf + 62));
    ptpClock->leap_59 = *(UInteger8*)(buf + 67);
    ptpClock->leap_61 = *(UInteger8*)(buf + 71);
    ptpClock->epoch_number = flip16(*(UInteger16*)(buf + 74));
    break;

  default:
    break;
  }

  return ptpClock->port_state;
}

void msgUnpackManagementPayload(void *buf, MsgManagement *manage)
{
  switch(manage->managementMessageKey)
  {
  case PTP_MM_CLOCK_IDENTITY:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_CLOCK_IDENTITY\n");
    manage->payload.clockIdentity.clockCommunicationTechnology = *(UInteger8*)(buf + 63);
    memcpy(manage->payload.clockIdentity.clockUuidField, buf + 64, PTP_UUID_LENGTH);
    manage->payload.clockIdentity.clockPortField = flip16(*(UInteger16*)(buf + 74));
    memcpy(manage->payload.clockIdentity.manufacturerIdentity, buf + 76, MANUFACTURER_ID_LENGTH);
    break;

  case PTP_MM_DEFAULT_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_DEFAULT_DATA_SET\n");
    manage->payload.defaultData.clockCommunicationTechnology = *(UInteger8*)(buf + 63);
    memcpy(manage->payload.defaultData.clockUuidField, buf + 64, PTP_UUID_LENGTH);
    manage->payload.defaultData.clockPortField = flip16(*(UInteger16*)(buf + 74));
    manage->payload.defaultData.clockStratum = *(UInteger16*)(buf + 79);
    memcpy(manage->payload.defaultData.clockIdentifier, buf + 80, PTP_CODE_STRING_LENGTH);
    manage->payload.defaultData.clockVariance = flip16(*(UInteger16*)(buf + 86));
    manage->payload.defaultData.clockFollowupCapable = *(UInteger8*)(buf + 91);
    manage->payload.defaultData.preferred = *(UInteger8*)(buf + 95);
    manage->payload.defaultData.initializable = *(UInteger8*)(buf + 99);
    manage->payload.defaultData.externalTiming = *(UInteger8*)(buf + 103);
    manage->payload.defaultData.isBoundaryClock = *(UInteger8*)(buf + 107);
    manage->payload.defaultData.syncInterval = *(UInteger8*)(buf + 111);
    memcpy(manage->payload.defaultData.subdomainName, buf + 112, PTP_SUBDOMAIN_NAME_LENGTH);
    manage->payload.defaultData.numberPorts = flip16(*(UInteger16*)(buf + 130));
    manage->payload.defaultData.numberForeignRecords = flip16(*(UInteger16*)(buf + 134));
    break;

  case PTP_MM_CURRENT_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_CURRENT_DATA_SET\n");
    manage->payload.current.stepsRemoved = flip16(*(UInteger16*)(buf + 62));
    manage->payload.current.offsetFromMaster.seconds = flip32(*(UInteger32*)(buf + 64));
    manage->payload.current.offsetFromMaster.nanoseconds = flip32(*(UInteger32*)(buf + 68));
    manage->payload.current.oneWayDelay.seconds = flip32(*(UInteger32*)(buf + 72));
    manage->payload.current.oneWayDelay.nanoseconds = flip32(*(Integer32*)(buf + 76));
    break;

  case PTP_MM_PARENT_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_PORT_DATA_SET\n");
    manage->payload.parent.parentCommunicationTechnology = *(UInteger8*)(buf + 63);
    memcpy(manage->payload.parent.parentUuid, buf + 64, PTP_UUID_LENGTH);
    manage->payload.parent.parentPortId = flip16(*(UInteger16*)(buf + 74));
    manage->payload.parent.parentLastSyncSequenceNumber = flip16(*(UInteger16*)(buf + 74));
    manage->payload.parent.parentFollowupCapable = *(UInteger8*)(buf + 83);
    manage->payload.parent.parentExternalTiming = *(UInteger8*)(buf + 87);
    manage->payload.parent.parentVariance = flip16(*(UInteger16*)(buf + 90));
    manage->payload.parent.parentStats = *(UInteger8*)(buf + 85);
    manage->payload.parent.observedVariance = flip16(*(Integer16*)(buf + 98));
    manage->payload.parent.observedDrift = flip32(*(Integer32*)(buf + 100));
    manage->payload.parent.utcReasonable = *(UInteger8*)(buf + 107);
    manage->payload.parent.grandmasterCommunicationTechnology = *(UInteger8*)(buf + 111);
    memcpy(manage->payload.parent.grandmasterUuidField, buf + 112, PTP_UUID_LENGTH);
    manage->payload.parent.grandmasterPortIdField = flip16(*(UInteger16*)(buf + 122));
    manage->payload.parent.grandmasterStratum = *(UInteger8*)(buf + 127);
    memcpy(manage->payload.parent.grandmasterIdentifier, buf + 128, PTP_CODE_STRING_LENGTH);
    manage->payload.parent.grandmasterVariance = flip16(*(Integer16*)(buf + 134));
    manage->payload.parent.grandmasterPreferred = *(UInteger8*)(buf + 139);
    manage->payload.parent.grandmasterIsBoundaryClock = *(UInteger8*)(buf + 144);
    manage->payload.parent.grandmasterSequenceNumber = flip16(*(UInteger16*)(buf + 146));
    break;

  case PTP_MM_PORT_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_FOREIGN_DATA_SET\n");
    manage->payload.port.returnedPortNumber = flip16(*(UInteger16*)(buf + 62));
    manage->payload.port.portState = *(UInteger8*)(buf + 67);
    manage->payload.port.lastSyncEventSequenceNumber = flip16(*(UInteger16*)(buf + 70));
    manage->payload.port.lastGeneralEventSequenceNumber = flip16(*(UInteger16*)(buf + 74));
    manage->payload.port.portCommunicationTechnology = *(UInteger8*)(buf + 79);
    memcpy(manage->payload.port.portUuidField, buf + 80, PTP_UUID_LENGTH);
    manage->payload.port.portIdField = flip16(*(UInteger16*)(buf + 90));
    manage->payload.port.burstEnabled = *(UInteger8*)(buf + 95);
    manage->payload.port.subdomainAddressOctets = *(UInteger8*)(buf + 97);
    manage->payload.port.eventPortAddressOctets = *(UInteger8*)(buf + 98);
    manage->payload.port.generalPortAddressOctets = *(UInteger8*)(buf + 99);
    memcpy(manage->payload.port.subdomainAddress, buf + 100, SUBDOMAIN_ADDRESS_LENGTH);
    memcpy(manage->payload.port.eventPortAddress, buf + 106, PORT_ADDRESS_LENGTH);
    memcpy(manage->payload.port.generalPortAddress, buf + 110, PORT_ADDRESS_LENGTH);
    break;

  case PTP_MM_GLOBAL_TIME_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_GLOBAL_TIME_DATA_SET\n");
    manage->payload.globalTime.localTime.seconds = flip32(*(UInteger32*)(buf + 60));
    manage->payload.globalTime.localTime.nanoseconds = flip32(*(Integer32*)(buf + 64));
    manage->payload.globalTime.currentUtcOffset = flip16(*(Integer16*)(buf + 70));
    manage->payload.globalTime.leap59 = *(UInteger8*)(buf + 75);
    manage->payload.globalTime.leap61 = *(UInteger8*)(buf + 79);
    manage->payload.globalTime.epochNumber = flip16(*(UInteger16*)(buf + 82));
    break;


  case PTP_MM_FOREIGN_DATA_SET:
    DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_FOREIGN_DATA_SET\n");
    manage->payload.foreign.returnedPortNumber = flip16(*(UInteger16*)(buf + 62));
    manage->payload.foreign.returnedRecordNumber = flip16(*(UInteger16*)(buf + 68));
    manage->payload.foreign.foreignMasterCommunicationTechnology = *(UInteger8*)(buf + 71);
    memcpy(manage->payload.foreign.foreignMasterUuid, buf + 72, PTP_UUID_LENGTH);
    manage->payload.foreign.foreignMasterPortId = flip16(*(UInteger16*)(buf + 82));
    manage->payload.foreign.foreignMasterSyncs = flip16(*(UInteger16*)(buf + 66));
    break;

  case PTP_MM_NULL:
    DBGV("msgUnloadManagementPayload: managementMessageKey NULL\n");
    break;

  default:
    DBGV("msgUnloadManagementPayload: managementMessageKey ?\n");
    break;
  }

  return;
}

void msgPackHeader(void *buf, PtpClock *ptpClock)
{
  *(Integer32*)(buf + 0) = shift16(flip16(VERSION_PTP), 0) | shift16(flip16(VERSION_NETWORK), 1);
  memcpy((buf + 4), ptpClock->subdomain_name, 16);
  *(Integer32*)(buf + 20) = shift8(ptpClock->port_communication_technology, 1);
  memcpy((buf + 22), ptpClock->port_uuid_field, 6);

  if(ptpClock->external_timing)
    setFlag((buf + 34), PTP_EXT_SYNC);
  if(ptpClock->clock_followup_capable)
    setFlag((buf + 34), PTP_ASSIST);
  if(ptpClock->is_boundary_clock)
    setFlag((buf + 34), PTP_BOUNDARY_CLOCK);
}

void msgPackSync(void *buf, Boolean burst,
  TimeRepresentation *originTimestamp, PtpClock *ptpClock)
{
  *(UInteger8*)(buf +20) = 1;  /* messageType */
  *(Integer32*)(buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_sync_event_sequence_number), 1);
  *(UInteger8*)(buf +32) = PTP_SYNC_MESSAGE;  /* control */
  if(ptpClock->burst_enabled && burst)
    setFlag((buf + 34), PTP_SYNC_BURST);
  else
    clearFlag((buf + 34), PTP_SYNC_BURST);
  if(ptpClock->parent_stats)
    setFlag((buf + 34), PARENT_STATS);
  else
    clearFlag((buf + 34), PARENT_STATS);


  *(Integer32*)(buf + 40) = flip32(originTimestamp->seconds);
  *(Integer32*)(buf + 44) = flip32(originTimestamp->nanoseconds);
  *(Integer32*)(buf + 48) = shift16(flip16(ptpClock->epoch_number), 0) | shift16(flip16(ptpClock->current_utc_offset), 1);
  *(Integer32*)(buf + 52) = shift8(ptpClock->grandmaster_communication_technology, 1);
  memcpy((buf + 54), ptpClock->grandmaster_uuid_field, 6);
  *(Integer32*)(buf + 60) = shift16(flip16(ptpClock->grandmaster_port_id_field), 0) | shift16(flip16(ptpClock->grandmaster_sequence_number), 1);
  *(Integer32*)(buf + 64) = shift8(ptpClock->grandmaster_stratum, 3);
  memcpy((buf + 68), ptpClock->grandmaster_identifier, 4);
  *(Integer32*)(buf + 72) = shift16(flip16(ptpClock->grandmaster_variance), 1);
  *(Integer32*)(buf + 76) = shift16(flip16(ptpClock->grandmaster_preferred), 0) | shift16(flip16(ptpClock->grandmaster_is_boundary_clock), 1);
  *(Integer32*)(buf + 80) = shift16(flip16(ptpClock->sync_interval), 1);
  *(Integer32*)(buf + 84) = shift16(flip16(ptpClock->clock_variance), 1);
  *(Integer32*)(buf + 88) = shift16(flip16(ptpClock->steps_removed), 1);
  *(Integer32*)(buf + 92) = shift8(ptpClock->clock_stratum, 3);
  memcpy((buf + 96), ptpClock->clock_identifier, 4);
  *(Integer32*)(buf + 100) = shift8(ptpClock->parent_communication_technology, 1);
  memcpy((buf + 102), ptpClock->parent_uuid, 6);
  *(Integer32*)(buf + 108) = shift16(flip16(ptpClock->parent_port_id), 1);
  *(Integer32*)(buf + 112) = shift16(flip16(ptpClock->observed_variance), 1);
  *(Integer32*)(buf + 116) = flip32(ptpClock->observed_drift);
  *(Integer32*)(buf + 120) = shift8(ptpClock->utc_reasonable, 3);
}

void msgPackDelayReq(void *buf, Boolean burst,
  TimeRepresentation *originTimestamp, PtpClock *ptpClock)
{
  *(UInteger8*)(buf + 20) = 1;  /* messageType */
  *(Integer32*)(buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_sync_event_sequence_number), 1);
  *(UInteger8*)(buf + 32) = PTP_DELAY_REQ_MESSAGE;  /* control */
  if(ptpClock->burst_enabled && burst)
    setFlag((buf + 34), PTP_SYNC_BURST);
  else
    clearFlag((buf + 34), PTP_SYNC_BURST);
  if(ptpClock->parent_stats)
    setFlag((buf + 34), PARENT_STATS);
  else
    clearFlag((buf + 34), PARENT_STATS);

  *(Integer32*)(buf + 40) = flip32(originTimestamp->seconds);
  *(Integer32*)(buf + 44) = flip32(originTimestamp->nanoseconds);
  *(Integer32*)(buf + 48) = shift16(flip16(ptpClock->epoch_number), 0) | shift16(flip16(ptpClock->current_utc_offset), 1);
  *(Integer32*)(buf + 52) = shift8(ptpClock->grandmaster_communication_technology, 1);
  memcpy((buf + 54), ptpClock->grandmaster_uuid_field, 6);
  *(Integer32*)(buf + 60) = shift16(flip16(ptpClock->grandmaster_port_id_field), 0) | shift16(flip16(ptpClock->grandmaster_sequence_number), 1);
  *(Integer32*)(buf + 64) = shift8(ptpClock->grandmaster_stratum, 3);
  memcpy((buf + 68), ptpClock->grandmaster_identifier, 4);
  *(Integer32*)(buf + 72) = shift16(flip16(ptpClock->grandmaster_variance), 1);
  *(Integer32*)(buf + 76) = shift16(flip16(ptpClock->grandmaster_preferred), 0) | shift16(flip16(ptpClock->grandmaster_is_boundary_clock), 1);