general: audited all cField and math

There is still some factorization to make, and other irrelevant size-related things, but I'd better wait a little and get confident this stuff is solid. Signed-off-by: Alessandro Rubini <rubini@gnudd.com>

general: audited all cField and math
There is still some factorization to make, and other irrelevant size-related things, but I'd better wait a little and get confident this stuff is solid. Signed-off-by: Alessandro Rubini <rubini@gnudd.com>
1357b4a4 · Alessandro Rubini · Adam Wujek · 9b94f288 · 1357b4a4 · 1357b4a4
Commit 1357b4a4 authored Feb 24, 2017 by Alessandro Rubini Committed by Adam Wujek Mar 03, 2017
9 changed files
--- a/include/ppsi/ieee1588_types.h
+++ b/include/ppsi/ieee1588_types.h
@@ -113,7 +113,7 @@ typedef struct MsgHeader {
 	UInteger16	messageLength;
 	UInteger8	domainNumber;
 	Octet		flagField[2];
-	struct pp_time	cField;;
+	struct pp_time	cField;
 	PortIdentity	sourcePortIdentity;
 	UInteger16	sequenceId;
 	/* UInteger8	controlField; -- receiver must ignore it */

--- a/include/ppsi/pp-instance.h
+++ b/include/ppsi/pp-instance.h
@@ -150,8 +150,7 @@ struct pp_instance {
 	/* Times, for the various offset computations */
 	struct pp_time t1, t2, t3, t4, t5, t6;		/* *the* stamps */
-	Integer32 t4_cf, t6_cf;				/* peer delay */
+	uint64_t syncCF;				/* transp. clocks */
-	struct pp_time cField;				/* transp. clocks */
 	struct pp_time last_rcv_time, last_snt_time;	/* two temporaries */
 	/* Page 85: each port shall maintain an implementation-specific

--- a/include/ppsi/ppsi.h
+++ b/include/ppsi/ppsi.h
@@ -161,8 +161,7 @@ struct pp_ext_hooks {
 	void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann);
 	int (*execute_slave)(struct pp_instance *ppi);
 	int (*handle_announce)(struct pp_instance *ppi);
-	int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig,
+	int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig);
-			       struct pp_time *correction_field);
 	int (*handle_preq) (struct pp_instance * ppi);
 	int (*handle_presp) (struct pp_instance * ppi);
 	int (*pack_announce)(struct pp_instance *ppi);
@@ -394,6 +393,7 @@ extern int msg_issue_pdelay_resp_followup(struct pp_instance *ppi,
 extern int msg_issue_pdelay_resp(struct pp_instance *ppi, struct pp_time *time);
 /* Functions for time math */
+extern void normalize_pp_time(struct pp_time *t);
 extern void pp_time_add(struct pp_time *t1, struct pp_time *t2);
 extern void pp_time_sub(struct pp_time *t1, struct pp_time *t2);
 extern void pp_time_div2(struct pp_time *t);

--- a/proto-ext-whiterabbit/hooks.c
+++ b/proto-ext-whiterabbit/hooks.c
@@ -71,18 +71,9 @@ static int wr_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
 	return 0;
 }
-static int wr_handle_preq(struct pp_instance *ppi)
-{
-	/* FIXME: why do we save this fractional part? */
-	clear_time(&ppi->cField);
-	ppi->cField.scaled_nsecs = ppi->last_rcv_time.scaled_nsecs & 0xffff;
-	return 0;
-}
 static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
 			 int msgtype)
 {
-	MsgHeader *hdr = &ppi->received_ptp_header;
 	MsgSignaling wrsig_msg;
 	struct pp_time *time = &ppi->last_rcv_time;
@@ -93,16 +84,12 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
 	/* This case is modified from the default one */
 	case PPM_DELAY_REQ:
-		/* there is no cField now, we pp_time includes it all */
-		clear_time(&hdr->cField);
 		msg_issue_delay_resp(ppi, time); /* no error check */
 		msgtype = PPM_NO_MESSAGE;
 		break;
 	case PPM_PDELAY_REQ:
-		if (CONFIG_HAS_P2P)
+		/* nothing to do */
-			wr_handle_preq(ppi);
-		/* normal management continues  */
 		break;
 	/* This is missing in the standard protocol */
@@ -202,8 +189,7 @@ static int wr_handle_announce(struct pp_instance *ppi)
 }
 static int wr_handle_followup(struct pp_instance *ppi,
-			      struct pp_time *precise_orig_timestamp,
+			      struct pp_time *precise_orig_timestamp)
-			      struct pp_time *correction_field)
 {
 	pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
 	if (!WR_DSPOR(ppi)->wrModeOn)
@@ -285,7 +271,6 @@ struct pp_ext_hooks pp_hooks = {
 	.handle_announce = wr_handle_announce,
 	.handle_followup = wr_handle_followup,
 #if CONFIG_HAS_P2P
-	.handle_preq = wr_handle_preq,
 	.handle_presp = wr_handle_presp,
 #endif
 	.pack_announce = wr_pack_announce,

--- a/proto-standard/arith.c
+++ b/proto-standard/arith.c
@@ -9,7 +9,7 @@
 #include <limits.h>
 #include <ppsi/ppsi.h>
-static void normalize_pp_time(struct pp_time *t)
+void normalize_pp_time(struct pp_time *t)
 {
 	/* no 64b division please, we'll rather loop a few times */
 	#define SNS_PER_S ((1000LL * 1000 * 1000) << 16)

--- a/proto-standard/common-fun.c
+++ b/proto-standard/common-fun.c
@@ -107,19 +107,22 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
 	if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT))
 		return 0;
-	/* t2 may be overriden by follow-up, cField is always valid */
+	/* t2 may be overriden by follow-up, save it immediately */
 	ppi->t2 = ppi->last_rcv_time;
-	/* FIXME: merge cField here? */
+	msg_unpack_sync(buf, &sync);
-	ppi->cField = hdr->cField;
 	if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
 		ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
 		ppi->recv_sync_sequence_id = hdr->sequenceId;
+		/* for two-step, the stamp comes later */
+		ppi->t1 = hdr->cField; /* most likely 0 */
 		return 0;
 	}
