From a6163888f3c56123b1db313743c6147ba498732c Mon Sep 17 00:00:00 2001 From: Yuval Adam Date: Fri, 8 Aug 2014 14:42:07 +0300 Subject: Add third_party libs --- third_party/ptpd-1.1.0/src/dep/constants_dep.h | 99 ++++ third_party/ptpd-1.1.0/src/dep/datatypes_dep.h | 37 ++ third_party/ptpd-1.1.0/src/dep/msg.c | 671 +++++++++++++++++++++++++ third_party/ptpd-1.1.0/src/dep/net.c | 570 +++++++++++++++++++++ third_party/ptpd-1.1.0/src/dep/ptpd_dep.h | 154 ++++++ third_party/ptpd-1.1.0/src/dep/servo.c | 215 ++++++++ third_party/ptpd-1.1.0/src/dep/startup.c | 364 ++++++++++++++ third_party/ptpd-1.1.0/src/dep/sys.c | 282 +++++++++++ third_party/ptpd-1.1.0/src/dep/timer.c | 96 ++++ 9 files changed, 2488 insertions(+) create mode 100644 third_party/ptpd-1.1.0/src/dep/constants_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep/datatypes_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep/msg.c create mode 100644 third_party/ptpd-1.1.0/src/dep/net.c create mode 100644 third_party/ptpd-1.1.0/src/dep/ptpd_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep/servo.c create mode 100644 third_party/ptpd-1.1.0/src/dep/startup.c create mode 100644 third_party/ptpd-1.1.0/src/dep/sys.c create mode 100644 third_party/ptpd-1.1.0/src/dep/timer.c (limited to 'third_party/ptpd-1.1.0/src/dep') diff --git a/third_party/ptpd-1.1.0/src/dep/constants_dep.h b/third_party/ptpd-1.1.0/src/dep/constants_dep.h new file mode 100644 index 0000000..54afaf1 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/constants_dep.h @@ -0,0 +1,99 @@ +/** + * @file constants_dep.h + * @date Tue Jul 20 16:16:23 2010 + * + * @brief Platform dependent constants + * + * + */ + +#ifndef CONSTANTS_DEP_H +#define CONSTANTS_DEP_H + +#include + +#if !defined(linux) && !defined(__NetBSD__) && !defined(__FreeBSD__) +#error Not ported to this architecture, please update. +#endif + +#ifdef linux +#include +#include +#include +#define IFACE_NAME_LENGTH IF_NAMESIZE +#define NET_ADDRESS_LENGTH INET_ADDRSTRLEN + +#define IFCONF_LENGTH 10 + +#include +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define PTPD_LSBF +#elif __BYTE_ORDER == __BIG_ENDIAN +#define PTPD_MSBF +#endif +#endif /* linux */ + + +#if defined(__NetBSD__) || defined(__FreeBSD__) +#include +#include +#include +#include +#include +#include +#if defined(__FreeBSD__) +#include +#include +#else +#include +#endif +#include +#define IFACE_NAME_LENGTH IF_NAMESIZE +#define NET_ADDRESS_LENGTH INET_ADDRSTRLEN + +#define IFCONF_LENGTH 10 + +#define adjtimex ntp_adjtime + +#include +#if BYTE_ORDER == LITTLE_ENDIAN +#define PTPD_LSBF +#elif BYTE_ORDER == BIG_ENDIAN +#define PTPD_MSBF +#endif +#endif + + +#define ADJ_FREQ_MAX 512000 + +/* UDP/IPv4 dependent */ + +#define SUBDOMAIN_ADDRESS_LENGTH 4 +#define PORT_ADDRESS_LENGTH 2 + +#define PACKET_SIZE 300 + +#define PTP_EVENT_PORT 319 +#define PTP_GENERAL_PORT 320 + +#define DEFAULT_PTP_DOMAIN_ADDRESS "224.0.1.129" +#define ALTERNATE_PTP_DOMAIN1_ADDRESS "224.0.1.130" +#define ALTERNATE_PTP_DOMAIN2_ADDRESS "224.0.1.131" +#define ALTERNATE_PTP_DOMAIN3_ADDRESS "224.0.1.132" + +#define HEADER_LENGTH 40 +#define SYNC_PACKET_LENGTH 124 +#define DELAY_REQ_PACKET_LENGTH 124 +#define FOLLOW_UP_PACKET_LENGTH 52 +#define DELAY_RESP_PACKET_LENGTH 60 +#define MANAGEMENT_PACKET_LENGTH 136 + +#define MM_STARTING_BOUNDARY_HOPS 0x7fff + +/* others */ + +#define SCREEN_BUFSZ 128 +#define SCREEN_MAXSZ 80 + +#define MAXTIMESTR 32 +#endif diff --git a/third_party/ptpd-1.1.0/src/dep/datatypes_dep.h b/third_party/ptpd-1.1.0/src/dep/datatypes_dep.h new file mode 100644 index 0000000..b105130 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/datatypes_dep.h @@ -0,0 +1,37 @@ +/** + * @file datatypes_dep.h + * @date Tue Jul 20 16:16:48 2010 + * + * @brief Platform dependent datatypes. + * + * + */ + +#ifndef DATATYPES_DEP_H +#define DATATYPES_DEP_H + +typedef enum { + FALSE = 0, TRUE +} Boolean; +typedef char Octet; +typedef signed char Integer8; +typedef signed short Integer16; +typedef signed int Integer32; +typedef unsigned char UInteger8; +typedef unsigned short UInteger16; +typedef unsigned int UInteger32; + +typedef struct { + Integer32 nsec_prev, y; +} offset_from_master_filter; + +typedef struct { + Integer32 nsec_prev, y; + Integer32 s_exp; +} one_way_delay_filter; + +typedef struct { + Integer32 eventSock, generalSock, multicastAddr, unicastAddr; +} NetPath; + +#endif diff --git a/third_party/ptpd-1.1.0/src/dep/msg.c b/third_party/ptpd-1.1.0/src/dep/msg.c new file mode 100644 index 0000000..cd2dbb9 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/msg.c @@ -0,0 +1,671 @@ +/** + * @file msg.c + * @author George Neville-Neil + * @date Tue Jul 20 16:17:05 2010 + * + * @brief Functions to pack and unpack messages. + * + * See spec annex d + */ + + +#include "../ptpd.h" + +Boolean +msgPeek(void *buf, ssize_t length) +{ + /* not imlpemented yet */ + return TRUE; +} + +void +msgUnpackHeader(void *buf, MsgHeader * header) +{ + header->versionPTP = flip16(*(UInteger16 *) (buf + 0)); + header->versionNetwork = flip16(*(UInteger16 *) (buf + 2)); + DBGV("msgUnpackHeader: versionPTP %d\n", header->versionPTP); + DBGV("msgUnpackHeader: versionNetwork %d\n", header->versionNetwork); + + memcpy(header->subdomain, (buf + 4), 16); + DBGV("msgUnpackHeader: subdomain %s\n", header->subdomain); + + header->messageType = *(UInteger8 *) (buf + 20); + header->sourceCommunicationTechnology = *(UInteger8 *) (buf + 21); + DBGV("msgUnpackHeader: messageType %d\n", header->messageType); + DBGV("msgUnpackHeader: sourceCommunicationTechnology %d\n", header->sourceCommunicationTechnology); + + memcpy(header->sourceUuid, (buf + 22), 6); + DBGV("msgUnpackHeader: sourceUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + header->sourceUuid[0], header->sourceUuid[1], header->sourceUuid[2], + header->sourceUuid[3], header->sourceUuid[4], header->sourceUuid[5]); + + header->sourcePortId = flip16(*(UInteger16 *) (buf + 28)); + header->sequenceId = flip16(*(UInteger16 *) (buf + 30)); + DBGV("msgUnpackHeader: sourcePortId %d\n", header->sourcePortId); + DBGV("msgUnpackHeader: sequenceId %d\n", header->sequenceId); + + header->control = *(UInteger8 *) (buf + 32); + DBGV("msgUnpackHeader: control %d\n", header->control); + + memcpy(header->flags, (buf + 34), 2); + DBGV("msgUnpackHeader: flags %02hhx %02hhx\n", header->flags[0], header->flags[1]); +} + +void +msgUnpackSync(void *buf, MsgSync * sync) +{ + sync->originTimestamp.seconds = flip32(*(UInteger32 *) (buf + 40)); + DBGV("msgUnpackSync: originTimestamp.seconds %u\n", sync->originTimestamp.seconds); + sync->originTimestamp.nanoseconds = flip32(*(Integer32 *) (buf + 44)); + DBGV("msgUnpackSync: originTimestamp.nanoseconds %d\n", sync->originTimestamp.nanoseconds); + sync->epochNumber = flip16(*(UInteger16 *) (buf + 48)); + DBGV("msgUnpackSync: epochNumber %d\n", sync->epochNumber); + sync->currentUTCOffset = flip16(*(Integer16 *) (buf + 50)); + DBGV("msgUnpackSync: currentUTCOffset %d\n", sync->currentUTCOffset); + sync->grandmasterCommunicationTechnology = *(UInteger8 *) (buf + 53); + DBGV("msgUnpackSync: grandmasterCommunicationTechnology %d\n", sync->grandmasterCommunicationTechnology); + memcpy(sync->grandmasterClockUuid, (buf + 54), 6); + DBGV("msgUnpackSync: grandmasterClockUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + sync->grandmasterClockUuid[0], sync->grandmasterClockUuid[1], sync->grandmasterClockUuid[2], + sync->grandmasterClockUuid[3], sync->grandmasterClockUuid[4], sync->grandmasterClockUuid[5]); + sync->grandmasterPortId = flip16(*(UInteger16 *) (buf + 60)); + DBGV("msgUnpackSync: grandmasterPortId %d\n", sync->grandmasterPortId); + sync->grandmasterSequenceId = flip16(*(UInteger16 *) (buf + 62)); + DBGV("msgUnpackSync: grandmasterSequenceId %d\n", sync->grandmasterSequenceId); + sync->grandmasterClockStratum = *(UInteger8 *) (buf + 67); + DBGV("msgUnpackSync: grandmasterClockStratum %d\n", sync->grandmasterClockStratum); + memcpy(sync->grandmasterClockIdentifier, (buf + 68), 4); + DBGV("msgUnpackSync: grandmasterClockIdentifier %c%c%c%c\n", + sync->grandmasterClockIdentifier[0], sync->grandmasterClockIdentifier[1], + sync->grandmasterClockIdentifier[2], sync->grandmasterClockIdentifier[3]); + sync->grandmasterClockVariance = flip16(*(Integer16 *) (buf + 74)); + DBGV("msgUnpackSync: grandmasterClockVariance %d\n", sync->grandmasterClockVariance); + sync->grandmasterPreferred = *(UInteger8 *) (buf + 77); + DBGV("msgUnpackSync: grandmasterPreferred %d\n", sync->grandmasterPreferred); + sync->grandmasterIsBoundaryClock = *(UInteger8 *) (buf + 79); + DBGV("msgUnpackSync: grandmasterIsBoundaryClock %d\n", sync->grandmasterIsBoundaryClock); + sync->syncInterval = *(Integer8 *) (buf + 83); + DBGV("msgUnpackSync: syncInterval %d\n", sync->syncInterval); + sync->localClockVariance = flip16(*(Integer16 *) (buf + 86)); + DBGV("msgUnpackSync: localClockVariance %d\n", sync->localClockVariance); + sync->localStepsRemoved = flip16(*(UInteger16 *) (buf + 90)); + DBGV("msgUnpackSync: localStepsRemoved %d\n", sync->localStepsRemoved); + sync->localClockStratum = *(UInteger8 *) (buf + 95); + DBGV("msgUnpackSync: localClockStratum %d\n", sync->localClockStratum); + memcpy(sync->localClockIdentifer, (buf + 96), PTP_CODE_STRING_LENGTH); + DBGV("msgUnpackSync: localClockIdentifer %c%c%c%c\n", + sync->localClockIdentifer[0], sync->localClockIdentifer[1], + sync->localClockIdentifer[2], sync->localClockIdentifer[3]); + sync->parentCommunicationTechnology = *(UInteger8 *) (buf + 101); + DBGV("msgUnpackSync: parentCommunicationTechnology %d\n", sync->parentCommunicationTechnology); + memcpy(sync->parentUuid, (buf + 102), PTP_UUID_LENGTH); + DBGV("msgUnpackSync: parentUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + sync->parentUuid[0], sync->parentUuid[1], sync->parentUuid[2], + sync->parentUuid[3], sync->parentUuid[4], sync->parentUuid[5]); + sync->parentPortField = flip16(*(UInteger16 *) (buf + 110)); + DBGV("msgUnpackSync: parentPortField %d\n", sync->parentPortField); + sync->estimatedMasterVariance = flip16(*(Integer16 *) (buf + 114)); + DBGV("msgUnpackSync: estimatedMasterVariance %d\n", sync->estimatedMasterVariance); + sync->estimatedMasterDrift = flip32(*(Integer32 *) (buf + 116)); + DBGV("msgUnpackSync: estimatedMasterDrift %d\n", sync->estimatedMasterDrift); + sync->utcReasonable = *(UInteger8 *) (buf + 123); + DBGV("msgUnpackSync: utcReasonable %d\n", sync->utcReasonable); +} + +void +msgUnpackDelayReq(void *buf, MsgDelayReq * req) +{ +} + +void +msgUnpackFollowUp(void *buf, MsgFollowUp * follow) +{ + follow->associatedSequenceId = flip16(*(UInteger16 *) (buf + 42)); + DBGV("msgUnpackFollowUp: associatedSequenceId %u\n", follow->associatedSequenceId); + follow->preciseOriginTimestamp.seconds = flip32(*(UInteger32 *) (buf + 44)); + DBGV("msgUnpackFollowUp: preciseOriginTimestamp.seconds %u\n", follow->preciseOriginTimestamp.seconds); + follow->preciseOriginTimestamp.nanoseconds = flip32(*(Integer32 *) (buf + 48)); + DBGV("msgUnpackFollowUp: preciseOriginTimestamp.nanoseconds %d\n", follow->preciseOriginTimestamp.nanoseconds); +} + +void +msgUnpackDelayResp(void *buf, MsgDelayResp * resp) +{ + resp->delayReceiptTimestamp.seconds = flip32(*(UInteger32 *) (buf + 40)); + DBGV("msgUnpackDelayResp: delayReceiptTimestamp.seconds %u\n", resp->delayReceiptTimestamp.seconds); + resp->delayReceiptTimestamp.nanoseconds = flip32(*(Integer32 *) (buf + 44)); + DBGV("msgUnpackDelayResp: delayReceiptTimestamp.nanoseconds %d\n", resp->delayReceiptTimestamp.nanoseconds); + resp->requestingSourceCommunicationTechnology = *(UInteger8 *) (buf + 49); + DBGV("msgUnpackDelayResp: requestingSourceCommunicationTechnology %d\n", resp->requestingSourceCommunicationTechnology); + memcpy(resp->requestingSourceUuid, (buf + 50), 6); + DBGV("msgUnpackDelayResp: requestingSourceUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + resp->requestingSourceUuid[0], resp->requestingSourceUuid[1], resp->requestingSourceUuid[2], + resp->requestingSourceUuid[3], resp->requestingSourceUuid[4], resp->requestingSourceUuid[5]); + resp->requestingSourcePortId = flip16(*(UInteger16 *) (buf + 56)); + DBGV("msgUnpackDelayResp: requestingSourcePortId %d\n", resp->requestingSourcePortId); + resp->requestingSourceSequenceId = flip16(*(UInteger16 *) (buf + 58)); + DBGV("msgUnpackDelayResp: requestingSourceSequenceId %d\n", resp->requestingSourceSequenceId); +} + +void +msgUnpackManagement(void *buf, MsgManagement * manage) +{ + manage->targetCommunicationTechnology = *(UInteger8 *) (buf + 41); + DBGV("msgUnpackManagement: targetCommunicationTechnology %d\n", manage->targetCommunicationTechnology); + memcpy(manage->targetUuid, (buf + 42), 6); + DBGV("msgUnpackManagement: targetUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + manage->targetUuid[0], manage->targetUuid[1], manage->targetUuid[2], + manage->targetUuid[3], manage->targetUuid[4], manage->targetUuid[5]); + manage->targetPortId = flip16(*(UInteger16 *) (buf + 48)); + DBGV("msgUnpackManagement: targetPortId %d\n", manage->targetPortId); + manage->startingBoundaryHops = flip16(*(Integer16 *) (buf + 50)); + DBGV("msgUnpackManagement: startingBoundaryHops %d\n", manage->startingBoundaryHops); + manage->boundaryHops = flip16(*(Integer16 *) (buf + 52)); + DBGV("msgUnpackManagement: boundaryHops %d\n", manage->boundaryHops); + manage->managementMessageKey = *(UInteger8 *) (buf + 55); + DBGV("msgUnpackManagement: managementMessageKey %d\n", manage->managementMessageKey); + manage->parameterLength = flip16(*(UInteger16 *) (buf + 58)); + DBGV("msgUnpackManagement: parameterLength %d\n", manage->parameterLength); + + if (manage->managementMessageKey == PTP_MM_GET_FOREIGN_DATA_SET) + manage->recordKey = flip16(*(UInteger16 *) (buf + 62)); +} + +UInteger8 +msgUnloadManagement(void *buf, MsgManagement * manage, + PtpClock * ptpClock, RunTimeOpts * rtOpts) +{ + TimeInternal internalTime; + TimeRepresentation externalTime; + + switch (manage->managementMessageKey) { + case PTP_MM_INITIALIZE_CLOCK: + if (ptpClock->initializable) + return PTP_INITIALIZING; + break; + + case PTP_MM_GOTO_FAULTY_STATE: + DBG("event FAULT_DETECTED (forced by management message)\n"); + return PTP_FAULTY; + break; + + case PTP_MM_DISABLE_PORT: + if (manage->targetPortId == 1) { + DBG("event DESIGNATED_DISABLED\n"); + return PTP_DISABLED; + } + break; + + case PTP_MM_ENABLE_PORT: + if (manage->targetPortId == 1) { + DBG("event DESIGNATED_ENABLED\n"); + return PTP_INITIALIZING; + } + break; + + case PTP_MM_CLEAR_DESIGNATED_PREFERRED_MASTER: + ptpClock->preferred = FALSE; + break; + + case PTP_MM_SET_DESIGNATED_PREFERRED_MASTER: + ptpClock->preferred = TRUE; + break; + + case PTP_MM_DISABLE_BURST: + break; + + case PTP_MM_ENABLE_BURST: + break; + + case PTP_MM_SET_SYNC_INTERVAL: + rtOpts->syncInterval = *(Integer8 *) (buf + 63); + break; + + case PTP_MM_SET_SUBDOMAIN: + memcpy(rtOpts->subdomainName, buf + 60, 16); + DBG("set subdomain to %s\n", rtOpts->subdomainName); + break; + + case PTP_MM_SET_TIME: + externalTime.seconds = flip32(*(UInteger32 *) (buf + 60)); + externalTime.nanoseconds = flip32(*(Integer32 *) (buf + 64)); + toInternalTime(&internalTime, &externalTime, &ptpClock->halfEpoch); + setTime(&internalTime); + break; + + 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); + *(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 +msgPackFollowUp(void *buf, UInteger16 associatedSequenceId, + TimeRepresentation * preciseOriginTimestamp, PtpClock * ptpClock) +{ + *(UInteger8 *) (buf + 20) = 2; /* messageType */ + *(Integer32 *) (buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1); + *(UInteger8 *) (buf + 32) = PTP_FOLLOWUP_MESSAGE; /* control */ + clearFlag((buf + 34), PTP_SYNC_BURST); + clearFlag((buf + 34), PARENT_STATS); + + *(Integer32 *) (buf + 40) = shift16(flip16(associatedSequenceId), 1); + *(Integer32 *) (buf + 44) = flip32(preciseOriginTimestamp->seconds); + *(Integer32 *) (buf + 48) = flip32(preciseOriginTimestamp->nanoseconds); +} + +void +msgPackDelayResp(void *buf, MsgHeader * header, + TimeRepresentation * delayReceiptTimestamp, PtpClock * ptpClock) +{ + *(UInteger8 *) (buf + 20) = 2; /* messageType */ + *(Integer32 *) (buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1); + *(UInteger8 *) (buf + 32) = PTP_DELAY_RESP_MESSAGE; /* control */ + clearFlag((buf + 34), PTP_SYNC_BURST); + clearFlag((buf + 34), PARENT_STATS); + + *(Integer32 *) (buf + 40) = flip32(delayReceiptTimestamp->seconds); + *(Integer32 *) (buf + 44) = flip32(delayReceiptTimestamp->nanoseconds); + *(Integer32 *) (buf + 48) = shift8(header->sourceCommunicationTechnology, 1); + memcpy(buf + 50, header->sourceUuid, 6); + *(Integer32 *) (buf + 56) = shift16(flip16(header->sourcePortId), 0) | shift16(flip16(header->sequenceId), 1); +} + +UInteger16 +msgPackManagement(void *buf, MsgManagement * manage, PtpClock * ptpClock) +{ + *(UInteger8 *) (buf + 20) = 2; /* messageType */ + *(Integer32 *) (buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1); + *(UInteger8 *) (buf + 32) = PTP_MANAGEMENT_MESSAGE; /* control */ + clearFlag((buf + 34), PTP_SYNC_BURST); + clearFlag((buf + 34), PARENT_STATS); + *(Integer32 *) (buf + 40) = shift8(manage->targetCommunicationTechnology, 1); + memcpy(buf + 42, manage->targetUuid, 6); + *(Integer32 *) (buf + 48) = shift16(flip16(manage->targetPortId), 0) | shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 1); + *(Integer32 *) (buf + 52) = shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 0); + + *(UInteger8 *) (buf + 55) = manage->managementMessageKey; + + switch (manage->managementMessageKey) { + case PTP_MM_GET_FOREIGN_DATA_SET: + *(UInteger16 *) (buf + 62) = manage->recordKey; + *(Integer32 *) (buf + 56) = shift16(flip16(4), 1); + return 64; + + default: + *(Integer32 *) (buf + 56) = shift16(flip16(0), 1); + return 60; + } +} + +UInteger16 +msgPackManagementResponse(void *buf, MsgHeader * header, MsgManagement * manage, PtpClock * ptpClock) +{ + TimeInternal internalTime; + TimeRepresentation externalTime; + + *(UInteger8 *) (buf + 20) = 2; /* messageType */ + *(Integer32 *) (buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1); + *(UInteger8 *) (buf + 32) = PTP_MANAGEMENT_MESSAGE; /* control */ + clearFlag((buf + 34), PTP_SYNC_BURST); + clearFlag((buf + 34), PARENT_STATS); + *(Integer32 *) (buf + 40) = shift8(header->sourceCommunicationTechnology, 1); + memcpy(buf + 42, header->sourceUuid, 6); + *(Integer32 *) (buf + 48) = shift16(flip16(header->sourcePortId), 0) | shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 1); + *(Integer32 *) (buf + 52) = shift16(flip16(manage->startingBoundaryHops - manage->boundaryHops + 1), 0); + + switch (manage->managementMessageKey) { + case PTP_MM_OBTAIN_IDENTITY: + *(UInteger8 *) (buf + 55) = PTP_MM_CLOCK_IDENTITY; + *(Integer32 *) (buf + 56) = shift16(flip16(64), 1); + *(Integer32 *) (buf + 60) = shift8(ptpClock->clock_communication_technology, 3); + memcpy(buf + 64, ptpClock->clock_uuid_field, 6); + *(Integer32 *) (buf + 72) = shift16(flip16(ptpClock->clock_port_id_field), 1); + memcpy((buf + 76), MANUFACTURER_ID, 48); + return 124; + + case PTP_MM_GET_DEFAULT_DATA_SET: + *(UInteger8 *) (buf + 55) = PTP_MM_DEFAULT_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(76), 1); + *(Integer32 *) (buf + 60) = shift8(ptpClock->clock_communication_technology, 3); + memcpy(buf + 64, ptpClock->clock_uuid_field, 6); + *(Integer32 *) (buf + 72) = shift16(flip16(ptpClock->clock_port_id_field), 1); + *(Integer32 *) (buf + 76) = shift8(ptpClock->clock_stratum, 3); + memcpy(buf + 80, ptpClock->clock_identifier, 4); + *(Integer32 *) (buf + 84) = shift16(flip16(ptpClock->clock_variance), 1); + *(Integer32 *) (buf + 88) = shift8(ptpClock->clock_followup_capable, 3); + *(Integer32 *) (buf + 92) = shift8(ptpClock->preferred, 3); + *(Integer32 *) (buf + 96) = shift8(ptpClock->initializable, 3); + *(Integer32 *) (buf + 100) = shift8(ptpClock->external_timing, 3); + *(Integer32 *) (buf + 104) = shift8(ptpClock->is_boundary_clock, 3); + *(Integer32 *) (buf + 108) = shift8(ptpClock->sync_interval, 3); + memcpy(buf + 112, ptpClock->subdomain_name, 16); + *(Integer32 *) (buf + 128) = shift16(flip16(ptpClock->number_ports), 1); + *(Integer32 *) (buf + 132) = shift16(flip16(ptpClock->number_foreign_records), 1); + return 136; + + case PTP_MM_GET_CURRENT_DATA_SET: + *(UInteger8 *) (buf + 55) = PTP_MM_CURRENT_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(20), 1); + *(Integer32 *) (buf + 60) = shift16(flip16(ptpClock->steps_removed), 1); + + fromInternalTime(&ptpClock->offset_from_master, &externalTime, 0); + *(Integer32 *) (buf + 64) = flip32(externalTime.seconds); + *(Integer32 *) (buf + 68) = flip32(externalTime.nanoseconds); + + fromInternalTime(&ptpClock->one_way_delay, &externalTime, 0); + *(Integer32 *) (buf + 72) = flip32(externalTime.seconds); + *(Integer32 *) (buf + 76) = flip32(externalTime.nanoseconds); + return 80; + + case PTP_MM_GET_PARENT_DATA_SET: + *(UInteger8 *) (buf + 55) = PTP_MM_PARENT_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(90), 1); + *(Integer32 *) (buf + 60) = shift8(ptpClock->parent_communication_technology, 3); + memcpy(buf + 64, ptpClock->parent_uuid, 6); + *(Integer32 *) (buf + 72) = shift16(flip16(ptpClock->parent_port_id), 1); + *(Integer32 *) (buf + 76) = shift16(flip16(ptpClock->parent_last_sync_sequence_number), 1); + *(Integer32 *) (buf + 80) = shift8(ptpClock->parent_followup_capable, 1); + *(Integer32 *) (buf + 84) = shift8(ptpClock->parent_external_timing, 3); + *(Integer32 *) (buf + 88) = shift16(flip16(ptpClock->parent_variance), 1); + *(Integer32 *) (buf + 92) = shift8(ptpClock->parent_stats, 3); + *(Integer32 *) (buf + 96) = shift16(flip16(ptpClock->observed_variance), 1); + *(Integer32 *) (buf + 100) = flip32(ptpClock->observed_drift); + *(Integer32 *) (buf + 104) = shift8(ptpClock->utc_reasonable, 3); + *(Integer32 *) (buf + 108) = shift8(ptpClock->grandmaster_communication_technology, 3); + memcpy(buf + 112, ptpClock->grandmaster_uuid_field, 6); + *(Integer32 *) (buf + 120) = shift16(flip16(ptpClock->grandmaster_port_id_field), 1); + *(Integer32 *) (buf + 124) = shift8(ptpClock->grandmaster_stratum, 3); + memcpy(buf + 128, ptpClock->grandmaster_identifier, 4); + *(Integer32 *) (buf + 132) = shift16(flip16(ptpClock->grandmaster_variance), 1); + *(Integer32 *) (buf + 136) = shift8(ptpClock->grandmaster_preferred, 3); + *(Integer32 *) (buf + 140) = shift8(ptpClock->grandmaster_is_boundary_clock, 3); + *(Integer32 *) (buf + 144) = shift16(flip16(ptpClock->grandmaster_sequence_number), 1); + return 148; + + case PTP_MM_GET_PORT_DATA_SET: + if (manage->targetPortId && manage->targetPortId != ptpClock->port_id_field) { + *(UInteger8 *) (buf + 55) = PTP_MM_NULL; + *(Integer32 *) (buf + 56) = shift16(flip16(0), 1); + return 0; + } + *(UInteger8 *) (buf + 55) = PTP_MM_PORT_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(52), 1); + *(Integer32 *) (buf + 60) = shift16(flip16(ptpClock->port_id_field), 1); + *(Integer32 *) (buf + 64) = shift8(ptpClock->port_state, 3); + *(Integer32 *) (buf + 68) = shift16(flip16(ptpClock->last_sync_event_sequence_number), 1); + *(Integer32 *) (buf + 72) = shift16(flip16(ptpClock->last_general_event_sequence_number), 1); + *(Integer32 *) (buf + 76) = shift8(ptpClock->port_communication_technology, 3); + memcpy(buf + 80, ptpClock->port_uuid_field, 6); + *(Integer32 *) (buf + 88) = shift16(flip16(ptpClock->port_id_field), 1); + *(Integer32 *) (buf + 92) = shift8(ptpClock->burst_enabled, 3); + *(Integer32 *) (buf + 96) = shift8(4, 1) | shift8(2, 2) | shift8(2, 3); + memcpy(buf + 100, ptpClock->subdomain_address, 4); + memcpy(buf + 106, ptpClock->event_port_address, 2); + memcpy(buf + 110, ptpClock->general_port_address, 2); + return 112; + + case PTP_MM_GET_GLOBAL_TIME_DATA_SET: + *(UInteger8 *) (buf + 55) = PTP_MM_GLOBAL_TIME_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(24), 1); + + getTime(&internalTime); + fromInternalTime(&internalTime, &externalTime, ptpClock->halfEpoch); + *(Integer32 *) (buf + 60) = flip32(externalTime.seconds); + *(Integer32 *) (buf + 64) = flip32(externalTime.nanoseconds); + + *(Integer32 *) (buf + 68) = shift16(flip16(ptpClock->current_utc_offset), 1); + *(Integer32 *) (buf + 72) = shift8(ptpClock->leap_59, 3); + *(Integer32 *) (buf + 76) = shift8(ptpClock->leap_61, 3); + *(Integer32 *) (buf + 80) = shift16(flip16(ptpClock->epoch_number), 1); + return 84; + + case PTP_MM_GET_FOREIGN_DATA_SET: + if ((manage->targetPortId && manage->targetPortId != ptpClock->port_id_field) + || !manage->recordKey || manage->recordKey > ptpClock->number_foreign_records) { + *(UInteger8 *) (buf + 55) = PTP_MM_NULL; + *(Integer32 *) (buf + 56) = shift16(flip16(0), 1); + return 0; + } + *(UInteger8 *) (buf + 55) = PTP_MM_FOREIGN_DATA_SET; + *(Integer32 *) (buf + 56) = shift16(flip16(28), 1); + *(Integer32 *) (buf + 60) = shift16(flip16(ptpClock->port_id_field), 1); + *(Integer32 *) (buf + 64) = shift16(flip16(manage->recordKey - 1), 1); + *(Integer32 *) (buf + 68) = shift8(ptpClock->foreign[manage->recordKey - 1].foreign_master_communication_technology, 3); + memcpy(buf + 72, ptpClock->foreign[manage->recordKey - 1].foreign_master_uuid, 6); + *(Integer32 *) (buf + 80) = shift16(flip16(ptpClock->foreign[manage->recordKey - 1].foreign_master_port_id), 1); + *(Integer32 *) (buf + 84) = shift16(flip16(ptpClock->foreign[manage->recordKey - 1].foreign_master_syncs), 1); + return 88; + + default: + return 0; + } +} diff --git a/third_party/ptpd-1.1.0/src/dep/net.c b/third_party/ptpd-1.1.0/src/dep/net.c new file mode 100644 index 0000000..76c1ac3 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/net.c @@ -0,0 +1,570 @@ +/** + * @file net.c + * @date Tue Jul 20 16:17:49 2010 + * + * @brief Functions to interact with the network sockets and NIC driver. + * + * + */ + +#include "../ptpd.h" + +Boolean +lookupSubdomainAddress(Octet * subdomainName, Octet * subdomainAddress) +{ + UInteger32 h; + + /* set multicast group address based on subdomainName */ + if (!memcmp(subdomainName, DEFAULT_PTP_DOMAIN_NAME, PTP_SUBDOMAIN_NAME_LENGTH)) + memcpy(subdomainAddress, DEFAULT_PTP_DOMAIN_ADDRESS, NET_ADDRESS_LENGTH); + else if (!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN1_NAME, PTP_SUBDOMAIN_NAME_LENGTH)) + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN1_ADDRESS, NET_ADDRESS_LENGTH); + else if (!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN2_NAME, PTP_SUBDOMAIN_NAME_LENGTH)) + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN2_ADDRESS, NET_ADDRESS_LENGTH); + else if (!memcmp(subdomainName, ALTERNATE_PTP_DOMAIN3_NAME, PTP_SUBDOMAIN_NAME_LENGTH)) + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN3_ADDRESS, NET_ADDRESS_LENGTH); + else { + h = crc_algorithm(subdomainName, PTP_SUBDOMAIN_NAME_LENGTH) % 3; + switch (h) { + case 0: + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN1_ADDRESS, NET_ADDRESS_LENGTH); + break; + case 1: + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN2_ADDRESS, NET_ADDRESS_LENGTH); + break; + case 2: + memcpy(subdomainAddress, ALTERNATE_PTP_DOMAIN3_ADDRESS, NET_ADDRESS_LENGTH); + break; + default: + ERROR("handle out of range for '%s'!\n", subdomainName); + return FALSE; + } + } + + return TRUE; +} + +UInteger8 +lookupCommunicationTechnology(UInteger8 communicationTechnology) +{ +#if defined(linux) + + switch (communicationTechnology) { + case ARPHRD_ETHER: + case ARPHRD_EETHER: + case ARPHRD_IEEE802: + return PTP_ETHER; + + default: + break; + } + +#elif defined(BSD_INTERFACE_FUNCTIONS) + +#endif + + return PTP_DEFAULT; +} + +UInteger32 +findIface(Octet * ifaceName, UInteger8 * communicationTechnology, + Octet * uuid, NetPath * netPath) +{ +#if defined(linux) + + /* depends on linux specific ioctls (see 'netdevice' man page) */ + int i, flags; + struct ifconf data; + struct ifreq device[IFCONF_LENGTH]; + + data.ifc_len = sizeof(device); + data.ifc_req = device; + + memset(data.ifc_buf, 0, data.ifc_len); + + flags = IFF_UP | IFF_RUNNING | IFF_MULTICAST; + + /* look for an interface if none specified */ + if (ifaceName[0] != '\0') { + i = 0; + memcpy(device[i].ifr_name, ifaceName, IFACE_NAME_LENGTH); + + if (ioctl(netPath->eventSock, SIOCGIFHWADDR, &device[i]) < 0) + DBGV("failed to get hardware address\n"); + else if ((*communicationTechnology = lookupCommunicationTechnology(device[i].ifr_hwaddr.sa_family)) == PTP_DEFAULT) + DBGV("unsupported communication technology (%d)\n", *communicationTechnology); + else + memcpy(uuid, device[i].ifr_hwaddr.sa_data, PTP_UUID_LENGTH); + } else { + /* no iface specified */ + /* get list of network interfaces */ + if (ioctl(netPath->eventSock, SIOCGIFCONF, &data) < 0) { + PERROR("failed query network interfaces"); + return 0; + } + if (data.ifc_len >= sizeof(device)) + DBG("device list may exceed allocated space\n"); + + /* search through interfaces */ + for (i = 0; i < data.ifc_len / sizeof(device[0]); ++i) { + DBGV("%d %s %s\n", i, device[i].ifr_name, inet_ntoa(((struct sockaddr_in *)&device[i].ifr_addr)->sin_addr)); + + if (ioctl(netPath->eventSock, SIOCGIFFLAGS, &device[i]) < 0) + DBGV("failed to get device flags\n"); + else if ((device[i].ifr_flags & flags) != flags) + DBGV("does not meet requirements (%08x, %08x)\n", device[i].ifr_flags, flags); + else if (ioctl(netPath->eventSock, SIOCGIFHWADDR, &device[i]) < 0) + DBGV("failed to get hardware address\n"); + else if ((*communicationTechnology = lookupCommunicationTechnology(device[i].ifr_hwaddr.sa_family)) == PTP_DEFAULT) + DBGV("unsupported communication technology (%d)\n", *communicationTechnology); + else { + DBGV("found interface (%s)\n", device[i].ifr_name); + + memcpy(uuid, device[i].ifr_hwaddr.sa_data, PTP_UUID_LENGTH); + memcpy(ifaceName, device[i].ifr_name, IFACE_NAME_LENGTH); + + break; + } + } + } + + if (ifaceName[0] == '\0') { + ERROR("failed to find a usable interface\n"); + return 0; + } + if (ioctl(netPath->eventSock, SIOCGIFADDR, &device[i]) < 0) { + PERROR("failed to get ip address"); + return 0; + } + return ((struct sockaddr_in *)&device[i].ifr_addr)->sin_addr.s_addr; + +#elif defined(BSD_INTERFACE_FUNCTIONS) + + struct ifaddrs *if_list, *ifv4, *ifh; + + if (getifaddrs(&if_list) < 0) { + PERROR("getifaddrs() failed"); + return FALSE; + } + /* find an IPv4, multicast, UP interface, right name(if supplied) */ + for (ifv4 = if_list; ifv4 != NULL; ifv4 = ifv4->ifa_next) { + if ((ifv4->ifa_flags & IFF_UP) == 0) + continue; + if ((ifv4->ifa_flags & IFF_RUNNING) == 0) + continue; + if ((ifv4->ifa_flags & IFF_LOOPBACK)) + continue; + if ((ifv4->ifa_flags & IFF_MULTICAST) == 0) + continue; + if (ifv4->ifa_addr->sa_family != AF_INET) /* must have IPv4 + * address */ + continue; + + if (ifaceName[0] && strncmp(ifv4->ifa_name, ifaceName, IF_NAMESIZE) != 0) + continue; + + break; + } + + if (ifv4 == NULL) { + if (ifaceName[0]) { + ERROR("interface \"%s\" does not exist, or is not appropriate\n", ifaceName); + return FALSE; + } + ERROR("no suitable interfaces found!"); + return FALSE; + } + /* find the AF_LINK info associated with the chosen interface */ + for (ifh = if_list; ifh != NULL; ifh = ifh->ifa_next) { + if (ifh->ifa_addr->sa_family != AF_LINK) + continue; + if (strncmp(ifv4->ifa_name, ifh->ifa_name, IF_NAMESIZE) == 0) + break; + } + + if (ifh == NULL) { + ERROR("could not get hardware address for interface \"%s\"\n", ifv4->ifa_name); + return FALSE; + } + /* check that the interface TYPE is OK */ + if (((struct sockaddr_dl *)ifh->ifa_addr)->sdl_type != IFT_ETHER) { + ERROR("\"%s\" is not an ethernet interface!\n", ifh->ifa_name); + return FALSE; + } + + DBG("==> %s %s %s\n", ifv4->ifa_name, + inet_ntoa(((struct sockaddr_in *)ifv4->ifa_addr)->sin_addr), + ether_ntoa((struct ether_addr *) + LLADDR((struct sockaddr_dl *)ifh->ifa_addr))); + + *communicationTechnology = PTP_ETHER; + memcpy(ifaceName, ifh->ifa_name, IFACE_NAME_LENGTH); + memcpy(uuid, LLADDR((struct sockaddr_dl *)ifh->ifa_addr), PTP_UUID_LENGTH); + + return ((struct sockaddr_in *)ifv4->ifa_addr)->sin_addr.s_addr; + +#endif +} + +/* start all of the UDP stuff */ +/* must specify 'subdomainName', optionally 'ifaceName', if not then pass ifaceName == "" */ +/* returns other args */ +/* on socket options, see the 'socket(7)' and 'ip' man pages */ +Boolean +netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + int temp, i; + struct in_addr interfaceAddr, netAddr; + struct sockaddr_in addr; + struct ip_mreq imr; + char addrStr[NET_ADDRESS_LENGTH]; + char *s; + + DBG("netInit\n"); + + /* open sockets */ + if ((netPath->eventSock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 + || (netPath->generalSock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) { + PERROR("failed to initalize sockets"); + return FALSE; + } + /* find a network interface */ + if (!(interfaceAddr.s_addr = findIface(rtOpts->ifaceName, &ptpClock->port_communication_technology, + ptpClock->port_uuid_field, netPath))) + return FALSE; + + temp = 1; /* allow address reuse */ + if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_REUSEADDR, &temp, sizeof(int)) < 0 + || setsockopt(netPath->generalSock, SOL_SOCKET, SO_REUSEADDR, &temp, sizeof(int)) < 0) { + DBG("failed to set socket reuse\n"); + } + /* bind sockets */ + /* + * need INADDR_ANY to allow receipt of multi-cast and uni-cast + * messages + */ + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(PTP_EVENT_PORT); + if (bind(netPath->eventSock, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) { + PERROR("failed to bind event socket"); + return FALSE; + } + addr.sin_port = htons(PTP_GENERAL_PORT); + if (bind(netPath->generalSock, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) { + PERROR("failed to bind general socket"); + return FALSE; + } + /* set general and port address */ + *(Integer16 *) ptpClock->event_port_address = PTP_EVENT_PORT; + *(Integer16 *) ptpClock->general_port_address = PTP_GENERAL_PORT; + + /* send a uni-cast address if specified (useful for testing) */ + if (rtOpts->unicastAddress[0]) { + if (!inet_aton(rtOpts->unicastAddress, &netAddr)) { + ERROR("failed to encode uni-cast address: %s\n", rtOpts->unicastAddress); + return FALSE; + } + netPath->unicastAddr = netAddr.s_addr; + } else + netPath->unicastAddr = 0; + + /* resolve PTP subdomain */ + if (!lookupSubdomainAddress(rtOpts->subdomainName, addrStr)) + return FALSE; + + if (!inet_aton(addrStr, &netAddr)) { + ERROR("failed to encode multi-cast address: %s\n", addrStr); + return FALSE; + } + netPath->multicastAddr = netAddr.s_addr; + + s = addrStr; + for (i = 0; i < SUBDOMAIN_ADDRESS_LENGTH; ++i) { + ptpClock->subdomain_address[i] = strtol(s, &s, 0); + + if (!s) + break; + + ++s; + } + + /* multicast send only on specified interface */ + imr.imr_multiaddr.s_addr = netAddr.s_addr; + imr.imr_interface.s_addr = interfaceAddr.s_addr; + if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_IF, &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0 + || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_IF, &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0) { + PERROR("failed to enable multi-cast on the interface"); + return FALSE; + } + /* join multicast group (for receiving) on specified interface */ + if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imr, sizeof(struct ip_mreq)) < 0 + || setsockopt(netPath->generalSock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imr, sizeof(struct ip_mreq)) < 0) { + PERROR("failed to join the multi-cast group"); + return FALSE; + } + /* set socket time-to-live */ + if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_TTL, &rtOpts->ttl, sizeof(int)) < 0 + || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_TTL, &rtOpts->ttl, sizeof(int)) < 0) { + PERROR("failed to set the multi-cast time-to-live"); + return FALSE; + } + /* enable loopback */ + temp = 1; + if (setsockopt(netPath->eventSock, IPPROTO_IP, IP_MULTICAST_LOOP, &temp, sizeof(int)) < 0 + || setsockopt(netPath->generalSock, IPPROTO_IP, IP_MULTICAST_LOOP, &temp, sizeof(int)) < 0) { + PERROR("failed to enable multi-cast loopback"); + return FALSE; + } + /* make timestamps available through recvmsg() */ + + temp = 1; +#if defined(linux) + if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMP, &temp, sizeof(int)) < 0 + || setsockopt(netPath->generalSock, SOL_SOCKET, SO_TIMESTAMP, &temp, sizeof(int)) < 0) { +#else /* BSD */ + if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_BINTIME, &temp, sizeof(int)) < 0 + || setsockopt(netPath->generalSock, SOL_SOCKET, SO_BINTIME, &temp, sizeof(int)) < 0) { +#endif /* linux or BSD */ + PERROR("failed to enable receive time stamps"); + return FALSE; + } + + return TRUE; +} + +/* shut down the UDP stuff */ +Boolean +netShutdown(NetPath * netPath) +{ + struct ip_mreq imr; + + imr.imr_multiaddr.s_addr = netPath->multicastAddr; + imr.imr_interface.s_addr = htonl(INADDR_ANY); + + setsockopt(netPath->eventSock, IPPROTO_IP, IP_DROP_MEMBERSHIP, &imr, sizeof(struct ip_mreq)); + setsockopt(netPath->generalSock, IPPROTO_IP, IP_DROP_MEMBERSHIP, &imr, sizeof(struct ip_mreq)); + + netPath->multicastAddr = 0; + netPath->unicastAddr = 0; + + if (netPath->eventSock > 0) + close(netPath->eventSock); + netPath->eventSock = -1; + + if (netPath->generalSock > 0) + close(netPath->generalSock); + netPath->generalSock = -1; + + return TRUE; +} + +int +netSelect(TimeInternal * timeout, NetPath * netPath) +{ + int ret, nfds; + fd_set readfds; + struct timeval tv, *tv_ptr; + + if (timeout < 0) + return FALSE; + + FD_ZERO(&readfds); + FD_SET(netPath->eventSock, &readfds); + FD_SET(netPath->generalSock, &readfds); + + if (timeout) { + tv.tv_sec = timeout->seconds; + tv.tv_usec = timeout->nanoseconds / 1000; + tv_ptr = &tv; + } else + tv_ptr = 0; + + if (netPath->eventSock > netPath->generalSock) + nfds = netPath->eventSock; + else + nfds = netPath->generalSock; + + ret = select(nfds + 1, &readfds, 0, 0, tv_ptr) > 0; + if (ret < 0) { + if (errno == EAGAIN || errno == EINTR) + return 0; + } + return ret; +} + +ssize_t +netRecvEvent(Octet * buf, TimeInternal * time, NetPath * netPath) +{ + ssize_t ret; + struct msghdr msg; + struct iovec vec[1]; + struct sockaddr_in from_addr; + union { + struct cmsghdr cm; + char control[CMSG_SPACE(sizeof(struct timeval))]; + } cmsg_un; + struct cmsghdr *cmsg; +#if defined(linux) + struct timeval *tv; +#else /* FreeBSD */ + struct timespec ts; +#endif /* FreeBSD or Linux */ + + vec[0].iov_base = buf; + vec[0].iov_len = PACKET_SIZE; + + memset(&msg, 0, sizeof(msg)); + memset(&from_addr, 0, sizeof(from_addr)); + memset(buf, 0, PACKET_SIZE); + memset(&cmsg_un, 0, sizeof(cmsg_un)); + + msg.msg_name = (caddr_t)&from_addr; + msg.msg_namelen = sizeof(from_addr); + msg.msg_iov = vec; + msg.msg_iovlen = 1; + msg.msg_control = cmsg_un.control; + msg.msg_controllen = sizeof(cmsg_un.control); + msg.msg_flags = 0; + + ret = recvmsg(netPath->eventSock, &msg, MSG_DONTWAIT); + if (ret <= 0) { + if (errno == EAGAIN || errno == EINTR) + return 0; + + return ret; + } + if (msg.msg_flags & MSG_TRUNC) { + ERROR("received truncated message\n"); + return 0; + } + /* get time stamp of packet */ + if (!time) { + ERROR("null receive time stamp argument\n"); + return 0; + } + if (msg.msg_flags & MSG_CTRUNC) { + ERROR("received truncated ancillary data\n"); + return 0; + } + if (msg.msg_controllen < sizeof(cmsg_un.control)) { + ERROR("received short ancillary data (%d/%d)\n", + msg.msg_controllen, (int)sizeof(cmsg_un.control)); + + return 0; + } + +#if defined(linux) + tv = 0; + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; + cmsg = CMSG_NXTHDR(&msg, cmsg)) + if (cmsg->cmsg_level == SOL_SOCKET && + cmsg->cmsg_type == SCM_TIMESTAMP) + tv = (struct timeval *)CMSG_DATA(cmsg); + + if (tv) { + time->seconds = tv->tv_sec; + time->nanoseconds = tv->tv_usec * 1000; + DBGV("kernel recv time stamp %us %dns\n", time->seconds, time->nanoseconds); + } else { + /* + * do not try to get by with recording the time here, better + * to fail because the time recorded could be well after the + * message receive, which would put a big spike in the + * offset signal sent to the clock servo + */ + DBG("no recieve time stamp\n"); + return 0; + } +#else /* FreeBSD */ + bzero(&ts, sizeof(ts)); + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; + cmsg = CMSG_NXTHDR(&msg, cmsg)) + if (cmsg->cmsg_level == SOL_SOCKET && + cmsg->cmsg_type == SCM_BINTIME) + bintime2timespec((struct bintime *)CMSG_DATA(cmsg), + &ts); + + if (ts.tv_sec != 0) { + time->seconds = ts.tv_sec; + time->nanoseconds = ts.tv_nsec; + DBGV("kernel recv time stamp %us %dns\n", time->seconds, time->nanoseconds); + } else { + /* + * do not try to get by with recording the time here, better + * to fail because the time recorded could be well after the + * message receive, which would put a big spike in the + * offset signal sent to the clock servo + */ + DBG("no recieve time stamp\n"); + return 0; + } +#endif /* FreeBSD or Linux */ + + return ret; +} + +ssize_t +netRecvGeneral(Octet * buf, NetPath * netPath) +{ + ssize_t ret; + struct sockaddr_in addr; + socklen_t addr_len = sizeof(struct sockaddr_in); + + ret = recvfrom(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT, (struct sockaddr *)&addr, &addr_len); + if (ret <= 0) { + if (errno == EAGAIN || errno == EINTR) + return 0; + + return ret; + } + return ret; +} + +ssize_t +netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath) +{ + ssize_t ret; + struct sockaddr_in addr; + + addr.sin_family = AF_INET; + addr.sin_port = htons(PTP_EVENT_PORT); + addr.sin_addr.s_addr = netPath->multicastAddr; + + ret = sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); + if (ret <= 0) + DBG("error sending multi-cast event message\n"); + + if (netPath->unicastAddr) { + addr.sin_addr.s_addr = netPath->unicastAddr; + + ret = sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); + if (ret <= 0) + DBG("error sending uni-cast event message\n"); + } + return ret; +} + +ssize_t +netSendGeneral(Octet * buf, UInteger16 length, NetPath * netPath) +{ + ssize_t ret; + struct sockaddr_in addr; + + addr.sin_family = AF_INET; + addr.sin_port = htons(PTP_GENERAL_PORT); + addr.sin_addr.s_addr = netPath->multicastAddr; + + ret = sendto(netPath->generalSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); + if (ret <= 0) + DBG("error sending multi-cast general message\n"); + + if (netPath->unicastAddr) { + addr.sin_addr.s_addr = netPath->unicastAddr; + + ret = sendto(netPath->eventSock, buf, length, 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); + if (ret <= 0) + DBG("error sending uni-cast general message\n"); + } + return ret; +} diff --git a/third_party/ptpd-1.1.0/src/dep/ptpd_dep.h b/third_party/ptpd-1.1.0/src/dep/ptpd_dep.h new file mode 100644 index 0000000..63cf674 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/ptpd_dep.h @@ -0,0 +1,154 @@ +/** + * @file ptpd_dep.h + * @date Tue Jul 20 16:18:39 2010 + * + * @brief External definitions for inclusion elsewhere. + * + * + */ + +#ifndef PTPD_DEP_H +#define PTPD_DEP_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* system messages */ +#define ERROR(x, ...) message(LOG_ERR, x, ##__VA_ARGS__) +#define PERROR(x, ...) message(LOG_ERR, x ": %m\n", ##__VA_ARGS__) +#define NOTIFY(x, ...) message(LOG_NOTICE, x, ##__VA_ARGS__) +#define INFO(x, ...) message(LOG_INFO, x, ##__VA_ARGS__) + +/* debug messages */ +#ifdef PTPD_DBGV +#define PTPD_DBG +#define DBGV(x, ...) message(LOG_DEBUG, x, ##__VA_ARGS__) +#else +#define DBGV(x, ...) +#endif + +#ifdef PTPD_DBG +#define DBG(x, ...) message(LOG_DEBUG, x, ##__VA_ARGS__) +#else +#define DBG(x, ...) +#endif + +/* endian corrections */ +#if defined(PTPD_MSBF) +#define shift8(x,y) ( (x) << ((3-y)<<3) ) +#define shift16(x,y) ( (x) << ((1-y)<<4) ) +#elif defined(PTPD_LSBF) +#define shift8(x,y) ( (x) << ((y)<<3) ) +#define shift16(x,y) ( (x) << ((y)<<4) ) +#endif + +#define flip16(x) htons(x) +#define flip32(x) htonl(x) + +/* i don't know any target platforms that do not have htons and htonl, + but here are generic funtions just in case */ +/* +#if defined(PTPD_MSBF) +#define flip16(x) (x) +#define flip32(x) (x) +#elif defined(PTPD_LSBF) +static inline Integer16 flip16(Integer16 x) +{ + return (((x) >> 8) & 0x00ff) | (((x) << 8) & 0xff00); +} + +static inline Integer32 flip32(x) +{ + return (((x) >> 24) & 0x000000ff) | (((x) >> 8 ) & 0x0000ff00) | + (((x) << 8 ) & 0x00ff0000) | (((x) << 24) & 0xff000000); +} +#endif +*/ + +/* bit array manipulation */ +#define getFlag(x,y) !!( *(UInteger8*)((x)+((y)<8?1:0)) & (1<<((y)<8?(y):(y)-8)) ) +#define setFlag(x,y) ( *(UInteger8*)((x)+((y)<8?1:0)) |= 1<<((y)<8?(y):(y)-8) ) +#define clearFlag(x,y) ( *(UInteger8*)((x)+((y)<8?1:0)) &= ~(1<<((y)<8?(y):(y)-8)) ) + + +/* msg.c */ +Boolean msgPeek(void *, ssize_t); +void msgUnpackHeader(void *, MsgHeader *); +void msgUnpackSync(void *, MsgSync *); +void msgUnpackDelayReq(void *, MsgDelayReq *); +void msgUnpackFollowUp(void *, MsgFollowUp *); +void msgUnpackDelayResp(void *, MsgDelayResp *); +void msgUnpackManagement(void *, MsgManagement *); +UInteger8 msgUnloadManagement(void *, MsgManagement *, PtpClock *, RunTimeOpts *); +void msgUnpackManagementPayload(void *buf, MsgManagement * manage); +void msgPackHeader(void *, PtpClock *); +void msgPackSync(void *, Boolean, TimeRepresentation *, PtpClock *); +void msgPackDelayReq(void *, Boolean, TimeRepresentation *, PtpClock *); +void msgPackFollowUp(void *, UInteger16, TimeRepresentation *, PtpClock *); +void msgPackDelayResp(void *, MsgHeader *, TimeRepresentation *, PtpClock *); +UInteger16 msgPackManagement(void *, MsgManagement *, PtpClock *); +UInteger16 msgPackManagementResponse(void *, MsgHeader *, MsgManagement *, PtpClock *); + +/* net.c */ +/* linux API dependent */ +Boolean netInit(NetPath *, RunTimeOpts *, PtpClock *); +Boolean netShutdown(NetPath *); +int netSelect(TimeInternal *, NetPath *); +ssize_t netRecvEvent(Octet *, TimeInternal *, NetPath *); +ssize_t netRecvGeneral(Octet *, NetPath *); +ssize_t netSendEvent(Octet *, UInteger16, NetPath *); +ssize_t netSendGeneral(Octet *, UInteger16, NetPath *); + +/* servo.c */ +void initClock(RunTimeOpts *, PtpClock *); +void +updateDelay(TimeInternal *, TimeInternal *, + one_way_delay_filter *, RunTimeOpts *, PtpClock *); +void +updateOffset(TimeInternal *, TimeInternal *, + offset_from_master_filter *, RunTimeOpts *, PtpClock *); +void updateClock(RunTimeOpts *, PtpClock *); + +/* startup.c */ +/* unix API dependent */ +int logToFile(); +int recordToFile(); +PtpClock *ptpdStartup(int, char **, Integer16 *, RunTimeOpts *); +void ptpdShutdown(void); + +/* sys.c */ +char *translatePortState(PtpClock *ptpClock); +/* unix API dependent */ +void message(int priority, const char *format, ...); +void displayStats(RunTimeOpts *, PtpClock *); +Boolean nanoSleep(TimeInternal *); +void getTime(TimeInternal *); +void setTime(TimeInternal *); +UInteger16 getRand(UInteger32 *); +Boolean adjFreq(Integer32); + +/* timer.c */ +void initTimer(void); +void timerUpdate(IntervalTimer *); +void timerStop(UInteger16, IntervalTimer *); +void timerStart(UInteger16, UInteger16, IntervalTimer *); +Boolean timerExpired(UInteger16, IntervalTimer *); + + +#endif diff --git a/third_party/ptpd-1.1.0/src/dep/servo.c b/third_party/ptpd-1.1.0/src/dep/servo.c new file mode 100644 index 0000000..b888593 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/servo.c @@ -0,0 +1,215 @@ +/** + * @file servo.c + * @date Tue Jul 20 16:19:19 2010 + * + * @brief Code which implements the clock servo in software. + * + * + */ + +#include "../ptpd.h" + +void +initClock(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + DBG("initClock\n"); + + /* clear vars */ + ptpClock->master_to_slave_delay.seconds = ptpClock->master_to_slave_delay.nanoseconds = 0; + ptpClock->slave_to_master_delay.seconds = ptpClock->slave_to_master_delay.nanoseconds = 0; + ptpClock->observed_variance = 0; + ptpClock->observed_drift = 0; /* clears clock servo accumulator (the + * I term) */ + ptpClock->owd_filt.s_exp = 0; /* clears one-way delay filter */ + ptpClock->halfEpoch = ptpClock->halfEpoch || rtOpts->halfEpoch; + rtOpts->halfEpoch = 0; + + /* level clock */ + if (!rtOpts->noAdjust) + adjFreq(0); +} + +void +updateDelay(TimeInternal * send_time, TimeInternal * recv_time, + one_way_delay_filter * owd_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + Integer16 s; + TimeInternal slave_to_master_delay; + + DBGV("updateDelay\n"); + + /* calc 'slave_to_master_delay' */ + subTime(&slave_to_master_delay, recv_time, send_time); + + if (rtOpts->maxDelay) { /* If maxDelay is 0 then it's OFF */ + if (slave_to_master_delay.seconds && rtOpts->maxDelay) { + INFO("updateDelay aborted, delay greater than 1" + " second."); + return; + } + + if (slave_to_master_delay.nanoseconds > rtOpts->maxDelay) { + INFO("updateDelay aborted, delay %d greater than " + "administratively set maximum %d\n", + slave_to_master_delay.nanoseconds, + rtOpts->maxDelay); + return; + } + } + + ptpClock->slave_to_master_delay = slave_to_master_delay; + + /* update 'one_way_delay' */ + addTime(&ptpClock->one_way_delay, &ptpClock->master_to_slave_delay, &ptpClock->slave_to_master_delay); + ptpClock->one_way_delay.seconds /= 2; + ptpClock->one_way_delay.nanoseconds /= 2; + + if (ptpClock->one_way_delay.seconds) { + /* cannot filter with secs, clear filter */ + owd_filt->s_exp = owd_filt->nsec_prev = 0; + return; + } + /* avoid overflowing filter */ + s = rtOpts->s; + while (abs(owd_filt->y) >> (31 - s)) + --s; + + /* crank down filter cutoff by increasing 's_exp' */ + if (owd_filt->s_exp < 1) + owd_filt->s_exp = 1; + else if (owd_filt->s_exp < 1 << s) + ++owd_filt->s_exp; + else if (owd_filt->s_exp > 1 << s) + owd_filt->s_exp = 1 << s; + + /* filter 'one_way_delay' */ + owd_filt->y = (owd_filt->s_exp - 1) * owd_filt->y / owd_filt->s_exp + + (ptpClock->one_way_delay.nanoseconds / 2 + owd_filt->nsec_prev / 2) / owd_filt->s_exp; + + owd_filt->nsec_prev = ptpClock->one_way_delay.nanoseconds; + ptpClock->one_way_delay.nanoseconds = owd_filt->y; + + DBG("delay filter %d, %d\n", owd_filt->y, owd_filt->s_exp); +} + +void +updateOffset(TimeInternal * send_time, TimeInternal * recv_time, + offset_from_master_filter * ofm_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeInternal master_to_slave_delay; + DBGV("updateOffset\n"); + + /* calc 'master_to_slave_delay' */ + subTime(&master_to_slave_delay, recv_time, send_time); + + if (rtOpts->maxDelay) { /* If maxDelay is 0 then it's OFF */ + if (master_to_slave_delay.seconds && rtOpts->maxDelay) { + INFO("updateDelay aborted, delay greater than 1" + " second."); + return; + } + + if (master_to_slave_delay.nanoseconds > rtOpts->maxDelay) { + INFO("updateDelay aborted, delay %d greater than " + "administratively set maximum %d\n", + master_to_slave_delay.nanoseconds, + rtOpts->maxDelay); + return; + } + } + ptpClock->master_to_slave_delay = master_to_slave_delay; + + /* update 'offset_from_master' */ + subTime(&ptpClock->offset_from_master, &ptpClock->master_to_slave_delay, &ptpClock->one_way_delay); + + if (ptpClock->offset_from_master.seconds) { + /* cannot filter with secs, clear filter */ + ofm_filt->nsec_prev = 0; + return; + } + /* filter 'offset_from_master' */ + ofm_filt->y = ptpClock->offset_from_master.nanoseconds / 2 + ofm_filt->nsec_prev / 2; + ofm_filt->nsec_prev = ptpClock->offset_from_master.nanoseconds; + ptpClock->offset_from_master.nanoseconds = ofm_filt->y; + + DBGV("offset filter %d\n", ofm_filt->y); +} + +void +updateClock(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + Integer32 adj; + TimeInternal timeTmp; + + DBGV("updateClock\n"); + + if (rtOpts->maxReset) { /* If maxReset is 0 then it's OFF */ + if (ptpClock->offset_from_master.seconds) { + INFO("updateClock aborted, offset greater than 1" + " second."); + goto display; + } + + if (ptpClock->offset_from_master.nanoseconds > + rtOpts->maxReset) { + INFO("updateClock aborted, offset %d greater than " + "administratively set maximum %d\n", + ptpClock->offset_from_master.nanoseconds, + rtOpts->maxReset); + goto display; + } + } + + if (ptpClock->offset_from_master.seconds) { + /* if secs, reset clock or set freq adjustment to max */ + if (!rtOpts->noAdjust) { + if (!rtOpts->noResetClock) { + getTime(&timeTmp); + subTime(&timeTmp, &timeTmp, &ptpClock->offset_from_master); + setTime(&timeTmp); + initClock(rtOpts, ptpClock); + } else { + adj = ptpClock->offset_from_master.nanoseconds > 0 ? ADJ_FREQ_MAX : -ADJ_FREQ_MAX; + adjFreq(-adj); + } + } + } else { + /* the PI controller */ + + /* no negative or zero attenuation */ + if (rtOpts->ap < 1) + rtOpts->ap = 1; + if (rtOpts->ai < 1) + rtOpts->ai = 1; + + /* the accumulator for the I component */ + ptpClock->observed_drift += ptpClock->offset_from_master.nanoseconds / rtOpts->ai; + + /* clamp the accumulator to ADJ_FREQ_MAX for sanity */ + if (ptpClock->observed_drift > ADJ_FREQ_MAX) + ptpClock->observed_drift = ADJ_FREQ_MAX; + else if (ptpClock->observed_drift < -ADJ_FREQ_MAX) + ptpClock->observed_drift = -ADJ_FREQ_MAX; + + adj = ptpClock->offset_from_master.nanoseconds / rtOpts->ap + ptpClock->observed_drift; + + /* apply controller output as a clock tick rate adjustment */ + if (!rtOpts->noAdjust) + adjFreq(-adj); + } + +display: + + if (rtOpts->displayStats) + displayStats(rtOpts, ptpClock); + + DBGV("master-to-slave delay: %10ds %11dns\n", + ptpClock->master_to_slave_delay.seconds, ptpClock->master_to_slave_delay.nanoseconds); + DBGV("slave-to-master delay: %10ds %11dns\n", + ptpClock->slave_to_master_delay.seconds, ptpClock->slave_to_master_delay.nanoseconds); + DBGV("one-way delay: %10ds %11dns\n", + ptpClock->one_way_delay.seconds, ptpClock->one_way_delay.nanoseconds); + DBG("offset from master: %10ds %11dns\n", + ptpClock->offset_from_master.seconds, ptpClock->offset_from_master.nanoseconds); + DBG("observed drift: %10d\n", ptpClock->observed_drift); +} diff --git a/third_party/ptpd-1.1.0/src/dep/startup.c b/third_party/ptpd-1.1.0/src/dep/startup.c new file mode 100644 index 0000000..edbcbfd --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/startup.c @@ -0,0 +1,364 @@ +/** + * @file startup.c + * @date Wed Jun 23 09:33:27 2010 + * + * @brief Code to handle daemon startup, including command line args + * + * The function in this file are called when the daemon starts up + * and include the getopt() command line argument parsing. + */ + +#include "../ptpd.h" + +PtpClock *ptpClock; + +/** + * Signal handler for catching a close signal + * + * @param sig + */ +void +catch_close(int sig) +{ + char *s; + + ptpdShutdown(); + + switch (sig) { + case SIGINT: + s = "interrupt"; + break; + + case SIGTERM: + s = "terminate"; + break; + + default: + s = "?"; + } + + NOTIFY("shutdown on %s signal\n", s); + + exit(0); +} + +/** + * Signal handler for HUP which tells us to swap the log file. + * + * @param sig + */ +void +catch_sighup(int sig) +{ + if(!logToFile()) + NOTIFY("SIGHUP failed\n"); + else + NOTIFY("I've been SIGHUP'd\n"); + if(!recordToFile()) + NOTIFY("SIGHUP failed\n"); + else + NOTIFY("I've been SIGHUP'd\n"); + +} + +/** + * Log output to a file + * + * + * @return True if success, False if failure + */ +int +logToFile() +{ + extern RunTimeOpts rtOpts; + if(rtOpts.logFd != -1) + close(rtOpts.logFd); + + if((rtOpts.logFd = creat(rtOpts.file, 0444)) != -1) { + dup2(rtOpts.logFd, STDOUT_FILENO); + dup2(rtOpts.logFd, STDERR_FILENO); + } + return (rtOpts.logFd != -1); +} + +/** + * Record quality data for later correlation + * + * + * @return True if success, False if failure + */ +int +recordToFile() +{ + extern RunTimeOpts rtOpts; + + if (rtOpts.recordFP != NULL) + fclose(rtOpts.recordFP); + + if ((rtOpts.recordFP = fopen(rtOpts.recordFile, "w")) == NULL) + PERROR("could not open sync recording file"); + else + setlinebuf(rtOpts.recordFP); + return (rtOpts.recordFP != NULL); +} + +void +ptpdShutdown() +{ + netShutdown(&ptpClock->netPath); + + free(ptpClock->foreign); + free(ptpClock); +} + +PtpClock * +ptpdStartup(int argc, char **argv, Integer16 * ret, RunTimeOpts * rtOpts) +{ + int c, nondaemon = 0, noclose = 0; + + /* parse command line arguments */ + while ((c = getopt(argc, argv, "?cf:dDR:xM:O:ta:w:b:u:l:o:e:hy:m:gpSs:i:v:n:k:rT:")) != -1) { + switch (c) { + case '?': + printf( + "\nUsage: ptpd [OPTION]\n\n" + "-? show this page\n" + "\n" + "-c run in command line (non-daemon) mode\n" + "-f FILE send stats to FILE\n" + "-S send output to syslog \n" + "-T set multicast time to live\n" + "-d display stats\n" + "-D display stats in .csv format\n" + "-R record data about sync packets in a file\n" + "\n" + "-x do not reset the clock if off by more than one second\n" + "-O do not reset the clock if offset is more than NUMBER nanoseconds\n" + "-M do not accept delay values of more than NUMBER nanoseconds\n" + "-t do not adjust the system clock\n" + "-a NUMBER,NUMBER specify clock servo P and I attenuations\n" + "-w NUMBER specify one way delay filter stiffness\n" + "\n" + "-b NAME bind PTP to network interface NAME\n" + "-u ADDRESS also send uni-cast to ADDRESS\n" + "-l NUMBER,NUMBER specify inbound, outbound latency in nsec\n" + "\n" + "-o NUMBER specify current UTC offset\n" + "-e NUMBER specify epoch NUMBER\n" + "-h specify half epoch\n" + "\n" + "-y NUMBER specify sync interval in 2^NUMBER sec\n" + "-m NUMBER specify max number of foreign master records\n" + "\n" + "-g run as slave only\n" + "-p make this a preferred clock\n" + "-s NUMBER specify system clock stratum\n" + "-i NAME specify system clock identifier\n" + "-v NUMBER specify system clock allen variance\n" + "\n" + "-n NAME specify PTP subdomain name (not related to IP or DNS)\n" + "\n" + "-k NUMBER,NUMBER send a management message of key, record, then exit\n" + "\n" + ); + *ret = 0; + return 0; + + case 'c': + nondaemon = 1; + break; + + case 'S': + rtOpts->useSysLog = TRUE; + break; + case 'T': + rtOpts->ttl = atoi(optarg); + break; + case 'f': + strncpy(rtOpts->file, optarg, PATH_MAX); + if(logToFile()) + noclose = 1; + else + PERROR("could not open output file"); + break; + + case 'd': +#ifndef PTPD_DBG + rtOpts->displayStats = TRUE; +#endif + break; + + case 'D': +#ifndef PTPD_DBG + rtOpts->displayStats = TRUE; + rtOpts->csvStats = TRUE; +#endif + break; + + case 'R': + strncpy(rtOpts->recordFile, optarg, PATH_MAX); + if (recordToFile()) + noclose = 1; + else + PERROR("could not open quality file"); + break; + + case 'x': + rtOpts->noResetClock = TRUE; + break; + + case 'O': + rtOpts->maxReset = atoi(optarg); + if (rtOpts->maxReset > 1000000000) { + PERROR("Use -x to prevent jumps of more" + " than one second."); + *ret = 1; + return (0); + } + break; + case 'M': + rtOpts->maxDelay = atoi(optarg); + if (rtOpts->maxDelay > 1000000000) { + PERROR("Use -x to prevent jumps of more" + " than one second."); + *ret = 1; + return (0); + } + break; + case 't': + rtOpts->noAdjust = TRUE; + break; + + case 'a': + rtOpts->ap = strtol(optarg, &optarg, 0); + if (optarg[0]) + rtOpts->ai = strtol(optarg + 1, 0, 0); + break; + + case 'w': + rtOpts->s = strtol(optarg, &optarg, 0); + break; + + case 'b': + memset(rtOpts->ifaceName, 0, IFACE_NAME_LENGTH); + strncpy(rtOpts->ifaceName, optarg, IFACE_NAME_LENGTH); + break; + + case 'u': + strncpy(rtOpts->unicastAddress, optarg, NET_ADDRESS_LENGTH); + break; + + case 'l': + rtOpts->inboundLatency.nanoseconds = strtol(optarg, &optarg, 0); + if (optarg[0]) + rtOpts->outboundLatency.nanoseconds = strtol(optarg + 1, 0, 0); + break; + + case 'o': + rtOpts->currentUtcOffset = strtol(optarg, &optarg, 0); + break; + + case 'e': + rtOpts->epochNumber = strtoul(optarg, &optarg, 0); + break; + + case 'h': + rtOpts->halfEpoch = TRUE; + break; + + case 'y': + rtOpts->syncInterval = strtol(optarg, 0, 0); + break; + + case 'm': + rtOpts->max_foreign_records = strtol(optarg, 0, 0); + if (rtOpts->max_foreign_records < 1) + rtOpts->max_foreign_records = 1; + break; + + case 'g': + rtOpts->slaveOnly = TRUE; + break; + + case 'p': + rtOpts->clockPreferred = TRUE; + break; + + case 's': + rtOpts->clockStratum = strtol(optarg, 0, 0); + if (rtOpts->clockStratum <= 0) + rtOpts->clockStratum = 255; + break; + + case 'i': + memset(rtOpts->clockIdentifier, 0, PTP_CODE_STRING_LENGTH); + strncpy(rtOpts->clockIdentifier, optarg, PTP_CODE_STRING_LENGTH); + break; + + case 'v': + rtOpts->clockVariance = strtol(optarg, 0, 0); + break; + + case 'n': + memset(rtOpts->subdomainName, 0, PTP_SUBDOMAIN_NAME_LENGTH); + strncpy(rtOpts->subdomainName, optarg, PTP_SUBDOMAIN_NAME_LENGTH); + break; + + case 'k': + rtOpts->probe = TRUE; + + rtOpts->probe_management_key = strtol(optarg, &optarg, 0); + if (optarg[0]) + rtOpts->probe_record_key = strtol(optarg + 1, 0, 0); + + nondaemon = 1; + break; + + case 'r': + ERROR("The '-r' option has been removed because it is now the default behaviour.\n"); + ERROR("Use the '-x' option to disable clock resetting.\n"); + *ret = 1; + return 0; + + default: + *ret = 1; + return 0; + } + } + + ptpClock = (PtpClock *) calloc(1, sizeof(PtpClock)); + if (!ptpClock) { + PERROR("failed to allocate memory for protocol engine data"); + *ret = 2; + return 0; + } else { + DBG("allocated %d bytes for protocol engine data\n", (int)sizeof(PtpClock)); + ptpClock->foreign = (ForeignMasterRecord *) calloc(rtOpts->max_foreign_records, sizeof(ForeignMasterRecord)); + if (!ptpClock->foreign) { + PERROR("failed to allocate memory for foreign master data"); + *ret = 2; + free(ptpClock); + return 0; + } else { + DBG("allocated %d bytes for foreign master data\n", (int)(rtOpts->max_foreign_records * sizeof(ForeignMasterRecord))); + } + } + +#ifndef PTPD_NO_DAEMON + if (!nondaemon) { + if (daemon(0, noclose) == -1) { + PERROR("failed to start as daemon"); + *ret = 3; + return 0; + } + DBG("running as daemon\n"); + } +#endif + + signal(SIGINT, catch_close); + signal(SIGTERM, catch_close); + signal(SIGHUP, catch_sighup); + + *ret = 0; + return ptpClock; +} diff --git a/third_party/ptpd-1.1.0/src/dep/sys.c b/third_party/ptpd-1.1.0/src/dep/sys.c new file mode 100644 index 0000000..9e1221e --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/sys.c @@ -0,0 +1,282 @@ +/** + * @file sys.c + * @date Tue Jul 20 16:19:46 2010 + * + * @brief Code to call kernel time routines and also display server statistics. + * + * + */ + +#include "../ptpd.h" + + +int +isTimeInternalNegative(const TimeInternal * p) +{ + return (p->seconds < 0) || (p->nanoseconds < 0); +} + + +int +snprint_TimeInternal(char *s, int max_len, const TimeInternal * p) +{ + int len = 0; + + if (isTimeInternalNegative(p)) + len += snprintf(&s[len], max_len - len, "-"); + + len += snprintf(&s[len], max_len - len, "%d.%09d", + abs(p->seconds), abs(p->nanoseconds)); + + return len; +} + + + +int +snprint_ClockIdentity(char *s, int max_len, const Octet uuid[PTP_UUID_LENGTH], const char *info) +{ + int len = 0; + int i; + + if (info) + len += snprintf(&s[len], max_len - len, "%s", info); + + for (i = 0; ;) { + len += snprintf(&s[len], max_len - len, "%02x", (unsigned char) uuid[i]); + + if (++i >= PTP_UUID_LENGTH) + break; + + // uncomment the line below to print a separator after each byte except the last one + // len += snprintf(&s[len], max_len - len, "%s", "-"); + } + + return len; +} + + +int +snprint_PortIdentity(char *s, int max_len, const Octet uuid[PTP_UUID_LENGTH], + UInteger16 portId, const char *info) +{ + int len = 0; + + if (info) + len += snprintf(&s[len], max_len - len, "%s", info); + + len += snprint_ClockIdentity(&s[len], max_len - len, uuid, NULL); + len += snprintf(&s[len], max_len - len, ":%02x", portId); + return len; +} + + +void +message(int priority, const char *format, ...) +{ + extern RunTimeOpts rtOpts; + va_list ap; + va_start(ap, format); + if(rtOpts.useSysLog) { + static Boolean logOpened; + if(!logOpened) { + openlog("ptpd", 0, LOG_USER); + logOpened = TRUE; + } + vsyslog(priority, format, ap); + } else { + fprintf(stderr, "(ptpd %s) ", + priority == LOG_EMERG ? "emergency" : + priority == LOG_ALERT ? "alert" : + priority == LOG_CRIT ? "critical" : + priority == LOG_ERR ? "error" : + priority == LOG_WARNING ? "warning" : + priority == LOG_NOTICE ? "notice" : + priority == LOG_INFO ? "info" : + priority == LOG_DEBUG ? "debug" : + "???"); + vfprintf(stderr, format, ap); + } + va_end(ap); +} + +char * +translatePortState(PtpClock *ptpClock) +{ + char *s; + switch(ptpClock->port_state) { + case PTP_INITIALIZING: s = "init"; break; + case PTP_FAULTY: s = "flt"; break; + case PTP_LISTENING: s = "lstn"; break; + case PTP_PASSIVE: s = "pass"; break; + case PTP_UNCALIBRATED: s = "uncl"; break; + case PTP_SLAVE: s = "slv"; break; + case PTP_PRE_MASTER: s = "pmst"; break; + case PTP_MASTER: s = "mst"; break; + case PTP_DISABLED: s = "dsbl"; break; + default: s = "?"; break; + } + return s; +} + +void +displayStats(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + static int start = 1; + static char sbuf[SCREEN_BUFSZ]; + int len = 0; + struct timeval now; + char time_str[MAXTIMESTR]; + + if (start && rtOpts->csvStats) { + start = 0; + printf("timestamp, state, clock ID, one way delay, " + "offset from master, master to slave, " + "slave to master, drift, variance"); + fflush(stdout); + } + memset(sbuf, ' ', sizeof(sbuf)); + + gettimeofday(&now, 0); + strftime(time_str, MAXTIMESTR, "%Y-%m-%d %X", localtime(&now.tv_sec)); + + len += snprintf(sbuf + len, sizeof(sbuf) - len, "%s%s:%06d, %s", + rtOpts->csvStats ? "\n" : "\rstate: ", + time_str, (int)now.tv_usec, + translatePortState(ptpClock)); + + if (ptpClock->port_state == PTP_SLAVE) { + len += snprint_PortIdentity(sbuf + len, sizeof(sbuf) - len, + ptpClock->parent_uuid, ptpClock->parent_port_id, ", "); + + /* + * if grandmaster ID differs from parent port ID then also + * print GM ID + */ + if (memcmp(ptpClock->grandmaster_uuid_field, + ptpClock->parent_uuid, PTP_UUID_LENGTH)) { + len += snprint_ClockIdentity(sbuf + len, + sizeof(sbuf) - len, + ptpClock->grandmaster_uuid_field, + " GM:"); + } + + len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); + + if (!rtOpts->csvStats) + len += snprintf(sbuf + len, + sizeof(sbuf) - len, "owd: "); + + len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, + &ptpClock->one_way_delay); + + len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); + + if (!rtOpts->csvStats) + len += snprintf(sbuf + len, sizeof(sbuf) - len, + "ofm: "); + + len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, + &ptpClock->offset_from_master); + + len += snprintf(sbuf + len, sizeof(sbuf) - len, + ", %s%d.%09d" ", %s%d.%09d", + rtOpts->csvStats ? "" : "stm: ", + ptpClock->slave_to_master_delay.seconds, + abs(ptpClock->slave_to_master_delay.nanoseconds), + rtOpts->csvStats ? "" : "mts: ", + ptpClock->master_to_slave_delay.seconds, + abs(ptpClock->master_to_slave_delay.nanoseconds)); + + len += snprintf(sbuf + len, sizeof(sbuf) - len, + ", %s%d", + rtOpts->csvStats ? "" : "drift: ", + ptpClock->observed_drift); + + len += snprintf(sbuf + len, sizeof(sbuf) - len, + ", %s%d", + rtOpts->csvStats ? "" : "var: ", + ptpClock->observed_variance); + } + else { + if (ptpClock->port_state == PTP_MASTER) { + len += snprint_ClockIdentity(sbuf + len, sizeof(sbuf) - len, + ptpClock->clock_uuid_field, " (ID:"); + len += snprintf(sbuf + len, sizeof(sbuf) - len, ")"); + } + } + write(1, sbuf, rtOpts->csvStats ? len : SCREEN_MAXSZ + 1); +} + +Boolean +nanoSleep(TimeInternal * t) +{ + struct timespec ts, tr; + + ts.tv_sec = t->seconds; + ts.tv_nsec = t->nanoseconds; + + if (nanosleep(&ts, &tr) < 0) { + t->seconds = tr.tv_sec; + t->nanoseconds = tr.tv_nsec; + return FALSE; + } + return TRUE; +} + +void +getTime(TimeInternal * time) +{ +#if defined(linux) + + struct timeval tv; + gettimeofday(&tv, 0); + time->seconds = tv.tv_sec; + time->nanoseconds = tv.tv_usec * 1000; + +#else /* FreeBSD */ + + struct timespec tp; + if (clock_gettime(CLOCK_REALTIME, &tp) < 0) { + PERROR("clock_gettime() failed, exiting."); + exit(0); + } + time->seconds = tp.tv_sec; + time->nanoseconds = tp.tv_nsec; +#endif /* FreeBSD or Linux */ +} + +void +setTime(TimeInternal * time) +{ + struct timeval tv; + + tv.tv_sec = time->seconds; + tv.tv_usec = time->nanoseconds / 1000; + settimeofday(&tv, 0); + + NOTIFY("resetting system clock to %ds %dns\n", + time->seconds, time->nanoseconds); +} + +UInteger16 +getRand(UInteger32 * seed) +{ + return rand_r((unsigned int *)seed); +} + +Boolean +adjFreq(Integer32 adj) +{ + struct timex t; + + if (adj > ADJ_FREQ_MAX) + adj = ADJ_FREQ_MAX; + else if (adj < -ADJ_FREQ_MAX) + adj = -ADJ_FREQ_MAX; + + t.modes = MOD_FREQUENCY; + t.freq = adj * ((1 << 16) / 1000); + + return !adjtimex(&t); +} diff --git a/third_party/ptpd-1.1.0/src/dep/timer.c b/third_party/ptpd-1.1.0/src/dep/timer.c new file mode 100644 index 0000000..4bd7f80 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/timer.c @@ -0,0 +1,96 @@ +/** + * @file timer.c + * @date Wed Jun 23 09:41:26 2010 + * + * @brief The timers which run the state machine. + * + * Timers in the PTP daemon are run off of the signal system. + */ + +#include "../ptpd.h" + +#define TIMER_INTERVAL 1 +int elapsed; + +void +catch_alarm(int sig) +{ + elapsed += TIMER_INTERVAL; + + DBGV("catch_alarm: elapsed %d\n", elapsed); +} + +void +initTimer(void) +{ + struct itimerval itimer; + + DBG("initTimer\n"); + + signal(SIGALRM, SIG_IGN); + + elapsed = 0; + itimer.it_value.tv_sec = itimer.it_interval.tv_sec = TIMER_INTERVAL; + itimer.it_value.tv_usec = itimer.it_interval.tv_usec = 0; + + signal(SIGALRM, catch_alarm); + setitimer(ITIMER_REAL, &itimer, 0); +} + +void +timerUpdate(IntervalTimer * itimer) +{ + int i, delta; + + delta = elapsed; + elapsed = 0; + + if (delta <= 0) + return; + + for (i = 0; i < TIMER_ARRAY_SIZE; ++i) { + if (itimer[i].interval > 0 && (itimer[i].left -= delta) <= 0) { + itimer[i].left = itimer[i].interval; + itimer[i].expire = TRUE; + DBGV("timerUpdate: timer %u expired\n", i); + } + } +} + +void +timerStop(UInteger16 index, IntervalTimer * itimer) +{ + if (index >= TIMER_ARRAY_SIZE) + return; + + itimer[index].interval = 0; +} + +void +timerStart(UInteger16 index, UInteger16 interval, IntervalTimer * itimer) +{ + if (index >= TIMER_ARRAY_SIZE) + return; + + itimer[index].expire = FALSE; + itimer[index].left = interval; + itimer[index].interval = itimer[index].left; + + DBGV("timerStart: set timer %d to %d\n", index, interval); +} + +Boolean +timerExpired(UInteger16 index, IntervalTimer * itimer) +{ + timerUpdate(itimer); + + if (index >= TIMER_ARRAY_SIZE) + return FALSE; + + if (!itimer[index].expire) + return FALSE; + + itimer[index].expire = FALSE; + + return TRUE; +} -- cgit v1.3.1