tc: Add the transparent clock implementation.
This patch adds code that sends an event messages received on one port out all the other ports and calculates the residence time. The correction, ingress port, and the original message are remembered in a TC transmit descriptor. These descriptors are recycled in a memory pool in a similar way to the message buffers. Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
Richard Cochran
parent
4cfbb2627f
commit
a13212fcbb
1
clock.c
1
clock.c
|
|
@ -282,6 +282,7 @@ void clock_destroy(struct clock *c)
|
||||||
}
|
}
|
||||||
memset(c, 0, sizeof(*c));
|
memset(c, 0, sizeof(*c));
|
||||||
msg_cleanup();
|
msg_cleanup();
|
||||||
|
tc_cleanup();
|
||||||
}
|
}
|
||||||
|
|
||||||
static int clock_fault_timeout(struct port *port, int set)
|
static int clock_fault_timeout(struct port *port, int set)
|
||||||
|
|
|
||||||
4
makefile
4
makefile
|
|
@ -25,8 +25,8 @@ LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
|
||||||
PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster
|
PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster
|
||||||
OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
|
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 \
|
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 \
|
pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o tc.o telecom.o \
|
||||||
transport.o tsproc.o udp.o udp6.o uds.o util.o version.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 \
|
OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
|
||||||
sysoff.o timemaster.o
|
sysoff.o timemaster.o
|
||||||
|
|
|
||||||
3
port.c
3
port.c
|
|
@ -36,6 +36,7 @@
|
||||||
#include "print.h"
|
#include "print.h"
|
||||||
#include "rtnl.h"
|
#include "rtnl.h"
|
||||||
#include "sk.h"
|
#include "sk.h"
|
||||||
|
#include "tc.h"
|
||||||
#include "tlv.h"
|
#include "tlv.h"
|
||||||
#include "tmv.h"
|
#include "tmv.h"
|
||||||
#include "tsproc.h"
|
#include "tsproc.h"
|
||||||
|
|
@ -1534,6 +1535,7 @@ void port_disable(struct port *p)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
|
||||||
|
tc_flush(p);
|
||||||
flush_last_sync(p);
|
flush_last_sync(p);
|
||||||
flush_delay_req(p);
|
flush_delay_req(p);
|
||||||
flush_peer_delay(p);
|
flush_peer_delay(p);
|
||||||
|
|
@ -2777,6 +2779,7 @@ struct port *port_open(int phc_index,
|
||||||
}
|
}
|
||||||
|
|
||||||
memset(p, 0, sizeof(*p));
|
memset(p, 0, sizeof(*p));
|
||||||
|
TAILQ_INIT(&p->tc_transmitted);
|
||||||
|
|
||||||
switch (type) {
|
switch (type) {
|
||||||
case CLOCK_TYPE_ORDINARY:
|
case CLOCK_TYPE_ORDINARY:
|
||||||
|
|
|
||||||
5
port.h
5
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,
|
void fault_interval(struct port *port, enum fault_type ft,
|
||||||
struct fault_interval *i);
|
struct fault_interval *i);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Release all of the memory in the TC transmit descriptor cache.
|
||||||
|
*/
|
||||||
|
void tc_cleanup(void);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -50,6 +50,13 @@ struct nrate_estimator {
|
||||||
int ratio_valid;
|
int ratio_valid;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct tc_txd {
|
||||||
|
TAILQ_ENTRY(tc_txd) list;
|
||||||
|
struct ptp_message *msg;
|
||||||
|
tmv_t residence;
|
||||||
|
int ingress_port;
|
||||||
|
};
|
||||||
|
|
||||||
struct port {
|
struct port {
|
||||||
LIST_ENTRY(port) list;
|
LIST_ENTRY(port) list;
|
||||||
char *name;
|
char *name;
|
||||||
|
|
@ -113,6 +120,7 @@ struct port {
|
||||||
int min_neighbor_prop_delay;
|
int min_neighbor_prop_delay;
|
||||||
int net_sync_monitor;
|
int net_sync_monitor;
|
||||||
int path_trace_enabled;
|
int path_trace_enabled;
|
||||||
|
int tc_spanning_tree;
|
||||||
Integer64 rx_timestamp_offset;
|
Integer64 rx_timestamp_offset;
|
||||||
Integer64 tx_timestamp_offset;
|
Integer64 tx_timestamp_offset;
|
||||||
enum link_state link_status;
|
enum link_state link_status;
|
||||||
|
|
@ -121,6 +129,8 @@ struct port {
|
||||||
unsigned int versionNumber; /*UInteger4*/
|
unsigned int versionNumber; /*UInteger4*/
|
||||||
/* foreignMasterDS */
|
/* foreignMasterDS */
|
||||||
LIST_HEAD(fm, foreign_clock) foreign_masters;
|
LIST_HEAD(fm, foreign_clock) foreign_masters;
|
||||||
|
/* TC book keeping */
|
||||||
|
TAILQ_HEAD(tct, tc_txd) tc_transmitted;
|
||||||
};
|
};
|
||||||
|
|
||||||
#define portnum(p) (p->portIdentity.portNumber)
|
#define portnum(p) (p->portIdentity.portNumber)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,510 @@
|
||||||
|
/**
|
||||||
|
* @file tc.c
|
||||||
|
* @note Copyright (C) 2018 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* 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 <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,
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,103 @@
|
||||||
|
/**
|
||||||
|
* @file tc.h
|
||||||
|
* @brief Provides Transparent Clock logic.
|
||||||
|
* @note Copyright (C) 2018 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* 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
|
||||||
Loading…
Reference in New Issue