-	msg_unpack_sync(buf, &sync);
+	/* one-step folllows */
 	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
 	ppi->t1 = sync.originTimestamp;
+	pp_time_add(&ppi->t1, &hdr->cField);
+	ppi->syncCF = 0;
 	if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
 		pp_servo_got_psync(ppi);
 	else
@@ -146,6 +149,8 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
 	    (ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
 		ppi->t4 = resp.requestReceiptTimestamp;
+		pp_time_add(&ppi->t4, &hdr->cField);
+		/* WARNING: should be "sub" (see README-cfield::BUG)  */
 		ppi->t6 = ppi->last_rcv_time;
 		if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0)
 			ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
@@ -191,6 +196,7 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
 	    (ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
 		ppi->t5 = respFllw.responseOriginTimestamp;
+		pp_time_add(&ppi->t5, &hdr->cField);
 		if (pp_hooks.handle_presp)
 			e = pp_hooks.handle_presp(ppi);
@@ -249,14 +255,14 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
 	msg_unpack_follow_up(buf, &follow);
 	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
-	ppi->t1 = follow.preciseOriginTimestamp;
+	/* t1 for calculations is T1 + Csyn + Cful -- see README-cfield */
+	pp_time_add(&ppi->t1, &follow.preciseOriginTimestamp);
-	/* Add correctionField in follow-up to sync correctionField, see 11.2 */
+	pp_time_add(&ppi->t1, &hdr->cField);
-	pp_time_add(&ppi->cField, &hdr->cField); /* FIXME: check this cField */
+	ppi->syncCF = hdr->cField.scaled_nsecs; /* for diag about TC */
 	/* Call the extension; it may do it all and ask to return */
 	if (pp_hooks.handle_followup)
-		ret = pp_hooks.handle_followup(ppi, &ppi->t1, &ppi->cField);
+		ret = pp_hooks.handle_followup(ppi, &ppi->t1);
 	if (ret == 1)
 		return 0;
 	if (ret < 0)

--- a/proto-standard/msg.c
+++ b/proto-standard/msg.c
@@ -127,6 +127,7 @@ void msg_unpack_sync(void *buf, MsgSync *sync)
 	secs |= htonl(*(UInteger32 *) (buf + 36));
 	nsecs = htonl(*(UInteger32 *) (buf + 40));
+	/* The cField is added in the caller according to 1-step vs. 2-step */
 	sync->originTimestamp.secs = secs;
 	sync->originTimestamp.scaled_nsecs = nsecs << 16;
 }
