Signed-off-by: Richard Cochran <[email protected]>
---
clock.c | 1 +
makefile | 2 +-
port.c | 3 +
port.h | 5 ++
port_private.h | 9 ++
tc.c | 265 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tc.h | 36 ++++++++
7 files changed, 320 insertions(+), 1 deletion(-)
create mode 100644 tc.c
create mode 100644 tc.h
diff --git a/clock.c b/clock.c
index da15882..0ec437f 100644
--- a/clock.c
+++ b/clock.c
@@ -284,6 +284,7 @@ void clock_destroy(struct clock *c)
clockcheck_destroy(c->sanity_check);
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 f898336..83e4787 100644
--- a/makefile
+++ b/makefile
@@ -25,7 +25,7 @@ LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
PRG = ptp4l pmc phc2sys hwstamp_ctl phc_ctl 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 tlv.o \
+ pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o tc.o tlv.o \
transport.o tsproc.o udp.o udp6.o uds.o util.o version.o
OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
diff --git a/port.c b/port.c
index eba46a0..0465d95 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"
@@ -1367,6 +1368,7 @@ void port_disable(struct port *p)
{
int i;
+ tc_flush(p);
flush_last_sync(p);
flush_delay_req(p);
flush_peer_delay(p);
@@ -2525,6 +2527,7 @@ struct port *port_open(int phc_index,
return NULL;
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 60fd0a4..5bf3f6a 100644
--- a/port.h
+++ b/port.h
@@ -313,4 +313,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 dc69df0..bcf6f02 100644
--- a/port_private.h
+++ b/port_private.h
@@ -43,6 +43,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;
@@ -109,6 +116,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..5939fb8
--- /dev/null
+++ b/tc.c
@@ -0,0 +1,265 @@
+/**
+ * @file tc.c
+ * @note Copyright (C) 2015 Richard Cochran <[email protected]>
+ *
+ * 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-1301 USA.
+ */
+#include <stdlib.h>
+
+#include "port.h"
+#include "print.h"
+#include "tc.h"
+#include "tmv.h"
+
+enum tc_match {
+ TC_MISMATCH,
+ TC_SYNC_FUP,
+ TC_FUP_SYNC,
+};
+
+static TAILQ_HEAD(tc_pool, tc_txd) tc_pool = TAILQ_HEAD_INITIALIZER(tc_pool);
+
+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 *p)
+{
+ if (portnum(p) == 0) {
+ return 1;
+ }
+ enum port_state s = port_state(p);
+ switch (s) {
+ case PS_INITIALIZING:
+ case PS_FAULTY:
+ case PS_DISABLED:
+ case PS_PASSIVE:
+ break;
+ case PS_LISTENING:
+ case PS_PRE_MASTER:
+ case PS_MASTER:
+ case PS_UNCALIBRATED:
+ case PS_SLAVE:
+ case PS_GRAND_MASTER:
+ return 0;
+ }
+ return 1;
+}
+
+static int tc_current(struct ptp_message *m, struct timespec now)
+{
+ int64_t t1, t2, 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 void tc_free(struct tc_txd *txd)
+{
+ TAILQ_INSERT_HEAD(&tc_pool, txd, list);
+}
+
+static int tc_match(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_complete(struct port *q, struct port *p, struct ptp_message
*msg,
+ tmv_t residence)
+{
+ struct ptp_message *fup;
+ struct tc_txd *txd;
+ enum tc_match type = TC_MISMATCH;
+ Integer64 c1, c2;
+ int cnt;
+
+ TAILQ_FOREACH(txd, &p->tc_transmitted, list) {
+ type = tc_match(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;
+ }
+ if (type != TC_MISMATCH) {
+ break;
+ }
+ }
+
+ if (type == TC_MISMATCH) {
+ txd = tc_allocate();
+ if (!txd) {
+ pr_err("low memory, TC failed to forward event");
+ port_dispatch(p, EV_FAULT_DETECTED, 0);
+ return;
+ }
+ msg_get(msg);
+ txd->msg = msg;
+ txd->residence = residence;
+ txd->ingress_port = port_number(q);
+ TAILQ_INSERT_TAIL(&p->tc_transmitted, txd, list);
+ return;
+ }
+
+ c1 = net2host64(fup->header.correction);
+ c2 = c1 + tmv_to_TimeInterval(residence);
+ fup->header.correction = host2net64(c2);
+ cnt = transport_send(p->trp, &p->fda, 0, fup);
+ if (cnt <= 0) {
+ port_dispatch(p, EV_FAULT_DETECTED, 0);
+ }
+ TAILQ_REMOVE(&p->tc_transmitted, txd, list);
+ msg_put(txd->msg);
+ tc_free(txd);
+}
+
+/* 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_free(txd);
+ }
+}
+
+int tc_forward(struct port *q, struct ptp_message *msg)
+{
+ struct port *p;
+ int cnt;
+
+ if (msg_pre_send(msg)) {
+ return -1;
+ }
+ for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) {
+ if (p == q || tc_blocked(p)) {
+ continue;
+ }
+ cnt = transport_send(p->trp, &p->fda, 0, msg);
+ if (cnt <= 0) {
+ /* egress port is faulty. */
+ port_dispatch(p, EV_FAULT_DETECTED, 0);
+ }
+ }
+ return 0;
+}
+
+int tc_fwd_event(struct port *q, struct ptp_message *msg)
+{
+ tmv_t egress, ingress = timespec_to_tmv(msg->hwts.ts), residence;
+ struct port *p;
+ int cnt;
+
+ clock_gettime(CLOCK_MONOTONIC, &msg->ts.host);
+ if (msg_pre_send(msg)) {
+ return -1;
+ }
+ for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) {
+ if (p == q || tc_blocked(p)) {
+ continue;
+ }
+ cnt = transport_send(p->trp, &p->fda, 1, msg);
+ if (cnt <= 0 || !msg_sots_valid(msg)) {
+ port_dispatch(p, EV_FAULT_DETECTED, 0);
+ continue;
+ }
+ ts_add(&msg->hwts.ts, p->tx_timestamp_offset);
+ egress = timespec_to_tmv(msg->hwts.ts);
+ residence = tmv_sub(egress, ingress);
+ tc_complete(q, p, msg, residence);
+ }
+ return 0;
+}
+
+int tc_fwd_folup(struct port *q, struct ptp_message *msg)
+{
+ struct Timestamp *ts = &msg->follow_up.preciseOriginTimestamp;
+ struct port *p;
+
+ clock_gettime(CLOCK_MONOTONIC, &msg->ts.host);
+
+ ts->seconds_lsb = msg->ts.pdu.sec & 0xFFFFFFFF;
+ ts->seconds_msb = msg->ts.pdu.sec >> 32;
+ ts->nanoseconds = msg->ts.pdu.nsec;
+
+ if (msg_pre_send(msg)) {
+ return -1;
+ }
+ for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) {
+ if (p == q || tc_blocked(p)) {
+ continue;
+ }
+ tc_complete(q, p, msg, tmv_zero());
+ }
+ return 0;
+}
+
+void tc_prune(struct port *q)
+{
+ struct tc_txd *txd;
+ struct timespec now;
+
+ 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_free(txd);
+ }
+}
diff --git a/tc.h b/tc.h
new file mode 100644
index 0000000..2463d87
--- /dev/null
+++ b/tc.h
@@ -0,0 +1,36 @@
+/**
+ * @file tc.h
+ * @brief Implements a Transparent Clock.
+ * @note Copyright (C) 2015 Richard Cochran <[email protected]>
+ *
+ * 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-1301 USA.
+ */
+#ifndef HAVE_TC_H
+#define HAVE_TC_H
+
+#include "msg.h"
+#include "port_private.h"
+
+void tc_flush(struct port *q);
+
+int tc_forward(struct port *q, struct ptp_message *msg);
+
+int tc_fwd_event(struct port *q, struct ptp_message *msg);
+
+int tc_fwd_folup(struct port *q, struct ptp_message *msg);
+
+void tc_prune(struct port *q);
+
+#endif
--
2.13.0
------------------------------------------------------------------------------
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
