Fixes Issue #137

This patch fixes the problem of lost PDelay messages. If the amount of
lost messages is bigger than a threshold, asCapable is set to false.

7 files changed, 101 insertions, 1 deletions

diff --git a/daemons/gptp/common/avbts_port.hpp b/daemons/gptp/common/avbts_port.hpp
index 0620ad4d..28469522 100644
--- a/daemons/gptp/common/avbts_port.hpp
+++ b/daemons/gptp/common/avbts_port.hpp
@@ -235,6 +235,7 @@ class IEEE1588Port {
 	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;
+	static const uint16_t LOSTPDELAY_RESP_THRESH = 3;
 
 	/* Signed value allows this to be negative result because of inaccurate
 	   timestamp */
@@ -249,6 +250,10 @@ class IEEE1588Port {
 	unsigned int seqIdAsCapableThresh;
 	unsigned int seqIdAsCapableThreshCounter;
 
+	/*Lost PDelayFUPs*/
+	uint16_t lastSeqId;
+	uint16_t lostPdelayRespThresh;
+
 	/* Implementation Specific data/methods */
 	IEEE1588Clock *clock;
 
@@ -942,16 +947,42 @@ class IEEE1588Port {
 		sync_receipt_thresh = th;
 	}
 
+    /**
+     * @brief  Sets the seqIdAsCapableThresh value
+     * @param  th value to be set
+     */
 	void setSeqIdAsCapableThresh(unsigned int th)
 	{
 		seqIdAsCapableThresh = th;
 	}
 
+    /**
+     * @brief  Gets the seqIdAsCapableThresh value
+     * @return seqIdAsCapableThresh content
+     */
 	unsigned int getSeqIdAsCapableThresh(void)
 	{
 		return seqIdAsCapableThresh;
 	}
 
+    /**
+     * @brief  Sets the lostPdelayRespThresh value
+     * @param  th value to be set
+     */
+	void setLostPdelayRespThresh(unsigned int th)
+	{
+		lostPdelayRespThresh = th;
+	}
+
+    /**
+     * @brief  Gets the lostPdelayRespThresh value
+     * @return lostPdelayRespThresh content
+     */
+	uint16_t getLostPdelayRespThresh(void)
+	{
+		return lostPdelayRespThresh;
+	}
+
 	/**
 	 * @brief  Gets the internal variabl sync_receipt_thresh, which is the
 	 * flag that monitors the amount of wrong syncs enabled before switching
@@ -1037,6 +1068,24 @@ class IEEE1588Port {
 	}
 
     /**
+     * @brief  Stores the last seqID on port object
+     * @param  seqid Value to be set
+     */
+    void setLastSeqID(uint16_t seqid)
+    {
+        lastSeqId = seqid;
+    }
+
+    /**
+     * @brief  Gets the last SeqID from Port object
+     * @return lastSeqID
+     */
+    uint16_t getLastSeqId(void)
+    {
+        return lastSeqId;
+    }
+
+    /**
      * @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 ab948e66..284244ae 100644
--- a/daemons/gptp/common/gptp_cfg.cpp
+++ b/daemons/gptp/common/gptp_cfg.cpp
@@ -136,6 +136,16 @@ int GptpIniParser::iniCallBack(void *user, const char *section, const char *name
                 parser->_config.seqIdAsCapableThresh = sidt;
             }
         }
+        else if( parseMatch( name, "lostPdelayRespThresh") )
+        {
+            errno = 0;
+            char *pEnd;
+            uint16_t lostpdelayth = strtoul(value, &pEnd, 10);
+            if( *pEnd == '\0' && errno == 0 ) {
+                valOK = true;
+                parser->_config.lostPdelayRespThresh = lostpdelayth;
+            }
+        }
     }
     else if( parseMatch(section, "eth") )
     {
diff --git a/daemons/gptp/common/gptp_cfg.hpp b/daemons/gptp/common/gptp_cfg.hpp
index 63056afc..4007fab9 100644
--- a/daemons/gptp/common/gptp_cfg.hpp
+++ b/daemons/gptp/common/gptp_cfg.hpp
@@ -61,6 +61,7 @@ class GptpIniParser
             unsigned int syncReceiptThresh;		//!< Number of wrong sync messages that will trigger a switch to master
             int64_t neighborPropDelayThresh;
             unsigned int seqIdAsCapableThresh;
+            uint16_t lostPdelayRespThresh;
             PortState port_state;
 
             /*ethernet adapter data set*/
@@ -161,7 +162,6 @@ class GptpIniParser
 
         /**
          * @brief  Reads the sync receipt threshold from the configuration file
-         * @param  getSyncReceiptThresh
          * @return syncRecepitThresh value from the .ini file
          */
         unsigned int getSyncReceiptThresh(void)
@@ -178,6 +178,15 @@ class GptpIniParser
             return _config.seqIdAsCapableThresh;
         }
 
+        /**
+         * @brief  Reads the lostPdelayRespThresh from the configuration file
+         * @return lostPdelayRespThresh value from the .ini file
+         */
+        uint16_t getLostPdelayRespThresh(void)
+        {
+            return _config.lostPdelayRespThresh;
+        }
+
     private:
         int _error;
         gptp_cfg_t _config;
diff --git a/daemons/gptp/common/ieee1588port.cpp b/daemons/gptp/common/ieee1588port.cpp
index 7b4c5085..89d1127e 100644
--- a/daemons/gptp/common/ieee1588port.cpp
+++ b/daemons/gptp/common/ieee1588port.cpp
@@ -112,6 +112,7 @@ IEEE1588Port::IEEE1588Port
 	seqIdAsCapableThresh = SEQID_ASCAPABLE_THRESHOLD;
 	wrongSeqIDCounter = 0;
 	seqIdAsCapableThreshCounter = 0;
+	lostPdelayRespThresh = LOSTPDELAY_RESP_THRESH;
 
 	_peer_rate_offset = 1.0;
 
@@ -132,6 +133,8 @@ IEEE1588Port::IEEE1588Port
 
 	pdelay_count = 0;
 	sync_count = 0;
+
+	lastSeqId = 0;
 }
 
 bool IEEE1588Port::init_port(int delay[4])
