diff options
Diffstat (limited to 'daemons/gptp/common')
-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 |
6 files changed, 89 insertions, 12 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 ); } } |