[Linuxptp-devel] [RFC PATCH v1 6/8] Add plumbing for interacting with the CMLDS

[Kishen M. <kis...@in...>]

This change furnishes the plumbing for interacting with CMLDS
Link Ports (i.e. ports with "run_cmlds=1").

It adds internal functions for PTP Ports which employ the
COMMON_P2P delay mechanism to issue requests to a CMLDS Link Port at
MID_CMLDS_INFO_NP and handle the response.

port_request_cmlds_info() encodes the CMLDS Link Port portNumber
associated with its physical port as read from its config file.
The request is sent to the configured cmlds_uds_address, i.e. the
uds_address of a ptp4l instance that hosts the CMLDS Link Port.

Upon receiving a request for MID_CMLDS_INFO_NP, the receiving ptp4l
instance matches the target portNumber against the CMLDS Link Port
portNumber that was assigned in its own config file and invokes
process_cmlds_response().

Ports that do not expose the CMLDS ('run_cmlds' set to 0) will not
respond to MID_CMLDS_INFO_NP queries.

Co-authored-by: Andrew Zaborowski <and...@in...>
Signed-off-by: Kishen Maloor <kis...@in...>
---
 clock.c        |  10 ++++
 clock.h        |   6 +++
 port.c         | 131 ++++++++++++++++++++++++++++++++++++++++++++++++-
 port_private.h |   2 +
 4 files changed, 147 insertions(+), 2 deletions(-)

diff --git a/clock.c b/clock.c
index 85f2a04867e5..626063849f2b 100644
--- a/clock.c
+++ b/clock.c
@@ -1650,6 +1650,11 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg)
 			return changed;
 		}
 		break;
+	case RESPONSE:
+		/* Let the destined Port handle a response from CMLDS */
+		if (mgt->id == MID_CMLDS_INFO_NP)
+			break;
+		return changed;
 	default:
 		return changed;
 	}
@@ -2280,3 +2285,8 @@ enum servo_state clock_servo_state(struct clock *c)
 {
 	return c->servo_state;
 }
+
+struct port *clock_uds_rw_port(struct clock *c)
+{
+	return c->uds_rw_port;
+}
diff --git a/clock.h b/clock.h
index ce9ae913eaaf..deb827f0afad 100644
--- a/clock.h
+++ b/clock.h
@@ -396,4 +396,10 @@ void clock_check_ts(struct clock *c, uint64_t ts);
  */
 double clock_rate_ratio(struct clock *c);
 
+/**
+ * Return a handle to the UDS RW port.
+ * @param c   The clock instance.
+ */
+struct port *clock_uds_rw_port(struct clock *c);
+
 #endif
diff --git a/port.c b/port.c
index 9040453fb2dc..14d2b71d0d73 100644
--- a/port.c
+++ b/port.c
@@ -1206,6 +1206,12 @@ static int port_management_fill_response(struct port *target,
 		cmlds->scaledNeighborRateRatio =
 			(Integer32) (target->nrate.ratio * POW2_41 - POW2_41);
 		datalen = sizeof(*cmlds);
+		/* Reflect the CMLDS sdoid/domainNumber/port id in
+		 * MID_CMLDS_INFO_NP responses
+		 */
+		rsp->header.tsmt = MANAGEMENT | CMLDS_TRANSPORTSPECIFIC;
+		rsp->header.domainNumber = CMLDS_DOMAINNUMBER;
+		rsp->header.sourcePortIdentity = target->cmlds_portIdentity;
 		break;
 	default:
 		/* The caller should *not* respond to this message. */
@@ -3240,10 +3246,26 @@ int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg)
 	struct management_tlv *mgt;
 	UInteger16 target = msg->management.targetPortIdentity.portNumber;
 
-	if (target != portnum(p) && target != 0xffff) {
+	mgt = (struct management_tlv *) msg->management.suffix;
+
+	/* Given a query to CMLDS and this port exposes CMLDS, try to match
+	 * the target portNumber with the CMLDS Link Port PortNumber.
+	 */
+	if (mgt->id == MID_CMLDS_INFO_NP &&
+	    management_action(msg) == GET) {
+		if (port_cmlds_enabled(p)) {
+			if (target != 0xffff &&
+			    target != p->cmlds_portIdentity.portNumber)
+				return 0;
+		} else {
+			/* Do not respond to MID_CMLDS_INFO_NP if this port
+			 * does not expose CMLDS
+			 */
+			return 0;
+		}
+	} else if (target != portnum(p) && target != 0xffff) {
 		return 0;
 	}
-	mgt = (struct management_tlv *) msg->management.suffix;
 
 	switch (management_action(msg)) {
 	case GET:
@@ -3256,6 +3278,12 @@ int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg)
 		break;
 	case COMMAND:
 		break;
+	case RESPONSE:
+		/* Handle a response from CMLDS */
+		if (mgt->id == MID_CMLDS_INFO_NP) {
+			return process_cmlds_response(p, msg);
+		}
+		break;
 	default:
 		return -1;
 	}
