adding delay asymmetry calculation

Delay asymmetry calculation based on the PTP port interface speed of
master obtained from TLV and the slave interface rate obtained by ethtool.

The calculation is done in attoseconds to accomodate interface bit period
less than 1ns. (ITU-T G.8275.2(06/2021) annex D).

v4: removing bit period calculation on every signalling message and updating
    the bit-period when link update happens
v3: updating network/host byte order handling.
v1: initial commit

Signed-off-by: Greg Armstrong <greg.armstrong.uw@renesas.com>
Signed-off-by: Leon Goldin <leon.goldin.nx@renesas.com>
Signed-off-by: Devasish Dey <devasish.dey@syncmonk.net>
Signed-off-by: Vipin Sharma <vipin.sharma@syncmonk.net>

interface.c

@@ -94,3 +94,8 @@ int interface_get_vclock(struct interface *iface)
 {
 	return iface->vclock;
 }
+
+uint64_t interface_bitperiod(struct interface *iface)
+{
+	return iface->if_info.iface_bit_period;
+}

interface.h

@@ -113,4 +113,11 @@ void interface_set_vclock(struct interface *iface, int vclock);
  */
 int interface_get_vclock(struct interface *iface);
 
+/**
+ * Obtains the interface bit period based on the speed.
+ * @param iface  The interface of interest.
+ * @return       return interface bitperiod in atto seconds.
+ */
+uint64_t interface_bitperiod(struct interface *iface);
+
 #endif

port_private.h

@@ -146,6 +146,7 @@ struct port {
 	UInteger8	    delay_response_counter;
 	UInteger8	    delay_response_timeout;
 	bool		    iface_rate_tlv;
+	Integer64	    portAsymmetry;
 	struct PortStats    stats;
 	struct PortServiceStats    service_stats;
 	/* foreignMasterDS */

port_signaling.c

@@ -103,10 +103,37 @@ static int process_interval_request(struct port *p,
 	return 0;
 }
 
+static int process_interface_rate(struct port *p,
+                                 struct msg_interface_rate_tlv *r)
+{
+       Integer64 delayAsymmetry;
+       double    nsDelay;
+       Integer64 slaveBitPeriod;
+       Integer64 masterBitPeriod;
+
+       if (p->iface_rate_tlv && interface_ifinfo_valid(p->iface)) {
+               slaveBitPeriod = interface_bitperiod(p->iface);
+               masterBitPeriod = r->interfaceBitPeriod;
+
+               /* Delay Asymmetry Calculation */
+               nsDelay = (double)(masterBitPeriod - slaveBitPeriod) / (2 * 1.0e9);
+               delayAsymmetry =
+                       (r->numberOfBitsAfterTimestamp - r->numberOfBitsBeforeTimestamp)  * nsDelay;
+
+               if (delayAsymmetry != p->portAsymmetry) {
+                       p->asymmetry += ((delayAsymmetry - p->portAsymmetry) << 16);
+                       p->portAsymmetry = delayAsymmetry;
+               }
+       }
+       return 0;
+}
+
 int process_signaling(struct port *p, struct ptp_message *m)
 {
 	struct tlv_extra *extra;
+	struct organization_tlv *org;
 	struct msg_interval_req_tlv *r;
+	struct msg_interface_rate_tlv *rate;
 	int err = 0, result;
 
 	switch (p->state) {
@@ -160,11 +187,17 @@ int process_signaling(struct port *p, struct ptp_message *m)
 			break;
 
 		case TLV_ORGANIZATION_EXTENSION:
-			r = (struct msg_interval_req_tlv *) extra->tlv;
+			org = (struct organization_tlv *)extra->tlv;
 
-			if (0 == memcmp(r->id, ieee8021_id, sizeof(ieee8021_id)) &&
-			    r->subtype[0] == 0 && r->subtype[1] == 0 && r->subtype[2] == 2)
+			if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id)) &&
+			    org->subtype[0] == 0 && org->subtype[1] == 0 && org->subtype[2] == 2) {
+				r = (struct msg_interval_req_tlv *) extra->tlv;
 				err = process_interval_request(p, r);
+			} else if (0 == memcmp(org->id, itu_t_id, sizeof(itu_t_id)) &&
+				   org->subtype[0] == 0 && org->subtype[1] == 0 && org->subtype[2] == 2) {
+				rate = (struct msg_interface_rate_tlv *) extra->tlv;
+				err = process_interface_rate(p, rate);
+			}
 			break;
 		}
 	}

ptp4l.8

@@ -697,6 +697,13 @@ The default is 0 (disabled).
 Print messages to the system log if enabled.
 The default is 1 (enabled).
 .TP
