This commit only focuses on time transfer and not BMCA changes.
Signed-off-by: Rodney Greenstreet
[email protected]<mailto:[email protected]>
---
clock.c | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
clock.h | 11 ++++++++
foreign.h | 2 +-
port.c | 92 ++++++++++++++++++++++++++++++++++++++++++---------------------
tmv.h | 16 +++++++++++
5 files changed, 171 insertions(+), 32 deletions(-)
diff --git a/clock.c b/clock.c
index b6afba9..0b8fb56 100644
--- a/clock.c
+++ b/clock.c
@@ -67,6 +67,12 @@ struct clock_stats {
unsigned int max_count;
};
+struct clock_port_sync_info {
+ tmv_t upstream_tx_time;
+ tmv_t origin_ts;
+ tmv_t fup_correction;
+};
+
struct clock_subscriber {
LIST_ENTRY(clock_subscriber) list;
uint8_t events[EVENT_BITMASK_CNT];
@@ -116,6 +122,7 @@ struct clock {
struct tsproc *tsproc;
struct freq_estimator fest;
struct time_status_np status;
+ struct clock_port_sync_info port_sync;
double nrr;
struct clock_description desc;
struct clock_stats stats;
@@ -1097,6 +1104,10 @@ struct clock *clock_create(enum clock_type type, struct
config *config,
pr_err("Failed to create time stamp processor");
return NULL;
}
+
+ c->port_sync.upstream_tx_time = tmv_zero();
+ c->port_sync.origin_ts = tmv_zero();
+ c->port_sync.fup_correction = tmv_zero();
c->nrr = 1.0;
c->stats_interval = config_get_int(config, NULL,
"summary_interval");
c->stats.offset = stats_create();
@@ -1215,6 +1226,58 @@ void clock_follow_up_info(struct clock *c, struct
follow_up_info_tlv *f)
c->status.gmTimeBaseIndicator = f->gmTimeBaseIndicator;
memcpy(&c->status.lastGmPhaseChange, &f->lastGmPhaseChange,
sizeof(c->status.lastGmPhaseChange));
+ pr_debug("clock_follow_up_info()");
+ pr_debug(" cumulativeScaledRateOffset: %d",
f->cumulativeScaledRateOffset);
+}
+
+void clock_get_follow_up_info(struct clock *c, struct follow_up_info_tlv *f)
+{
+ f->cumulativeScaledRateOffset = (Integer32) (
+
(clock_to_gm_rate_ratio(c) - 1.0) * POW2_41);
+ f->scaledLastGmPhaseChange = c->status.scaledLastGmPhaseChange;
+ f->gmTimeBaseIndicator = c->status.gmTimeBaseIndicator;
+ f->lastGmPhaseChange = c->status.lastGmPhaseChange;
+
+ pr_debug("clock_get_follow_up_info()");
+ pr_debug(" cumulativeScaledRateOffset: %d",
f->cumulativeScaledRateOffset);
+}
+
+void clock_port_sync(struct clock *c, tmv_t ingress_ts, tmv_t origin_ts,
+ tmv_t correction)
+{
+ c->port_sync.upstream_tx_time = tmv_sub(ingress_ts,
c->path_delay);
+ c->port_sync.origin_ts = origin_ts;
+ c->port_sync.fup_correction = correction;
+
+ pr_debug("clock_port_sync()");
+ pr_debug(" upstream_tx_time: %"PRIu64,
tmv_to_nanoseconds(c->port_sync.upstream_tx_time));
+ pr_debug(" origin_ts: %"PRIu64,
tmv_to_nanoseconds(c->port_sync.origin_ts));
+ pr_debug(" fup_correction: %"PRIu64,
tmv_to_nanoseconds(c->port_sync.fup_correction));
+}
+
+tmv_t clock_sync_origin_ts(struct clock *c)
+{
+ return c->port_sync.origin_ts;
+}
+
+tmv_t clock_sync_upstream_tx_time(struct clock *c)
+{
+ return c->port_sync.upstream_tx_time;
+}
+
+tmv_t clock_sync_fup_correction(struct clock *c)
+{
+ return c->port_sync.fup_correction;
+}
+
+double clock_to_gm_rate_ratio(struct clock *c)
+{
+ pr_debug("clock_to_gm_rate_ratio()");
+ pr_debug(" nrr: %0.9lf", c->nrr);
+ pr_debug(" cumulativeScaledRateOffset: %d",
c->status.cumulativeScaledRateOffset);
+ pr_debug(" cumulativeScaledRateOffset / 2^41: %0.9lf", ((double)
(c->status.cumulativeScaledRateOffset)) / POW2_41);
+ pr_debug(" return value: %0.9lf", ((double)
(c->status.cumulativeScaledRateOffset)) / POW2_41 + c->nrr);
+ return ((double) (c->status.cumulativeScaledRateOffset)) /
POW2_41 + c->nrr;
}
int clock_gm_capable(struct clock *c)
@@ -1222,6 +1285,15 @@ int clock_gm_capable(struct clock *c)
return c->grand_master_capable;
}
+int clock_is_gm(struct clock *c)
+{
+ pr_debug("clock_is_gm()");
+ pr_debug(" this clock: %s", cid2str(&c->dds.clockIdentity));
+ pr_debug(" best clock: %s", cid2str(&c->best_id));
+ pr_debug(" cid_eq: %d", cid_eq(&c->dds.clockIdentity,
&c->best_id));
+ return cid_eq(&c->dds.clockIdentity, &c->best_id);
+}
+
struct ClockIdentity clock_identity(struct clock *c)
{
return c->dds.clockIdentity;
@@ -1742,6 +1814,16 @@ static void handle_state_decision_event(struct clock *c)
tsproc_reset(c->tsproc, 1);
c->ingress_ts = tmv_zero();
c->path_delay = 0;
+ c->status.cumulativeScaledRateOffset = 0;
+ c->status.scaledLastGmPhaseChange = 0;
+ c->status.gmTimeBaseIndicator = 0;
+ c->status.lastGmPhaseChange.nanoseconds_msb = 0;
+ c->status.lastGmPhaseChange.nanoseconds_lsb = 0;
+
c->status.lastGmPhaseChange.fractional_nanoseconds = 0;
+ c->status.scaledLastGmPhaseChange = 0;
+ c->port_sync.upstream_tx_time = tmv_zero();
+ c->port_sync.origin_ts = tmv_zero();
+ c->port_sync.fup_correction = tmv_zero();
c->nrr = 1.0;
fresh_best = 1;
}
diff --git a/clock.h b/clock.h
index fcd9328..c127ed7 100644
--- a/clock.h
+++ b/clock.h
@@ -118,6 +118,15 @@ struct port *clock_first_port(struct clock *c);
* @param f Pointer to the TLV.
*/
void clock_follow_up_info(struct clock *c, struct follow_up_info_tlv *f);
+void clock_get_follow_up_info(struct clock *c, struct follow_up_info_tlv *f);
+
+void clock_port_sync(struct clock *c, tmv_t ingress_ts, tmv_t origin_ts,
+ tmv_t correction);
+
+tmv_t clock_sync_origin_ts(struct clock *c);
+tmv_t clock_sync_upstream_tx_time(struct clock *c);
+tmv_t clock_sync_fup_correction(struct clock *c);
+double clock_to_gm_rate_ratio(struct clock *c);
/**
* Obtain the gmCapable flag from a clock's default data set.
@@ -127,6 +136,8 @@ void clock_follow_up_info(struct clock *c, struct
follow_up_info_tlv *f);
*/
int clock_gm_capable(struct clock *c);
+int clock_is_gm(struct clock *c);
+
/**
* Obtain a clock's identity from its default data set.
* @param c The clock instance.
diff --git a/foreign.h b/foreign.h
index db24b84..0fdc8e3 100644
--- a/foreign.h
+++ b/foreign.h
@@ -54,7 +54,7 @@ struct foreign_clock {
/**
* Contains the information from the latest announce message
- * in a form suitable for comparision in the BMCA.
+ * in a form suitable for comparison in the BMCA.
*/
struct dataset dataset;
};
diff --git a/port.c b/port.c
index ec02825..aeddfbf 100644
--- a/port.c
+++ b/port.c
@@ -23,6 +23,7 @@
#include <string.h>
#include <unistd.h>
#include <sys/queue.h>
+#include <math.h>
#include "bmc.h"
#include "clock.h"
@@ -430,6 +431,7 @@ static int follow_up_info_append(struct port *p, struct
ptp_message *m)
memcpy(fui->id, ieee8021_id, sizeof(ieee8021_id));
fui->subtype[2] = 1;
m->tlv_count = 1;
+ clock_get_follow_up_info(p->clock, fui);
return sizeof(*fui);
}
@@ -1026,23 +1028,18 @@ static void port_slave_priority_warning(struct port *p)
pr_warning("port %hu: defaultDS.priority1 probably
misconfigured", n);
}
-static void port_synchronize(struct port *p,
- struct timespec ingress_ts,
- struct timestamp origin_ts,
- Integer64 correction1,
Integer64 correction2)
+static void port_synchronize(struct port *p, tmv_t ingress_ts, tmv_t origin_ts,
+ tmv_t sync_correction, tmv_t
fup_correction)
{
enum servo_state state;
- tmv_t t1, t1c, t2, c1, c2;
+ tmv_t origin_ts_c;
port_set_sync_rx_tmo(p);
- t1 = timestamp_to_tmv(origin_ts);
- t2 = timespec_to_tmv(ingress_ts);
- c1 = correction_to_tmv(correction1);
- c2 = correction_to_tmv(correction2);
- t1c = tmv_add(t1, tmv_add(c1, c2));
+ origin_ts_c = tmv_add(origin_ts,
+ tmv_add(sync_correction,
fup_correction));
- state = clock_synchronize(p->clock, t2, t1c);
+ state = clock_synchronize(p->clock, ingress_ts, origin_ts_c);
switch (state) {
case SERVO_UNLOCKED:
port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0);
@@ -1074,6 +1071,7 @@ static void port_syfufsm(struct port *p, enum syfu_event
event,
struct ptp_message *m)
{
struct ptp_message *syn, *fup;
+ Boolean have_match = FALSE;
switch (p->syfu) {
case SF_EMPTY:
@@ -1112,11 +1110,8 @@ static void port_syfufsm(struct port *p, enum syfu_event
event,
break;
case FUP_MATCH:
syn = p->last_syncfup;
- port_synchronize(p,
syn->hwts.ts, m->ts.pdu,
-
syn->header.correction,
-
m->header.correction);
- msg_put(p->last_syncfup);
- p->syfu = SF_EMPTY;
+ fup = m;
+ have_match = TRUE;
break;
}
break;
@@ -1130,12 +1125,9 @@ static void port_syfufsm(struct port *p, enum syfu_event
event,
p->syfu = SF_HAVE_SYNC;
break;
case SYNC_MATCH:
+ syn = m;
fup = p->last_syncfup;
- port_synchronize(p, m->hwts.ts,
fup->ts.pdu,
-
m->header.correction,
-
fup->header.correction);
- msg_put(p->last_syncfup);
- p->syfu = SF_EMPTY;
+ have_match = TRUE;
break;
case FUP_MISMATCH:
msg_put(p->last_syncfup);
@@ -1147,6 +1139,33 @@ static void port_syfufsm(struct port *p, enum syfu_event
event,
}
break;
}
+
+ if (have_match == TRUE) {
+ tmv_t origin_ts, ingress_ts, sync_correction,
fup_correction;
+
+ origin_ts = timestamp_to_tmv(fup->ts.pdu);
+ ingress_ts = timespec_to_tmv(syn->hwts.ts);
+ sync_correction =
correction_to_tmv(syn->header.correction);
+ fup_correction =
correction_to_tmv(fup->header.correction);
+
+ if (p->follow_up_info) {
+ struct follow_up_info_tlv *fui =
+
follow_up_info_extract(fup);
+ if (!fui)
+ return;
+ clock_follow_up_info(p->clock,
fui);
+ }
+
+ if (port_is_ieee8021as(p)) {
+ clock_port_sync(p->clock,
ingress_ts, origin_ts,
+
fup_correction);
+ }
+
+ port_synchronize(p, ingress_ts, origin_ts,
sync_correction,
+ fup_correction);
+ msg_put(p->last_syncfup);
+ p->syfu = SF_EMPTY;
+ }
}
static int port_pdelay_request(struct port *p)
@@ -1370,7 +1389,23 @@ static int port_tx_sync(struct port *p)
fup->header.control = CTL_FOLLOW_UP;
fup->header.logMessageInterval = p->logSyncInterval;
- ts_to_timestamp(&msg->hwts.ts,
&fup->follow_up.preciseOriginTimestamp);
+ if (port_is_ieee8021as(p) && !clock_is_gm(p->clock)) {
+ tmv_t fup_correction;
+
+ fup->follow_up.preciseOriginTimestamp =
tmv_to_Timestamp(
+
clock_sync_origin_ts(p->clock));
+ fup_correction = timespec_to_tmv(msg->hwts.ts) -
+
clock_sync_upstream_tx_time(p->clock);
+ fup_correction =
dbl_tmv(round(tmv_dbl(fup_correction) *
+
clock_to_gm_rate_ratio(p->clock)));
+ fup_correction +=
clock_sync_fup_correction(p->clock);
+ fup->header.correction =
tmv_to_correction(fup_correction);
+
+ pr_debug("port_tx_sync()");
+ pr_debug(" fup_correction: %"PRIu64,
fup_correction);
+ } else{
+ ts_to_timestamp(&msg->hwts.ts,
&fup->follow_up.preciseOriginTimestamp);
+ }
err = port_prepare_and_send(p, fup, 0);
if (err)
@@ -1727,13 +1762,6 @@ static void process_follow_up(struct port *p, struct
ptp_message *m)
if (memcmp(&master, &m->header.sourcePortIdentity,
sizeof(master)))
return;
- if (p->follow_up_info) {
- struct follow_up_info_tlv *fui =
follow_up_info_extract(m);
- if (!fui)
- return;
- clock_follow_up_info(p->clock, fui);
- }
-
if (p->syfu == SF_HAVE_SYNC &&
p->last_syncfup->header.sequenceId == m->header.sequenceId) {
event = FUP_MATCH;
@@ -2002,8 +2030,10 @@ static void process_sync(struct port *p, struct
ptp_message *m)
m->header.correction += p->asymmetry;
if (one_step(m)) {
- port_synchronize(p, m->hwts.ts, m->ts.pdu,
-
m->header.correction, 0);
+ port_synchronize(p, timespec_to_tmv(m->hwts.ts),
+
timestamp_to_tmv(m->ts.pdu),
+
correction_to_tmv(m->header.correction),
+ tmv_zero());
flush_last_sync(p);
return;
}
diff --git a/tmv.h b/tmv.h
index 30b41ee..ca8a5ea 100644
--- a/tmv.h
+++ b/tmv.h
@@ -76,6 +76,11 @@ static inline tmv_t correction_to_tmv(Integer64 c)
return c >> 16;
}
+static inline Integer64 tmv_to_correction(tmv_t c)
+{
+ return c << 16;
+}
+
static inline double tmv_dbl(tmv_t x)
{
return (double) x;
@@ -106,4 +111,15 @@ static inline tmv_t timestamp_to_tmv(struct timestamp ts)
return ts.sec * NS_PER_SEC + ts.nsec;
}
+static inline struct Timestamp tmv_to_Timestamp(tmv_t x)
+{
+ struct Timestamp ts;
+
+ ts.seconds_msb = 0;
+ ts.seconds_lsb = x / NS_PER_SEC;
+ ts.nanoseconds = x % NS_PER_SEC;
+
+ return ts;
+}
+
#endif
--
2.7.4
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
