This patch adds new code that handles unicast service contracts,
sending messages according the negotiated schedule.
Signed-off-by: Richard Cochran <richardcoch...@gmail.com>
---
makefile | 2 +-
missing.h | 2 +
port_private.h | 2 +
unicast_service.c | 525 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
unicast_service.h | 49 +++++
5 files changed, 579 insertions(+), 1 deletion(-)
create mode 100644 unicast_service.c
create mode 100644 unicast_service.h
diff --git a/makefile b/makefile
index fdc9e5e..693e75d 100644
--- a/makefile
+++ b/makefile
@@ -27,7 +27,7 @@ OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o
e2e_tc.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 port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.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 unicast_client.o unicast_fsm.o util.o version.o
+ uds.o unicast_client.o unicast_fsm.o unicast_service.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/missing.h b/missing.h
index b186aa7..2f7adb9 100644
--- a/missing.h
+++ b/missing.h
@@ -127,6 +127,8 @@ enum {
#else
+#define TFD_TIMER_ABSTIME (1 << 0)
+
static inline int clock_nanosleep(clockid_t clock_id, int flags,
const struct timespec *request,
struct timespec *remain)
diff --git a/port_private.h b/port_private.h
index 464699b..b876b86 100644
--- a/port_private.h
+++ b/port_private.h
@@ -135,6 +135,8 @@ struct port {
TAILQ_HEAD(tct, tc_txd) tc_transmitted;
/* unicast client mode */
struct unicast_master_table *unicast_master_table;
+ /* unicast service mode */
+ struct unicast_service *unicast_service;
};
#define portnum(p) (p->portIdentity.portNumber)
diff --git a/unicast_service.c b/unicast_service.c
new file mode 100644
index 0000000..6318a24
--- /dev/null
+++ b/unicast_service.c
@@ -0,0 +1,525 @@
+/**
+ * @file unicast_service.c
+ * @brief Unicast service
+ * @note Copyright (C) 2018 Richard Cochran <richardcoch...@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 <sys/queue.h>
+#include <time.h>
+
+#include "address.h"
+#include "clock.h"
+#include "missing.h"
+#include "port.h"
+#include "port_private.h"
+#include "pqueue.h"
+#include "print.h"
+#include "unicast_service.h"
+#include "util.h"
+
+#define QUEUE_LEN 16
+
+struct unicast_client_address {
+ LIST_ENTRY(unicast_client_address) list;
+ struct PortIdentity portIdentity;
+ unsigned int message_types;
+ struct address addr;
+ time_t grant_tmo;
+};
+
+struct unicast_service_interval {
+ LIST_HEAD(uca, unicast_client_address) clients;
+ LIST_ENTRY(unicast_service_interval) list;
+ struct timespec incr;
+ struct timespec tmo;
+ int log_period;
+};
+
+struct unicast_service {
+ LIST_HEAD(usi, unicast_service_interval) intervals;
+ struct pqueue *queue;
+};
+
+static struct timespec log_to_timespec(int log_seconds);
+static void timespec_normalize(struct timespec *ts);
+static int timespec_compare(struct timespec *a, struct timespec *b);
+
+static int attach_grant(struct ptp_message *msg,
+ struct request_unicast_xmit_tlv *req,
+ int duration)
+{
+ struct grant_unicast_xmit_tlv *g;
+ struct tlv_extra *extra;
+
+ extra = msg_tlv_append(msg, sizeof(*g));
+ if (!extra) {
+ return -1;
+ }
+ g = (struct grant_unicast_xmit_tlv *) extra->tlv;
+ g->type = TLV_GRANT_UNICAST_TRANSMISSION;
+ g->length = sizeof(*g) - sizeof(g->type) - sizeof(g->length);
+ g->message_type = req->message_type;
+ g->logInterMessagePeriod = req->logInterMessagePeriod;
+ g->durationField = duration;
+ g->flags = GRANT_UNICAST_RENEWAL_INVITED;
+
+ return 0;
+}
+
+static int compare_timeout(void *ain, void *bin)
+{
+ struct unicast_service_interval *a, *b;
+
+ a = (struct unicast_service_interval *) ain;
+ b = (struct unicast_service_interval *) bin;
+
+ return timespec_compare(&a->tmo, &b->tmo);
+}
+
+static void initialize_interval(struct unicast_service_interval *interval,
+ int log_period)
+{
+ LIST_INIT(&interval->clients);
+ interval->incr = log_to_timespec(log_period);
+ clock_gettime(CLOCK_MONOTONIC, &interval->tmo);
+ interval->tmo.tv_nsec += 10000000;
+ timespec_normalize(&interval->tmo);
+ interval->log_period = log_period;
+}
+
+static void interval_increment(struct unicast_service_interval *i)
+{
+ i->tmo.tv_sec += i->incr.tv_sec;
+ i->tmo.tv_nsec += i->incr.tv_nsec;
+ timespec_normalize(&i->tmo);
+}
+
+static struct timespec log_to_timespec(int log_seconds)
+{
+ struct timespec ts = {0, 0};
+ uint64_t ns;
+ int i;
+
+ if (log_seconds < 0) {
+ log_seconds *= -1;
+ for (i = 1, ns = 500000000ULL; i < log_seconds; i++) {
+ ns >>= 1;
+ }
+ ts.tv_nsec = ns;
+ timespec_normalize(&ts);
+ } else {
+ ts.tv_sec = 1 << log_seconds;
+ }
+ return ts;
+}
+
+/*
+ * Returns:
+ * 1 if 'a' is before 'b'
+ * -1 if 'a' is after 'b'
+ * 0 otherwise
+ */
+static int timespec_compare(struct timespec *a, struct timespec *b)
+{
+ if (a->tv_sec < b->tv_sec) {
+ return 1;
+ }
+ if (b->tv_sec < a->tv_sec) {
+ return -1;
+ }
+ if (a->tv_nsec < b->tv_nsec) {
+ return 1;
+ }
+ if (b->tv_nsec < a->tv_nsec) {
+ return -1;
+ }
+ return 0;
+}
+
+static void timespec_normalize(struct timespec *ts)
+{
+ while (ts->tv_nsec >= NS_PER_SEC) {
+ ts->tv_nsec -= NS_PER_SEC;
+ ts->tv_sec++;
+ }
+}
+
+static int unicast_service_clients(struct port *p,
+ struct unicast_service_interval *interval)
+{
+ struct unicast_client_address *client, *next;
+ struct timespec now;
+ int err = 0;
+
+ err = clock_gettime(CLOCK_MONOTONIC, &now);
+ if (err) {
+ pr_err("clock_gettime failed: %m");
+ return err;
+ }
+ LIST_FOREACH_SAFE(client, &interval->clients, list, next) {
+ pr_debug("%s wants 0x%x", pid2str(&client->portIdentity),
+ client->message_types);
+ if (now.tv_sec > client->grant_tmo) {
+ pr_debug("%s service of 0x%x expired",
+ pid2str(&client->portIdentity),
+ client->message_types);
+ LIST_REMOVE(client, list);
+ free(client);
+ continue;
+ }
+ if (client->message_types & (1 << ANNOUNCE)) {
+ if (port_tx_announce(p, &client->addr)) {
+ err = -1;
+ }
+ }
+ if (client->message_types & (1 << SYNC)) {
+ if (port_tx_sync(p, &client->addr)) {
+ err = -1;
+ }
+ }
+ }
+ return err;
+}
+
+static void unicast_service_extend(struct unicast_client_address *client,
+ struct request_unicast_xmit_tlv *req)
+{
+ struct timespec now;
+ time_t tmo;
+ int err;
+
+ err = clock_gettime(CLOCK_MONOTONIC, &now);
+ if (err) {
+ pr_err("clock_gettime failed: %m");
+ return;
+ }
+ tmo = now.tv_sec + req->durationField;
+ if (tmo > client->grant_tmo) {
+ client->grant_tmo = tmo;
+ pr_debug("%s grant of 0x%x extended to %ld",
+ pid2str(&client->portIdentity),
+ client->message_types, tmo);
+ }
+}
+
+static int unicast_service_rearm_timer(struct port *p)
+{
+ struct unicast_service_interval *interval;
+ struct itimerspec tmo;
+ int fd;
+
+ fd = p->fda.fd[FD_UNICAST_SRV_TIMER];
+ memset(&tmo, 0, sizeof(tmo));
+ interval = pqueue_peek(p->unicast_service->queue);
+ if (interval) {
+ tmo.it_value = interval->tmo;
+ pr_debug("arming timer tmo={%ld,%ld}",
+ interval->tmo.tv_sec, interval->tmo.tv_nsec);
+ } else {
+ pr_debug("stopping unicast service timer");
+ }
+ return timerfd_settime(fd, TFD_TIMER_ABSTIME, &tmo, NULL);
+}
+
+static int unicast_service_reply(struct port *p, struct ptp_message *dst,
+ struct request_unicast_xmit_tlv *req,
+ int duration)
+{
+ struct ptp_message *msg;
+ int err;
+
+ msg = port_signaling_construct(p, &dst->address,
+ &dst->header.sourcePortIdentity);
+ if (!msg) {
+ return -1;
+ }
+ err = attach_grant(msg, req, duration);
+ if (err) {
+ goto out;
+ }
+ err = port_prepare_and_send(p, msg, TRANS_GENERAL);
+ if (err) {
+ pr_err("port %hu: signaling message failed", portnum(p));
+ }
+out:
+ msg_put(msg);
+ return err;
+}
+
+/* public methods */
+
+int unicast_service_add(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra)
+{
+ struct unicast_client_address *client = NULL, *ctmp, *next;
+ struct unicast_service_interval *interval = NULL, *itmp;
+ struct request_unicast_xmit_tlv *req;
+ unsigned int mask;
+ uint8_t mtype;
+
+ if (!p->unicast_service) {
+ return SERVICE_DISABLED;
+ }
+
+ req = (struct request_unicast_xmit_tlv *) extra->tlv;
+ mtype = req->message_type >> 4;
+ mask = 1 << mtype;
+
+ switch (mtype) {
+ case ANNOUNCE:
+ case SYNC:
+ break;
+ case DELAY_RESP:
+ case PDELAY_RESP:
+ return SERVICE_GRANTED;
+ default:
+ return SERVICE_DENIED;
+ }
+
+ LIST_FOREACH(itmp, &p->unicast_service->intervals, list) {
+ /*
+ * Remember the interval of interest.
+ */
+ if (itmp->log_period == req->logInterMessagePeriod) {
+ interval = itmp;
+ }
+ /*
+ * Find any client records, and remove any stale contract.
+ */
+ LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, next) {
+ if (!addreq(transport_type(p->trp),
+ &ctmp->addr, &m->address)) {
+ continue;
+ }
+ if (interval == itmp) {
+ if (ctmp->message_types & mask) {
+ /* Contract is unchanged. */
+ unicast_service_extend(ctmp, req);
+ return SERVICE_GRANTED;
+ }
+ /* This is the one to use. */
+ client = ctmp;
+ continue;
+ }
+ /* Clear any stale contracts. */
+ ctmp->message_types &= ~mask;
+ if (!ctmp->message_types) {
+ LIST_REMOVE(ctmp, list);
+ free(ctmp);
+ }
+ }
+ }
+
+ if (client) {
+ client->message_types |= mask;
+ unicast_service_extend(client, req);
+ return SERVICE_GRANTED;
+ }
+
+ client = calloc(1, sizeof(*client));
+ if (!client) {
+ return SERVICE_DENIED;
+ }
+ client->portIdentity = m->header.sourcePortIdentity;
+ client->message_types = mask;
+ client->addr = m->address;
+ unicast_service_extend(client, req);
+
+ if (!interval) {
+ interval = calloc(1, sizeof(*interval));
+ if (!interval) {
+ free(client);
+ return SERVICE_DENIED;
+ }
+ initialize_interval(interval, req->logInterMessagePeriod);
+ LIST_INSERT_HEAD(&p->unicast_service->intervals, interval,
list);
+ if (pqueue_insert(p->unicast_service->queue, interval)) {
+ LIST_REMOVE(interval, list);
+ free(interval);
+ free(client);
+ return SERVICE_DENIED;
+ }
+ unicast_service_rearm_timer(p);
+ }
+ LIST_INSERT_HEAD(&interval->clients, client, list);
+ return SERVICE_GRANTED;
+}
+
+void unicast_service_cleanup(struct port *p)
+{
+ struct unicast_service_interval *itmp, *inext;
+ struct unicast_client_address *ctmp, *cnext;
+
+ if (!p->unicast_service) {
+ return;
+ }
+ LIST_FOREACH_SAFE(itmp, &p->unicast_service->intervals, list, inext) {
+ LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, cnext) {
+ LIST_REMOVE(ctmp, list);
+ free(ctmp);
+ }
+ LIST_REMOVE(itmp, list);
+ free(itmp);
+ }
+ pqueue_destroy(p->unicast_service->queue);
+ free(p->unicast_service);
+}
+
+int unicast_service_deny(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra)
+{
+ struct request_unicast_xmit_tlv *req =
+ (struct request_unicast_xmit_tlv *) extra->tlv;
+
+ return unicast_service_reply(p, m, req, 0);
+}
+
+int unicast_service_grant(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra)
+{
+ struct request_unicast_xmit_tlv *req =
+ (struct request_unicast_xmit_tlv *) extra->tlv;
+
+ return unicast_service_reply(p, m, req, req->durationField);
+}
+
+int unicast_service_initialize(struct port *p)
+{
+ struct config *cfg = clock_config(p->clock);
+
+ if (!config_get_int(cfg, p->name, "unicast_listen")) {
+ return 0;
+ }
+
+ p->unicast_service = calloc(1, sizeof(*p->unicast_service));
+ if (!p->unicast_service) {
+ return -1;
+ }
+ LIST_INIT(&p->unicast_service->intervals);
+
+ p->unicast_service->queue = pqueue_create(QUEUE_LEN, compare_timeout);
+ if (!p->unicast_service->queue) {
+ free(p->unicast_service);
+ return -1;
+ }
+
+ return 0;
+}
+
+void unicast_service_remove(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra)
+{
+ struct unicast_client_address *ctmp, *next;
+ struct cancel_unicast_xmit_tlv *cancel;
+ struct unicast_service_interval *itmp;
+ unsigned int mask;
+ uint8_t mtype;
+
+ if (!p->unicast_service) {
+ return;
+ }
+ cancel = (struct cancel_unicast_xmit_tlv *) extra->tlv;
+ if (cancel->message_type_flags & CANCEL_UNICAST_MAINTAIN_REQUEST) {
+ return;
+ }
+ mtype = cancel->message_type_flags >> 4;
+ mask = 1 << mtype;
+
+ switch (mtype) {
+ case ANNOUNCE:
+ case SYNC:
+ break;
+ case DELAY_RESP:
+ case PDELAY_RESP:
+ default:
+ return;
+ }
+
+ LIST_FOREACH(itmp, &p->unicast_service->intervals, list) {
+ LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, next) {
+ if (!addreq(transport_type(p->trp),
+ &ctmp->addr, &m->address)) {
+ continue;
+ }
+ ctmp->message_types &= ~mask;
+ if (!ctmp->message_types) {
+ LIST_REMOVE(ctmp, list);
+ free(ctmp);
+ }
+ }
+ }
+}
+
+int unicast_service_timer(struct port *p)
+{
+ struct unicast_service_interval *interval;
+ int err = 0, master = 0;
+ struct timespec now;
+
+ if (!p->unicast_service) {
+ return 0;
+ }
+ clock_gettime(CLOCK_MONOTONIC, &now);
+
+ switch (p->state) {
+ case PS_INITIALIZING:
+ case PS_FAULTY:
+ case PS_DISABLED:
+ case PS_LISTENING:
+ case PS_PRE_MASTER:
+ case PS_PASSIVE:
+ case PS_UNCALIBRATED:
+ case PS_SLAVE:
+ break;
+ case PS_MASTER:
+ case PS_GRAND_MASTER:
+ master = 1;
+ break;
+ }
+
+ while ((interval = pqueue_peek(p->unicast_service->queue)) != NULL) {
+
+ pr_debug("peek i={2^%d} tmo={%ld,%ld}", interval->log_period,
+ interval->tmo.tv_sec, interval->tmo.tv_nsec);
+
+ if (timespec_compare(&now, &interval->tmo) >= 0) {
+ break;
+ }
+ interval = pqueue_extract(p->unicast_service->queue);
+
+ if (master && unicast_service_clients(p, interval)) {
+ err = -1;
+ }
+
+ if (LIST_EMPTY(&interval->clients)) {
+ pr_debug("retire interval 2^%d", interval->log_period);
+ LIST_REMOVE(interval, list);
+ free(interval);
+ continue;
+ }
+
+ interval_increment(interval);
+ pr_debug("next i={2^%d} tmo={%ld,%ld}", interval->log_period,
+ interval->tmo.tv_sec, interval->tmo.tv_nsec);
+ pqueue_insert(p->unicast_service->queue, interval);
+ }
+
+ if (unicast_service_rearm_timer(p)) {
+ err = -1;
+ }
+ return err;
+}
diff --git a/unicast_service.h b/unicast_service.h
new file mode 100644
index 0000000..2c510a8
--- /dev/null
+++ b/unicast_service.h
@@ -0,0 +1,49 @@
+/**
+ * @file unicast_service.h
+ * @brief Unicast service
+ * @note Copyright (C) 2018 Richard Cochran <richardcoch...@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_UNICAST_SERVICE_H
+#define HAVE_UNICAST_SERVICE_H
+
+struct port;
+struct ptp_message;
+struct tlv_extra;
+
+#define SERVICE_GRANTED 0
+#define SERVICE_DENIED 1
+#define SERVICE_DISABLED 2
+
+int unicast_service_add(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra);
+
+void unicast_service_cleanup(struct port *p);
+
+int unicast_service_deny(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra);
+
+int unicast_service_grant(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra);
+
+int unicast_service_initialize(struct port *p);
+
+void unicast_service_remove(struct port *p, struct ptp_message *m,
+ struct tlv_extra *extra);
+
+int unicast_service_timer(struct port *p);
+
+#endif
--
2.11.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
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