+.B interface_rate_tlv
+When the client and server are operating are operating at different interface rate,
+delay asymmetry caused due to different interface rate needs to be compensated.
+The server sends its interface rate using interface rate TLV
+as per G.8275.2 Annex D.
+The default is 0 (does not support interface rate tlv).
+.TP
 .B summary_interval
 The time interval in which are printed summary statistics of the clock. It is
 specified as a power of two in seconds. The statistics include offset root mean

tlv.c

@@ -681,6 +681,7 @@ static void nsm_resp_pre_send(struct tlv_extra *extra)
 static int org_post_recv(struct organization_tlv *org)
 {
 	struct follow_up_info_tlv *f;
+	struct msg_interface_rate_tlv *m;
 
 	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
 		if (org->subtype[0] || org->subtype[1]) {
@@ -701,6 +702,21 @@ static int org_post_recv(struct organization_tlv *org)
 			if (org->length + sizeof(struct TLV) != sizeof(struct msg_interval_req_tlv))
 				goto bad_length;
 		}
+	} else if (0 == memcmp(org->id, itu_t_id, sizeof(itu_t_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return 0;
+		}
+		switch (org->subtype[2]) {
+		case 2:
+			if (org->length + sizeof(struct TLV) != sizeof(struct msg_interface_rate_tlv))
+				goto bad_length;
+			m = (struct msg_interface_rate_tlv *)org;
+			m->interfaceBitPeriod = net2host64(m->interfaceBitPeriod);
+			m->numberOfBitsBeforeTimestamp = ntohs(m->numberOfBitsBeforeTimestamp);
+			m->numberOfBitsAfterTimestamp = ntohs(m->numberOfBitsAfterTimestamp);
+			break;
+		}
+
 	}
 	return 0;
 bad_length:
@@ -710,6 +726,7 @@ static int org_post_recv(struct organization_tlv *org)
 static void org_pre_send(struct organization_tlv *org)
 {
 	struct follow_up_info_tlv *f;
+	struct msg_interface_rate_tlv *m;
 
 	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
 		if (org->subtype[0] || org->subtype[1]) {
@@ -724,6 +741,18 @@ static void org_pre_send(struct organization_tlv *org)
 			f->scaledLastGmPhaseChange = htonl(f->scaledLastGmPhaseChange);
 			break;
 		}
+	} else if (0 == memcmp(org->id, itu_t_id, sizeof(itu_t_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return;
+		}
+		switch (org->subtype[2]) {
+		case 2:
+			m = (struct msg_interface_rate_tlv *)org;
+			m->interfaceBitPeriod = host2net64(m->interfaceBitPeriod);
+			m->numberOfBitsBeforeTimestamp = htons(m->numberOfBitsBeforeTimestamp);
+			m->numberOfBitsAfterTimestamp = htons(m->numberOfBitsAfterTimestamp);
+			break;
+		}
 	}
 }
 

unicast_service.c

@@ -84,6 +84,30 @@ static int attach_grant(struct ptp_message *msg,
 	return 0;
 }
 
+static int attach_interface_rate(struct ptp_message *msg,
+                                uint64_t iface_bit_period,
+                                uint16_t  no_of_bits_before_ts,
+                                uint16_t  no_of_bits_after_ts)
+{
+	struct msg_interface_rate_tlv *mir;
+	struct tlv_extra *extra;
+
+	extra = msg_tlv_append(msg, sizeof(*mir));
+	if (!extra) {
+		return -1;
+	}
+	mir = (struct msg_interface_rate_tlv *) extra->tlv;
+	mir->type = TLV_ORGANIZATION_EXTENSION;
+	mir->length = sizeof(*mir) - sizeof(mir->type) - sizeof(mir->length);
+	memcpy(mir->id, itu_t_id, sizeof(itu_t_id));
+	mir->subtype[2] = 2;
+	mir->interfaceBitPeriod = iface_bit_period;
+	mir->numberOfBitsBeforeTimestamp = no_of_bits_before_ts;
+	mir->numberOfBitsAfterTimestamp = no_of_bits_after_ts;
+
+	return 0;
+}
+
 static int compare_timeout(void *ain, void *bin)
 {
 	struct unicast_service_interval *a, *b;
@@ -256,6 +280,14 @@ static int unicast_service_reply(struct port *p, struct ptp_message *dst,
 	if (err) {
 		goto out;
 	}
+	if (p->iface_rate_tlv && duration > 0 && interface_ifinfo_valid(p->iface)) {
+		err = attach_interface_rate(msg,
+				interface_bitperiod(p->iface), 64, 720);
+		if (err) {
+			goto out;
+		}
+	}
+
 	err = port_prepare_and_send(p, msg, TRANS_GENERAL);
 	if (err) {
 		pr_err("%s: signaling message failed", p->log_name);