diff options
| author | Yuval Adam <yuv.adm@gmail.com> | 2014-08-08 14:42:07 +0300 |
|---|---|---|
| committer | Yuval Adam <yuv.adm@gmail.com> | 2014-08-08 14:42:07 +0300 |
| commit | a6163888f3c56123b1db313743c6147ba498732c (patch) | |
| tree | ff7d15d991d1d09ba6cbc0cec80924f57445bfdd /third_party/ptpd-1.1.0/src/dep-tiva | |
| parent | c3e4c9a25c2910d2d66d52215b3406b13d5b23d5 (diff) | |
Add third_party libs
Diffstat (limited to 'third_party/ptpd-1.1.0/src/dep-tiva')
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/constants_dep.h | 87 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/datatypes_dep.h | 42 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/ptpd_dep.h | 138 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/ptpd_msg.c | 655 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/ptpd_net.c | 401 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/ptpd_servo.c | 179 | ||||
| -rw-r--r-- | third_party/ptpd-1.1.0/src/dep-tiva/ptpd_timer.c | 75 |
7 files changed, 1577 insertions, 0 deletions
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 <string.h>
+
+/*
+ * 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)
+ ++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)
+{
+ 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;
+}
+
|