@@ -3707,3 +3735,102 @@ void port_update_unicast_state(struct port *p)
 		p->unicast_state_dirty = false;
 	}
 }
+
+int process_cmlds_response(struct port *p,
+			   struct ptp_message *msg)
+{
+	struct management_tlv *mgt = (struct management_tlv *)
+					msg->management.suffix;
+	struct cmlds_info_np *cmlds;
+
+	if (port_delay_mechanism(p) != DM_COMMON_P2P ||
+	    msg_tlv_count(msg) != 1) {
+		return -1;
+	}
+
+	pr_debug("Response\t%s seq %hu ",
+		pid2str(&msg->header.sourcePortIdentity),
+		msg->header.sequenceId);
+
+	cmlds = (struct cmlds_info_np *) mgt->data;
+
+	pr_debug("CMLDS INFO\n"
+		 "\tserviceMeasurementValid %i\n"
+		 "\tmeanLinkDelay %" PRId64 "\n"
+		 "\tscaledNeighborRateRatio %" PRId32,
+		 cmlds->serviceMeasurementValid, cmlds->meanLinkDelay,
+		 cmlds->scaledNeighborRateRatio);
+
+	/* COMMON_P2P DM implementation goes here */
+
+	return 0;
+}
+
+int port_request_cmlds_info(struct port *p)
+{
+	struct port *uds = clock_uds_rw_port(p->clock);
+	struct management_tlv *mgt;
+	struct tlv_extra *extra;
+	struct ptp_message *msg;
+	int err;
+
+	if (port_delay_mechanism(p) != DM_COMMON_P2P) {
+		return -1;
+	}
+
+	msg = msg_allocate();
+	if (!msg)
+		return -ENOMEM;
+
+	msg->hwts.type = p->timestamping;
+
+	msg->header.tsmt               = MANAGEMENT | CMLDS_TRANSPORTSPECIFIC;
+	msg->header.ver                = PTP_VERSION;
+	msg->header.domainNumber       = CMLDS_DOMAINNUMBER;
+	msg->header.sourcePortIdentity = p->portIdentity;
+	msg->header.sequenceId         = p->seqnum.delayreq++;
+	msg->header.control            = CTL_MANAGEMENT;
+	msg->header.logMessageInterval = 0x7f;
+
+	/* Set the target cid to the wildcard as this request is being
+	 * sent to the specific ptp4l instance that exposes the CMLDS
+	 */
+	msg->management.targetPortIdentity.clockIdentity =
+	  (struct ClockIdentity){
+		.id = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}
+	};
+	/* Set the target CMLDS Link Port portNumber */
+	msg->management.targetPortIdentity.portNumber = p->cmlds_portIdentity.portNumber;
+
+	msg->management.flags = GET;
+
+	mgt = (struct management_tlv *) msg->management.suffix;
+	mgt->type = TLV_MANAGEMENT;
+	mgt->length = sizeof(mgt->id);
+	mgt->id = MID_CMLDS_INFO_NP;
+	msg->header.messageLength = sizeof(struct management_msg) + sizeof(*mgt);
+
+	extra = tlv_extra_alloc();
+	if (!extra) {
+		pr_err("failed to allocate TLV descriptor");
+		msg_put(msg);
+		return -ENOMEM;
+	}
+	extra->tlv = (struct TLV *) msg->management.suffix;
+	msg_tlv_attach(msg, extra);
+
+	err = msg_pre_send(msg);
+	if (err) {
+		pr_err("msg_pre_send failed");
+		return -1;
+	}
+
+	/* Set the target CMLDS UDS address */
+	msg->address = p->cmlds_uds_address;
+	err = port_forward_to(uds, msg);
+
+	pr_debug("CMLDS request sent: %s", strerror(err < 0 ? -err : 0));
+
+	msg_put(msg);
+	return err;
+}
diff --git a/port_private.h b/port_private.h
index 7d36a8200896..10ce05f3a895 100644
--- a/port_private.h
+++ b/port_private.h
@@ -205,6 +205,8 @@ int port_tx_interval_request(struct port *p,
 			     Integer8 timeSyncInterval,
 			     Integer8 linkDelayInterval);
 int port_tx_sync(struct port *p, struct address *dst, uint16_t sequence_id);
+int port_request_cmlds_info(struct port *p);
+int process_cmlds_response(struct port *p, struct ptp_message *msg);
 int process_announce(struct port *p, struct ptp_message *m);
 void process_delay_resp(struct port *p, struct ptp_message *m);
 void process_follow_up(struct port *p, struct ptp_message *m);
-- 
2.31.1