From 7b7e046e912f6f45a721a226e61394b707be0ae6 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Thu, 29 Aug 2013 10:54:55 +0200
Subject: [PATCH] Add an optional extra check on sync and follow up message
 ordering.

Because of packet reordering that can occur in the network, in the
hardware, or in the networking stack, a follow up message can appear
to arrive in the application before the matching sync message. As this
is a normal occurrence, and the sequenceID message field ensures
proper matching, the ptp4l program accepts out of order packets.

This patch adds an additional check using the software time stamps
from the networking stack to verify that the sync message did arrive
first. This check is only useful if the sequence IDs generated by
the master might possibly be incorrect.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 port.c      | 16 ++++++++++++++++
 raw.c       |  3 +++
 sk.c        | 27 +++++++++++++++++++++++++--
 sk.h        | 14 ++++++++++++++
 transport.h |  1 +
 udp.c       |  3 +++
 udp6.c      |  3 +++
 7 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/port.c b/port.c
index e5c079e..43105d1 100644
--- a/port.c
+++ b/port.c
@@ -366,6 +366,21 @@ static void free_foreign_masters(struct port *p)
 	}
 }
 
+static int fup_sync_ok(struct ptp_message *fup, struct ptp_message *sync)
+{
+	int64_t tfup, tsync;
+	tfup = tmv_to_nanoseconds(timespec_to_tmv(fup->hwts.sw));
+	tsync = tmv_to_nanoseconds(timespec_to_tmv(sync->hwts.sw));
+	/*
+	 * NB - If the sk_check_fupsync option is not enabled, then
+	 * both of these time stamps will be zero.
+	 */
+	if (tfup < tsync) {
+		return 0;
+	}
+	return 1;
+}
+
 static int incapable_ignore(struct port *p, struct ptp_message *m)
 {
 	if (port_capable(p)) {
@@ -1808,6 +1823,7 @@ static void process_sync(struct port *p, struct ptp_message *m)
 	}
 
 	if (p->syfu == SF_HAVE_FUP &&
+	    fup_sync_ok(p->last_syncfup, m) &&
 	    p->last_syncfup->header.sequenceId == m->header.sequenceId) {
 		event = SYNC_MATCH;
 	} else {
diff --git a/raw.c b/raw.c
index 326ccd2..d87edbf 100644
--- a/raw.c
+++ b/raw.c
@@ -209,6 +209,9 @@ static int raw_open(struct transport *t, char *name,
 	if (sk_timestamping_init(efd, name, ts_type, TRANS_IEEE_802_3))
 		goto no_timestamping;
 
+	if (sk_general_init(gfd))
+		goto no_timestamping;
+
 	fda->fd[FD_EVENT] = efd;
 	fda->fd[FD_GENERAL] = gfd;
 	return 0;
diff --git a/sk.c b/sk.c
index a13d14d..fe74164 100644
--- a/sk.c
+++ b/sk.c
@@ -36,6 +36,7 @@
 /* globals */
 
 int sk_tx_timeout = 1;
+int sk_check_fupsync;
 
 /* private methods */
 
@@ -91,6 +92,16 @@ int sk_interface_index(int fd, char *name)
 	return ifreq.ifr_ifindex;
 }
 
+int sk_general_init(int fd)
+{
+	int on = sk_check_fupsync ? 1 : 0;
+	if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPNS, &on, sizeof(on)) < 0) {
+		pr_err("ioctl SO_TIMESTAMPNS failed: %m");
+		return -1;
+	}
+	return 0;
+}
+
 int sk_get_ts_info(char *name, struct sk_ts_info *sk_info)
 {
 #ifdef ETHTOOL_GET_TS_INFO
@@ -205,7 +216,7 @@ int sk_receive(int fd, void *buf, int buflen,
 	struct cmsghdr *cm;
 	struct iovec iov = { buf, buflen };
 	struct msghdr msg;
-	struct timespec *ts = NULL;
+	struct timespec *sw, *ts = NULL;
 
 	memset(control, 0, sizeof(control));
 	memset(&msg, 0, sizeof(msg));
@@ -241,7 +252,14 @@ int sk_receive(int fd, void *buf, int buflen,
 				return -1;
 			}
 			ts = (struct timespec *) CMSG_DATA(cm);
-			break;
+		}
+		if (SOL_SOCKET == level && SO_TIMESTAMPNS == type) {
+			if (cm->cmsg_len < sizeof(*sw)) {
+				pr_warning("short SO_TIMESTAMPNS message");
+				return -1;
+			}
+			sw = (struct timespec *) CMSG_DATA(cm);
+			hwts->sw = *sw;
 		}
 	}
 
@@ -325,5 +343,10 @@ int sk_timestamping_init(int fd, char *device, enum timestamp_type type,
 		return -1;
 	}
 
+	/* Enable the sk_check_fupsync option, perhaps. */
+	if (sk_general_init(fd)) {
+		return -1;
+	}
+
 	return 0;
 }
diff --git a/sk.h b/sk.h
index 6070537..895840f 100644
--- a/sk.h
+++ b/sk.h
@@ -46,6 +46,13 @@ struct sk_ts_info {
  */
 int sk_interface_index(int fd, char *device);
 
+/**
+ * Prepare a given socket for PTP "general" messages.
+ * @param fd  An open socket.
+ * @return    Zero on success, non-zero otherwise.
+ */
+int sk_general_init(int fd);
+
 /**
  * Obtain supported timestamping information
  * @param name	    The name of the interface
@@ -102,4 +109,11 @@ int sk_timestamping_init(int fd, char *device, enum timestamp_type type,
  */
 extern int sk_tx_timeout;
 
+/**
+ * Enables the SO_TIMESTAMPNS socket option on the both the event and
+ * general sockets in order to test the order of paired sync and
+ * follow up messages using their network stack receipt time stamps.
+ */
+extern int sk_check_fupsync;
+
 #endif
diff --git a/transport.h b/transport.h
index 46af456..34934fe 100644
--- a/transport.h
+++ b/transport.h
@@ -57,6 +57,7 @@ enum timestamp_type {
 struct hw_timestamp {
 	enum timestamp_type type;
 	struct timespec ts;
+	struct timespec sw;
 };
 
 struct transport;
diff --git a/udp.c b/udp.c
index fea0122..be7f2b7 100644
--- a/udp.c
+++ b/udp.c
@@ -179,6 +179,9 @@ static int udp_open(struct transport *t, char *name, struct fdarray *fda,
 	if (sk_timestamping_init(efd, name, ts_type, TRANS_UDP_IPV4))
 		goto no_timestamping;
 
+	if (sk_general_init(gfd))
+		goto no_timestamping;
+
 	fda->fd[FD_EVENT] = efd;
 	fda->fd[FD_GENERAL] = gfd;
 	return 0;
diff --git a/udp6.c b/udp6.c
index 2cb0179..e0d1256 100644
--- a/udp6.c
+++ b/udp6.c
@@ -191,6 +191,9 @@ static int udp6_open(struct transport *t, char *name, struct fdarray *fda,
 	if (sk_timestamping_init(efd, name, ts_type, TRANS_UDP_IPV6))
 		goto no_timestamping;
 
+	if (sk_general_init(gfd))
+		goto no_timestamping;
+
 	fda->fd[FD_EVENT] = efd;
 	fda->fd[FD_GENERAL] = gfd;
 	return 0;