diff options
author | andrew-elder <aelder@audioscience.com> | 2016-05-17 10:01:40 -0400 |
---|---|---|
committer | andrew-elder <aelder@audioscience.com> | 2016-05-17 10:01:40 -0400 |
commit | ff44a1edf0f97d067eb23e6925a7a49a43651ebf (patch) | |
tree | b0bc0a5507f381ec195ad0a679ed0a7053fa1e74 | |
parent | 5edf47732f95cc88403046acc323eb88a6539521 (diff) | |
parent | 6b5eaae86e3b1e0bc6d809d9f5132cca2586e3da (diff) | |
download | Open-AVB-feature-gptp-avnu-automotive-profile.tar.gz |
Merge pull request #393 from kencarlino/feature-gptp-avnu-automotive-profilefeature-gptp-avnu-automotive-profile
Phase error violation detection added
-rw-r--r-- | daemons/gptp/common/avbts_clock.hpp | 30 | ||||
-rw-r--r-- | daemons/gptp/common/avbts_osthread.hpp | 7 | ||||
-rw-r--r-- | daemons/gptp/common/avbts_port.hpp | 14 | ||||
-rw-r--r-- | daemons/gptp/common/ieee1588clock.cpp | 30 | ||||
-rw-r--r-- | daemons/gptp/common/ieee1588port.cpp | 4 | ||||
-rw-r--r-- | daemons/gptp/common/ptp_message.cpp | 16 | ||||
-rw-r--r-- | daemons/gptp/linux/src/linux_hal_common.cpp | 22 | ||||
-rw-r--r-- | daemons/gptp/linux/src/linux_hal_common.hpp | 18 | ||||
-rw-r--r-- | daemons/gptp/linux/src/linux_hal_generic_adj.cpp | 4 | ||||
-rw-r--r-- | daemons/gptp/windows/daemon_cl/windows_hal.hpp | 11 |
10 files changed, 132 insertions, 24 deletions
diff --git a/daemons/gptp/common/avbts_clock.hpp b/daemons/gptp/common/avbts_clock.hpp index 760eb5b0..f40cc575 100644 --- a/daemons/gptp/common/avbts_clock.hpp +++ b/daemons/gptp/common/avbts_clock.hpp @@ -49,6 +49,18 @@ #define UPPER_FREQ_LIMIT 250.0 /*!< Upper frequency limit */ #define LOWER_FREQ_LIMIT -250.0 /*!< Lower frequency limit */ +#define UPPER_LIMIT_PPM 250 +#define LOWER_LIMIT_PPM -250 +#define PPM_OFFSET_TO_RATIO(ppm) ((ppm) / ((FrequencyRatio)US_PER_SEC) + 1) + + +/* This is the threshold in ns for which frequency adjustments will be made */ +#define PHASE_ERROR_THRESHOLD (1000000000) + +/* This is the maximum count of phase error, outside of the threshold before + adjustment is performed */ +#define PHASE_ERROR_MAX_COUNT (6) + /** * Provides the clock quality abstraction. * Represents the quality of the clock @@ -106,6 +118,7 @@ private: bool _syntonize; bool _new_syntonization_set_point; float _ppm; + int _phase_error_violation; IEEE1588Port *port_list[MAX_PORTS]; @@ -482,6 +495,23 @@ public: } /** + * @brief Restart PDelays on all ports + * @return void + */ + void restartPDelayAll() { + int number_ports, i, j = 0; + IEEE1588Port **ports; + + getPortList( number_ports, ports ); + + for( i = 0; i < number_ports; ++i ) { + while( ports[j] == NULL ) ++j; + ports[j]->restartPDelay(); + } + } + + + /** * @brief Gets all TX locks * @return void */ diff --git a/daemons/gptp/common/avbts_osthread.hpp b/daemons/gptp/common/avbts_osthread.hpp index 67bd49c4..c9c069d0 100644 --- a/daemons/gptp/common/avbts_osthread.hpp +++ b/daemons/gptp/common/avbts_osthread.hpp @@ -62,6 +62,13 @@ public: virtual bool start(OSThreadFunction function, void *arg) = 0; /** + * @brief Name a new thread + * @param name The name to give to the calling thread + * @return void + */ + virtual void setName(const char *name) = 0; + + /** * @brief Joins the thread * @param exit_code OSThreadExitCode enumeration * @return Implementation specific diff --git a/daemons/gptp/common/avbts_port.hpp b/daemons/gptp/common/avbts_port.hpp index c5f04504..ed7a30b7 100644 --- a/daemons/gptp/common/avbts_port.hpp +++ b/daemons/gptp/common/avbts_port.hpp @@ -390,10 +390,6 @@ class IEEE1588Port { PTPMessageSync *last_sync; - OSThread *listening_thread; - - OSThread *link_thread; - OSCondition *port_ready_condition; OSLock *pdelay_rx_lock; @@ -426,6 +422,8 @@ class IEEE1588Port { public: bool forceSlave; //!< Forces port to be slave. Added for testing. + OSThread *listening_thread; + OSThread *link_thread; /** * @brief Serializes (i.e. copy over buf pointer) the information from @@ -549,6 +547,14 @@ class IEEE1588Port { } /** + * @brief Restart PDelay + * @return void + */ + void restartPDelay() { + _peer_offset_init = false; + } + + /** * @brief Gets the asCapable flag * @return asCapable flag */ diff --git a/daemons/gptp/common/ieee1588clock.cpp b/daemons/gptp/common/ieee1588clock.cpp index 73f954ce..e2082adb 100644 --- a/daemons/gptp/common/ieee1588clock.cpp +++ b/daemons/gptp/common/ieee1588clock.cpp @@ -45,6 +45,8 @@ #include <string.h> +#include <math.h> + std::string ClockIdentity::getIdentityString() { uint8_t cid[PTP_CLOCK_IDENTITY_LENGTH]; @@ -99,6 +101,8 @@ IEEE1588Clock::IEEE1588Clock _new_syntonization_set_point = false; _ppm = 0; + _phase_error_violation = 0; + _master_local_freq_offset_init = false; _local_system_freq_offset_init = false; _timestamper = timestamper; @@ -346,30 +350,42 @@ void IEEE1588Clock::setMasterOffset } if( _syntonize ) { - if( _new_syntonization_set_point ) { + if( _new_syntonization_set_point || _phase_error_violation > PHASE_ERROR_MAX_COUNT ) { _new_syntonization_set_point = false; + _phase_error_violation = 0; if( _timestamper ) { /* Make sure that there are no transmit operations in progress */ getTxLockAll(); - _timestamper->HWTimestamper_adjclockphase - ( -master_local_offset ); + + GPTP_LOG_DEBUG("before adjust: local_time=%lldns", TIMESTAMP_TO_NS(local_time)); + + _timestamper->HWTimestamper_adjclockphase( -master_local_offset ); _master_local_freq_offset_init = false; + restartPDelayAll(); putTxLockAll(); master_local_offset = 0; } } // Adjust for frequency offset long double phase_error = (long double) -master_local_offset; - _ppm += (float) (INTEGRAL*phase_error + - PROPORTIONAL*((master_local_freq_offset-1.0)*1000000)); - if( _ppm < LOWER_FREQ_LIMIT ) _ppm = LOWER_FREQ_LIMIT; - if( _ppm > UPPER_FREQ_LIMIT ) _ppm = UPPER_FREQ_LIMIT; + if( fabsl(phase_error) > PHASE_ERROR_THRESHOLD ) { + ++_phase_error_violation; + } else { + _phase_error_violation = 0; + _ppm += (float) (INTEGRAL*phase_error + PROPORTIONAL*((master_local_freq_offset-1.0)*1000000)); + + GPTP_LOG_DEBUG("phase_error = %Lf, ppm = %f", phase_error, _ppm ); + } + + if( _ppm < LOWER_LIMIT_PPM ) _ppm = LOWER_LIMIT_PPM; + if( _ppm > UPPER_LIMIT_PPM ) _ppm = UPPER_LIMIT_PPM; if( _timestamper ) { if( !_timestamper->HWTimestamper_adjclockrate( _ppm )) { GPTP_LOG_ERROR( "Failed to adjust clock rate" ); } } + } return; diff --git a/daemons/gptp/common/ieee1588port.cpp b/daemons/gptp/common/ieee1588port.cpp index 21bf816f..416b9be1 100644 --- a/daemons/gptp/common/ieee1588port.cpp +++ b/daemons/gptp/common/ieee1588port.cpp @@ -59,6 +59,7 @@ OSThreadExitCode watchNetLinkWrapper(void *arg) IEEE1588Port *port; port = (IEEE1588Port *) arg; + port->link_thread->setName("gPTPLinkWatch"); if (port->watchNetlink() == NULL) return osthread_ok; else @@ -70,6 +71,7 @@ OSThreadExitCode openPortWrapper(void *arg) IEEE1588Port *port; port = (IEEE1588Port *) arg; + port->listening_thread->setName("gPTPListener"); if (port->openPort(port) == NULL) return osthread_ok; else @@ -177,6 +179,8 @@ IEEE1588Port::IEEE1588Port(IEEE1588PortInit_t *portInit) pdelay_count = 0; sync_count = 0; + _peer_offset_init = false; + if (testMode) { if (isGM) { avbSyncState = 1; diff --git a/daemons/gptp/common/ptp_message.cpp b/daemons/gptp/common/ptp_message.cpp index e78fff96..be07c85f 100644 --- a/daemons/gptp/common/ptp_message.cpp +++ b/daemons/gptp/common/ptp_message.cpp @@ -960,6 +960,12 @@ void PTPMessageFollowUp::processMessage(IEEE1588Port * port) goto done; } + if (sync->getTimestamp()._version != port->getTimestampVersion()) + { + GPTP_LOG_ERROR("Received Follow Up but timestamp version indicates Sync is out of date"); + goto done; + } + sync_arrival = sync->getTimestamp(); delay = port->getLinkDelay(); @@ -1342,6 +1348,7 @@ PTPMessagePathDelayRespFollowUp::~PTPMessagePathDelayRespFollowUp() delete requestingPortIdentity; } +#define US_PER_SEC 1000000 void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port) { Timestamp remote_resp_tx_timestamp(0, 0, 0); @@ -1502,12 +1509,19 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port) Timestamp prev_peer_ts_theirs; FrequencyRatio rate_offset; if( port->getPeerOffset( prev_peer_ts_mine, prev_peer_ts_theirs )) { + FrequencyRatio upper_ratio_limit, lower_ratio_limit; + upper_ratio_limit = PPM_OFFSET_TO_RATIO(UPPER_LIMIT_PPM); + lower_ratio_limit = PPM_OFFSET_TO_RATIO(LOWER_LIMIT_PPM); + mine_elapsed = TIMESTAMP_TO_NS(request_tx_timestamp)-TIMESTAMP_TO_NS(prev_peer_ts_mine); theirs_elapsed = TIMESTAMP_TO_NS(remote_req_rx_timestamp)-TIMESTAMP_TO_NS(prev_peer_ts_theirs); theirs_elapsed -= port->getLinkDelay(); theirs_elapsed += link_delay < 0 ? 0 : link_delay; rate_offset = ((FrequencyRatio) mine_elapsed)/theirs_elapsed; - port->setPeerRateOffset(rate_offset); + + if( rate_offset < upper_ratio_limit && rate_offset > lower_ratio_limit ) { + port->setPeerRateOffset(rate_offset); + } port->setAsCapable( true ); } } diff --git a/daemons/gptp/linux/src/linux_hal_common.cpp b/daemons/gptp/linux/src/linux_hal_common.cpp index a8cac2a0..4f208cfb 100644 --- a/daemons/gptp/linux/src/linux_hal_common.cpp +++ b/daemons/gptp/linux/src/linux_hal_common.cpp @@ -54,6 +54,7 @@ #include <sys/ioctl.h> #include <sys/types.h> #include <sys/stat.h> +#include <sys/prctl.h> #include <sys/socket.h> #include <net/if.h> @@ -61,6 +62,7 @@ #include <linux/netlink.h> #include <linux/rtnetlink.h> + Timestamp tsToTimestamp(struct timespec *ts) { Timestamp ret; @@ -116,10 +118,9 @@ net_result LinuxNetworkInterface::send } -void LinuxNetworkInterface::disable_clear_rx_queue() { +void LinuxNetworkInterface::disable_rx_queue() { struct packet_mreq mr_8021as; int err; - char buf[256]; if( !net_lock.lock() ) { fprintf( stderr, "D rx lock failed\n" ); @@ -141,15 +142,16 @@ void LinuxNetworkInterface::disable_clear_rx_queue() { return; } - while( recvfrom( sd_event, buf, 256, MSG_DONTWAIT, NULL, 0 ) != -1 ); - return; } -void LinuxNetworkInterface::reenable_rx_queue() { +void LinuxNetworkInterface::clear_reenable_rx_queue() { struct packet_mreq mr_8021as; + char buf[256]; int err; + while( recvfrom( sd_event, buf, 256, MSG_DONTWAIT, NULL, 0 ) != -1 ); + memset( &mr_8021as, 0, sizeof( mr_8021as )); mr_8021as.mr_ifindex = ifindex; mr_8021as.mr_type = PACKET_MR_MULTICAST; @@ -288,6 +290,9 @@ void *LinuxTimerQueueHandler( void *arg ) { struct timespec timeout; timeout.tv_sec = 0; timeout.tv_nsec = 100000000; /* 100 ms */ + // Ingoring the return value + /* s = */ prctl(PR_SET_NAME, "gPTPTimerQueue", NULL, NULL, NULL); + sigemptyset( &waitfor ); while( !timerq->stop ) { @@ -684,6 +689,13 @@ bool LinuxThread::start(OSThreadFunction function, void *arg) { return true; } +void LinuxThread::setName(const char *name) +{ + // Ingoring the return value + /* s = */ prctl(PR_SET_NAME, name, NULL, NULL, NULL); +} + + bool LinuxThread::join(OSThreadExitCode & exit_code) { int err; err = pthread_join(_private->thread_id, NULL); diff --git a/daemons/gptp/linux/src/linux_hal_common.hpp b/daemons/gptp/linux/src/linux_hal_common.hpp index 4c306929..df0aa75e 100644 --- a/daemons/gptp/linux/src/linux_hal_common.hpp +++ b/daemons/gptp/linux/src/linux_hal_common.hpp @@ -180,16 +180,17 @@ public: ( LinkLayerAddress *addr, uint8_t *payload, size_t &length, struct phy_delay *delay ); /** - * @brief Disables rx socket descriptor and and clears the rx queue - * @return void + * @brief Disables rx socket descriptor + * @return void */ - void disable_clear_rx_queue(); + void disable_rx_queue(); /** - * @brief Enables the rx socket descriptor + * @brief Enables the rx socket descriptor and clears the rx + * queue * @return void */ - void reenable_rx_queue(); + void clear_reenable_rx_queue(); /** * @brief Gets the local link layer address @@ -533,6 +534,13 @@ class LinuxThread : public OSThread { virtual bool start(OSThreadFunction function, void *arg); /** + * @brief Name a new thread + * @param name The name to give to the calling thread + * @return void + */ + virtual void setName(const char *name); + + /** * @brief Joins a new thread * @param exit_code Callback's return code * @return TRUE if ok, FALSE if error. diff --git a/daemons/gptp/linux/src/linux_hal_generic_adj.cpp b/daemons/gptp/linux/src/linux_hal_generic_adj.cpp index bcfc7395..d1122a09 100644 --- a/daemons/gptp/linux/src/linux_hal_generic_adj.cpp +++ b/daemons/gptp/linux/src/linux_hal_generic_adj.cpp @@ -59,7 +59,7 @@ bool LinuxTimestamperGeneric::HWTimestamper_adjclockphase( int64_t phase_adjust for ( iface_iter = iface_list.begin(); iface_iter != iface_list.end(); ++iface_iter ) { - (*iface_iter)->disable_clear_rx_queue(); + (*iface_iter)->disable_rx_queue(); } rxTimestampList.clear(); @@ -87,7 +87,7 @@ bool LinuxTimestamperGeneric::HWTimestamper_adjclockphase( int64_t phase_adjust iface_iter = iface_list.begin(); for( iface_iter = iface_list.begin(); iface_iter != iface_list.end(); ++iface_iter ) { - (*iface_iter)->reenable_rx_queue(); + (*iface_iter)->clear_reenable_rx_queue(); } delete timer; diff --git a/daemons/gptp/windows/daemon_cl/windows_hal.hpp b/daemons/gptp/windows/daemon_cl/windows_hal.hpp index 5acd7b55..17e72232 100644 --- a/daemons/gptp/windows/daemon_cl/windows_hal.hpp +++ b/daemons/gptp/windows/daemon_cl/windows_hal.hpp @@ -548,6 +548,17 @@ public: if( thread_id == NULL ) return false; else return true; } + + /** + * @brief Name a new thread + * @param name The name to give to the calling thread + * @return void + */ + virtual void setName(const char *name) + { + // No Windows support for named threads + } + /** * @brief Joins a terminated thread * @param exit_code [out] Thread's return code |