ptp4l: use ts label to get ts info

Now the ts label will be either the bond active slave or the interface
name, which is the exactly interface we need to get ts info.

When the link down/up or there is a fail over and ts_label changed, the
phc index may also changed. So we need to check get new ts info and check
clock_required_modes. We will set the link to LINK_DOWN by force if
the new ts_label's timestamp do not support required mode.

If all good, then we set phc index to new one. Also sync clock interval
after switch phc.

Signed-off-by: Hangbin Liu <liuhangbin@gmail.com>

clock.c

@@ -38,6 +38,7 @@
 #include "servo.h"
 #include "stats.h"
 #include "print.h"
+#include "rtnl.h"
 #include "tlv.h"
 #include "tsproc.h"
 #include "uds.h"
@@ -832,6 +833,16 @@ int clock_required_modes(struct clock *c)
 	return required_modes;
 }
 
+/*
+ * If we do not have a slave or the rtnl query failed, then use our
+ * own interface name as the time stamping interface name.
+ */
+static void ensure_ts_label(struct interface *iface)
+{
+	if (iface->ts_label[0] == '\0')
+		strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE);
+}
+
 struct clock *clock_create(enum clock_type type, struct config *config,
 			   const char *phc_device)
 {
@@ -943,6 +954,9 @@ struct clock *clock_create(enum clock_type type, struct config *config,
 	c->timestamping = timestamping;
 	required_modes = clock_required_modes(c);
 	STAILQ_FOREACH(iface, &config->interfaces, list) {
+		rtnl_get_ts_label(iface);
+		ensure_ts_label(iface);
+		sk_get_ts_info(iface->ts_label, &iface->ts_info);
 		if (iface->ts_info.valid &&
 		    ((iface->ts_info.so_timestamping & required_modes) != required_modes)) {
 			pr_err("interface '%s' does not support "

config.c

@@ -633,7 +633,6 @@ struct interface *config_create_interface(char *name, struct config *cfg)
 	}
 
 	strncpy(iface->name, name, MAX_IFNAME_SIZE);
-	sk_get_ts_info(iface->name, &iface->ts_info);
 	STAILQ_INSERT_TAIL(&cfg->interfaces, iface, list);
 	cfg->n_interfaces++;
 

port.c

@@ -60,6 +60,7 @@ enum link_state {
 	LINK_DOWN  = (1<<0),
 	LINK_UP  = (1<<1),
 	LINK_STATE_CHANGED = (1<<3),
+	TS_LABEL_CHANGED  = (1<<4),
 };
 
 struct nrate_estimator {
@@ -2231,6 +2232,8 @@ static void port_link_status(void *ctx, int linkup, int ts_index)
 {
 	struct port *p = ctx;
 	int link_state;
+	char ts_label[MAX_IFNAME_SIZE + 1] = {0};
+	int required_modes;
 
 	link_state = linkup ? LINK_UP : LINK_DOWN;
 	if (p->link_status & link_state) {
@@ -2240,6 +2243,39 @@ static void port_link_status(void *ctx, int linkup, int ts_index)
 		pr_notice("port %hu: link %s", portnum(p), linkup ? "up" : "down");
 	}
 
+	/* ts_label changed */
+	if (if_indextoname(ts_index, ts_label) && strcmp(p->iface->ts_label, ts_label)) {
+		strncpy(p->iface->ts_label, ts_label, MAX_IFNAME_SIZE);
+		p->link_status |= TS_LABEL_CHANGED;
+		pr_notice("port %hu: ts label changed to %s", portnum(p), ts_label);
+	}
+
+	/* Both link down/up and change ts_label may change phc index. */
+	if (p->link_status & LINK_UP &&
+	    (p->link_status & LINK_STATE_CHANGED || p->link_status & TS_LABEL_CHANGED)) {
+		sk_get_ts_info(p->iface->ts_label, &p->iface->ts_info);
+
+		/* Only switch phc with HW time stamping mode */
+		if (p->phc_index >= 0 && p->iface->ts_info.valid) {
+			required_modes = clock_required_modes(p->clock);
+			if ((p->iface->ts_info.so_timestamping & required_modes) != required_modes) {
+				pr_err("interface '%s' does not support requested "
+				       "timestamping mode, set link status down by force.",
+				       p->iface->ts_label);
+				p->link_status = LINK_DOWN | LINK_STATE_CHANGED;
+			} else if (p->phc_index != p->iface->ts_info.phc_index) {
+				p->phc_index = p->iface->ts_info.phc_index;
+
+				if (clock_switch_phc(p->clock, p->phc_index)) {
+					p->last_fault_type = FT_SWITCH_PHC;
+					port_dispatch(p, EV_FAULT_DETECTED, 0);
+					return;
+				}
+				clock_sync_interval(p->clock, p->log_sync_interval);
+			}
+		}
+	}
+
 	/*
 	 * A port going down can affect the BMCA result.
 	 * Force a state decision event.
@@ -2292,7 +2328,8 @@ enum fsm_event port_event(struct port *p, int fd_index)
 		rtnl_link_status(fd, p->name, port_link_status, p);
 		if (p->link_status == (LINK_UP | LINK_STATE_CHANGED))
 			return EV_FAULT_CLEARED;
-		else if (p->link_status == (LINK_DOWN | LINK_STATE_CHANGED))
+		else if ((p->link_status == (LINK_DOWN | LINK_STATE_CHANGED)) ||
+			 (p->link_status & TS_LABEL_CHANGED))
 			return EV_FAULT_DETECTED;
 		else
 			return EV_NONE;