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 --- .../ptpd-1.1.0/src/dep-tiva/constants_dep.h | 87 +++ .../ptpd-1.1.0/src/dep-tiva/datatypes_dep.h | 42 ++ third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h | 138 +++++ third_party/ptpd-1.1.0/src/dep-tiva/ptpd_msg.c | 655 +++++++++++++++++++++ third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c | 401 +++++++++++++ third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c | 179 ++++++ third_party/ptpd-1.1.0/src/dep-tiva/ptpd_timer.c | 75 +++ 7 files changed, 1577 insertions(+) create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/constants_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/datatypes_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/ptpd_msg.c create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c create mode 100644 third_party/ptpd-1.1.0/src/dep-tiva/ptpd_timer.c (limited to 'third_party/ptpd-1.1.0/src/dep-tiva') diff --git a/third_party/ptpd-1.1.0/src/dep-tiva/constants_dep.h b/third_party/ptpd-1.1.0/src/dep-tiva/constants_dep.h new file mode 100644 index 0000000..952bddd --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/constants_dep.h @@ -0,0 +1,87 @@ +/* constants_dep.h */ + +#ifndef CONSTANTS_DEP_H +#define CONSTANTS_DEP_H + +/* platform dependent */ + +#ifdef IF_NAMESIZE + #define IFACE_NAME_LENGTH IF_NAMESIZE +#else + #define IFACE_NAME_LENGTH 16 +#endif + +#ifdef INET_ADDRSTRLEN + #define NET_ADDRESS_LENGTH INET_ADDRSTRLEN +#else + #define NET_ADDRESS_LENGTH 16 +#endif + +#define IFCONF_LENGTH 10 + +#define PTPD_LSBF + +#define ADJ_MAX 10000000 + +/* 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 PBUF_QUEUE_SIZE 16 + +/* override default values */ +#ifdef DEFAULT_AP +# undef DEFAULT_AP +# define DEFAULT_AP 2 +#endif + +#ifdef DEFAULT_AI +# undef DEFAULT_AI +# define DEFAULT_AI 200 +#endif + +#define MAX_AP 10 +#define MAX_AI 1000 + +#ifdef DEFAULT_INBOUND_LATENCY +# undef DEFAULT_INBOUND_LATENCY +# define DEFAULT_INBOUND_LATENCY 16500 +#endif + +#ifdef DEFAULT_OUTBOUND_LATENCY +# undef DEFAULT_OUTBOUND_LATENCY +# define DEFAULT_OUTBOUND_LATENCY 16500 +#endif + +#ifdef CLOCK_FOLLOWUP +# undef CLOCK_FOLLOWUP +# define CLOCK_FOLLOWUP FALSE +#endif + +#endif /* #ifndef CONSTANTS_DEP_H */ + diff --git a/third_party/ptpd-1.1.0/src/dep-tiva/datatypes_dep.h b/third_party/ptpd-1.1.0/src/dep-tiva/datatypes_dep.h new file mode 100644 index 0000000..af9f67d --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/datatypes_dep.h @@ -0,0 +1,42 @@ +/* datatypes_dep.h */ + +#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 { + void *pbuf[PBUF_QUEUE_SIZE]; + Integer32 get; + Integer32 put; + Integer32 count; +} BufQueue; + +typedef struct { + Integer32 multicastAddr; + Integer32 unicastAddr; + void *eventPcb; + void *generalPcb; + BufQueue eventQ; + BufQueue generalQ; + void *eventTxBuf; + void *generalTxBuf; +} NetPath; + +#endif diff --git a/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h new file mode 100644 index 0000000..10b297e --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h @@ -0,0 +1,138 @@ +/* ptpd_dep.h */ + +#ifndef PTPD_DEP_H +#define PTPD_DEP_H + +#include + +/* + * Remove the following #if 0 and its matching #endif to enable PTPd debug + * output on UART0. This is disabled by default even in debug builds due to + * the volume of information transmitted. + */ +#if 0 +#ifdef DEBUG +#include "utils/uartstdio.h" +#define PTPD_DBG +/* + * Define the following to enable verbose debug. + */ +#undef PTPD_DBGV +#endif +#endif + +#ifndef INT_MAX +#define INT_MAX 2147483647 +#endif +#ifndef size_t +#define size_t long +#endif +#ifndef ssize_t +#define ssize_t long +#endif + +/* system messages */ +#ifndef PTPD_DBG +#define ERROR(...) +#define PERROR(...) +#define NOTIFY(...) +#else +#define ERROR(x, ...) UARTprintf("(ptpd error) " x, ##__VA_ARGS__) +#define PERROR(x, ...) UARTprintf("(ptpd error) " x ": %m\n", ##__VA_ARGS__) +#define NOTIFY(x, ...) UARTprintf("(ptpd notice) " x, ##__VA_ARGS__) +#endif + +/* debug messages */ +#ifdef PTPD_DBGV +#define PTPD_DBG +#define DBGV(x, ...) UARTprintf("(ptpd debug) " x, ##__VA_ARGS__) +#else +#define DBGV(...) +#endif + +#ifdef PTPD_DBG +#define DBG(x, ...) UARTprintf("(ptpd debug) " x, ##__VA_ARGS__) +#else +#define DBG(...) +#endif + +/* endian corrections */ +#if defined(PTPD_MSBF) +#define shift8(x,y) ( (x) << ((3-y)<<3) ) +#define shift16(x,y) ( (x) << ((1-y)<<4) ) +#define flip16(x) (x) +#define flip32(x) (x) + +#elif defined(PTPD_LSBF) +#define shift8(x,y) ( (x) << ((y)<<3) ) +#define shift16(x,y) ( (x) << ((y)<<4) ) +#define flip16(x) ((((x) >> 8) & 0x00ff) | (((x) << 8) & 0xff00)) +#define flip32(x) ((((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)) ) + + +#define labs(x) (((x) >= 0) ? (x) : -(x)) + +/* msg.c */ +Boolean msgPeek(char*,size_t); +void msgUnpackHeader(char*,MsgHeader*); +void msgUnpackSync(char*,MsgSync*); +void msgUnpackDelayReq(char*,MsgDelayReq*); +void msgUnpackFollowUp(char*,MsgFollowUp*); +void msgUnpackDelayResp(char*,MsgDelayResp*); +void msgUnpackManagement(char*,MsgManagement*); +UInteger8 msgUnloadManagement(char*,MsgManagement*,PtpClock*,RunTimeOpts*); +void msgUnpackManagementPayload(char *buf, MsgManagement *manage); +void msgPackHeader(char*,PtpClock*); +void msgPackSync(char*,Boolean,TimeRepresentation*,PtpClock*); +void msgPackDelayReq(char*,Boolean,TimeRepresentation*,PtpClock*); +void msgPackFollowUp(char*,UInteger16,TimeRepresentation*,PtpClock*); +void msgPackDelayResp(char*,MsgHeader*,TimeRepresentation*,PtpClock*); +UInteger16 msgPackManagement(char*,MsgManagement*,PtpClock*); +UInteger16 msgPackManagementResponse(char*,MsgHeader*,MsgManagement*,PtpClock*); + +/* net.c */ +Boolean netInit(NetPath*,RunTimeOpts*,PtpClock*); +Boolean netShutdown(NetPath*); +int netSelect(TimeInternal*,NetPath*); +size_t netRecvEvent(Octet*,TimeInternal*,NetPath*); +size_t netRecvGeneral(Octet*,NetPath*); +size_t netSendEvent(Octet*,UInteger16,NetPath*); +size_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*); + +/* sys.c */ +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 timerTick(int); +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-tiva/ptpd_msg.c b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_msg.c new file mode 100644 index 0000000..6827ec0 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_msg.c @@ -0,0 +1,655 @@ +/* ptpd_msg.c */ +/* see spec annex d */ + +#include "../ptpd.h" + +Boolean msgPeek(char *buf, size_t length) +{ + /* not imlpemented yet */ + return TRUE; +} + +void msgUnpackHeader(char *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 %02x:%02x:%02x:%02x:%02x:%02x\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 %02x %02x\n", header->flags[0], header->flags[1]); +} + +void msgUnpackSync(char *buf, MsgSync *sync) +{ + sync->originTimestamp.seconds = flip32(*(UInteger32*)(buf + 40)); + DBG("msgUnpackSync: originTimestamp.seconds %u\n", sync->originTimestamp.seconds); + sync->originTimestamp.nanoseconds = flip32(*(Integer32*)(buf + 44)); + DBG("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 %02x:%02x:%02x:%02x:%02x:%02x\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 %02x:%02x:%02x:%02x:%02x:%02x\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(char *buf, MsgDelayReq *req) {} + +void msgUnpackFollowUp(char *buf, MsgFollowUp *follow) +{ + follow->associatedSequenceId = flip16(*(UInteger16*)(buf + 42)); + DBGV("msgUnpackFollowUp: associatedSequenceId %u\n", follow->associatedSequenceId); + follow->preciseOriginTimestamp.seconds = flip32(*(UInteger32*)(buf + 44)); + DBG("msgUnpackFollowUp: preciseOriginTimestamp.seconds %u\n", follow->preciseOriginTimestamp.seconds); + follow->preciseOriginTimestamp.nanoseconds = flip32(*(Integer32*)(buf + 48)); + DBG("msgUnpackFollowUp: preciseOriginTimestamp.nanoseconds %d\n", follow->preciseOriginTimestamp.nanoseconds); +} + +void msgUnpackDelayResp(char *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 %02x:%02x:%02x:%02x:%02x:%02x\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(char *buf, MsgManagement *manage) +{ + manage->targetCommunicationTechnology = *(UInteger8*)(buf + 41); + DBGV("msgUnpackManagement: targetCommunicationTechnology %d\n", manage->targetCommunicationTechnology); + memcpy(manage->targetUuid, (buf + 42), 6); + DBGV("msgUnpackManagement: targetUuid %02x:%02x:%02x:%02x:%02x:%02x\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(char *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(char *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(char *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(char *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(char *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(char *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(char *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(char *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(char *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-tiva/ptpd_net.c b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c new file mode 100644 index 0000000..5826ebc --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c @@ -0,0 +1,401 @@ +/* net.c */ + +#include "utils/lwiplib.h" +#include "utils/ptpdlib.h" +#include "lwip/inet.h" + +/* Network Buffer Queue Functions. */ +static void netQInit(BufQueue *pQ) +{ + pQ->get = 0; + pQ->put = 0; + pQ->count = 0; +} + +static Integer32 netQPut(BufQueue *pQ, void *pbuf) +{ + if(pQ->count >= PBUF_QUEUE_SIZE) + return FALSE; + pQ->pbuf[pQ->put] = pbuf; + pQ->put = (pQ->put + 1) % PBUF_QUEUE_SIZE; + pQ->count++; + return TRUE; +} + +void *netQGet(BufQueue *pQ) +{ + void *pbuf; + + if(!pQ->count) + return NULL; + pbuf = pQ->pbuf[pQ->get]; + pQ->get = (pQ->get + 1) % PBUF_QUEUE_SIZE; + pQ->count--; + return pbuf; +} + +static Integer32 netQCheck(BufQueue *pQ) +{ + if(!pQ->count) + return FALSE; + return TRUE; +} + +/* Processing an incoming message on the Event port. */ +static void eventRecv(void *arg, struct udp_pcb *pcb, struct pbuf *p, + struct ip_addr *addr, u16_t port) +{ + NetPath *netPath = (NetPath *)arg; + + /* prevent warnings about unused arguments */ + (void)pcb; (void)addr; (void)port; + + /* Place the incoming message on the Event Port QUEUE. */ + if(!netQPut(&netPath->eventQ, p)) + PERROR("Event Queue Full!\n"); +} + +/* Processing an incoming message on the Event port. */ +static void generalRecv(void *arg, struct udp_pcb *pcb, struct pbuf *p, + struct ip_addr *addr, u16_t port) +{ + NetPath *netPath = (NetPath *)arg; + + /* prevent warnings about unused arguments */ + (void)pcb; (void)addr; (void)port; + + /* Place the incoming message on the Event Port QUEUE. */ + if(!netQPut(&netPath->generalQ, p)) + { + PERROR("Event Queue Full!\n"); + } +} + +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; +} + +Boolean netInit(NetPath *netPath, RunTimeOpts *rtOpts, PtpClock *ptpClock) +{ + int i; + struct in_addr netAddr; + char addrStr[NET_ADDRESS_LENGTH]; + + DBG("netInit\n"); + + /* Allocate tx buffer for the event port. */ + netPath->eventTxBuf = pbuf_alloc(PBUF_TRANSPORT, PACKET_SIZE, PBUF_RAM); + if(netPath->eventTxBuf == NULL) + { + PERROR("Failed to allocate Event Tx Buffer\n"); + return FALSE; + } + + /* Allocate tx buffer for the general port. */ + netPath->generalTxBuf = pbuf_alloc(PBUF_TRANSPORT, PACKET_SIZE, PBUF_RAM); + if(netPath->generalTxBuf == NULL) + { + PERROR("Failed to allocate Event Tx Buffer\n"); + pbuf_free(netPath->eventTxBuf); + return FALSE; + } + + /* Open lwIP raw udp interfaces for the event port. */ + netPath->eventPcb = udp_new(); + if(netPath->eventPcb == NULL) + { + PERROR("Failed to open Event UDP PCB\n"); + pbuf_free(netPath->eventTxBuf); + pbuf_free(netPath->generalTxBuf); + return FALSE; + } + + /* Open lwIP raw udp interfaces for the general port. */ + netPath->generalPcb = udp_new(); + if(netPath->generalPcb == NULL) + { + PERROR("Failed to open General UDP PCB\n"); + udp_remove(netPath->eventPcb); + pbuf_free(netPath->eventTxBuf); + pbuf_free(netPath->generalTxBuf); + return FALSE; + } + + /* Initialize the buffer queues. */ + netQInit(&netPath->eventQ); + netQInit(&netPath->generalQ); + + /* Configure network (broadcast/unicast) addresses. */ + netPath->unicastAddr = 0; + if(!lookupSubdomainAddress(rtOpts->subdomainName, addrStr)) + { + udp_disconnect(netPath->eventPcb); + udp_disconnect(netPath->generalPcb); + udp_remove(netPath->eventPcb); + udp_remove(netPath->generalPcb); + pbuf_free(netPath->eventTxBuf); + pbuf_free(netPath->generalTxBuf); + return FALSE; + } + if(!inet_aton(addrStr, &netAddr)) + { + ERROR("failed to encode multi-cast address: %s\n", addrStr); + udp_disconnect(netPath->eventPcb); + udp_disconnect(netPath->generalPcb); + udp_remove(netPath->eventPcb); + udp_remove(netPath->generalPcb); + pbuf_free(netPath->eventTxBuf); + pbuf_free(netPath->generalTxBuf); + return FALSE; + } + netPath->multicastAddr = netAddr.s_addr; + + /* Setup subdomain address string. */ + for(i = 0; i < SUBDOMAIN_ADDRESS_LENGTH; ++i) + { + ptpClock->subdomain_address[i] = (netAddr.s_addr >> (i * 8)) & 0xff; + } + + /* Establish the appropriate UDP bindings/connections for events. */ + + udp_recv(netPath->eventPcb, eventRecv, netPath); + udp_bind(netPath->eventPcb, IP_ADDR_ANY, PTP_EVENT_PORT); +// udp_connect(netPath->eventPcb, IP_ADDR_ANY, PTP_EVENT_PORT); + *(Integer16*)ptpClock->event_port_address = PTP_EVENT_PORT; + + /* Establish the appropriate UDP bindings/connections for general. */ + udp_recv(netPath->generalPcb, generalRecv, netPath); + udp_bind(netPath->generalPcb, IP_ADDR_ANY, PTP_GENERAL_PORT); +// udp_connect(netPath->generalPcb, IP_ADDR_ANY, PTP_GENERAL_PORT); + *(Integer16*)ptpClock->general_port_address = PTP_GENERAL_PORT; + + /* Return a success code. */ + return TRUE; +} + +/* shut down the UDP stuff */ +Boolean netShutdown(NetPath *netPath) +{ + /* Disconnect and close the Event UDP interface */ + if(netPath->eventPcb) + { + udp_disconnect(netPath->eventPcb); + udp_remove(netPath->eventPcb); + } + + /* Disconnect and close the General UDP interface */ + if(netPath->generalPcb) + { + udp_disconnect(netPath->generalPcb); + udp_remove(netPath->generalPcb); + } + + /* Free up the Event and General Tx PBUFs. */ + if(netPath->eventTxBuf) + { + pbuf_free(netPath->eventTxBuf); + } + if(netPath->generalTxBuf) + { + pbuf_free(netPath->generalTxBuf); + } + + /* Clear the network addresses. */ + netPath->multicastAddr = 0; + netPath->unicastAddr = 0; + + /* Return a success code. */ + return TRUE; +} + +/* Wait for a packet to come in on either port. For now, there is no wait. + * Simply check to see if a packet is available on either port and return 1, + * otherwise return 0. */ +int netSelect(TimeInternal *timeout, NetPath *netPath) +{ + /* Check the packet queues. If there is data, return TRUE. */ + if(netQCheck(&netPath->eventQ) || netQCheck(&netPath->generalQ)) + return 1; + return 0; +} + +/* Pop a message off of the event queue and copy it to the passed in buffer. */ +size_t netRecvEvent(Octet *buf, TimeInternal *time, NetPath *netPath) +{ + int i, j; + int iPacketSize; + struct pbuf *p, *pcopy; + + /* Attempt to get a buffer from the Q. If none is available, + * return length of 0. */ + + p = netQGet(&netPath->eventQ); + if(p == NULL) + return 0; + pcopy = p; + + + /* Here, p points to a valid PBUF structure. Verify that we have + * enough space to store the contents. */ + if(p->tot_len > PACKET_SIZE) + { + ERROR("received truncated message\n"); + return 0; + } + + /* Copy the PBUF payload into the buffer. */ + j = 0; + iPacketSize = p->tot_len; + for(i = 0; i < iPacketSize; i++) + { + buf[i] = ((u8_t *)pcopy->payload)[j++]; + if(j == pcopy->len) + { + pcopy = pcopy->next; + j = 0; + } + } + + /* Get the timestamp information. */ + time->seconds = p->time_s; + time->nanoseconds = p->time_ns; + + /* Free up the pbuf (chain). */ + pbuf_free(p); + + /* Return the length of data copied. */ + return iPacketSize; +} + +/* Pop a message off of the general queue and copy to the passed in buffer. */ +size_t netRecvGeneral(Octet *buf, NetPath *netPath) +{ + int i, j; + int iPacketSize; + struct pbuf *p, *pcopy; + + + /* Attempt to get a buffer from the Q. If none is available, + * return length of 0. */ + p = netQGet(&netPath->generalQ); + if(p == NULL) + return 0; + pcopy = p; + + /* Here, p points to a valid PBUF structure. Verify that we have + * enough space to store the contents. */ + if(p->tot_len > PACKET_SIZE) + { + ERROR("received truncated message\n"); + return 0; + } + + /* Copy the PBUF payload into the buffer. */ + j = 0; + iPacketSize = p->tot_len; + for(i = 0; i < iPacketSize; i++) + { + buf[i] = ((u8_t *)pcopy->payload)[j++]; + if(j == pcopy->len) + { + pcopy = pcopy->next; + j = 0; + } + } + + /* Free up the pbuf (chain). */ + pbuf_free(p); + + /* Return the length of data copied. */ + return iPacketSize; +} + +/* Transmit a packet on the Event Port. */ +size_t netSendEvent(Octet *buf, UInteger16 length, NetPath *netPath) +{ + int i, j; + struct pbuf *pcopy; + + /* Reallocate the tx pbuf based on the current size. */ + pbuf_realloc(netPath->eventTxBuf, length); + pcopy = netPath->eventTxBuf; + + /* Copy the incoming data into the pbuf payload. */ + j = 0; + for(i = 0; i < length; i++) + { + ((u8_t *)pcopy->payload)[j++] = buf[i]; + if(j == pcopy->len) + { + pcopy = pcopy->next; + j = 0; + } + } + + /* send the buffer. */ + udp_sendto(netPath->eventPcb, netPath->eventTxBuf, + (void *)&netPath->multicastAddr, PTP_EVENT_PORT); + + return(length); +} + +/* Transmit a packet on the General Port. */ +size_t netSendGeneral(Octet *buf, UInteger16 length, NetPath *netPath) +{ + int i, j; + struct pbuf *pcopy; + + /* Reallocate the tx pbuf based on the current size. */ + pbuf_realloc(netPath->generalTxBuf, length); + pcopy = netPath->generalTxBuf; + + /* Copy the incoming data into the pbuf payload. */ + j = 0; + for(i = 0; i < length; i++) + { + ((u8_t *)pcopy->payload)[j++] = buf[i]; + if(j == pcopy->len) + { + pcopy = pcopy->next; + j = 0; + } + } + + /* send the buffer. */ + udp_sendto(netPath->eventPcb, netPath->generalTxBuf, + (void *)&netPath->multicastAddr, PTP_GENERAL_PORT); + + return(length); +} diff --git a/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c new file mode 100644 index 0000000..c6a3eae --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c @@ -0,0 +1,179 @@ +/* ptpd_servo.c */ + +#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; + + rtOpts->ai = DEFAULT_AI; + rtOpts->ap = DEFAULT_AP; + + /* 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; + + DBGV("updateDelay\n"); + + /* calc 'slave_to_master_delay' */ + subTime(&ptpClock->slave_to_master_delay, recv_time, send_time); + + /* update 'one_way_delay' */ + addTime(&ptpClock->one_way_delay, &ptpClock->master_to_slave_delay, &ptpClock->slave_to_master_delay); + + // + // TODO: This looks wrong. If seconds is odd, surely we need to add half + // a second's worth of nanoseconds to the result? I'm assuming that + // these two lines intend to divide the one_way_delay as a whole by 2. + // + 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(labs(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_exp; + else if(owd_filt->s_exp > 1<s_exp = 1<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) +{ + DBGV("updateOffset\n"); + + /* calc 'master_to_slave_delay' */ + subTime(&ptpClock->master_to_slave_delay, recv_time, send_time); + + /* 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; + static unsigned long ulCount = 0; + + DBGV("updateClock\n"); + + 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_MAX : -ADJ_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; + + /* adjust the filter components each time, till we reach the max */ + ulCount++; + if(!ulCount % 2) + { + rtOpts->ai++; + if(rtOpts->ai > MAX_AI) + { + rtOpts->ai = MAX_AI; + } + rtOpts->ap = rtOpts->ai / 100; + if(rtOpts->ap < DEFAULT_AP) + { + rtOpts->ap = DEFAULT_AP; + } + } + + /* the accumulator for the I component */ + ptpClock->observed_drift += ptpClock->offset_from_master.nanoseconds/rtOpts->ai; + + 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); + } + + 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-tiva/ptpd_timer.c b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_timer.c new file mode 100644 index 0000000..47b0bb5 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep-tiva/ptpd_timer.c @@ -0,0 +1,75 @@ +/* ptpd_timer.c */ + +#include "../ptpd.h" + +static int iElapsedMilliSeconds = 0; + +void initTimer(void) +{ + DBG("initTimer\n"); + + iElapsedMilliSeconds = 0; +} + +void timerTick(int iTickMilliSeconds) +{ + iElapsedMilliSeconds += iTickMilliSeconds; +} + +void timerUpdate(IntervalTimer *itimer) +{ + int i, delta; + + delta = iElapsedMilliSeconds / 1000; + + if(delta <= 0) + return; + + iElapsedMilliSeconds %= 1000; + + 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