diff --git a/daemons/gptp/common/ptp_message.cpp b/daemons/gptp/common/ptp_message.cpp
index ded34b1e..6d0204d1 100644
--- a/daemons/gptp/common/ptp_message.cpp
+++ b/daemons/gptp/common/ptp_message.cpp
@@ -1360,6 +1360,22 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	req->getPortIdentity(&req_id);
 	resp->getRequestingPortIdentity(&resp_id);
 
+    /*We need to keep track of lost respFUPs. If we have more
+     * than a certain number of lost FUPs we should set asCapable
+     * to false
+     */
+    if( sequenceId != (port->getLastSeqId() + 1) )
+    {
+        XPTPD_ERROR("Current seqID: %d. %d are missing since the last one sent",
+                sequenceId, sequenceId-port->getLastSeqId());
+        if( (sequenceId - port->getLastSeqId() ) > port->getLostPdelayRespThresh() )
+        {
+            port->setAsCapable( false );
+        }
+    }
+    port->setLastSeqID(sequenceId);
+
+
     // Check if we have received a response
     /* Count wrong seqIDs and after a threshold (from .ini), mark
      * gPTP as not asCapable
diff --git a/daemons/gptp/gptp_cfg.ini b/daemons/gptp/gptp_cfg.ini
index d48b49db..dd89ea9b 100644
--- a/daemons/gptp/gptp_cfg.ini
+++ b/daemons/gptp/gptp_cfg.ini
@@ -34,6 +34,13 @@ syncReceiptThresh = 8
 # is allowed to receive before it is considered not AS Capable.
 seqIdAsCapableThresh = 5
 
+# Lost PDelay response threshold
+# A gPTP device must keep count of consecutively lost responses to Pdelay_Req
+# frames. A lost response is considered any late or invalid Pdelay_Resp or
+# Pdelay_Resp_Follow_Up. If more than 3 consecutive responses are lost,
+# then both isMeasuringDelay and asCapable must be set to FALSE.
+lostPdelayRespThresh = 3
+
 [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 0f664bb3..6c9ee9ba 100644
--- a/daemons/gptp/linux/src/daemon_cl.cpp
+++ b/daemons/gptp/linux/src/daemon_cl.cpp
@@ -321,6 +321,7 @@ int main(int argc, char **argv)
             fprintf(stdout, "neighborPropDelayThresh: %ld\n", iniParser.getNeighborPropDelayThresh());
             fprintf(stdout, "syncReceiptThreshold: %d\n", iniParser.getSyncReceiptThresh());
             fprintf(stdout, "seqIdAsCapableThresh: %d\n", iniParser.getSeqIdAsCapableThresh());
+            fprintf(stdout, "lostPdelayRespThresh %d\n", iniParser.getLostPdelayRespThresh());
 
             /* If using config file, set the neighborPropDelayThresh.
              * Otherwise it will use its default value (800ns) */
@@ -336,6 +337,11 @@ int main(int argc, char **argv)
              */
             port->setSeqIdAsCapableThresh(iniParser.getSeqIdAsCapableThresh());
 
+            /* If using config file, set the lostPdelayRespThresh, otherwise
+             * it will use the default value (LOSTPDELAY_RESP_THRESH)
+             */
+            port->setLostPdelayRespThresh(iniParser.getLostPdelayRespThresh());
+
             /*Only overwrites phy_delay default values if not input_delay switch enabled*/
             if(!input_delay)
             {