@@ -231,6 +232,9 @@ static int msg_pack_follow_up(struct pp_instance *ppi,
 	*(UInteger16 *)(buf + 34) = htons(prec_orig_tstamp->secs >> 32);
 	*(UInteger32 *)(buf + 36) = htonl(prec_orig_tstamp->secs);
 	*(UInteger32 *)(buf + 40) = htonl(prec_orig_tstamp->scaled_nsecs >> 16);
+	/* Fractional part in cField */
+	*(UInteger32 *)(buf + 12) =
+		htonl(prec_orig_tstamp->scaled_nsecs & 0xffff);
 	return len;
 }
@@ -244,9 +248,13 @@ static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
 	/* Header */
 	*(UInteger8 *) (buf + 4) = hdr->domainNumber; /* FIXME: why? */
-	/* copy the correction field, 11.4.3 c.3) */
+	/* We should copy the correction field and add our fractional part */
+	hdr->cField.scaled_nsecs
+		+= htonl(prec_orig_tstamp->scaled_nsecs & 0xffff);
+	normalize_pp_time(&hdr->cField);
 	*(Integer32 *) (buf + 8) = htonl(hdr->cField.scaled_nsecs >> 32);
 	*(Integer32 *) (buf + 12) = htonl((int)hdr->cField.scaled_nsecs);
 	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
 	/* requestReceiptTimestamp */
@@ -271,6 +279,7 @@ void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
 	secs |= htonl(*(UInteger32 *) (buf + 36));
 	nsecs = htonl(*(UInteger32 *) (buf + 40));
+	/* cField add1ed by the caller, from already-converted header */
 	flwup->preciseOriginTimestamp.secs = secs;
 	flwup->preciseOriginTimestamp.scaled_nsecs = nsecs << 16;
 }
@@ -279,25 +288,18 @@ void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
 void msg_unpack_pdelay_resp_follow_up(void *buf,
 				      MsgPDelayRespFollowUp * pdelay_resp_flwup)
 {
-	int64_t secs, nsecs, cf;
+	int64_t secs, nsecs;
 	secs = htons(*(UInteger16 *) (buf + 34));
 	secs <<= 32;
 	secs |= htonl(*(UInteger32 *) (buf + 36));
 	nsecs = htonl(*(UInteger32 *) (buf + 40));
+	/* cField added by the caller, as it's already converted */
 	pdelay_resp_flwup->responseOriginTimestamp.secs = secs;
 	pdelay_resp_flwup->responseOriginTimestamp.scaled_nsecs =
 		nsecs << 16;
-	/* add correction factor, already_scaled */
-	cf = ntohl(*(UInteger32 *) (buf + 8));
-	cf <<= 32;
-	cf |= ntohl(*(UInteger32 *) (buf + 12));
-	pdelay_resp_flwup->responseOriginTimestamp += cf;
-	/* FIXME: normalize? */
 	memcpy(&pdelay_resp_flwup->requestingPortIdentity.clockIdentity,
 	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
 	pdelay_resp_flwup->requestingPortIdentity.portNumber =
@@ -306,7 +308,7 @@ void msg_unpack_pdelay_resp_follow_up(void *buf,
 /* pack DelayReq message into out buffer of ppi */
 static int msg_pack_delay_req(struct pp_instance *ppi,
-			       struct pp_time *orig_tstamp)
+			       struct pp_time *now)
 {
 	void *buf = ppi->tx_ptp;
 	int len = __msg_pack_header(ppi, PPM_DELAY_REQ);
@@ -315,16 +317,16 @@ static int msg_pack_delay_req(struct pp_instance *ppi,
 	/* Header */
 	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_DELAY_REQ]);
-	/* Delay_req message */
+	/* Delay_req message - we may send zero instead */
-	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
+	*(UInteger16 *) (buf + 34) = htons(now->secs >> 32);
-	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
+	*(UInteger32 *) (buf + 36) = htonl(now->secs);
-	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
+	*(UInteger32 *) (buf + 40) = htonl(now->scaled_nsecs >> 16);
 	return len;
 }
 /* pack DelayReq message into out buffer of ppi */
 static int msg_pack_pdelay_req(struct pp_instance *ppi,
-				struct pp_time *orig_tstamp)
+				struct pp_time *now)
 {
 	void *buf = ppi->tx_ptp;
 	int len = __msg_pack_header(ppi, PPM_PDELAY_REQ);
@@ -333,10 +335,10 @@ static int msg_pack_pdelay_req(struct pp_instance *ppi,
 	/* Header */
 	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_PDELAY_REQ]);
