ptpd: add FSL QorIQ DPAA and eTSEC support

This patch is for Freescale QorIQ DPAA and eTSEC which support 1588 hardware
assist module, and mainly uses SO_TIMESTAMPING API for HW timestamp and PHC
API.

Signed-off-by: Yangbo Lu <yangbo.lu@freescale.com>
Signed-off-by: Zhenhua Luo <zhenhua.luo@freescale.com>
Signed-off-by: Martin Jansa <Martin.Jansa@gmail.com>

2 files changed, 624 insertions, 1 deletions

diff --git a/meta-networking/recipes-daemons/ptpd/ptpd/ptpd-add-dpaa-etsec-support.patch b/meta-networking/recipes-daemons/ptpd/ptpd/ptpd-add-dpaa-etsec-support.patch
new file mode 100644
index 000000000..abb393310
--- /dev/null
+++ b/meta-networking/recipes-daemons/ptpd/ptpd/ptpd-add-dpaa-etsec-support.patch
@@ -0,0 +1,621 @@
+Add FSL QorIQ DPAA and eTSEC support
+
+This patch is for Freescale QorIQ DPAA and eTSEC which support 1588 hardware
+assist module, and mainly uses SO_TIMESTAMPING API for HW timestamp and PHC
+API.
+
+Signed-off-by: Lu Yangbo <yangbo.lu@freescale.com>
+Upstream-Status: Submitted [http://sourceforge.net/p/ptpd/patches/56/]
+
+diff --git a/src/bmc.c b/src/bmc.c
+index f4c74bd..925daba 100644
+--- a/src/bmc.c
++++ b/src/bmc.c
+@@ -249,6 +249,7 @@ void s1(MsgHeader *header,MsgAnnounce *announce,PtpClock *ptpClock, const RunTim
+         ptpClock->timePropertiesDS.ptpTimescale = IS_SET(header->flagField1, PTPT);
+         ptpClock->timePropertiesDS.timeSource = announce->timeSource;
+ 
++#ifndef FSL_1588
+ #if defined(MOD_TAI) &&  NTP_API == 4
+        /*
+         * update kernel TAI offset, but only if timescale is
+@@ -261,6 +262,7 @@ void s1(MsgHeader *header,MsgAnnounce *announce,PtpClock *ptpClock, const RunTim
+                INFO("Set kernel UTC offset to %d\n", ptpClock->timePropertiesDS.currentUtcOffset);
+         }
+ #endif /* MOD_TAI */
++#endif
+ 
+        /* Leap second handling */
+ 
+@@ -280,9 +282,11 @@ void s1(MsgHeader *header,MsgAnnounce *announce,PtpClock *ptpClock, const RunTim
+                        ptpClock->leapSecondPending = FALSE;
+                        ptpClock->leapSecondInProgress = FALSE;
+                        timerStop(LEAP_SECOND_PAUSE_TIMER, ptpClock->itimer);
++#ifndef FSL_1588
+ #ifdef HAVE_SYS_TIMEX_H
+                        unsetTimexFlags(STA_INS | STA_DEL,TRUE);
+ #endif /* HAVE_SYS_TIMEX_H */
++#endif /* FSL_1588 */
+                }
+ 
+                /*
+@@ -295,6 +299,7 @@ void s1(MsgHeader *header,MsgAnnounce *announce,PtpClock *ptpClock, const RunTim
+                    ((!previousLeap59 && ptpClock->timePropertiesDS.leap59) ||
+                    (!previousLeap61 && ptpClock->timePropertiesDS.leap61)))) {
+ #ifdef HAVE_SYS_TIMEX_H
++#ifndef FSL_1588
+                        WARNING("Leap second pending! Setting kernel to %s "
+                                "one second at midnight\n",
+                                ptpClock->timePropertiesDS.leap61 ? "add" : "delete");
+@@ -304,6 +309,7 @@ void s1(MsgHeader *header,MsgAnnounce *announce,PtpClock *ptpClock, const RunTim
+                            setTimexFlags(ptpClock->timePropertiesDS.leap61 ? STA_INS : STA_DEL,
+                                          FALSE);
+                    }
++#endif
+ #else
+                        WARNING("Leap second pending! No kernel leap second "
+                                "API support - expect a clock jump at "
+diff --git a/src/dep/net.c b/src/dep/net.c
+index 34d23ad..9075847 100644
+--- a/src/dep/net.c
++++ b/src/dep/net.c
+@@ -85,6 +85,9 @@
+ #include <linux/ethtool.h>
+ #endif /* SO_TIMESTAMPING */
+ 
++#if defined(FSL_1588)
++char fsl_1588_if_name[IFACE_NAME_LENGTH];
++#endif
+ /**
+  * shutdown the IPv4 multicast for specific address
+  *
+@@ -461,6 +464,9 @@ testInterface(char * ifaceName, RunTimeOpts* rtOpts)
+        if(getInterfaceInfo(ifaceName, &info) != 1)
+                return FALSE;
+ 
++#if defined(FSL_1588)
++       memcpy(fsl_1588_if_name, ifaceName, IFACE_NAME_LENGTH);
++#endif
+        switch(rtOpts->transport) {
+ 
+            case UDP_IPV4:
+@@ -666,6 +672,27 @@ end:
+ }
+ #endif /* SO_TIMESTAMPING */
+ 
++#if defined(FSL_1588)
++/* select HWTSTAMP_TX_ON or HWTSTAMP_TX_OFF */
++void hwtstamp_tx_ctl(NetPath *netPath, Boolean enable)
++{
++       struct ifreq hwtstamp;
++       struct hwtstamp_config hwconfig;
++
++       memset(&hwtstamp, 0, sizeof(hwtstamp));
++       strncpy(hwtstamp.ifr_name, fsl_1588_if_name, sizeof(hwtstamp.ifr_name));
++       hwtstamp.ifr_data = (void *)&hwconfig;
++       memset(&hwconfig, 0, sizeof(hwconfig));
++       hwconfig.tx_type =
++               enable ?
++               HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
++       hwconfig.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_SYNC;
++       if (ioctl(netPath->eventSock, SIOCSHWTSTAMP, &hwtstamp) < 0
++               || ioctl(netPath->generalSock, SIOCSHWTSTAMP, &hwtstamp) < 0)
++                       printf("error:hwtstamp_tx_ctl\n");
++}
++#endif
++
+ 
+ /**
+  * Initialize timestamping of packets
+@@ -682,10 +709,17 @@ netInitTimestamping(NetPath * netPath, RunTimeOpts * rtOpts)
+        Boolean result = TRUE;
+ #if defined(SO_TIMESTAMPING) && defined(SO_TIMESTAMPNS)/* Linux - current API */
+        DBG("netInitTimestamping: trying to use SO_TIMESTAMPING\n");
++#if defined(FSL_1588)
++       val = SOF_TIMESTAMPING_TX_HARDWARE |
++           SOF_TIMESTAMPING_RX_HARDWARE |
++           SOF_TIMESTAMPING_RAW_HARDWARE;
++#else
+        val = SOF_TIMESTAMPING_TX_SOFTWARE |
+            SOF_TIMESTAMPING_RX_SOFTWARE |
+            SOF_TIMESTAMPING_SOFTWARE;
++#endif
+ 
++#ifndef FSL_1588
+ /* unless compiled with PTPD_EXPERIMENTAL, check if we support the desired tstamp capabilities */
+ #ifndef PTPD_EXPERIMENTAL
+ #ifdef ETHTOOL_GET_TS_INFO
+@@ -716,6 +750,7 @@ netInitTimestamping(NetPath * netPath, RunTimeOpts * rtOpts)
+        val = 1;
+ #endif /* ETHTOOL_GET_TS_INFO */
+ #endif /* PTPD_EXPERIMENTAL */
++#endif /* FSL_1588 */
+ 
+        if(val == 1) {
+            if (setsockopt(netPath->eventSock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0) {
+@@ -902,6 +937,9 @@ netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock)
+ 
+        DBG("Listening on IP: %s\n",inet_ntoa(
+                ((struct sockaddr_in*)&(netPath->interfaceInfo.afAddress))->sin_addr));
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(&ptpClock->netPath, FALSE);/* HWTSTAMP_TX_OFF */
++#endif
+ 
+ #ifdef PTPD_PCAP
+        if (rtOpts->pcap == TRUE) {
+@@ -1319,7 +1357,11 @@ netRecvEvent(Octet * buf, TimeInternal * time, NetPath * netPath, int flags)
+ #if defined(SO_TIMESTAMPING) && defined(SO_TIMESTAMPNS)
+                                if(cmsg->cmsg_type == SO_TIMESTAMPING || 
+                                    cmsg->cmsg_type == SO_TIMESTAMPNS) {
++#if defined(FSL_1588)
++                                       ts = (struct timespec *)CMSG_DATA(cmsg) + 2;
++#else
+                                        ts = (struct timespec *)CMSG_DATA(cmsg);
++#endif
+                                        time->seconds = ts->tv_sec;
+                                        time->nanoseconds = ts->tv_nsec;
+                                        timestampValid = TRUE;
+@@ -1520,6 +1562,9 @@ netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
+        ssize_t ret;
+        struct sockaddr_in addr;
+ 
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(netPath, TRUE);/* HWTSTAMP_TX_ON */
++#endif
+        addr.sin_family = AF_INET;
+        addr.sin_port = htons(PTP_EVENT_PORT);
+ 
+@@ -1559,6 +1604,9 @@ netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
+                                DBG("Error sending unicast event message\n");
+                        else
+                                netPath->sentPackets++;
++#if defined(FSL_1588)
++                       usleep(100);
++#endif
+ #ifndef SO_TIMESTAMPING
+                        /* 
+                         * Need to forcibly loop back the packet since
+@@ -1574,10 +1622,14 @@ netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
+ #else
+                        if(!netPath->txTimestampFailure) {
+                                if(!getTxTimestamp(netPath, tim)) {
++#if defined(FSL_1588)
++                                       printf("getTxTimestamp: get tx timestamp error\n");
++#else
+                                        netPath->txTimestampFailure = TRUE;
+                                        if (tim) {
+                                                clearTime(tim);
+                                        }
++#endif
+                                }
+                        }
+ 
+@@ -1609,9 +1661,15 @@ netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
+                                DBG("Error sending multicast event message\n");
+                        else
+                                netPath->sentPackets++;
++#if defined(FSL_1588)
++                       usleep(100);
++#endif
+ #ifdef SO_TIMESTAMPING
+                        if(!netPath->txTimestampFailure) {
+                                if(!getTxTimestamp(netPath, tim)) {
++#if defined(FSL_1588)
++                                       printf("getTxTimestamp: get tx timestamp error\n");
++#else
+                                        if (tim) {
+                                                clearTime(tim);
+                                        }
+@@ -1620,6 +1678,7 @@ netSendEvent(Octet * buf, UInteger16 length, NetPath * netPath,
+ 
+                                        /* Try re-enabling MULTICAST_LOOP */
+                                        netSetMulticastLoopback(netPath, TRUE);
++#endif
+                                }
+                        }
+ #endif /* SO_TIMESTAMPING */
+@@ -1638,6 +1697,9 @@ netSendGeneral(Octet * buf, UInteger16 length, NetPath * netPath,
+        ssize_t ret;
+        struct sockaddr_in addr;
+ 
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(netPath, TRUE);/* HWTSTAMP_TX_ON */
++#endif
+        addr.sin_family = AF_INET;
+        addr.sin_port = htons(PTP_GENERAL_PORT);
+ 
+@@ -1707,6 +1769,9 @@ netSendPeerGeneral(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpt
+        ssize_t ret;
+        struct sockaddr_in addr;
+ 
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(netPath, TRUE);/* HWTSTAMP_TX_ON */
++#endif
+        addr.sin_family = AF_INET;
+        addr.sin_port = htons(PTP_GENERAL_PORT);
+ 
+@@ -1762,6 +1827,9 @@ netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts
+        ssize_t ret;
+        struct sockaddr_in addr;
+ 
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(netPath, TRUE);/* HWTSTAMP_TX_ON */
++#endif
+        addr.sin_family = AF_INET;
+        addr.sin_port = htons(PTP_EVENT_PORT);
+ 
+@@ -1788,6 +1856,9 @@ netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts
+                        DBG("Error sending unicast peer event message\n");
+                else
+                        netPath->sentPackets++;
++#if defined(FSL_1588)
++               usleep(100);
++#endif
+ 
+ #ifndef SO_TIMESTAMPING
+                /* 
+@@ -1804,10 +1875,14 @@ netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts
+ #else
+                if(!netPath->txTimestampFailure) {
+                        if(!getTxTimestamp(netPath, tim)) {
++#if defined(FSL_1588)
++                               printf("getTxTimestamp: get tx timestamp error\n");
++#else
+                                netPath->txTimestampFailure = TRUE;
+                                if (tim) {
+                                        clearTime(tim);
+                                }
++#endif
+                        }
+                }
+ 
+@@ -1839,9 +1914,15 @@ netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts
+                        DBG("Error sending multicast peer event message\n");
+                else
+                        netPath->sentPackets++;
++#if defined(FSL_1588)
++               usleep(100);
++#endif
+ #ifdef SO_TIMESTAMPING
+                if(!netPath->txTimestampFailure) {
+                        if(!getTxTimestamp(netPath, tim)) {
++#if defined(FSL_1588)
++                               printf("getTxTimestamp: get tx timestamp error\n");
++#else
+                                if (tim) {
+                                        clearTime(tim);
+                                }
+@@ -1850,6 +1931,7 @@ netSendPeerEvent(Octet * buf, UInteger16 length, NetPath * netPath, RunTimeOpts
+ 
+                                /* Try re-enabling MULTICAST_LOOP */
+                                netSetMulticastLoopback(netPath, TRUE);
++#endif
+                        }
+                }
+ #endif /* SO_TIMESTAMPING */
+diff --git a/src/dep/ntpengine/ntpdcontrol.c b/src/dep/ntpengine/ntpdcontrol.c
+index d773284..e84af9e 100644
+--- a/src/dep/ntpengine/ntpdcontrol.c
++++ b/src/dep/ntpengine/ntpdcontrol.c
+@@ -170,6 +170,15 @@ get_systime(
+ {
+         double dtemp;
+ 
++#if defined(FSL_1588)
++       struct timespec tp;
++       clock_gettime(clkid, &tp);
++       tv.tv_sec = tp.tv_sec;
++       tv.tv_usec = tp.tv_nsec / 1000;
++
++       now->l_i = tv.tv_sec + JAN_1970;
++       dtemp = tv.tv_usec / 1e6;
++#else /* FSL_1588 */
+ #if defined(HAVE_CLOCK_GETTIME) || defined(HAVE_GETCLOCK)
+         struct timespec ts;     /* seconds and nanoseconds */
+ 
+@@ -195,6 +204,7 @@ get_systime(
+         dtemp = tv.tv_usec / 1e6;
+ 
+ #endif /* HAVE_CLOCK_GETTIME || HAVE_GETCLOCK */
++#endif /* FSL_1588 */
+ 
+         /*
+          * Renormalize to seconds past 1900 and fraction.
+diff --git a/src/dep/ptpd_dep.h b/src/dep/ptpd_dep.h
+index 8778988..03ae642 100644
+--- a/src/dep/ptpd_dep.h
++++ b/src/dep/ptpd_dep.h
+@@ -14,6 +14,7 @@
+ #define PTPD_DBGV
+ #endif
+ 
++#define FSL_1588 1
+  /** \name System messages*/
+  /**\{*/
+ 
+@@ -280,6 +281,11 @@ UInteger16 msgPackManagementResponse(Octet * buf,MsgHeader*,MsgManagement*,PtpCl
+  * -Init network stuff, send and receive datas*/
+  /**\{*/
+ 
++#if defined(FSL_1588)
++extern char fsl_1588_if_name[IFACE_NAME_LENGTH];
++/* select HWTSTAMP_TX_ON or HWTSTAMP_TX_OFF */
++void hwtstamp_tx_ctl(NetPath *netPath, Boolean enable);
++#endif
+ Boolean testInterface(char* ifaceName, RunTimeOpts* rtOpts);
+ Boolean netInit(NetPath*,RunTimeOpts*,PtpClock*);
+ Boolean netShutdown(NetPath*);
+@@ -357,6 +363,10 @@ void restartLogging(RunTimeOpts* rtOpts);
+ void logStatistics(RunTimeOpts *rtOpts, PtpClock *ptpClock);
+ void displayStatus(PtpClock *ptpClock, const char *prefixMessage);
+ void displayPortIdentity(PortIdentity *port, const char *prefixMessage);
++#if defined(FSL_1588)
++clockid_t get_clockid(int fd);
++int clock_adjtime(clockid_t id, struct timex *tx);
++#endif
+ Boolean nanoSleep(TimeInternal*);
+ void getTime(TimeInternal*);
+ void setTime(TimeInternal*);
+@@ -378,12 +388,15 @@ void adjTime(Integer32);
+ void adjFreq_wrapper(RunTimeOpts * rtOpts, PtpClock * ptpClock, double adj);
+ Boolean adjFreq(double);
+ double getAdjFreq(void);
++#ifndef FSL_1588
+ void informClockSource(PtpClock* ptpClock);
++#endif
+ 
+ /* Observed drift save / recovery functions */
+ void restoreDrift(PtpClock * ptpClock, RunTimeOpts * rtOpts, Boolean quiet);
+ void saveDrift(PtpClock * ptpClock, RunTimeOpts * rtOpts, Boolean quiet);
+ 
++#ifndef FSL_1588
+ /* Helper function to manage ntpadjtime / adjtimex flags */
+ void setTimexFlags(int flags, Boolean quiet);
+ void unsetTimexFlags(int flags, Boolean quiet);
+@@ -393,6 +406,7 @@ Boolean checkTimexFlags(int flags);
+ #if defined(MOD_TAI) &&  NTP_API == 4
+ void setKernelUtcOffset(int utc_offset);
+ #endif /* MOD_TAI */
++#endif
+ 
+ #endif /* HAVE_SYS_TIMEX_H */
+ 
+diff --git a/src/dep/servo.c b/src/dep/servo.c
+index 06215e6..6651a20 100644
+--- a/src/dep/servo.c
++++ b/src/dep/servo.c
+@@ -1013,10 +1013,12 @@ if(rtOpts->ntpOptions.enableEngine && rtOpts->panicModeNtp) {
+                /* Adjust the clock first -> the PI controller runs here */
+                adjFreq_wrapper(rtOpts, ptpClock, runPIservo(&ptpClock->servo, ptpClock->offsetFromMaster.nanoseconds));
+                warn_operator_fast_slewing(rtOpts, ptpClock, ptpClock->servo.observedDrift);
++#ifndef FSL_1588
+                /* Unset STA_UNSYNC */
+                unsetTimexFlags(STA_UNSYNC, TRUE);
+                /* "Tell" the clock about maxerror, esterror etc. */
+                informClockSource(ptpClock);
++#endif /* FSL_1588 */
+ #endif /* HAVE_SYS_TIMEX_H */
+        }
+ 
+diff --git a/src/dep/sys.c b/src/dep/sys.c
+index 52b274c..8551724 100644
+--- a/src/dep/sys.c
++++ b/src/dep/sys.c
+@@ -344,7 +344,14 @@ int writeMessage(FILE* destination, int priority, const char * format, va_list a
+                 * it also can cause problems in nested debug statements (which are solved by turning the signal
+                 *  handling synchronous, and not calling this function inside asycnhronous signal processing)
+                 */
++#if defined(FSL_1588)
++               struct timespec tp;
++               clock_gettime(clkid, &tp);
++               now.tv_sec = tp.tv_sec;
++               now.tv_usec = tp.tv_nsec / 1000;
++#else
+                gettimeofday(&now, 0);
++#endif
+                strftime(time_str, MAXTIMESTR, "%F %X", localtime((time_t*)&now.tv_sec));
+                fprintf(destination, "%s.%06d ", time_str, (int)now.tv_usec  );
+                fprintf(destination,PTPD_PROGNAME"[%d].%s (%-9s ",
+@@ -1174,13 +1181,33 @@ nanoSleep(TimeInternal * t)
+        return TRUE;
+ }
+ 
++#if defined(FSL_1588)
++clockid_t get_clockid(int fd)
++{
++#define CLOCKFD 3
++#define FD_TO_CLOCKID(fd)       ((~(clockid_t) (fd) << 3) | CLOCKFD)
++       return FD_TO_CLOCKID(fd);
++}
++
++/* When glibc offers the syscall, this will go away. */
++#include <sys/syscall.h>
++int clock_adjtime(clockid_t id, struct timex *tx)
++{
++       return syscall(__NR_clock_adjtime, id, tx);
++}
++#endif
++
+ void
+ getTime(TimeInternal * time)
+ {
+-#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
++#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) || defined(FSL_1588)
+ 
+        struct timespec tp;
++#if defined(FSL_1588)
++       if (clock_gettime(clkid, &tp) < 0) {
++#else
+        if (clock_gettime(CLOCK_REALTIME, &tp) < 0) {
++#endif
+                PERROR("clock_gettime() failed, exiting.");
+                exit(0);
+        }
+@@ -1201,7 +1228,7 @@ void
+ setTime(TimeInternal * time)
+ {
+ 
+-#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
++#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) || defined(FSL_1588)
+ 
+        struct timespec tp;
+        tp.tv_sec = time->seconds;
+@@ -1215,9 +1242,13 @@ setTime(TimeInternal * time)
+ 
+ #endif /* _POSIX_TIMERS */
+ 
+-#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
+ 
++#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) || defined(FSL_1588)
++#if defined(FSL_1588)
++       if (clock_settime(clkid, &tp) < 0) {
++#else
+        if (clock_settime(CLOCK_REALTIME, &tp) < 0) {
++#endif
+                PERROR("Could not set system time");
+                return;
+        }
+@@ -1595,7 +1626,11 @@ adjFreq(double adj)
+ #endif /* HAVE_STRUCT_TIMEX_TICK */
+        DBG2("        adj is %.09f;  t freq is %d       (float: %.09f)\n", adj, t.freq,  dFreq);
+        
++#if defined(FSL_1588)
++       return !clock_adjtime(clkid, &t);
++#else
+        return !adjtimex(&t);
++#endif
+ }
+ 
+ 
+@@ -1609,7 +1644,11 @@ getAdjFreq(void)
+ 
+        memset(&t, 0, sizeof(t));
+        t.modes = 0;
++#if defined(FSL_1588)
++       clock_adjtime(clkid, &t);
++#else
+        adjtimex(&t);
++#endif
+ 
+        dFreq = (t.freq + 0.0) / ((1<<16) / 1000.0);
+ 
+@@ -1764,6 +1803,7 @@ saveDrift(PtpClock * ptpClock, RunTimeOpts * rtOpts, Boolean quiet)
+        fclose(driftFP);
+ }
+ 
++#ifndef FSL_1588
+ void
+ setTimexFlags(int flags, Boolean quiet)
+ {
+@@ -1922,6 +1962,7 @@ setKernelUtcOffset(int utc_offset) {
+        }
+ }
+ #endif /* MOD_TAI */
++#endif /* FSL_1588 */
+ 
+ 
+ #else
+diff --git a/src/protocol.c b/src/protocol.c
+index c73728b..90554f3 100644
+--- a/src/protocol.c
++++ b/src/protocol.c
+@@ -615,6 +615,7 @@ if(!rtOpts->panicModeNtp || !ptpClock->panicMode)
+                timerStart(STATISTICS_UPDATE_TIMER, rtOpts->statsUpdateInterval, ptpClock->itimer);
+ #endif /* PTPD_STATISTICS */
+ 
++#ifndef FSL_1588
+ #ifdef HAVE_SYS_TIMEX_H
+ 
+                /* 
+@@ -632,6 +633,7 @@ if(!rtOpts->panicModeNtp || !ptpClock->panicMode)
+                        unsetTimexFlags(STA_INS | STA_DEL, TRUE);
+                }
+ #endif /* HAVE_SYS_TIMEX_H */
++#endif /* FSL_1588 */
+                break;
+        default:
+                DBG("to unrecognized state\n");
+@@ -934,6 +936,7 @@ doState(RunTimeOpts *rtOpts, PtpClock *ptpClock)
+                             WARNING("Leap second event imminent - pausing "
+                                    "clock and offset updates\n");
+                             ptpClock->leapSecondInProgress = TRUE;
++#ifndef FSL_1588
+ #ifdef HAVE_SYS_TIMEX_H
+                             if(!checkTimexFlags(ptpClock->timePropertiesDS.leap61 ? 
+                                                STA_INS : STA_DEL)) {
+@@ -944,6 +947,7 @@ doState(RunTimeOpts *rtOpts, PtpClock *ptpClock)
+                                                  STA_INS : STA_DEL, FALSE);
+                             }
+ #endif /* HAVE_SYS_TIMEX_H */
++#endif /* FSL_1588 */
+                            /*
+                             * start pause timer from now until [pause] after
+                             * midnight, plus an extra second if inserting
+@@ -1290,6 +1294,9 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
+     int ret;
+     ssize_t length = -1;
+ 
++#if defined(FSL_1588)
++       hwtstamp_tx_ctl(&ptpClock->netPath, FALSE);/* HWTSTAMP_TX_OFF */
++#endif
+     TimeInternal timeStamp = { 0, 0 };
+     fd_set readfds;
+ 
+@@ -1437,9 +1444,11 @@ handleAnnounce(MsgHeader *header, ssize_t length,
+                                        ptpClock->leapSecondInProgress=FALSE;
+                                        ptpClock->timePropertiesDS.leap59 = FALSE;
+                                        ptpClock->timePropertiesDS.leap61 = FALSE;
++#ifndef FSL_1588
+ #ifdef HAVE_SYS_TIMEX_H
+                                        unsetTimexFlags(STA_INS | STA_DEL, TRUE);
+ #endif /* HAVE_SYS_TIMEX_H */
++#endif /* FSL_1588 */
+                                }
+                        }
+                        DBG2("___ Announce: received Announce from current Master, so reset the Announce timer\n");
+diff --git a/src/ptpd.c b/src/ptpd.c
+index 3fad379..471b1e9 100644
+--- a/src/ptpd.c
++++ b/src/ptpd.c
+@@ -78,6 +78,21 @@ main(int argc, char **argv)
+        PtpClock *ptpClock;
+        Integer16 ret;
+ 
++#if defined(FSL_1588)
++       char device[] = "/dev/ptp0";
++       int fd;
++
++       fd = open(device, O_RDWR);
++       if (fd < 0) {
++               fprintf(stderr, "opening %s: %s\n", device, strerror(errno));
++               return -1;
++       }
++       clkid = get_clockid(fd);
++       if (clkid == -1) {
++               fprintf(stderr, "failed to read clock id\n");
++               return -1;
++       }
++#endif
+        startupInProgress = TRUE;
+ 
+        /* Initialize run time options with command line arguments */
+diff --git a/src/ptpd.h b/src/ptpd.h
+index 009f51c..6641649 100644
+--- a/src/ptpd.h
++++ b/src/ptpd.h
+@@ -172,6 +172,15 @@
+ #include <linux/rtc.h>
+ #endif /* HAVE_LINUX_RTC_H */
+ 
++#ifdef FSL_1588
++#ifndef SO_TIMESTAMPING
++#define SO_TIMESTAMPING 37
++#endif
++#ifndef HAVE_SYS_TIMEX_H
++#define HAVE_SYS_TIMEX_H 1
++#endif
++clockid_t clkid;
++#endif
+ /** \name arith.c
+  * -Timing management and arithmetic*/
+  /**\{*/
diff --git a/meta-networking/recipes-daemons/ptpd/ptpd_2.3.1-rc2.bb b/meta-networking/recipes-daemons/ptpd/ptpd_2.3.1-rc2.bb
index db74e4463..274ea331b 100644
--- a/meta-networking/recipes-daemons/ptpd/ptpd_2.3.1-rc2.bb
+++ b/meta-networking/recipes-daemons/ptpd/ptpd_2.3.1-rc2.bb
@@ -21,7 +21,9 @@ def get_sub(d):
     except:
         return parts[0]
 
-SRC_URI = "http://downloads.sourceforge.net/project/ptpd/ptpd/${@get_sub(d)}/ptpd-${PV}.tar.gz"
+SRC_URI = "http://downloads.sourceforge.net/project/ptpd/ptpd/${@get_sub(d)}/ptpd-${PV}.tar.gz \
+        file://ptpd-add-dpaa-etsec-support.patch \
+"
 
 SRC_URI[md5sum] = "1ef2f1f2825080a865bbce0eb61246d4"
 SRC_URI[sha256sum] = "2802aab758649bb222859dfcb62a5d282709ccb4d3f1df3f26f739cc091d0c8d"