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 41d3a20..df6cb72 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..8189a34 --- /dev/null +++ b/unicast_service.c @@ -0,0 +1,527 @@ +/** + * @file unicast_service.c + * @brief Unicast service + * @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 +#include + +#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 int timespec_compare(struct timespec *a, struct timespec *b); +static void timespec_normalize(struct timespec *ts); + +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; + } 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; + } + if (ctmp->message_types & mask) { + ctmp->message_types &= ~mask; + if (!ctmp->message_types) { + LIST_REMOVE(ctmp, list); + free(ctmp); + } + return; + } + } + } +} + +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..f0d6487 --- /dev/null +++ b/unicast_service.h @@ -0,0 +1,90 @@ +/** + * @file unicast_service.h + * @brief Unicast service + * @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_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 + +/** + * Handle a request for unicast service. + * @param p The port on which the signaling message was received. + * @param m The signaling message containing the request. + * @param extra The TLV containing the request. + * @return SERVICE_GRANTED, SERVICE_DENIED, or SERVICE_DISABLED. + */ +int unicast_service_add(struct port *p, struct ptp_message *m, + struct tlv_extra *extra); + +/** + * Frees all of the resources associated with a port's unicast service. + * @param p The port in question. + */ +void unicast_service_cleanup(struct port *p); + +/** + * Responds to a unicast service request with a denial. + * @param p The port on which the signaling message was received. + * @param m The signaling message containing the request. + * @param extra The TLV containing the request. + * @return Zero on success, non-zero otherwise. + */ +int unicast_service_deny(struct port *p, struct ptp_message *m, + struct tlv_extra *extra); + +/** + * Responds to a unicast service request with a grant. + * @param p The port on which the signaling message was received. + * @param m The signaling message containing the request. + * @param extra The TLV containing the request. + * @return Zero on success, non-zero otherwise. + */ +int unicast_service_grant(struct port *p, struct ptp_message *m, + struct tlv_extra *extra); + +/** + * Initializes unicast service on a given port. + * @param p The port in question. + * @return Zero on success, non-zero otherwise. + */ +int unicast_service_initialize(struct port *p); + +/** + * Handle a unicast service cancellation. + * @param p The port on which the signaling message was received. + * @param m The signaling message containing the cancellation. + * @param extra The TLV containing the cancellation. + */ +void unicast_service_remove(struct port *p, struct ptp_message *m, + struct tlv_extra *extra); + +/** + * Handles the unicast service timer, sending messages according to schedule. + * @param p The port in question. + * @return Zero on success, non-zero otherwise. + */ +int unicast_service_timer(struct port *p); + +#endif