-	/* PDelay_req message */
+	/* PDelay_req message - we may send zero instead */
-	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
+	*(UInteger16 *) (buf + 34) = htons(now->secs >> 32);
-	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
+	*(UInteger32 *) (buf + 36) = htonl(now->secs);
-	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
+	*(UInteger32 *) (buf + 40) = htonl(now->scaled_nsecs >> 16);
 	memset(buf + 44, 0, 10); /* reserved to match pdelay_resp length */
 	return len;
 }
@@ -353,6 +355,10 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi,
 	flags8[0] = PP_TWO_STEP_FLAG; /* Table 20) */
 	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
+	/* cField: shdould be the fractional negated (see README-cfield) */
+	*(UInteger32 *) (buf + 12)
+		= htonl(rcv_tstamp->scaled_nsecs & 0xffff);
 	/* requestReceiptTimestamp */
 	*(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secs >> 32);
 	*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secs);
@@ -373,7 +379,10 @@ static int msg_pack_delay_resp(struct pp_instance *ppi,
 	int len = __msg_pack_header(ppi, PPM_DELAY_RESP);
 	/* Header */
-	/* Copy correctionField of delayReqMessage */
+	/*
+	 * We should copy the cField of the request, and then subract
+	 * our fractional part. However, we add it (see README-cfield::BUG)
+	 */
 	*(Integer32 *) (buf + 8) = 0;
 	*(Integer32 *) (buf + 12) = htonl(rcv_tstamp->scaled_nsecs & 0xffff);
 	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
@@ -421,23 +430,17 @@ void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req)
 /* Unpack delayResp message from IN buffer of ppi to internal structure */
 void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
 {
-	int64_t secs, nsecs, cf;
+	int64_t secs, nsecs;
 	secs = htons(*(UInteger16 *) (buf + 34));
 	secs <<= 32;
 	secs |= htonl(*(UInteger32 *) (buf + 36));
 	nsecs = htonl(*(UInteger32 *) (buf + 40));
+	/* cfield added in the caller */
 	resp->receiveTimestamp.secs = secs;
 	resp->receiveTimestamp.scaled_nsecs = nsecs << 16;
-	/* add correction factor, already_scaled */
-	cf = ntohl(*(UInteger32 *) (buf + 8));
-	cf <<= 32;
-	cf |= ntohl(*(UInteger32 *) (buf + 12));
-	resp->receiveTimestamp.scaled_nsecs += cf;
-	/* FIXME: normalize? */
 	memcpy(&resp->requestingPortIdentity.clockIdentity,
 	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
 	resp->requestingPortIdentity.portNumber =
@@ -447,23 +450,17 @@ void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
 /* Unpack PDelayResp message from IN buffer of ppi to internal structure */
 void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp)
 {
-	int64_t secs, nsecs, cf;
+	int64_t secs, nsecs;
 	secs = ntohs(*(UInteger16 *) (buf + 34));
 	secs <<= 32;
 	secs |= ntohl(*(UInteger32 *) (buf + 36));
 	nsecs = ntohl(*(UInteger32 *) (buf + 40));
+	/* cfield added in the caller */
 	presp->requestReceiptTimestamp.secs = secs;
 	presp->requestReceiptTimestamp.scaled_nsecs = nsecs << 16;
-	/* add correction factor, already_scaled */
-	cf = ntohl(*(UInteger32 *) (buf + 8));
-	cf <<= 32;
-	cf |= ntohl(*(UInteger32 *) (buf + 12));
-	presp->requestReceiptTimestamp.scaled_nsecs += cf;
-	/* FIXME: normalize? */
 	memcpy(&presp->requestingPortIdentity.clockIdentity,
 	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
 	presp->requestingPortIdentity.portNumber =

--- a/proto-standard/servo.c
+++ b/proto-standard/servo.c
@@ -61,16 +61,11 @@ void pp_servo_got_sync(struct pp_instance *ppi)
 	struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
 	/*
-	 * calc 'master_to_slave_delay', removing the correction field
+	 * calc 'master_to_slave_delay'; no correction field
-	 * added by transparent clocks in the path.
+	 * appears in the formulas because it's already merged with t1
-	 * cField contains the sum of sync and followup messages
-	 * correctionField(s)
 	 */
 	*m_to_s_dly = ppi->t2;
 	pp_time_sub(m_to_s_dly, &ppi->t1);
-	pp_time_sub(m_to_s_dly, &ppi->cField); /* FIXME: check cField */
-	pp_diag(ppi, servo, 3, "correction field 1: %s\n",
-		fmt_ppt(&ppi->cField));
 }
 /* Called by slave and uncalib when we have t1 and t2 */
@@ -85,14 +80,11 @@ void pp_servo_got_psync(struct pp_instance *ppi)
 	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
 	/*
-	 * calc 'master_to_slave_delay', removing the correction field
+	 * calc 'master_to_slave_delay'; no correction field
-	 * added by transparent clocks in the path.
+	 * appears in the formulas because it's already merged with t1
 	 */
 	*m_to_s_dly = ppi->t2;
 	pp_time_sub(m_to_s_dly, &ppi->t1);
-	pp_time_sub(m_to_s_dly, &ppi->cField); /* FIXME: check cField */
-	pp_diag(ppi, servo, 3, "correction field 1: %s\n",
-		fmt_ppt(&ppi->cField));
 	/* update 'offsetFromMaster' and possibly jump in time */
 	if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
@@ -136,9 +128,6 @@ void pp_servo_got_resp(struct pp_instance *ppi)
 	 */
 	*s_to_m_dly = ppi->t4;
 	pp_time_sub(s_to_m_dly, &ppi->t3);
-	pp_time_sub(s_to_m_dly, &ppi->cField); /* FIXME: cField check */
-	pp_diag(ppi, servo, 3, "correction field 2: %s\n",
-		fmt_ppt(&ppi->cField));
 	pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
 	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
@@ -193,9 +182,6 @@ void pp_servo_got_presp(struct pp_instance *ppi)
 	 */
 	*s_to_m_dly = ppi->t6;
 	pp_time_sub(s_to_m_dly, &ppi->t5);
-	pp_time_sub(s_to_m_dly,  &ppi->cField); /* FIXME: check cField */
-	pp_diag(ppi, servo, 3, "correction field 2: %s\n",
-		fmt_ppt(&ppi->cField));
 	*m_to_s_dly = ppi->t4;
 	pp_time_sub(m_to_s_dly, &ppi->t3);

--- a/proto-standard/state-slave.c
+++ b/proto-standard/state-slave.c
@@ -52,6 +52,8 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
 	}
 	ppi->t4 = resp.receiveTimestamp;
+	pp_time_add(&ppi->t4, &hdr->cField);
+	/* WARNING: should be "sub" (see README-cfield::BUG)  */
 	if (pp_hooks.handle_resp)
 		e = pp_hooks.handle_resp(ppi);