From 8f7fb11fbc60f4c5c53b4e09c26f55a590625233 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Wed, 21 Oct 2015 10:48:43 +0200
Subject: [PATCH] Perform the time stamping mode check in the clock module.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 clock.c | 31 +++++++++++++++++++++++++++++++
 ptp4l.c | 34 +---------------------------------
 2 files changed, 32 insertions(+), 33 deletions(-)

diff --git a/clock.c b/clock.c
index ab238fb..d18cab0 100644
--- a/clock.c
+++ b/clock.c
@@ -17,6 +17,7 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 #include <errno.h>
+#include <linux/net_tstamp.h>
 #include <poll.h>
 #include <stdlib.h>
 #include <string.h>
@@ -805,6 +806,7 @@ struct clock *clock_create(struct config *config, int phc_index)
 	int fadj = 0, max_adj = 0, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0;
 	enum servo_type servo = config_get_int(config, NULL, "clock_servo");
 	struct clock *c = &the_clock;
+	int required_modes = 0;
 	struct port *p;
 	unsigned char oui[OUI_LEN];
 	char phc[32], *tmp;
@@ -892,6 +894,35 @@ struct clock *clock_create(struct config *config, int phc_index)
 		}
 	}
 
+	/* Check the time stamping mode on each interface. */
+	switch (config_get_int(config, NULL, "time_stamping")) {
+	case TS_SOFTWARE:
+		required_modes |= SOF_TIMESTAMPING_TX_SOFTWARE |
+			SOF_TIMESTAMPING_RX_SOFTWARE |
+			SOF_TIMESTAMPING_SOFTWARE;
+		break;
+	case TS_LEGACY_HW:
+		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |
+			SOF_TIMESTAMPING_RX_HARDWARE |
+			SOF_TIMESTAMPING_SYS_HARDWARE;
+		break;
+	case TS_HARDWARE:
+	case TS_ONESTEP:
+		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |
+			SOF_TIMESTAMPING_RX_HARDWARE |
+			SOF_TIMESTAMPING_RAW_HARDWARE;
+		break;
+	}
+	STAILQ_FOREACH(iface, &config->interfaces, list) {
+		if (iface->ts_info.valid &&
+		    ((iface->ts_info.so_timestamping & required_modes) != required_modes)) {
+			pr_err("interface '%s' does not support "
+			       "requested timestamping mode", iface->name);
+			return NULL;
+		}
+	}
+
+	iface = STAILQ_FIRST(&config->interfaces);
 	if (generate_clock_identity(&c->dds.clockIdentity, iface->name)) {
 		pr_err("failed to generate a clock identity");
 		return NULL;
diff --git a/ptp4l.c b/ptp4l.c
index 0eb2490..d48324b 100644
--- a/ptp4l.c
+++ b/ptp4l.c
@@ -22,7 +22,6 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
-#include <linux/net_tstamp.h>
 
 #include "clock.h"
 #include "config.h"
@@ -78,7 +77,7 @@ int main(int argc, char *argv[])
 	struct interface *iface;
 	struct clock *clock = NULL;
 	struct config *cfg;
-	int phc_index = -1, print_level, required_modes = 0;
+	int phc_index = -1, print_level;
 
 	if (handle_term_signals())
 		return -1;
@@ -198,37 +197,6 @@ int main(int argc, char *argv[])
 		goto out;
 	}
 
-	switch (config_get_int(cfg, NULL, "time_stamping")) {
-	case TS_SOFTWARE:
-		required_modes |= SOF_TIMESTAMPING_TX_SOFTWARE |
-			SOF_TIMESTAMPING_RX_SOFTWARE |
-			SOF_TIMESTAMPING_SOFTWARE;
-		break;
-	case TS_LEGACY_HW:
-		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |
-			SOF_TIMESTAMPING_RX_HARDWARE |
-			SOF_TIMESTAMPING_SYS_HARDWARE;
-		break;
-	case TS_HARDWARE:
-	case TS_ONESTEP:
-		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |
-			SOF_TIMESTAMPING_RX_HARDWARE |
-			SOF_TIMESTAMPING_RAW_HARDWARE;
-		break;
-	}
-
-	/* Init interface configs and check whether timestamping mode is
-	 * supported. */
-	STAILQ_FOREACH(iface, &cfg->interfaces, list) {
-		if (iface->ts_info.valid &&
-		    ((iface->ts_info.so_timestamping & required_modes) != required_modes)) {
-			fprintf(stderr, "interface '%s' does not support "
-				        "requested timestamping mode.\n",
-				iface->name);
-			goto out;
-		}
-	}
-
 	/* determine PHC Clock index */
 	iface = STAILQ_FIRST(&cfg->interfaces);
 	if (config_get_int(cfg, NULL, "free_running")) {