Merge pull request #393 from kencarlino/feature-gptp-avnu-automotive-profilefeature-gptp-avnu-automotive-profile

Phase error violation detection added

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