diff --git a/clock.c b/clock.c index a66d189..667cb79 100644 --- a/clock.c +++ b/clock.c @@ -46,7 +46,6 @@ #include "util.h" #define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer */ -#define POW2_41 ((double)(1ULL << 41)) struct interface { STAILQ_ENTRY(interface) list; @@ -130,6 +129,7 @@ struct clock { int stats_interval; struct clockcheck *sanity_check; struct interface *udsif; + struct syfu_relay_info syfu_relay; LIST_HEAD(clock_subscribers_head, clock_subscriber) subscribers; struct monitor *slave_event_monitor; }; @@ -1161,6 +1161,8 @@ struct clock *clock_create(enum clock_type type, struct config *config, } } + memset(&c->syfu_relay, 0, sizeof(struct syfu_relay_info)); + /* Initialize the parentDS. */ clock_update_grandmaster(c); c->dad.pds.parentStats = 0; @@ -1258,6 +1260,15 @@ void clock_follow_up_info(struct clock *c, struct follow_up_info_tlv *f) sizeof(c->status.lastGmPhaseChange)); } +static void clock_get_follow_up_info(struct clock *c, struct follow_up_info_tlv *f) +{ + f->cumulativeScaledRateOffset = c->status.cumulativeScaledRateOffset; + f->scaledLastGmPhaseChange = c->status.scaledLastGmPhaseChange; + f->gmTimeBaseIndicator = c->status.gmTimeBaseIndicator; + memcpy(&f->lastGmPhaseChange, &c->status.lastGmPhaseChange, + sizeof(f->lastGmPhaseChange)); +} + int clock_free_running(struct clock *c) { return c->free_running ? 1 : 0; @@ -1645,6 +1656,16 @@ struct monitor *clock_slave_monitor(struct clock *c) return c->slave_event_monitor; } +tmv_t clock_get_path_delay(struct clock *c) +{ + return c->path_delay; +} + +double clock_get_nrr(struct clock *c) +{ + return c->nrr; +} + int clock_slave_only(struct clock *c) { return c->dds.flags & DDS_SLAVE_ONLY; @@ -1867,6 +1888,7 @@ static void handle_state_decision_event(struct clock *c) c->master_local_rr = 1.0; c->nrr = 1.0; fresh_best = 1; + clock_disable_syfu_relay(c); } c->best = best; @@ -1938,3 +1960,22 @@ enum servo_state clock_servo_state(struct clock *c) { return c->servo_state; } + +void clock_prepare_syfu_relay(struct clock *c, struct ptp_message *sync, + struct ptp_message *fup) +{ + c->syfu_relay.precise_origin_ts = timestamp_to_tmv(fup->ts.pdu); + c->syfu_relay.correction = fup->header.correction; + clock_get_follow_up_info(c, &c->syfu_relay.fup_info_tlv); + c->syfu_relay.avail = 1; +} + +void clock_disable_syfu_relay(struct clock *c) +{ + c->syfu_relay.avail = 0; +} + +struct syfu_relay_info *clock_get_syfu_relay(struct clock *c) +{ + return &c->syfu_relay; +} diff --git a/clock.h b/clock.h index e7daf97..ddd1205 100644 --- a/clock.h +++ b/clock.h @@ -30,8 +30,18 @@ #include "tmv.h" #include "transport.h" +#define POW2_41 ((double)(1ULL << 41)) + struct ptp_message; /*forward declaration*/ +struct syfu_relay_info { + tmv_t precise_origin_ts; + Integer64 correction; + struct follow_up_info_tlv fup_info_tlv; + /* Auxiliary info */ + int avail; +}; + /** Opaque type. */ struct clock; @@ -240,6 +250,20 @@ void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx); void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx, double nrr); +/** + * Get the path delay as measured on a slave port. + * @param c The clock instance. + * @return The path delay as measured on a slave port. + */ +tmv_t clock_get_path_delay(struct clock *c); + +/** + * Get the neighbor rate ratio as measured on a slave port. + * @param c The clock instance. + * @return The neighbor rate ratio as measured on a slave port. + */ +double clock_get_nrr(struct clock *c); + /** * Set clock sde * @param c A pointer to a clock instance obtained with clock_create(). @@ -367,4 +391,25 @@ void clock_check_ts(struct clock *c, uint64_t ts); */ double clock_rate_ratio(struct clock *c); +/** + * Prepare sync/follow_up relay. + * @param c The clock instance. + * @param sync The sync message. + * @param fup The follow_up message. + */ +void clock_prepare_syfu_relay(struct clock *c, struct ptp_message *sync, + struct ptp_message *fup); + +/** + * Disable sync/follow_up relay. + * @param c The clock instance. + */ +void clock_disable_syfu_relay(struct clock *c); + +/** + * Get sync/follow_up relay information. + * @param c The clock instance. + * @return The sync/follow_up relay information. + */ +struct syfu_relay_info *clock_get_syfu_relay(struct clock *c); #endif diff --git a/port.c b/port.c index fa49663..277db0a 100644 --- a/port.c +++ b/port.c @@ -1282,6 +1282,10 @@ static void port_syfufsm(struct port *p, enum syfu_event event, break; case FUP_MATCH: syn = p->last_syncfup; + if (port_is_ieee8021as(p)) + clock_prepare_syfu_relay(p->clock, syn, m); + else + clock_disable_syfu_relay(p->clock); port_synchronize(p, syn->header.sequenceId, syn->hwts.ts, m->ts.pdu, syn->header.correction, @@ -1304,6 +1308,10 @@ static void port_syfufsm(struct port *p, enum syfu_event event, break; case SYNC_MATCH: fup = p->last_syncfup; + if (port_is_ieee8021as(p)) + clock_prepare_syfu_relay(p->clock, m, fup); + else + clock_disable_syfu_relay(p->clock); port_synchronize(p, fup->header.sequenceId, m->hwts.ts, fup->ts.pdu, m->header.correction, @@ -1495,6 +1503,60 @@ int port_tx_announce(struct port *p, struct address *dst) return err; } +static void port_syfu_relay_info_insert(struct port *p, + struct ptp_message *sync, + struct ptp_message *fup) +{ + struct syfu_relay_info *syfu_relay = clock_get_syfu_relay(p->clock); + struct follow_up_info_tlv *fui_relay = &syfu_relay->fup_info_tlv; + struct follow_up_info_tlv *fui = follow_up_info_extract(fup); + tmv_t ingress, egress, residence, path_delay; + double gm_rr, nrr, rr; + struct timestamp ts; + + if (syfu_relay->avail == 0) + return; + + fup->follow_up.preciseOriginTimestamp = + tmv_to_Timestamp(syfu_relay->precise_origin_ts); + fup->header.correction = syfu_relay->correction; + + /* Calculate residence time. */ + ingress = clock_ingress_time(p->clock); + egress = sync->hwts.ts; + residence = tmv_sub(egress, ingress); + rr = clock_rate_ratio(p->clock); + if (rr != 1.0) { + residence = dbl_tmv(tmv_dbl(residence) * rr); + } + + gm_rr = 1.0 + (fui_relay->cumulativeScaledRateOffset + 0.0) / POW2_41; + nrr = clock_get_nrr(p->clock); + + /* Add corrected residence time into correction. */ + fup->header.correction += tmv_to_TimeInterval(residence) * gm_rr * nrr; + + /* Add corrected path delay into correction. */ + path_delay = clock_get_path_delay(p->clock); + fup->header.correction += tmv_to_TimeInterval(path_delay) * gm_rr; + + /* Update follow_up TLV */ + gm_rr *= nrr; + fui->cumulativeScaledRateOffset = gm_rr * POW2_41 - POW2_41; + fui->scaledLastGmPhaseChange = fui_relay->scaledLastGmPhaseChange; + fui->gmTimeBaseIndicator = fui_relay->gmTimeBaseIndicator; + memcpy(&fui->lastGmPhaseChange, &fui_relay->lastGmPhaseChange, + sizeof(fui->lastGmPhaseChange)); + + ts.sec = fup->follow_up.preciseOriginTimestamp.seconds_msb; + ts.sec = ts.sec << 32 | fup->follow_up.preciseOriginTimestamp.seconds_lsb; + ts.nsec = fup->follow_up.preciseOriginTimestamp.nanoseconds; + pr_debug("port %hu: syfu_relay info:", portnum(p)); + pr_debug("port %hu: precise_origin_ts %" PRIu64 ".%u", portnum(p), ts.sec, ts.nsec); + pr_debug("port %hu: correction %" PRId64, portnum(p), fup->header.correction >> 16); + pr_debug("port %hu: fup_info %.9f", portnum(p), gm_rr); +} + int port_tx_sync(struct port *p, struct address *dst) { struct ptp_message *msg, *fup; @@ -1588,10 +1650,15 @@ int port_tx_sync(struct port *p, struct address *dst) fup->address = *dst; fup->header.flagField[0] |= UNICAST; } - if (p->follow_up_info && follow_up_info_append(fup)) { - pr_err("port %hu: append fup info failed", portnum(p)); - err = -1; - goto out; + + if (p->follow_up_info) { + if (follow_up_info_append(fup)) { + pr_err("port %hu: append fup info failed", portnum(p)); + err = -1; + goto out; + } + + port_syfu_relay_info_insert(p, msg, fup); } err = port_prepare_and_send(p, fup, TRANS_GENERAL);