Introduce unicast service.
This patch adds new code that handles unicast service contracts, sending messages according the negotiated schedule. Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
Richard Cochran
parent
dfa9e65785
commit
63598688b7
2
makefile
2
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 \
|
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 \
|
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 \
|
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 \
|
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
|
||||||
|
|
|
||||||
|
|
@ -127,6 +127,8 @@ enum {
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
|
#define TFD_TIMER_ABSTIME (1 << 0)
|
||||||
|
|
||||||
static inline int clock_nanosleep(clockid_t clock_id, int flags,
|
static inline int clock_nanosleep(clockid_t clock_id, int flags,
|
||||||
const struct timespec *request,
|
const struct timespec *request,
|
||||||
struct timespec *remain)
|
struct timespec *remain)
|
||||||
|
|
|
||||||
|
|
@ -135,6 +135,8 @@ struct port {
|
||||||
TAILQ_HEAD(tct, tc_txd) tc_transmitted;
|
TAILQ_HEAD(tct, tc_txd) tc_transmitted;
|
||||||
/* unicast client mode */
|
/* unicast client mode */
|
||||||
struct unicast_master_table *unicast_master_table;
|
struct unicast_master_table *unicast_master_table;
|
||||||
|
/* unicast service mode */
|
||||||
|
struct unicast_service *unicast_service;
|
||||||
};
|
};
|
||||||
|
|
||||||
#define portnum(p) (p->portIdentity.portNumber)
|
#define portnum(p) (p->portIdentity.portNumber)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,527 @@
|
||||||
|
/**
|
||||||
|
* @file unicast_service.c
|
||||||
|
* @brief Unicast service
|
||||||
|
* @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 <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 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,90 @@
|
||||||
|
/**
|
||||||
|
* @file unicast_service.h
|
||||||
|
* @brief Unicast service
|
||||||
|
* @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_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
|
||||||
Loading…
Reference in New Issue