SWPTP-1434: prefer the last receiver of a Sync as the active LACP intf

CHANGELOG.md

@@ -19,6 +19,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/).
   - populate PTP correctionField to reflect sub-nanosecond timestamps (sfptpd
     already used subnanosecond corrections received over the PTP network)
   - see notes below for current limitations
+- Improve LACP support. (SWPTP-738)
+  - select the local reference clock to be the one on which the latest sync
+    message was received (SWPTP-1434).
 - Add --socket option for sfptpdctl to control multiple processes. (SWPTP-624)
 - Add configurable patterns for log, state, control and stats paths. (SWPTP-649)
   - supports logging to a directory shared between hosts

src/include/sfptpd_interface.h

@@ -104,6 +104,12 @@ void sfptpd_interface_get_mac_addr(struct sfptpd_interface *interface,
  */
 const char *sfptpd_interface_get_fw_version(struct sfptpd_interface *interface);
 
+/** Get the ifindex of a given interface
+ * @param interface Pointer to interface instance
+ * @return The ifindex of the provided interface, or 0 on error.
+ */
+int sfptpd_interface_get_ifindex(struct sfptpd_interface *interface);
+
 /** Set the clock associated with an interface
  * @param interface Pointer to interface instance
  * @param clock Clock to be associated with this interface

src/ptp/ptpd2/datatypes.h

@@ -783,6 +783,7 @@ struct ptpd_port_context {
 	UInteger32 portAlarms;
 	Integer8 logMinDelayReqInterval;
 	Integer8 logDelayRespReceiptTimeout;
+	UInteger32 lastSyncIfindex;
 
 	/* Configurable members */
 	Integer8 logAnnounceInterval;

src/ptp/ptpd2/dep/net.c

@@ -92,6 +92,11 @@
 #define SO_TIMESTAMPING  37
 #endif
 
+#ifndef SOF_TIMESTAMPING_OPT_PKTINFO
+#define SOF_TIMESTAMPING_OPT_PKTINFO (1<<13)
+#define SCM_TIMESTAMPING_PKTINFO     58
+#endif
+
 #endif /*linux*/
 
 #include "../ptpd.h"
@@ -1057,9 +1062,10 @@ static int getTxTimestamp(PtpClock *ptpClock, char *pdu, int pdulen,
 /* Used to get receive timestamps  */
 static Boolean getRxTimestamp(PtpInterface *ptpInterface, char *pdu, int pduLength,
 			      struct msghdr *msg, ptpd_timestamp_type_e tsType,
-			      struct sfptpd_timespec *timestamp)
+			      struct sfptpd_timespec *timestamp, UInteger32 *rxPhysIfindex)
 {
 	struct cmsghdr *cmsg;
+	Boolean timestampValid = FALSE;
 
 	if (msg->msg_controllen <= 0) {
 		DBG2("received PTP event packet with no timestamp (%zu)\n",
@@ -1070,6 +1076,7 @@ static Boolean getRxTimestamp(PtpInterface *ptpInterface, char *pdu, int pduLeng
 	for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; cmsg = CMSG_NXTHDR(msg, cmsg)) {
 		struct timeval *tv;
 		struct timespec *ts;
+		struct scm_ts_pktinfo *ts_pktinfo;
 
 		DUMP("CM", cmsg, cmsg->cmsg_len);
 
@@ -1086,7 +1093,7 @@ static Boolean getRxTimestamp(PtpInterface *ptpInterface, char *pdu, int pduLeng
 				return FALSE;
 			}
 			sfptpd_time_init(timestamp, tv->tv_sec, tv->tv_usec * 1000, 0);
-			return TRUE;
+			timestampValid = TRUE;
 			break;
 
 		case SCM_TIMESTAMPNS:
@@ -1098,19 +1105,57 @@ static Boolean getRxTimestamp(PtpInterface *ptpInterface, char *pdu, int pduLeng
 				return FALSE;
 			}
 			sfptpd_time_init(timestamp, ts->tv_sec, ts->tv_nsec, 0);
-			return TRUE;
+			timestampValid = TRUE;
 			break;
 
 		case SO_TIMESTAMPING:
-			return parse_timestamp(CMSG_DATA(cmsg), cmsg->cmsg_len,
-					       tsType,
-					       ptpInterface->ts_fmt,
-					       timestamp, FALSE);
+			timestampValid = parse_timestamp(CMSG_DATA(cmsg), cmsg->cmsg_len,
+											 tsType,
+											 ptpInterface->ts_fmt,
+											 timestamp, FALSE);
+			break;
+
+		case SCM_TIMESTAMPING_PKTINFO:
+			ts_pktinfo = (struct scm_ts_pktinfo*)CMSG_DATA(cmsg);
+
+			if(cmsg->cmsg_len < CMSG_LEN(sizeof(*ts_pktinfo))) {
+				ERROR("received short SCM_TIMESTAMPING_PKTINFO (%zu)\n",
+				      cmsg->cmsg_len);
+				return FALSE;
+			}
+			*rxPhysIfindex = ts_pktinfo->if_index;
+			break;
+
+		}
+	}
+
+	if( ! timestampValid )
+		DBG("failed to retrieve rx time stamp\n");
+
+	return timestampValid;
+}
+
 
+/* Attempt to use SO_TIMESTAMPING with the provided flags, and try to include
+ * SOF_TIMESTAMPING_OPT_PKTINFO if possible. */
+static Boolean netTryEnableTimestampingPktinfo(PtpInterface *ptpInterface, int flags)
+{
+	int has_pktinfo_flag;
+
+	/* The option SOF_TIMESTAMPING_OPT_PKTINFO was only introduced in the
+	 * linux kernel version 4.13, and is not necessary for sfptpd. Older
+	 * kernels will return `-EINVAL` on setsockopt, so we try both with
+	 * and without this option and let other code deal with the presence/
+	 * absence of pktinfo for timestamping. */
+	flags |= SOF_TIMESTAMPING_OPT_PKTINFO;
+	for (has_pktinfo_flag = 1; has_pktinfo_flag >= 0; has_pktinfo_flag--) {
+		if (setsockopt(ptpInterface->transport.eventSock, SOL_SOCKET,
+			       SO_TIMESTAMPING, &flags, sizeof(flags)) == 0) {
+			return TRUE;
 		}
+		flags &= ~SOF_TIMESTAMPING_OPT_PKTINFO;
 	}
 
-	DBG("failed to retrieve rx time stamp\n");
 	return FALSE;
 }
 
@@ -1157,8 +1202,7 @@ netInitTimestamping(PtpInterface *ptpInterface, InterfaceOpts *ifOpts)
 		      | SOF_TIMESTAMPING_RX_SOFTWARE
 		      | SOF_TIMESTAMPING_SOFTWARE;
 
-		if (setsockopt(ptpInterface->transport.eventSock, SOL_SOCKET,
-			       SO_TIMESTAMPING, &flags, sizeof(flags)) == 0) {
+		if (netTryEnableTimestampingPktinfo(ptpInterface, flags)) {
 			ptpInterface->tsMethod = TS_METHOD_SO_TIMESTAMPING;
 			INFO("using SO_TIMESTAMPING software timestamps\n");
 			return TRUE;
@@ -1230,8 +1274,7 @@ netInitTimestamping(PtpInterface *ptpInterface, InterfaceOpts *ifOpts)
 	else
 		flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
 
-	if (setsockopt(ptpInterface->transport.eventSock, SOL_SOCKET,
-		       SO_TIMESTAMPING, &flags, sizeof(flags)) == 0) {
+	if (netTryEnableTimestampingPktinfo(ptpInterface, flags)) {
 		ptpInterface->tsMethod = TS_METHOD_SO_TIMESTAMPING;
 		INFO("using SO_TIMESTAMPING hardware timestamps\n");
 		return TRUE;
@@ -1569,7 +1612,8 @@ netSelect(struct sfptpd_timespec *timeout, struct ptpd_transport *transport, fd_
  */
 ssize_t
 netRecvEvent(Octet *buf, PtpInterface *ptpInterface, InterfaceOpts *ifOpts,
-	     struct sfptpd_timespec *timestamp, Boolean *timestampValid)
+	     struct sfptpd_timespec *timestamp, Boolean *timestampValid,
+	     UInteger32 *rxPhysIfindex)
 {
 	ssize_t ret;
 	struct msghdr msg;
@@ -1626,7 +1670,7 @@ netRecvEvent(Octet *buf, PtpInterface *ptpInterface, InterfaceOpts *ifOpts,
 	transport->receivedPackets++;
 
 	*timestampValid = getRxTimestamp(ptpInterface, buf, ret, &msg,
-					 ifOpts->timestampType, timestamp);
+					 ifOpts->timestampType, timestamp, rxPhysIfindex);
 
 	return ret;
 }

src/ptp/ptpd2/dep/ptpd_dep.h

@@ -227,7 +227,7 @@ Boolean netInit(struct ptpd_transport*,InterfaceOpts*,PtpInterface*);
 Boolean netInitPort(PtpClock *ptpClock, RunTimeOpts *rtOpts);
 Boolean netShutdown(struct ptpd_transport*);
 int netSelect(struct sfptpd_timespec*,struct ptpd_transport*,fd_set*);
-ssize_t netRecvEvent(Octet*,PtpInterface*,InterfaceOpts*,struct sfptpd_timespec*,Boolean*);
+ssize_t netRecvEvent(Octet*,PtpInterface*,InterfaceOpts*,struct sfptpd_timespec*,Boolean*,UInteger32*);
 ssize_t netRecvGeneral(Octet*,struct ptpd_transport*);
 
 /* These functions all return 0 for success or an errno in the case of failure */

src/ptp/ptpd2/protocol.c

@@ -63,7 +63,7 @@
 #include "sfptpd_time.h"
 
 static void handleAnnounce(MsgHeader*, ssize_t, Boolean, RunTimeOpts*, PtpClock*);
-static void handleSync(const MsgHeader*, ssize_t, struct sfptpd_timespec*, Boolean, Boolean, RunTimeOpts*, PtpClock*);
+static void handleSync(const MsgHeader*, ssize_t, struct sfptpd_timespec*, Boolean, UInteger32, Boolean, RunTimeOpts*, PtpClock*);
 static void handleFollowUp(const MsgHeader*, ssize_t, const MsgFollowUp*, Boolean, Boolean, RunTimeOpts*, PtpClock*);
 static void handlePDelayReq(MsgHeader*, ssize_t, struct sfptpd_timespec*, Boolean, Boolean, RunTimeOpts*, PtpClock*);
 static void handleDelayReq(const MsgHeader*, ssize_t, struct sfptpd_timespec*, Boolean, Boolean, RunTimeOpts*, PtpClock*);
@@ -83,16 +83,17 @@ static void issueDelayResp(struct sfptpd_timespec*, MsgHeader*, RunTimeOpts*, Pt
 static void issuePDelayRespFollowUp(struct sfptpd_timespec*, MsgHeader*, RunTimeOpts*, PtpClock*, const UInteger16);
 static void issueManagementRespOrAck(MsgManagement*, RunTimeOpts*, PtpClock*, const struct sockaddr_storage *address, socklen_t addressLength);
 static void issueManagementErrorStatus(MsgManagement*, RunTimeOpts*, PtpClock*, const struct sockaddr_storage *address, socklen_t addressLength);
-static void processMessage(InterfaceOpts *ifOpts, PtpInterface *ptpInterface, struct sfptpd_timespec *timestamp, Boolean timestampValid, ssize_t length);
-static void processPortMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock, struct sfptpd_timespec *timestamp, Boolean timestampValid, ssize_t length, int offset, acl_bitmap_t acls_checked, acl_bitmap_t acls_passed);
+static void processMessage(InterfaceOpts *ifOpts, PtpInterface *ptpInterface, struct sfptpd_timespec *timestamp, Boolean timestampValid, UInteger32 rxPhysIfindex, ssize_t length);
+static void processPortMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock, struct sfptpd_timespec *timestamp, Boolean timestampValid, UInteger32 rxPhysIfindex, ssize_t length, int offset, acl_bitmap_t acls_checked, acl_bitmap_t acls_passed);
 static ssize_t unpackPortMessage(PtpClock *ptpClock, ssize_t safe_length);
 static bool processTLVs(RunTimeOpts *rtOpts, PtpClock *ptpClock, int payload_offset,
 			ssize_t  unpack_result, ssize_t safe_length, Boolean isFromSelf,
 			struct sfptpd_timespec *timestamp, Boolean timestampValid,
 			acl_bitmap_t acls_checked, acl_bitmap_t acls_passed);
 static void handleMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock,
 			  ssize_t safe_length, Boolean isFromSelf,
-			  struct sfptpd_timespec *timestamp, Boolean timestampValid);
+			  struct sfptpd_timespec *timestamp, Boolean timestampValid,
+			  UInteger32 rxPhysIfindex);
 static void processSyncFromSelf(const struct sfptpd_timespec *tint, RunTimeOpts *rtOpts, PtpClock *ptpClock, const UInteger16 sequenceId);
 static void processDelayReqFromSelf(const struct sfptpd_timespec *tint, RunTimeOpts *rtOpts, PtpClock *ptpClock);
 static void processPDelayReqFromSelf(const struct sfptpd_timespec *tint, RunTimeOpts *rtOpts, PtpClock *ptpClock);
@@ -670,6 +671,9 @@ doTimerTick(RunTimeOpts *rtOpts, PtpClock *ptpClock)
 			SYNC_MODULE_ALARM_SET(ptpClock->portAlarms, NO_SYNC_PKTS);
 			ptpClock->counters.syncTimeouts++;
 
+			/* Reset the last sync index */
+			ptpClock->lastSyncIfindex = 0;
+
 			/* Record the fact that the data is missing */
 			servo_missing_m2s_ts(&ptpClock->servo);
 
@@ -936,7 +940,7 @@ static bool checkACLmask(acl_bitmap_t mask,
 
 static void
 processMessage(InterfaceOpts *ifOpts, PtpInterface *ptpInterface, struct sfptpd_timespec *timestamp,
-	       Boolean timestampValid, ssize_t length)
+	       Boolean timestampValid, UInteger32 rxPhysIfindex, ssize_t length)
 {
 	PtpClock *port;
 	ssize_t unpack_result;
@@ -1019,8 +1023,8 @@ processMessage(InterfaceOpts *ifOpts, PtpInterface *ptpInterface, struct sfptpd_
 		     port->portIdentity.portNumber,
 		     port->rtOpts.name);
 		processPortMessage(&port->rtOpts, port, timestamp,
-				   timestampValid, length, UNPACK_GET_SIZE(unpack_result),
-				   acls_checked, acls_passed);
+				   timestampValid, rxPhysIfindex, length,
+				   UNPACK_GET_SIZE(unpack_result), acls_checked, acls_passed);
 	} else {
 		DBG2("ignoring message from %s for unhandled domainNumber %d\n",
 		     ptpInterface->ifOpts.ifaceName,
@@ -1041,7 +1045,7 @@ processMessage(InterfaceOpts *ifOpts, PtpInterface *ptpInterface, struct sfptpd_
 static void
 processPortMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock,
 		   struct sfptpd_timespec *timestamp, Boolean timestampValid,
-		   ssize_t length, int offset,
+		   UInteger32 rxPhysIfindex, ssize_t length, int offset,
 		   acl_bitmap_t acls_checked, acl_bitmap_t acls_passed)
 {
 	PtpInterface *ptpInterface;
@@ -1134,7 +1138,8 @@ processPortMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock,
 			handleMessage(rtOpts,
 				      ptpClock,
 				      safe_length, isFromSelf,
-				      timestamp, timestampValid);
+				      timestamp, timestampValid,
+				      rxPhysIfindex);
 		}
 	}
 }
@@ -1428,7 +1433,8 @@ processTLVs(RunTimeOpts *rtOpts, PtpClock *ptpClock, int payload_offset,
 static void
 handleMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock,
 	      ssize_t safe_length, Boolean isFromSelf,
-	      struct sfptpd_timespec *timestamp, Boolean timestampValid)
+	      struct sfptpd_timespec *timestamp, Boolean timestampValid,
+	      UInteger32 rxPhysIfindex)
 {
 	PtpInterface *ptpInterface;
 
@@ -1451,7 +1457,7 @@ handleMessage(RunTimeOpts *rtOpts, PtpClock *ptpClock,
 		break;
 	case PTPD_MSG_SYNC:
 		handleSync(&ptpInterface->msgTmpHeader, safe_length,
-			   timestamp, timestampValid,
+			   timestamp, timestampValid, rxPhysIfindex,
 			   isFromSelf, rtOpts, ptpClock);
 		break;
 	case PTPD_MSG_FOLLOW_UP:
@@ -1500,10 +1506,11 @@ doHandleSockets(InterfaceOpts *ifOpts, PtpInterface *ptpInterface,
 	ssize_t length;
 	Boolean timestampValid = FALSE;
 	struct sfptpd_timespec timestamp = { 0, 0, 0 };
+	UInteger32 rxPhysIfindex = 0;
 
 	if (event) {
 		length = netRecvEvent(ptpInterface->msgIbuf, ptpInterface, ifOpts,
-				      &timestamp, &timestampValid);
+				      &timestamp, &timestampValid, &rxPhysIfindex);
 		if (length < 0) {
 			PERROR("failed to receive on the event socket\n");
 			toStateAllPorts(PTPD_FAULTY, ptpInterface);
@@ -1512,7 +1519,8 @@ doHandleSockets(InterfaceOpts *ifOpts, PtpInterface *ptpInterface,
 		}
 
 		if (length > 0)
-			processMessage(ifOpts, ptpInterface, &timestamp, timestampValid, length);
+			processMessage(ifOpts, ptpInterface, &timestamp, timestampValid,
+						   rxPhysIfindex, length);
 	}
 
 	if (general) {
@@ -1525,7 +1533,8 @@ doHandleSockets(InterfaceOpts *ifOpts, PtpInterface *ptpInterface,
 		}
 
 		if (length > 0)
-			processMessage(ifOpts, ptpInterface, &timestamp, FALSE, length);
+			processMessage(ifOpts, ptpInterface, &timestamp, FALSE,
+						   rxPhysIfindex, length);
 	}
 }
 
@@ -1652,7 +1661,8 @@ handleAnnounce(MsgHeader *header, ssize_t length,
 
 static void
 handleSync(const MsgHeader *header, ssize_t length,
-	   struct sfptpd_timespec *time, Boolean timestampValid, Boolean isFromSelf,
+	   struct sfptpd_timespec *time, Boolean timestampValid,
+	   UInteger32 rxPhysIfindex, Boolean isFromSelf,
 	   RunTimeOpts *rtOpts, PtpClock *ptpClock)
 {
 	Integer8 msgInterval;
@@ -1707,6 +1717,12 @@ handleSync(const MsgHeader *header, ssize_t length,
 				break;
 			}
 
+			/* If the ifindex is valid, then store it to be used later */
+			if (rxPhysIfindex != 0 &&
+				rxPhysIfindex != ptpClock->lastSyncIfindex) {
+				ptpClock->lastSyncIfindex = rxPhysIfindex;
+			}
+
 			/* Clear the RX timestamp alarm */
 			SYNC_MODULE_ALARM_CLEAR(ptpClock->portAlarms, NO_RX_TIMESTAMPS);
 

src/ptp/ptpd2/ptpd_lib.c

@@ -646,6 +646,7 @@ int ptpd_get_snapshot(struct ptpd_port_context *ptpd, struct ptpd_port_snapshot
 	snapshot->port.domain_number = ptpd->domainNumber;
 	snapshot->port.slave_only = ptpd->slaveOnly;
 	snapshot->port.master_only = ptpd->masterOnly;
+	snapshot->port.last_sync_ifindex = ptpd->lastSyncIfindex;
 	snapshot->current.servo_outlier_threshold = servo_get_outlier_threshold(&ptpd->servo);
 	snapshot->port.effective_comm_caps = ptpd->effective_comm_caps;
 

src/ptp/ptpd2/ptpd_lib.h

@@ -42,6 +42,7 @@ struct ptpd_port_snapshot {
 		UInteger8 domain_number;
 		Boolean slave_only;
 		Boolean master_only;
+		UInteger32 last_sync_ifindex;
 		PortCommunicationCapabilities effective_comm_caps;
 	} port;