ptp4l: use ethtool operation to double check PHC

If the new ethtool operation is supported, then use it to verify that the PHC
selected by the user is correct. If the user doesn't specify a PHC and ethtool
is supported then automatically select the PHC device.

If the user specifies a PHC device, and the ethtool operation is suppported,
automatically confirm that the PHC device requested is correct. This check is
performed for all ports, in order to verify that a boundary clock setup is
valid.

The check for PHC device validity is not done in the transport because the
only thing necessary for performing the check is the port name. Handled this
in the port_open code instead.

Signed-off-by: Jacob Keller <jacob.e.keller@intel.com>

clock.c

@@ -174,18 +174,20 @@ UInteger8 clock_class(struct clock *c)
 	return c->dds.clockQuality.clockClass;
 }
 
-struct clock *clock_create(char *phc, struct interface *iface, int count,
+struct clock *clock_create(int phc_index, struct interface *iface, int count,
 			   struct defaultDS *ds, struct port_defaults *pod)
 {
 	int i, max_adj, sw_ts = 0;
 	struct clock *c = &the_clock;
+	char phc[32];
 
 	srandom(time(NULL));
 
 	if (c->nports)
 		clock_destroy(c);
 
-	if (phc) {
+	if (phc_index >= 0) {
+		snprintf(phc, 31, "/dev/ptp%d", phc_index);
 		c->clkid = phc_open(phc);
 		if (c->clkid == CLOCK_INVALID) {
 			pr_err("Failed to open %s: %m", phc);
@@ -235,7 +237,7 @@ struct clock *clock_create(char *phc, struct interface *iface, int count,
 	c->fault_timeout = FAULT_RESET_SECONDS;
 
 	for (i = 0; i < count; i++) {
-		c->port[i] = port_open(pod, iface[i].name, iface[i].transport,
+		c->port[i] = port_open(pod, phc_index, iface[i].name, iface[i].transport,
 				       iface[i].timestamping, 1+i, iface[i].dm, c);
 		if (!c->port[i]) {
 			pr_err("failed to open port %s", iface[i].name);

clock.h

@@ -66,15 +66,15 @@ UInteger8 clock_class(struct clock *c);
  * Create a clock instance. There can only be one clock in any system,
  * so subsequent calls will destroy the previous clock instance.
  *
- * @param phc         PTP hardware clock device to use.
+ * @param phc_index   PTP hardware clock device to use.
- *                    Pass NULL to select CLOCK_REALTIME.
+ *                    Pass -1 to select CLOCK_REALTIME.
  * @param interface   An array of network interfaces.
  * @param count       The number of elements in @a interfaces.
  * @param ds          A pointer to a default data set for the clock.
  * @param pod         A pointer to a default port data set for the clock.
  * @return            A pointer to the single global clock instance.
  */
-struct clock *clock_create(char *phc, struct interface *iface, int count,
+struct clock *clock_create(int phc_index, struct interface *iface, int count,
 			   struct defaultDS *ds, struct port_defaults *pod);
 
 /**

port.c

@@ -33,6 +33,7 @@
 #include "tmtab.h"
 #include "tmv.h"
 #include "util.h"
+#include "sk.h"
 
 #define PORT_MAVE_LENGTH 10
 
@@ -1328,6 +1329,7 @@ enum fsm_event port_event(struct port *p, int fd_index)
 }
 
 struct port *port_open(struct port_defaults *pod,
+		       int phc_index,
 		       char *name,
 		       enum transport_type transport,
 		       enum timestamp_type timestamping,
@@ -1336,11 +1338,22 @@ struct port *port_open(struct port_defaults *pod,
 		       struct clock *clock)
 {
 	struct port *p = malloc(sizeof(*p));
+	int checked_phc_index = -1;
+
 	if (!p)
 		return NULL;
 
 	memset(p, 0, sizeof(*p));
 
+	if (sk_interface_phc(name, &checked_phc_index))
+		pr_warning("port %d: get_ts_info not supported", number);
+	else if (phc_index >= 0 && phc_index != checked_phc_index) {
+		pr_err("port %d: PHC device mismatch", number);
+		pr_err("port %d: /dev/ptp%d requested, but /dev/ptp%d attached",
+		       number, phc_index, checked_phc_index);
+		return NULL;
+	}
+
 	p->pod = *pod;
 	p->name = name;
 	p->clock = clock;

port.h

@@ -80,6 +80,7 @@ enum fsm_event port_event(struct port *port, int fd_index);
 /**
  * Open a network port.
  * @param pod           A pointer to a default port data set for this port.
+ * @param phc_index     The PHC device index for the network device.
  * @param name          The name of the network interface.
  * @param transport     The network transport type to use on this port.
  * @param timestamping  The flavor of time stamping to use on this port.
@@ -89,6 +90,7 @@ enum fsm_event port_event(struct port *port, int fd_index);
  * @return A pointer to an open port on success, or NULL otherwise.
  */
 struct port *port_open(struct port_defaults *pod,
+		       int phc_index,
 		       char *name,
 		       enum transport_type transport,
 		       enum timestamp_type timestamping,

ptp4l.c

@@ -77,23 +77,24 @@ static void usage(char *progname)
 		"           (may be specified multiple times)\n"
 		" -l [num]  set the logging level to 'num'\n"
 		" -m        slave only mode (overrides configuration file)\n"
-		" -p [dev]  PTP hardware clock device to use, default '%s'\n"
+		" -p [dev]  PTP hardware clock device to use, default auto\n"
 		"           (ignored for SOFTWARE/LEGACY HW time stamping)\n"
 		" -q        quiet mode, do not use syslog(3)\n"
 		" -v        verbose mode, print messages to stdout\n"
 		"\n",
-		progname, DEFAULT_PHC);
+		progname);
 }
 
 int main(int argc, char *argv[])
 {
-	char *config = NULL, *phc = DEFAULT_PHC, *progname;
+	char *config = NULL, *req_phc = NULL, *progname;
 	int c, i, nports = 0, slaveonly = 0;
 	struct interface iface[MAX_PORTS];
 	enum delay_mechanism dm = DM_E2E;
 	enum transport_type transport = TRANS_UDP_IPV4;
 	enum timestamp_type timestamping = TS_HARDWARE;
 	struct clock *clock;
+	int phc_index = -1;
 
 	/* Process the command line arguments. */
 	progname = strrchr(argv[0], '/');
@@ -139,7 +140,7 @@ int main(int argc, char *argv[])
 			dm = DM_P2P;
 			break;
 		case 'p':
-			phc = optarg;
+			req_phc = optarg;
 			break;
 		case 'q':
 			print_no_syslog();
@@ -176,8 +177,23 @@ int main(int argc, char *argv[])
 	for (i = 0; i < nports; i++) {
 		iface[i].timestamping = timestamping;
 	}
+
+	/* determine PHC Clock index */
 	if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) {
-		phc = NULL;
+		phc_index = -1;
+	} else if (req_phc) {
+		if (1 != sscanf(req_phc, "/dev/ptp%d", &phc_index)) {
+			fprintf(stderr, "bad ptp device string\n");
+			return -1;
+		}
+	} else if (sk_interface_phc(iface[0].name, &phc_index)) {
+		fprintf(stderr, "get_ts_info not supported\n"
+				"please specify ptp device\n");
+		return -1;
+	}
+
+	if (phc_index >= 0) {
+		pr_info("selected /dev/ptp%d as PTP clock\n", phc_index);
 	}
 
 	ds.slaveOnly = FALSE;
@@ -213,7 +229,7 @@ int main(int argc, char *argv[])
 		ds.clockQuality.clockClass = 255;
 	}
 
-	clock = clock_create(phc, iface, nports, &ds, &pod);
+	clock = clock_create(phc_index, iface, nports, &ds, &pod);
 	if (!clock) {
 		fprintf(stderr, "failed to create a clock\n");
 		return -1;

sk.c

@@ -20,6 +20,7 @@
 #include <errno.h>
 #include <linux/net_tstamp.h>
 #include <linux/sockios.h>
+#include <linux/ethtool.h>
 #include <net/if.h>
 #include <netinet/in.h>
 #include <string.h>
@@ -83,6 +84,41 @@ int sk_interface_index(int fd, char *name)
 	return ifreq.ifr_ifindex;
 }
 
+int sk_interface_phc(char *name, int *index)
+{
+#ifdef ETHTOOL_GET_TS_INFO
+	struct ethtool_ts_info info;
+	struct ifreq ifr;
+	int fd, err;
+
+	memset(&ifr, 0, sizeof(ifr));
+	memset(&info, 0, sizeof(info));
+	info.cmd = ETHTOOL_GET_TS_INFO;
+	strcpy(ifr.ifr_name, name);
+	ifr.ifr_data = (char *) &info;
+	fd = socket(AF_INET, SOCK_DGRAM, 0);
+
+	if (fd < 0) {
+		pr_err("socket failed: %m");
+		return -1;
+	}
+
+	err = ioctl(fd, SIOCETHTOOL, &ifr);
+	if (err < 0) {
+		pr_err("ioctl SIOCETHTOOL failed: %m");
+		close(fd);
+		return -1;
+	}
+
+	close(fd);
+	*index = info.phc_index;
+
+	return 0;
+#else
+	return -1;
+#endif
+}
+
 int sk_interface_macaddr(char *name, unsigned char *mac, int len)
 {
 	struct ifreq ifreq;

sk.h

@@ -30,6 +30,13 @@
  */
 int sk_interface_index(int fd, char *device);
 
+/**
+ * Obtain the PHC device index of a network interface.
+ * @param name	    The name of the interface
+ * @return index    The phc index associated with this iface
+ */
+int sk_interface_phc(char *name, int *index);
+
 /**
  * Obtain the MAC address of a network interface.
  * @param name  The name of the interface