Halt PDelayReq messages when peer is misbehaving.

Adding verification for duplicated PdelayResponse messages
that are coming from different sources. These messages normally have the
same sequenceID and have different SourcePortIdentity.

When receiving multiple duplicated messages in sequence (3), gPTP halts
the PdelayRequest messages for 5 minutes.

This patch fixes Issue #302.

4 files changed, 156 insertions, 14 deletions

diff --git a/daemons/gptp/common/avbts_port.hpp b/daemons/gptp/common/avbts_port.hpp
index 7e9d0cb3..406324a5 100644
--- a/daemons/gptp/common/avbts_port.hpp
+++ b/daemons/gptp/common/avbts_port.hpp
@@ -234,6 +234,10 @@ class IEEE1588Port {
 	static const int64_t INVALID_LINKDELAY = 3600000000000;
 	static const int64_t NEIGHBOR_PROP_DELAY_THRESH = 800;
 	static const unsigned int DEFAULT_SYNC_RECEIPT_THRESH = 5;
+	static const unsigned int DUPLICATE_RESP_THRESH = 3;
+
+	unsigned int duplicate_resp_counter;
+	uint16_t last_invalid_seqid;
 
 	/* Signed value allows this to be negative result because of inaccurate
 	   timestamp */
@@ -304,6 +308,7 @@ class IEEE1588Port {
 	OSConditionFactory *condition_factory;
 
 	bool pdelay_started;
+	bool pdelay_halted;
  public:
 	bool forceSlave;	//!< Forces port to be slave. Added for testing.
 
@@ -358,6 +363,30 @@ class IEEE1588Port {
 	void startPDelay();
 
 	/**
+	 * @brief Stops PDelay event timer
+	 * @return void
+	 */
+	void stopPDelay();
+
+	/**
+	 * @brief Enable/Disable PDelay Request messages
+	 * @param hlt True to HALT (stop sending), False to resume (start sending).
+	 */
+	void haltPdelay(bool hlt)
+	{
+		pdelay_halted = hlt;
+	}
+
+	/**
+	 * @brief Get the status of pdelayHalted condition.
+	 * @return True PdelayRequest halted. False when PDelay Request is running
+	 */
+	bool pdelayHalted(void)
+	{
+		return pdelay_halted;
+	}
+
+	/**
 	 * @brief  Starts announce event timer
 	 * @return void
 	 */
@@ -995,6 +1024,52 @@ class IEEE1588Port {
 		return ret;
 	}
 
+	/**
+	 * @brief Sets the value of last duplicated SeqID
+	 * @param seqid Value to set
+	 * @return void
+	 */
+	void setLastInvalidSeqID(uint16_t seqid)
+	{
+		last_invalid_seqid = seqid;
+	}
+
+	/**
+	 * @brief  Get the value of last invalid seqID
+	 * @return Last invalid seq id
+	 */
+	uint16_t getLastInvalidSeqID(void)
+	{
+		return last_invalid_seqid;
+	}
+
+	/**
+	 * @brief  Sets the duplicate pdelay_resp counter.
+	 * @param  cnt Value to be set
+	 */
+	void setDuplicateRespCounter(unsigned int cnt)
+	{
+		duplicate_resp_counter = cnt;
+	}
+
+	/**
+	 * @brief  Gets the current value of pdelay_resp duplicate messages counter
+	 * @return Counter value
+	 */
+	unsigned int getDuplicateRespCounter(void)
+	{
+		return duplicate_resp_counter;
+	}
+
+	/**
+	 * @brief  Increment the duplicate PDelayResp message counter
+	 * @return True if it equals the threshold, False otherwise
+	 */
+	bool incrementDuplicateRespCounter(void)
+	{
+		return ++duplicate_resp_counter == DUPLICATE_RESP_THRESH;
+	}
+
     /**
      * @brief  Sets the neighbor propagation delay threshold
      * @param  delay Delay in nanoseconds
diff --git a/daemons/gptp/common/ieee1588.hpp b/daemons/gptp/common/ieee1588.hpp
index 921116f5..220efad7 100644
--- a/daemons/gptp/common/ieee1588.hpp
+++ b/daemons/gptp/common/ieee1588.hpp
@@ -94,6 +94,7 @@ typedef enum {
 	FAULT_DETECTED,						//!< A fault was detected.
 	PDELAY_DEFERRED_PROCESSING,			//!< Defers pdelay processing
 	PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES,	//!< Pdelay response message timeout
+	PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES,	//!< Timeout for peer misbehaving. This even will re-enable the PDelay Requests
 } Event;
 
 /**
diff --git a/daemons/gptp/common/ieee1588port.cpp b/daemons/gptp/common/ieee1588port.cpp
index f1a6bb08..a5b0df59 100644
--- a/daemons/gptp/common/ieee1588port.cpp
+++ b/daemons/gptp/common/ieee1588port.cpp
@@ -93,6 +93,10 @@ IEEE1588Port::IEEE1588Port
 	sync_sequence_id = 0;
 
 	pdelay_started = false;
+	pdelay_halted = false;
+
+	duplicate_resp_counter = 0;
+	last_invalid_seqid = 0;
 
     /*TODO: Add intervals below to a config interface*/
 	log_mean_sync_interval = -3;
@@ -164,8 +168,16 @@ bool IEEE1588Port::init_port(int delay[4])
 }
 
 void IEEE1588Port::startPDelay() {
-	pdelay_started = true;
-	clock->addEventTimer( this, PDELAY_INTERVAL_TIMEOUT_EXPIRES, 32000000 );
+	if(!pdelayHalted()) {
+		pdelay_started = true;
+		clock->addEventTimer( this, PDELAY_INTERVAL_TIMEOUT_EXPIRES, 32000000 );
+	}
+}
+
+void IEEE1588Port::stopPDelay() {
+	haltPdelay(true);
+	pdelay_started = false;
+	clock->deleteEventTimer( this, PDELAY_INTERVAL_TIMEOUT_EXPIRES);
 }
 
 void IEEE1588Port::startAnnounce() {
@@ -886,6 +898,17 @@ void IEEE1588Port::processEvent(Event e)
 		setAsCapable(false);
 		pdelay_count = 0;
 		break;
+
+	case PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES:
+		XPTPD_INFO("Timeout expired! Restarting PDelay");
+
+		haltPdelay(false);
+		if( port_state != PTP_SLAVE && port_state != PTP_MASTER ) {
+			XPTPD_PRINTF("Starting PDelay\n" );
+			startPDelay();
+		}
+		break;
+
 	default:
 		XPTPD_INFO
 		    ("Unhandled event type in IEEE1588Port::processEvent(), %d",
diff --git a/daemons/gptp/common/ptp_message.cpp b/daemons/gptp/common/ptp_message.cpp
index 7476abae..691abc3c 100644
--- a/daemons/gptp/common/ptp_message.cpp
+++ b/daemons/gptp/common/ptp_message.cpp
@@ -1177,6 +1177,9 @@ done:
 void PTPMessagePathDelayReq::sendPort(IEEE1588Port * port,
 				      PortIdentity * destIdentity)
 {
+	if(port->pdelayHalted())
+		return;
+
 	uint8_t buf_t[256];
 	uint8_t *buf_ptr = buf_t + port->getPayloadOffset();
 	unsigned char tspec_msg_t = 0;
@@ -1186,7 +1189,6 @@ void PTPMessagePathDelayReq::sendPort(IEEE1588Port * port,
 	messageLength = PTP_COMMON_HDR_LENGTH + PTP_PDELAY_REQ_LENGTH;
 	tspec_msg_t |= messageType & 0xF;
 	buildCommonHeader(buf_ptr);
-
 	port->sendEventPort(buf_t, messageLength, MCAST_PDELAY, destIdentity);
 	return;
 }
@@ -1228,11 +1230,58 @@ void PTPMessagePathDelayResp::processMessage(IEEE1588Port * port)
 		return;
 	}
 
+	PortIdentity resp_id;
+	PortIdentity oldresp_id;
+	uint16_t resp_port_number;
+	uint16_t oldresp_port_number;
+
 	PTPMessagePathDelayResp *old_pdelay_resp = port->getLastPDelayResp();
+	if( old_pdelay_resp == NULL ) {
+		goto bypass_verify_duplicate;
+	}
+
+	old_pdelay_resp->getPortIdentity(&oldresp_id);
+	oldresp_id.getPortNumber(&oldresp_port_number);
+	getPortIdentity(&resp_id);
+	resp_id.getPortNumber(&resp_port_number);
+
+	/* In the case where we have multiple PDelay responses for the same
+	 * PDelay request, and they come from different sources, it is necessary
+	 * to verify if this happens 3 times (sequentially). If it does, PDelayRequests
+	 * are halted for 5 minutes
+	 */
+	if( getSequenceId() == old_pdelay_resp->getSequenceId() )
+	{
+		/*If the duplicates are in sequence and from different sources*/
+		if( (resp_port_number != oldresp_port_number ) && (
+					(port->getLastInvalidSeqID() + 1 ) == getSequenceId() ||
+					port->getDuplicateRespCounter() == 0 ) ){
+			XPTPD_ERROR("Two responses for same Request. seqID %d. First Response Port# %hu. Second Port# %hu. Counter %d",
+				getSequenceId(), oldresp_port_number, resp_port_number, port->getDuplicateRespCounter());
+
+			if( port->incrementDuplicateRespCounter() ) {
+				XPTPD_ERROR("Remote misbehaving. Stopping PDelay Requests for 5 minutes.");
+				port->stopPDelay();
+				port->getClock()->addEventTimerLocked
+					(port, PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES, 300 * 1000000000.0);
+			}
+		}
+		else {
+			port->setDuplicateRespCounter(0);
+		}
+		port->setLastInvalidSeqID(getSequenceId());
+	}
+	else
+	{
+		port->setDuplicateRespCounter(0);
+	}
+
+bypass_verify_duplicate:
+	port->setLastPDelayResp(this);
+
 	if (old_pdelay_resp != NULL) {
 		delete old_pdelay_resp;
 	}
-	port->setLastPDelayResp(this);
 
 	port->putPDelayRxLock();
 	_gc = false;
@@ -1380,8 +1429,7 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	}
 
 	/*
-	 * According to Figure 11-8 of subclause 11.2.15.3, a condition to leave the state
-	 * WAITING_FOR_PDELAY_RESP is if the response seqID is the same as the requested.
+	 * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3
 	 */
 	if (resp->getSequenceId() != sequenceId) {
 		XPTPD_ERROR
@@ -1394,9 +1442,7 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	}
 
 	/*
-	 * According to Figure 11-8 of subclause 11.2.15.3, a condition to leave the state
-	 * WAITING_FOR_PDELAY_RESP is if the clock identity in the response is the same
-	 * as this clock
+	 * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3
 	 */
 	if (req_clkId != resp_clkId ) {
 		XPTPD_ERROR
@@ -1406,8 +1452,7 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	}
 
 	/*
-	 * According to Figure 11-8 of subclause 11.2.15.3, a condition to leave the state
-	 * WAITING_FOR_PDELAY_RESP is if the response portID is the same as the requested.
+	 * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3
 	 */
 	if ( resp_port_number != req_port_number ) {
 		XPTPD_ERROR
@@ -1418,9 +1463,7 @@ void PTPMessagePathDelayRespFollowUp::processMessage(IEEE1588Port * port)
 	}
 
 	/*
-	 * According to Figure 11-8 of subclause 11.2.15.3, a condition to leave the state
-	 * WAITING_FOR_PDELAY_RESP is if the sourcePortIdentity from PDelay Response and PDelay
-	 * response Follow-UP (FUP) are the same.
+	 * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3
 	 */
 	if ( fup_sourcePortIdentity != resp_sourcePortIdentity ) {
 		XPTPD_ERROR("Source port identity from PDelay Response/FUP differ");