diff --git a/clock.c b/clock.c index 30348dd..bec23c3 100644 --- a/clock.c +++ b/clock.c @@ -282,6 +282,7 @@ void clock_destroy(struct clock *c) } memset(c, 0, sizeof(*c)); msg_cleanup(); + tc_cleanup(); } static int clock_fault_timeout(struct port *port, int set) diff --git a/makefile b/makefile index d34eb6b..707c5d4 100644 --- a/makefile +++ b/makefile @@ -25,8 +25,8 @@ LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \ filter.o fsm.o hash.o linreg.o mave.o mmedian.o msg.o ntpshm.o nullf.o phc.o \ - pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o telecom.o tlv.o \ - transport.o tsproc.o udp.o udp6.o uds.o util.o version.o + pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o tc.o telecom.o \ + tlv.o transport.o tsproc.o udp.o udp6.o uds.o util.o version.o OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \ sysoff.o timemaster.o diff --git a/port.c b/port.c index d4352b0..55216ae 100644 --- a/port.c +++ b/port.c @@ -36,6 +36,7 @@ #include "print.h" #include "rtnl.h" #include "sk.h" +#include "tc.h" #include "tlv.h" #include "tmv.h" #include "tsproc.h" @@ -1534,6 +1535,7 @@ void port_disable(struct port *p) { int i; + tc_flush(p); flush_last_sync(p); flush_delay_req(p); flush_peer_delay(p); @@ -2777,6 +2779,7 @@ struct port *port_open(int phc_index, } memset(p, 0, sizeof(*p)); + TAILQ_INIT(&p->tc_transmitted); switch (type) { case CLOCK_TYPE_ORDINARY: diff --git a/port.h b/port.h index d828875..9639193 100644 --- a/port.h +++ b/port.h @@ -322,4 +322,9 @@ enum fault_type last_fault_type(struct port *port); void fault_interval(struct port *port, enum fault_type ft, struct fault_interval *i); +/** + * Release all of the memory in the TC transmit descriptor cache. + */ +void tc_cleanup(void); + #endif diff --git a/port_private.h b/port_private.h index df6e39e..76dcab2 100644 --- a/port_private.h +++ b/port_private.h @@ -50,6 +50,13 @@ struct nrate_estimator { int ratio_valid; }; +struct tc_txd { + TAILQ_ENTRY(tc_txd) list; + struct ptp_message *msg; + tmv_t residence; + int ingress_port; +}; + struct port { LIST_ENTRY(port) list; char *name; @@ -113,6 +120,7 @@ struct port { int min_neighbor_prop_delay; int net_sync_monitor; int path_trace_enabled; + int tc_spanning_tree; Integer64 rx_timestamp_offset; Integer64 tx_timestamp_offset; enum link_state link_status; @@ -121,6 +129,8 @@ struct port { unsigned int versionNumber; /*UInteger4*/ /* foreignMasterDS */ LIST_HEAD(fm, foreign_clock) foreign_masters; + /* TC book keeping */ + TAILQ_HEAD(tct, tc_txd) tc_transmitted; }; #define portnum(p) (p->portIdentity.portNumber) diff --git a/tc.c b/tc.c new file mode 100644 index 0000000..9b1d219 --- /dev/null +++ b/tc.c @@ -0,0 +1,510 @@ +/** + * @file tc.c + * @note Copyright (C) 2018 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335 USA. + */ +#include + +#include "port.h" +#include "print.h" +#include "tc.h" +#include "tmv.h" + +enum tc_match { + TC_MISMATCH, + TC_SYNC_FUP, + TC_FUP_SYNC, + TC_DELAY_REQRESP, +}; + +static TAILQ_HEAD(tc_pool, tc_txd) tc_pool = TAILQ_HEAD_INITIALIZER(tc_pool); + +static int tc_match_delay(int ingress_port, struct ptp_message *resp, + struct tc_txd *txd); +static int tc_match_syfup(int ingress_port, struct ptp_message *msg, + struct tc_txd *txd); +static void tc_recycle(struct tc_txd *txd); + +static struct tc_txd *tc_allocate(void) +{ + struct tc_txd *txd = TAILQ_FIRST(&tc_pool); + + if (txd) { + TAILQ_REMOVE(&tc_pool, txd, list); + memset(txd, 0, sizeof(*txd)); + return txd; + } + txd = calloc(1, sizeof(*txd)); + return txd; +} + +static int tc_blocked(struct port *q, struct port *p, struct ptp_message *m) +{ + enum port_state s; + + if (q == p) { + return 1; + } + if (portnum(p) == 0) { + return 1; + } + if (!q->tc_spanning_tree) { + return 0; + } + /* Ingress state */ + s = port_state(q); + switch (s) { + case PS_INITIALIZING: + case PS_FAULTY: + case PS_DISABLED: + case PS_LISTENING: + case PS_PRE_MASTER: + case PS_PASSIVE: + return 1; + case PS_MASTER: + case PS_GRAND_MASTER: + /* Delay_Req swims against the stream. */ + if (msg_type(m) != DELAY_REQ) { + return 1; + } + break; + case PS_UNCALIBRATED: + case PS_SLAVE: + break; + } + /* Egress state */ + s = port_state(p); + switch (s) { + case PS_INITIALIZING: + case PS_FAULTY: + case PS_DISABLED: + case PS_LISTENING: + case PS_PRE_MASTER: + case PS_PASSIVE: + return 1; + case PS_UNCALIBRATED: + case PS_SLAVE: + /* Delay_Req swims against the stream. */ + if (msg_type(m) != DELAY_REQ) { + return 1; + } + break; + case PS_MASTER: + case PS_GRAND_MASTER: + /* No use forwarding Delay_Req out the wrong port. */ + if (msg_type(m) == DELAY_REQ) { + return 1; + } + break; + } + return 0; +} + +static void tc_complete_request(struct port *q, struct port *p, + struct ptp_message *req, tmv_t residence) +{ + struct tc_txd *txd = tc_allocate(); + if (!txd) { + port_dispatch(p, EV_FAULT_DETECTED, 0); + return; + } +#ifdef DEBUG + pr_err("stash delay request from port %hd to %hd seqid %hu residence %lu", + portnum(q), portnum(p), ntohs(req->header.sequenceId), + (unsigned long) tmv_to_nanoseconds(residence)); +#endif + msg_get(req); + txd->msg = req; + txd->residence = residence; + txd->ingress_port = portnum(q); + TAILQ_INSERT_TAIL(&p->tc_transmitted, txd, list); +} + +static void tc_complete_response(struct port *q, struct port *p, + struct ptp_message *resp, tmv_t residence) +{ + enum tc_match type = TC_MISMATCH; + struct tc_txd *txd; + Integer64 c1, c2; + int cnt; + +#ifdef DEBUG + pr_err("complete delay response from port %hd to %hd seqid %hu", + portnum(q), portnum(p), ntohs(resp->header.sequenceId)); +#endif + TAILQ_FOREACH(txd, &q->tc_transmitted, list) { + type = tc_match_delay(portnum(p), resp, txd); + if (type == TC_DELAY_REQRESP) { + residence = txd->residence; + break; + } + } + if (type != TC_DELAY_REQRESP) { + return; + } + c1 = net2host64(resp->header.correction); + c2 = c1 + tmv_to_TimeInterval(residence); + resp->header.correction = host2net64(c2); + cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, resp); + if (cnt <= 0) { + pr_err("tc failed to forward response on port %d", portnum(p)); + port_dispatch(p, EV_FAULT_DETECTED, 0); + } + /* Restore original correction value for next egress port. */ + resp->header.correction = host2net64(c1); + TAILQ_REMOVE(&q->tc_transmitted, txd, list); + msg_put(txd->msg); + tc_recycle(txd); +} + +static void tc_complete_syfup(struct port *q, struct port *p, + struct ptp_message *msg, tmv_t residence) +{ + enum tc_match type = TC_MISMATCH; + struct ptp_message *fup; + struct tc_txd *txd; + Integer64 c1, c2; + int cnt; + + TAILQ_FOREACH(txd, &p->tc_transmitted, list) { + type = tc_match_syfup(portnum(q), msg, txd); + switch (type) { + case TC_MISMATCH: + break; + case TC_SYNC_FUP: + fup = msg; + residence = txd->residence; + break; + case TC_FUP_SYNC: + fup = txd->msg; + break; + case TC_DELAY_REQRESP: + pr_err("tc: unexpected match of delay request - sync!"); + return; + } + if (type != TC_MISMATCH) { + break; + } + } + + if (type == TC_MISMATCH) { + txd = tc_allocate(); + if (!txd) { + port_dispatch(p, EV_FAULT_DETECTED, 0); + return; + } + msg_get(msg); + txd->msg = msg; + txd->residence = residence; + txd->ingress_port = portnum(q); + TAILQ_INSERT_TAIL(&p->tc_transmitted, txd, list); + return; + } + + c1 = net2host64(fup->header.correction); + c2 = c1 + tmv_to_TimeInterval(residence); + c2 += tmv_to_TimeInterval(q->peer_delay); + c2 += q->asymmetry; + fup->header.correction = host2net64(c2); + cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, fup); + if (cnt <= 0) { + pr_err("tc failed to forward follow up on port %d", portnum(p)); + port_dispatch(p, EV_FAULT_DETECTED, 0); + } + /* Restore original correction value for next egress port. */ + fup->header.correction = host2net64(c1); + TAILQ_REMOVE(&p->tc_transmitted, txd, list); + msg_put(txd->msg); + tc_recycle(txd); +} + +static void tc_complete(struct port *q, struct port *p, + struct ptp_message *msg, tmv_t residence) +{ + switch (msg_type(msg)) { + case SYNC: + case FOLLOW_UP: + tc_complete_syfup(q, p, msg, residence); + break; + case DELAY_REQ: + tc_complete_request(q, p, msg, residence); + break; + case DELAY_RESP: + tc_complete_response(q, p, msg, residence); + break; + } +} + +static int tc_current(struct ptp_message *m, struct timespec now) +{ + int64_t t1, t2, tmo; + + tmo = 1LL * NSEC2SEC; + t1 = m->ts.host.tv_sec * NSEC2SEC + m->ts.host.tv_nsec; + t2 = now.tv_sec * NSEC2SEC + now.tv_nsec; + + return t2 - t1 < tmo; +} + +static int tc_fwd_event(struct port *q, struct ptp_message *msg) +{ + tmv_t egress, ingress = msg->hwts.ts, residence; + struct port *p; + int cnt, err; + double rr; + + clock_gettime(CLOCK_MONOTONIC, &msg->ts.host); + + /* First send the event message out. */ + for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) { + if (tc_blocked(q, p, msg)) { + continue; + } + cnt = transport_send(p->trp, &p->fda, TRANS_DEFER_EVENT, msg); + if (cnt <= 0) { + pr_err("failed to forward event from port %hd to %hd", + portnum(q), portnum(p)); + port_dispatch(p, EV_FAULT_DETECTED, 0); + } + } + + /* Go back and gather the transmit time stamps. */ + for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) { + if (tc_blocked(q, p, msg)) { + continue; + } + err = transport_txts(p->trp, &p->fda, msg); + if (err || !msg_sots_valid(msg)) { + pr_err("failed to fetch txts on port %hd to %hd event", + portnum(q), portnum(p)); + port_dispatch(p, EV_FAULT_DETECTED, 0); + continue; + } + ts_add(&msg->hwts.ts, p->tx_timestamp_offset); + egress = msg->hwts.ts; + residence = tmv_sub(egress, ingress); + rr = clock_rate_ratio(q->clock); + if (rr != 1.0) { + residence = dbl_tmv(tmv_dbl(residence) * rr); + } + tc_complete(q, p, msg, residence); + } + + return 0; +} + +static int tc_match_delay(int ingress_port, struct ptp_message *resp, + struct tc_txd *txd) +{ + struct ptp_message *req = txd->msg; + + if (ingress_port != txd->ingress_port) { + return TC_MISMATCH; + } + if (req->header.sequenceId != resp->header.sequenceId) { + return TC_MISMATCH; + } + if (!pid_eq(&req->header.sourcePortIdentity, + &resp->delay_resp.requestingPortIdentity)) { + return TC_MISMATCH; + } + if (msg_type(req) == DELAY_REQ && msg_type(resp) == DELAY_RESP) { + return TC_DELAY_REQRESP; + } + return TC_MISMATCH; +} + +static int tc_match_syfup(int ingress_port, struct ptp_message *msg, + struct tc_txd *txd) +{ + if (ingress_port != txd->ingress_port) { + return TC_MISMATCH; + } + if (msg->header.sequenceId != txd->msg->header.sequenceId) { + return TC_MISMATCH; + } + if (!source_pid_eq(msg, txd->msg)) { + return TC_MISMATCH; + } + if (msg_type(txd->msg) == SYNC && msg_type(msg) == FOLLOW_UP) { + return TC_SYNC_FUP; + } + if (msg_type(txd->msg) == FOLLOW_UP && msg_type(msg) == SYNC) { + return TC_FUP_SYNC; + } + return TC_MISMATCH; +} + +static void tc_recycle(struct tc_txd *txd) +{ + TAILQ_INSERT_HEAD(&tc_pool, txd, list); +} + +/* public methods */ + +void tc_cleanup(void) +{ + struct tc_txd *txd; + + while ((txd = TAILQ_FIRST(&tc_pool)) != NULL) { + TAILQ_REMOVE(&tc_pool, txd, list); + free(txd); + } +} + +void tc_flush(struct port *q) +{ + struct tc_txd *txd; + + while ((txd = TAILQ_FIRST(&q->tc_transmitted)) != NULL) { + TAILQ_REMOVE(&q->tc_transmitted, txd, list); + msg_put(txd->msg); + tc_recycle(txd); + } +} + +int tc_forward(struct port *q, struct ptp_message *msg) +{ + uint16_t steps_removed; + struct port *p; + int cnt; + + if (q->tc_spanning_tree && msg_type(msg) == ANNOUNCE) { + steps_removed = ntohs(msg->announce.stepsRemoved); + msg->announce.stepsRemoved = htons(1 + steps_removed); + } + + for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) { + if (tc_blocked(q, p, msg)) { + continue; + } + cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, msg); + if (cnt <= 0) { + pr_err("tc failed to forward message on port %d", + portnum(p)); + port_dispatch(p, EV_FAULT_DETECTED, 0); + } + } + return 0; +} + +int tc_fwd_folup(struct port *q, struct ptp_message *msg) +{ + struct port *p; + + clock_gettime(CLOCK_MONOTONIC, &msg->ts.host); + + for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) { + if (tc_blocked(q, p, msg)) { + continue; + } + tc_complete(q, p, msg, tmv_zero()); + } + return 0; +} + +int tc_fwd_request(struct port *q, struct ptp_message *msg) +{ + return tc_fwd_event(q, msg); +} + +int tc_fwd_response(struct port *q, struct ptp_message *msg) +{ + struct port *p; + + clock_gettime(CLOCK_MONOTONIC, &msg->ts.host); + + for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) { + if (tc_blocked(q, p, msg)) { + continue; + } + tc_complete(q, p, msg, tmv_zero()); + } + return 0; +} + +int tc_fwd_sync(struct port *q, struct ptp_message *msg) +{ + struct ptp_message *fup = NULL; + int err; + + if (one_step(msg)) { + fup = msg_allocate(); + if (!fup) { + return -1; + } + fup->header.tsmt = FOLLOW_UP | (msg->header.tsmt & 0xf0); + fup->header.ver = msg->header.ver; + fup->header.messageLength = sizeof(struct follow_up_msg); + fup->header.domainNumber = msg->header.domainNumber; + fup->header.sourcePortIdentity = msg->header.sourcePortIdentity; + fup->header.sequenceId = msg->header.sequenceId; + fup->header.control = CTL_FOLLOW_UP; + fup->header.logMessageInterval = msg->header.logMessageInterval; + fup->follow_up.preciseOriginTimestamp = msg->sync.originTimestamp; + msg->header.flagField[0] |= TWO_STEP; + } + err = tc_fwd_event(q, msg); + if (err) { + return err; + } + if (fup) { + err = tc_fwd_folup(q, fup); + msg_put(fup); + } + return err; +} + +int tc_ignore(struct port *p, struct ptp_message *m) +{ + struct ClockIdentity c1, c2; + + if (p->match_transport_specific && + msg_transport_specific(m) != p->transportSpecific) { + return 1; + } + if (pid_eq(&m->header.sourcePortIdentity, &p->portIdentity)) { + return 1; + } + if (m->header.domainNumber != clock_domain_number(p->clock)) { + return 1; + } + + c1 = clock_identity(p->clock); + c2 = m->header.sourcePortIdentity.clockIdentity; + + if (0 == memcmp(&c1, &c2, sizeof(c1))) { + return 1; + } + return 0; +} + +void tc_prune(struct port *q) +{ + struct timespec now; + struct tc_txd *txd; + + clock_gettime(CLOCK_MONOTONIC, &now); + + while ((txd = TAILQ_FIRST(&q->tc_transmitted)) != NULL) { + if (tc_current(txd->msg, now)) { + break; + } + TAILQ_REMOVE(&q->tc_transmitted, txd, list); + msg_put(txd->msg); + tc_recycle(txd); + } +} diff --git a/tc.h b/tc.h new file mode 100644 index 0000000..ef82351 --- /dev/null +++ b/tc.h @@ -0,0 +1,103 @@ +/** + * @file tc.h + * @brief Provides Transparent Clock logic. + * @note Copyright (C) 2018 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335 USA. + */ +#ifndef HAVE_TC_H +#define HAVE_TC_H + +#include "msg.h" +#include "port_private.h" + +/** + * Flushes the list of remembered residence times. + * @param q Port whose list should be flushed + */ +void tc_flush(struct port *q); + +/** + * Forwards a given general message out all other ports. + * @param q The ingress port + * @param msg The message to be sent + * @return Zero on success, non-zero otherwise. + */ +int tc_forward(struct port *q, struct ptp_message *msg); + +/** + * Forwards a given Follow-Up message out all other ports. + * + * This function adds the unique, per egress port residence time into + * the correction field for the transmitted follow up message. + * + * @param q The ingress port + * @param msg The message to be sent + * @return Zero on success, non-zero otherwise. + */ +int tc_fwd_folup(struct port *q, struct ptp_message *msg); + +/** + * Forwards a given delay request message out all other ports. + * + * This function computes the unique residence time for each egress + * port, remembering it in that egress port. + * + * @param q The ingress port + * @param msg The message to be sent + * @return Zero on success, non-zero otherwise. + */ +int tc_fwd_request(struct port *q, struct ptp_message *msg); + +/** + * Forwards a given response message out all other ports. + * + * This function adds the unique, per egress port residence time into + * the correction field for the transmitted delay response message. + * + * @param q The ingress port + * @param msg The message to be sent + * @return Zero on success, non-zero otherwise. + */ +int tc_fwd_response(struct port *q, struct ptp_message *msg); + +/** + * Forwards a given sync message out all other ports. + * + * This function computes the unique residence time for each egress + * port, remembering it in that egress port. + * + * @param q The ingress port + * @param msg The message to be sent + * @return Zero on success, non-zero otherwise. + */ +int tc_fwd_sync(struct port *q, struct ptp_message *msg); + +/** + * Determines whether the local clock should ignore a given message. + * + * @param q The ingress port + * @param msg The message to test + * @return One if the message should be ignored, zero otherwise. + */ +int tc_ignore(struct port *q, struct ptp_message *m); + +/** + * Prunes stale entries from the list of remembered residence times. + * @param q Port whose list should be pruned. + */ +void tc_prune(struct port *q); + +#endif