Commit 1357b4a4 authored by Alessandro Rubini's avatar Alessandro Rubini Committed by Adam Wujek

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's avatarAlessandro Rubini <rubini@gnudd.com>
parent 9b94f288
......@@ -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 */
......
......@@ -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 */
struct pp_time cField; /* transp. clocks */
uint64_t syncCF; /* transp. clocks */
struct pp_time last_rcv_time, last_snt_time; /* two temporaries */
/* Page 85: each port shall maintain an implementation-specific
......
......@@ -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,
struct pp_time *correction_field);
int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig);
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);
......
......@@ -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)
wr_handle_preq(ppi);
/* normal management continues */
/* nothing to do */
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 *correction_field)
struct pp_time *precise_orig_timestamp)
{
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,
......
......@@ -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)
......
......@@ -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? */
ppi->cField = hdr->cField;
msg_unpack_sync(buf, &sync);
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;
/* Add correctionField in follow-up to sync correctionField, see 11.2 */
pp_time_add(&ppi->cField, &hdr->cField); /* FIXME: check this cField */
/* t1 for calculations is T1 + Csyn + Cful -- see README-cfield */
pp_time_add(&ppi->t1, &follow.preciseOriginTimestamp);
pp_time_add(&ppi->t1, &hdr->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)
......
......@@ -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 */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
/* Delay_req message - we may send zero instead */
*(UInteger16 *) (buf + 34) = htons(now->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(now->secs);
*(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 */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
/* PDelay_req message - we may send zero instead */
*(UInteger16 *) (buf + 34) = htons(now->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(now->secs);
*(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 =
......
......@@ -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
* added by transparent clocks in the path.
* cField contains the sum of sync and followup messages
* correctionField(s)
* calc 'master_to_slave_delay'; no correction field
* 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));
}
/* 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
* added by transparent clocks in the path.
* calc 'master_to_slave_delay'; no correction field
* 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);
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment