Fixes Issue #136

A limit variable was created on the configuration file (.ini).
A counter was added to port object, and now we count how many
wrong seqIds we have received, and if it is bigger than the set
limit, we mark the gPTP as not AsCapable.

7 files changed, 101 insertions, 11 deletions

diff --git a/daemons/gptp/common/avbts_port.hpp b/daemons/gptp/common/avbts_port.hpp
index 49ed9285..0620ad4d 100644
--- a/daemons/gptp/common/avbts_port.hpp
+++ b/daemons/gptp/common/avbts_port.hpp
@@ -234,6 +234,8 @@ class IEEE1588Port {
 	static const int64_t INVALID_LINKDELAY = 3600000000000;
 	static const int64_t NEIGHBOR_PROP_DELAY_THRESH = 800;
 	static const unsigned int SYNC_RECEIPT_THRESH = 5;
+	static const unsigned int SEQID_ASCAPABLE_THRESHOLD = 5;
+
 	/* Signed value allows this to be negative result because of inaccurate
 	   timestamp */
 	int64_t one_way_delay;
@@ -243,6 +245,10 @@ class IEEE1588Port {
 	unsigned int sync_receipt_thresh;
 	unsigned int wrongSeqIDCounter;
 
+    /*SeqID threshold*/
+	unsigned int seqIdAsCapableThresh;
+	unsigned int seqIdAsCapableThreshCounter;
+
 	/* Implementation Specific data/methods */
 	IEEE1588Clock *clock;
 
@@ -936,6 +942,16 @@ class IEEE1588Port {
 		sync_receipt_thresh = th;
 	}
 
+	void setSeqIdAsCapableThresh(unsigned int th)
+	{
+		seqIdAsCapableThresh = th;
+	}
+
+	unsigned int getSeqIdAsCapableThresh(void)
+	{
+		return seqIdAsCapableThresh;
+	}
+
 	/**
 	 * @brief  Gets the internal variabl sync_receipt_thresh, which is the
 	 * flag that monitors the amount of wrong syncs enabled before switching
@@ -964,11 +980,11 @@ class IEEE1588Port {
 	 */
 	bool getWrongSeqIDCounter(unsigned int *cnt)
 	{
-		if( cnt == NULL )
-		{
-			return false;
-		}
-		*cnt = wrongSeqIDCounter;
+        if( cnt == NULL )
+        {
+            return false;
+        }
+        *cnt = wrongSeqIDCounter;
 
 		return( *cnt < getSyncReceiptThresh() );
 	}
@@ -981,16 +997,46 @@ class IEEE1588Port {
 
 	bool incWrongSeqIDCounter(unsigned int *cnt)
 	{
-		if( cnt == NULL )
-		{
-			return false;
-		}
+        if( cnt == NULL )
+        {
+            return false;
+        }
 		*cnt = ++wrongSeqIDCounter;
 
 		return ( *cnt < getSyncReceiptThresh() );
 	}
 
     /**
+     * @brief  Set the seqIdAsCapableThreshCounter value
+     * @param  c Value to be set to.
+     * @return void
+     */
+	void setSeqIdAsCapableThreshCounter(unsigned int c)
+	{
+		seqIdAsCapableThreshCounter = c;
+	}
+
+    /**
+     * @brief  Gets the content of seqIdAsCapableThreshCounter
+     * @return seqIdAsCapableThreshCounter value
+     */
+	unsigned int getSeqIdAsCapableThreshCounter(void)
+	{
+		return seqIdAsCapableThreshCounter;
+	}
+
+    /**
+     * @brief  Increments the seqIdAsCapableThreshCounter value
+     * @param  incSeqIdAsCapableThreshCounter
+     * @return TRUE if incremented value is lower than the seqIdAsCapableThresh.
+     * FALSE otherwise.
+     */
+	bool incSeqIdAsCapableThreshCounter(void)
+	{
+		return( ++seqIdAsCapableThreshCounter < getSeqIdAsCapableThresh() );
+	}
+
+    /**
      * @brief  Sets the neighbor propagation delay threshold
      * @param  delay Delay in nanoseconds
      * @return void
diff --git a/daemons/gptp/common/gptp_cfg.cpp b/daemons/gptp/common/gptp_cfg.cpp
index 2a9945bf..ab948e66 100644
--- a/daemons/gptp/common/gptp_cfg.cpp
+++ b/daemons/gptp/common/gptp_cfg.cpp
@@ -126,6 +126,16 @@ int GptpIniParser::iniCallBack(void *user, const char *section, const char *name
                 parser->_config.syncReceiptThresh = st;
             }
         }
+        else if( parseMatch(name, "seqIdAsCapableThresh") )
+        {
+            errno = 0;
+            char *pEnd;
+            unsigned int sidt = strtoul(value, &pEnd, 10);
+            if( *pEnd == '\0' && errno == 0) {
+                valOK = true;
+                parser->_config.seqIdAsCapableThresh = sidt;
+            }
+        }
     }
     else if( parseMatch(section, "eth") )
     {
diff --git a/daemons/gptp/common/gptp_cfg.hpp b/daemons/gptp/common/gptp_cfg.hpp
index 7efeec48..63056afc 100644
--- a/daemons/gptp/common/gptp_cfg.hpp
+++ b/daemons/gptp/common/gptp_cfg.hpp
@@ -60,6 +60,7 @@ class GptpIniParser
             unsigned int syncReceiptTimeout;
             unsigned int syncReceiptThresh;		//!< Number of wrong sync messages that will trigger a switch to master
             int64_t neighborPropDelayThresh;
+            unsigned int seqIdAsCapableThresh;
             PortState port_state;
 
             /*ethernet adapter data set*/
@@ -168,6 +169,15 @@ class GptpIniParser
             return _config.syncReceiptThresh;
         }
 
+        /**
+         * @brief  Gets the value from seqIdAsCapableThresh from the configuration file
+         * @return seqIdAsCapableThresh content
+         */
+        unsigned int getSeqIdAsCapableThresh(void)
+        {
+            return _config.seqIdAsCapableThresh;
+        }
+
     private:
         int _error;
         gptp_cfg_t _config;
diff --git a/daemons/gptp/common/ieee1588port.cpp b/daemons/gptp/common/ieee1588port.cpp
index 1d32c75e..7b4c5085 100644
--- a/daemons/gptp/common/ieee1588port.cpp
+++ b/daemons/gptp/common/ieee1588port.cpp
@@ -109,7 +109,9 @@ IEEE1588Port::IEEE1588Port
 	one_way_delay = ONE_WAY_DELAY_DEFAULT;
 	neighbor_prop_delay_thresh = NEIGHBOR_PROP_DELAY_THRESH;
 	sync_receipt_thresh = SYNC_RECEIPT_THRESH;
+	seqIdAsCapableThresh = SEQID_ASCAPABLE_THRESHOLD;
 	wrongSeqIDCounter = 0;
+	seqIdAsCapableThreshCounter = 0;
 
 	_peer_rate_offset = 1.0;
 
diff --git a/daemons/gptp/common/ptp_message.cpp b/daemons/gptp/common/ptp_message.cpp
index ab6792b3..3de216cd 100644
--- a/daemons/gptp/common/ptp_message.cpp
+++ b/daemons/gptp/common/ptp_message.cpp
@@ -244,6 +244,7 @@ PTPMessageCommon *buildPTPMessage
 			pdelay_req_msg->messageType = messageType;
 
 #if 0
+            /*TODO: Do we need the code below? Can we remove it?*/
 			// The origin timestamp for PDelay Request packets has been eliminated since it is unused
 			// Copy in v2 PDelay Request specific fields
 			memcpy(&(pdelay_req_msg->originTimestamp.seconds_ms),
@@ -1371,20 +1372,30 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	}
 
 	// Check if we have received a response
+    /* Count wrong seqIDs and after a threshold (from .ini), mark
+     * gPTP as not asCapable
+     */
 	if (resp->getSequenceId() != sequenceId
 	    || resp_id != *requestingPortIdentity) {
 		uint16_t resp_port_number;
 		uint16_t req_port_number;
 		resp_id.getPortNumber(&resp_port_number);
 		requestingPortIdentity->getPortNumber(&req_port_number);
+		if( !port->incSeqIdAsCapableThreshCounter() )
+		{
+			port->setAsCapable( false );
+		}
 		XPTPD_ERROR
-		    ("Received PDelay Response Follow Up but cannot find "
+			("Received PDelay Response Follow Up but cannot find "
 			 "corresponding response");
 		XPTPD_ERROR("%hu, %hu, %hu, %hu", resp->getSequenceId(),
-			    sequenceId, resp_port_number, req_port_number);
+				sequenceId, resp_port_number, req_port_number);
+		XPTPD_ERROR("Counter: %d Threshold: %d", port->getSeqIdAsCapableThreshCounter(),
+				port->getSeqIdAsCapableThresh());
 
 		goto abort;
 	}
+	port->setSeqIdAsCapableThreshCounter(0);
 
 	XPTPD_INFO("Request Sequence Id: %u", req->getSequenceId());
 	XPTPD_INFO("Response Sequence Id: %u", resp->getSequenceId());
diff --git a/daemons/gptp/gptp_cfg.ini b/daemons/gptp/gptp_cfg.ini
index 35a00848..d48b49db 100644
--- a/daemons/gptp/gptp_cfg.ini
+++ b/daemons/gptp/gptp_cfg.ini
@@ -29,6 +29,11 @@ neighborPropDelayThresh = 800
 # up to 1 second of wrong messages before switching
 syncReceiptThresh = 8
 
+# Sequence ID AS Capable threshold
+# This flag is the the amount of wrong seqID messages that gPTP
+# is allowed to receive before it is considered not AS Capable.
+seqIdAsCapableThresh = 5
+
 [eth]
 
 # PHY delay GB TX in nanoseconds
diff --git a/daemons/gptp/linux/src/daemon_cl.cpp b/daemons/gptp/linux/src/daemon_cl.cpp
index 41d7ae4b..0f664bb3 100644
--- a/daemons/gptp/linux/src/daemon_cl.cpp
+++ b/daemons/gptp/linux/src/daemon_cl.cpp
@@ -320,6 +320,7 @@ int main(int argc, char **argv)
             fprintf(stdout, "phy_delay_mb_rx: %d\n", iniParser.getPhyDelayMbRx());
             fprintf(stdout, "neighborPropDelayThresh: %ld\n", iniParser.getNeighborPropDelayThresh());
             fprintf(stdout, "syncReceiptThreshold: %d\n", iniParser.getSyncReceiptThresh());
+            fprintf(stdout, "seqIdAsCapableThresh: %d\n", iniParser.getSeqIdAsCapableThresh());
 
             /* If using config file, set the neighborPropDelayThresh.
              * Otherwise it will use its default value (800ns) */
@@ -330,6 +331,11 @@ int main(int argc, char **argv)
              */
             port->setSyncReceiptThresh(iniParser.getSyncReceiptThresh());
 
+            /* If using config file, set the seqIdAsCapableThresh, otherwise
+             * it will use the default value (SEQID_ASCAPABLE_THRESHOLD)
+             */
+            port->setSeqIdAsCapableThresh(iniParser.getSeqIdAsCapableThresh());
+
             /*Only overwrites phy_delay default values if not input_delay switch enabled*/
             if(!input_delay)
             {