[Linuxptp-devel] [PATCH v2 5/6] Implement the COMMON_P2P delay mechanism.

[Richard C. <ric...@gm...>]

If a given port selects the COMMON_P2P delay mechanism, let it open a
local PTP Management channel to the Common Mean Link Delay Service
over a UNIX domain socket.

Signed-off-by: Richard Cochran <ric...@gm...>
---
 config.c            |   6 ++
 configs/default.cfg |   5 ++
 dm.h                |   3 +
 fd.h                |   1 +
 makefile            |   4 +-
 port.c              | 180 +++++++++++++++++++++++++++++++++++++++++---
 port_private.h      |   7 ++
 ptp4l.8             |  35 +++++++++
 8 files changed, 228 insertions(+), 13 deletions(-)

diff --git a/config.c b/config.c
index fe65b76..8cf1620 100644
--- a/config.c
+++ b/config.c
@@ -171,6 +171,7 @@ static struct config_enum delay_filter_enu[] = {
 
 static struct config_enum delay_mech_enu[] = {
 	{ "Auto", DM_AUTO },
+	{ "COMMON_P2P", DM_COMMON_P2P },
 	{ "E2E",  DM_E2E },
 	{ "P2P",  DM_P2P },
 	{ "NONE", DM_NO_MECHANISM },
@@ -249,6 +250,11 @@ struct config_item config_tab[] = {
 	GLOB_ITEM_INT("clock_class_threshold", CLOCK_CLASS_THRESHOLD_DEFAULT, 6, CLOCK_CLASS_THRESHOLD_DEFAULT),
 	GLOB_ITEM_ENU("clock_servo", CLOCK_SERVO_PI, clock_servo_enu),
 	GLOB_ITEM_ENU("clock_type", CLOCK_TYPE_ORDINARY, clock_type_enu),
+	PORT_ITEM_STR("cmlds.client_address", "/var/run/cmlds_cleint"),
+	PORT_ITEM_INT("cmlds.domainNumber", 0, 0, 255),
+	PORT_ITEM_INT("cmlds.majorSdoId", 2, 0, 0x0F),
+	PORT_ITEM_INT("cmlds.port", 0, 0, UINT16_MAX),
+	PORT_ITEM_STR("cmlds.server_address", "/var/run/cmlds_server"),
 	GLOB_ITEM_ENU("dataset_comparison", DS_CMP_IEEE1588, dataset_comp_enu),
 	PORT_ITEM_INT("delayAsymmetry", 0, INT_MIN, INT_MAX),
 	PORT_ITEM_ENU("delay_filter", FILTER_MOVING_MEDIAN, delay_filter_enu),
diff --git a/configs/default.cfg b/configs/default.cfg
index 0c7661c..29a34fd 100644
--- a/configs/default.cfg
+++ b/configs/default.cfg
@@ -93,6 +93,11 @@ write_phase_mode	0
 #
 # Transport options
 #
+cmlds.client_address	/var/run/cmlds_cleint
+cmlds.domainNumber	0
+cmlds.majorSdoId	2
+cmlds.port		0
+cmlds.server_address	/var/run/cmlds_server
 transportSpecific	0x0
 ptp_dst_mac		01:1B:19:00:00:00
 p2p_dst_mac		01:80:C2:00:00:0E
diff --git a/dm.h b/dm.h
index 47bd847..80d1ce5 100644
--- a/dm.h
+++ b/dm.h
@@ -34,6 +34,9 @@ enum delay_mechanism {
 	/** Peer delay mechanism. */
 	DM_P2P,
 
+	/** Peer delay as measured by CMLDS. */
+	DM_COMMON_P2P,
+
 	/** No Delay Mechanism. */
 	DM_NO_MECHANISM = 0xFE,
 };
diff --git a/fd.h b/fd.h
index 16420d7..9a072ab 100644
--- a/fd.h
+++ b/fd.h
@@ -39,6 +39,7 @@ enum {
 	FD_SYNC_TX_TIMER,
 	FD_UNICAST_REQ_TIMER,
 	FD_UNICAST_SRV_TIMER,
+	FD_CMLDS,
 	FD_RTNL,
 	N_POLLFD,
 };
diff --git a/makefile b/makefile
index 7fc5f6f..e9c1ccc 100644
--- a/makefile
+++ b/makefile
@@ -30,8 +30,8 @@ TS2PHC	= ts2phc.o lstab.o nmea.o serial.o sock.o ts2phc_generic_pps_source.o \
  ts2phc_nmea_pps_source.o ts2phc_phc_pps_source.o ts2phc_pps_sink.o ts2phc_pps_source.o
 OBJ	= bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
  e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o monitor.o msg.o phc.o \
- port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) \
- sk.o stats.o tc.o $(TRANSP) telecom.o tlv.o tsproc.o unicast_client.o \
+ pmc_common.o port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o \
+ $(SERVOS) sk.o stats.o tc.o $(TRANSP) telecom.o tlv.o tsproc.o unicast_client.o \
  unicast_fsm.o unicast_service.o util.o version.o
 
 OBJECTS	= $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_agent.o \
diff --git a/port.c b/port.c
index 23f021c..88d794e 100644
--- a/port.c
+++ b/port.c
@@ -46,6 +46,8 @@
 #include "util.h"
 
 #define ANNOUNCE_SPAN 1
+#define CMLDS_SUBSCRIPTION_INTERVAL	60 /*seconds*/
+#define CMLDS_UPDATE_INTERVAL		(CMLDS_SUBSCRIPTION_INTERVAL / 2)
 
 enum syfu_event {
 	SYNC_MISMATCH,
@@ -965,7 +967,8 @@ static int port_management_fill_response(struct port *target,
 		ptp_text_copy(cd->userDescription, &desc->userDescription);
 		buf += sizeof(struct PTPText) + cd->userDescription->length;
 
-		if (target->delayMechanism == DM_P2P) {
+		if (target->delayMechanism == DM_P2P ||
+		    target->delayMechanism == DM_COMMON_P2P) {
 			memcpy(buf, profile_id_p2p, PROFILE_ID_LEN);
 		} else {
 			struct config *cfg = clock_config(target->clock);
@@ -1275,14 +1278,16 @@ int port_set_delay_tmo(struct port *p)
 	if (p->inhibit_delay_req) {
 		return 0;
 	}
-
-	if (p->delayMechanism == DM_P2P) {
+	switch (p->delayMechanism) {
+	case DM_COMMON_P2P:
+	case DM_P2P:
 		return set_tmo_log(p->fda.fd[FD_DELAY_TIMER], 1,
-			       p->logPdelayReqInterval);
-	} else {
-		return set_tmo_random(p->fda.fd[FD_DELAY_TIMER], 0, 2,
-				p->logMinDelayReqInterval);
+				   p->logPdelayReqInterval);
+	default:
+		break;
 	}
+	return set_tmo_random(p->fda.fd[FD_DELAY_TIMER], 0, 2,
+			      p->logMinDelayReqInterval);
 }
 
 static int port_set_manno_tmo(struct port *p)
@@ -1528,6 +1533,45 @@ static void port_syfufsm(struct port *p, enum syfu_event event,
 	}
 }
 
+static int port_cmlds_renew(struct port *p, time_t now)
+{
+	struct subscribe_events_np sen = {
+		.duration = CMLDS_SUBSCRIPTION_INTERVAL,
+	};
+	int err;
+
+	event_bitmask_set(sen.bitmask, NOTIFY_CMLDS, TRUE);
+	err = pmc_send_set_action(p->cmlds.pmc, MID_SUBSCRIBE_EVENTS_NP,
+				  &sen, sizeof(sen));
+	if (err) {
+		return err;
+	}
+	p->cmlds.last_renewal = now;
+	return 0;
+}
+
+static enum fsm_event port_cmlds_timeout(struct port *p)
+{
+	struct timespec now;
+	int err;
+
+	if (!p->cmlds.pmc) {
+		return EV_NONE;
+	}
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	if (now.tv_sec - p->cmlds.last_renewal > CMLDS_UPDATE_INTERVAL) {
+		err = port_cmlds_renew(p, now.tv_sec);
+		if (err) {
+			return EV_FAULT_DETECTED;
+		}
+	}
+	p->cmlds.timer_count++;
+	if (p->cmlds.timer_count > p->allowedLostResponses) {
+		return EV_FAULT_DETECTED;
+	}
+	return EV_NONE;
+}
+
 static int port_pdelay_request(struct port *p)
 {
 	struct ptp_message *msg;
@@ -1871,6 +1915,33 @@ static void port_clear_fda(struct port *p, int count)
 		p->fda.fd[i] = -1;
 }
 
+static int port_cmlds_initialize(struct port *p)
+{
+	struct config *cfg = clock_config(p->clock);
+	const int zero_datalen = 1;
+	const UInteger8 hops = 0;
+	struct timespec now;
+
+	p->cmlds.port = config_get_int(cfg, p->name, "cmlds.port");
+	if (!p->cmlds.port) {
+		p->cmlds.port = portnum(p);
+	}
+	p->cmlds.pmc = pmc_create(cfg, TRANS_UDS,
+				  config_get_string(cfg, p->name, "cmlds.client_address"),
+				  config_get_string(cfg, p->name, "cmlds.server_address"),
+				  hops,
+				  config_get_int(cfg, p->name, "cmlds.domainNumber"),
+				  config_get_int(cfg, p->name, "cmlds.majorSdoId") << 4,
+				  zero_datalen);
+	if (!p->cmlds.pmc) {
+		return -1;
+	}
+	p->cmlds.timer_count = 0;
+	p->fda.fd[FD_CMLDS] = pmc_get_transport_fd(p->cmlds.pmc);
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	return port_cmlds_renew(p, now.tv_sec);
+}
+
 void port_disable(struct port *p)
 {
 	int i;
@@ -1888,6 +1959,12 @@ void port_disable(struct port *p)
 		close(p->fda.fd[FD_FIRST_TIMER + i]);
 	}
 
+	if (p->cmlds.pmc) {
+		pmc_destroy(p->cmlds.pmc);
+		p->fda.fd[FD_CMLDS] = -1;
+		p->cmlds.pmc = NULL;
+	}
+
 	/* Keep rtnl socket to get link status info. */
 	port_clear_fda(p, FD_RTNL);
 	clock_fda_changed(p->clock);
@@ -1935,7 +2012,8 @@ int port_initialize(struct port *p)
 		pr_err("inhibit_delay_req can only be set when asCapable == 'true'.");
 		return -1;
 	}
-	if (port_delay_mechanism(p) == DM_NO_MECHANISM) {
+	if (port_delay_mechanism(p) == DM_COMMON_P2P ||
+	    port_delay_mechanism(p) == DM_NO_MECHANISM) {
 		p->inhibit_delay_req = 1;
 	}
 
@@ -1963,6 +2041,10 @@ int port_initialize(struct port *p)
 		goto no_tmo;
 	}
 
+	if (port_delay_mechanism(p) == DM_COMMON_P2P && port_cmlds_initialize(p)) {
+		goto no_tmo;
+	}
+
 	/* No need to open rtnl socket on UDS port. */
 	if (!port_is_uds(p)) {
 		/*
@@ -2104,6 +2186,65 @@ int process_announce(struct port *p, struct ptp_message *m)
 	return result;
 }
 
+static int process_cmlds(struct port *p)
+{
+	struct cmlds_info_np *cmlds;
+	struct management_tlv *mgt;
+	struct ptp_message *msg;
+	struct TLV *tlv;
+	int err = 0;
+
+	msg = pmc_recv(p->cmlds.pmc);
+	if (!msg) {
+		pr_err("%s: pmc_recv failed", p->log_name);
+		return -1;
+	}
+	if (msg_type(msg) != MANAGEMENT) {
+		pr_err("%s: pmc_recv bad message", p->log_name);
+		err = -1;
+		goto out;
+	}
+	tlv = (struct TLV *) msg->management.suffix;
+	if (tlv->type != TLV_MANAGEMENT) {
+		pr_err("%s: pmc_recv bad message", p->log_name);
+		err = -1;
+		goto out;
+	}
+	mgt = (struct management_tlv *) msg->management.suffix;
+	if (mgt->length == 2 && mgt->id != MID_NULL_MANAGEMENT) {
+		pr_err("%s: pmc_recv bad length", p->log_name);
+		goto out;
+	}
+
+	switch (mgt->id) {
+	case MID_CMLDS_INFO_NP:
+		if (msg->header.sourcePortIdentity.portNumber != p->cmlds.port) {
+			break;
+		}
+		cmlds = (struct cmlds_info_np *) mgt->data;
+		p->peer_delay = nanoseconds_to_tmv(cmlds->meanLinkDelay >> 16);
+		p->peerMeanPathDelay = cmlds->meanLinkDelay;
+		p->nrate.ratio = 1.0 + (double) cmlds->scaledNeighborRateRatio / POW2_41;
+		p->asCapable = cmlds->as_capable;
+		p->cmlds.timer_count = 0;
+		if (p->state == PS_UNCALIBRATED || p->state == PS_SLAVE) {
+			const tmv_t tx = tmv_zero();
+			clock_peer_delay(p->clock, p->peer_delay, tx, tx, p->nrate.ratio);
+		}
+		break;
+	case MID_SUBSCRIBE_EVENTS_NP:
+		break;
+	default:
+		pr_err("%s: pmc_recv bad mgt id 0x%x", p->log_name, mgt->id);
+		err = -1;
+		break;
+	}
+
+out:
+	msg_put(msg);
+	return err;
+}
+
 static int process_delay_req(struct port *p, struct ptp_message *m)
 {
 	struct ptp_message *msg;
@@ -2115,7 +2256,7 @@ static int process_delay_req(struct port *p, struct ptp_message *m)
 		return 0;
 	}
 
-	if (p->delayMechanism == DM_P2P) {
+	if (p->delayMechanism == DM_P2P || p->delayMechanism == DM_COMMON_P2P) {
 		pr_warning("%s: delay request on P2P port", p->log_name);
 		return 0;
 	}
@@ -2280,6 +2421,9 @@ int process_pdelay_req(struct port *p, struct ptp_message *m)
 		return -1;
 	}
 
+	if (p->delayMechanism == DM_COMMON_P2P) {
+		return 0;
+	}
 	if (p->delayMechanism == DM_E2E) {
 		pr_warning("%s: pdelay_req on E2E port", p->log_name);
 		return 0;
@@ -2475,6 +2619,9 @@ calc:
 
 int process_pdelay_resp(struct port *p, struct ptp_message *m)
 {
+	if (p->delayMechanism == DM_COMMON_P2P) {
+		return 0;
+	}
 	if (p->peer_delay_resp) {
                 if (!p->multiple_pdr_detected) {
                         pr_err("%s: multiple peer responses", p->log_name);
@@ -2750,10 +2897,14 @@ static void bc_dispatch(struct port *p, enum fsm_event event, int mdiff)
 		return;
 	}
 
-	if (p->delayMechanism == DM_P2P) {
+	switch (p->delayMechanism) {
+	case DM_COMMON_P2P:
+	case DM_P2P:
 		port_p2p_transition(p, p->state);
-	} else {
+		break;
+	default:
 		port_e2e_transition(p, p->state);
+		break;
 	}
 
 	if (p->jbod && p->state == PS_UNCALIBRATED && p->phc_index >= 0 ) {
@@ -2901,6 +3052,9 @@ static enum fsm_event bc_event(struct port *p, int fd_index)
 	case FD_DELAY_TIMER:
 		pr_debug("%s: delay timeout", p->log_name);
 		port_set_delay_tmo(p);
+		if (p->delayMechanism == DM_COMMON_P2P) {
+			return port_cmlds_timeout(p);
+		}
 		delay_req_prune(p);
 		p->service_stats.delay_timeout++;
 		if (port_delay_request(p)) {
@@ -2947,6 +3101,10 @@ static enum fsm_event bc_event(struct port *p, int fd_index)
 		p->service_stats.unicast_request_timeout++;
 		return unicast_client_timer(p) ? EV_FAULT_DETECTED : EV_NONE;
 
+	case FD_CMLDS:
+		pr_debug("%s: CMLDS push notification", p->log_name);
+		return process_cmlds(p) ? EV_FAULT_DETECTED : EV_NONE;
+
 	case FD_RTNL:
 		pr_debug("%s: received link status notification", p->log_name);
 		rtnl_link_status(fd, p->name, port_link_status, p);
diff --git a/port_private.h b/port_private.h
index 1072ad6..4f158cd 100644
--- a/port_private.h
+++ b/port_private.h
@@ -26,6 +26,7 @@
 #include "fsm.h"
 #include "monitor.h"
 #include "msg.h"
+#include "pmc_common.h"
 #include "power_profile.h"
 #include "tmv.h"
 #include "util.h"
@@ -164,6 +165,12 @@ struct port {
 	/* slave event monitoring */
 	struct monitor *slave_event_monitor;
 	bool unicast_state_dirty;
+	struct {
+		unsigned int timer_count;
+		time_t last_renewal;
+		struct pmc *pmc;
+		int port;
+	} cmlds;
 };
 
 #define portnum(p) (p->portIdentity.portNumber)
diff --git a/ptp4l.8 b/ptp4l.8
index 4cb9adb..c53e769 100644
--- a/ptp4l.8
+++ b/ptp4l.8
@@ -159,6 +159,41 @@ collection of clocks must be synchronized by an external program, for
 example phc2sys(8) in "automatic" mode.
 The default is 0 (disabled).
 
+.TP
+.B cmlds.client_address
+Specifies the source address for the UNIX domain socket that receives
+peer delay measurement when using the "COMMON_P2P" delay mechanism.
+The default is /var/run/cmlds_cleint.
+
+.TP
+.B cmlds.domainNumber
+Specifies the domainNumber message field for communications with the
+local Common Mean Link Delay Service, used with the "COMMON_P2P" delay
+mechanism.
+The default is 0.
+
+.TP
+.B cmlds.majorSdoId
+Specifies the transportSpecific message field for communications with
+the local Common Mean Link Delay Service, used with the "COMMON_P2P"
+delay mechanism.
+The default is 2.
+
+.TP
+.B cmlds.port
+Specifies the port number of local Common Mean Link Delay Service from
+which to accept delay measurements.  The range of valid port numbers
+is one to 65535, but the special value zero configures the port number
+of the port requesting the measurements.
+The default is 0, meaning the requesting port number.
+
+.TP
+.B
+cmlds.server_address
+Specifies the UNIX domain socket address of the local Common Mean Link
+Delay Service, for use with the "COMMON_P2P" delay mechanism.
+The default is /var/run/cmlds_server.
+
 .TP
 .B delayAsymmetry
 The time difference in nanoseconds of the transmit and receive
-- 
2.39.2