diff --git a/clock.c b/clock.c index a85efde..4562078 100644 --- a/clock.c +++ b/clock.c @@ -22,6 +22,7 @@ #include #include +#include "address.h" #include "bmc.h" #include "clock.h" #include "clockadj.h" @@ -59,6 +60,14 @@ struct clock_stats { unsigned int max_count; }; +struct clock_subscriber { + LIST_ENTRY(clock_subscriber) list; + uint8_t events[EVENT_BITMASK_CNT]; + struct PortIdentity targetPortIdentity; + struct address addr; + UInteger16 sequenceId; +}; + struct clock { clockid_t clkid; struct servo *servo; @@ -99,6 +108,7 @@ struct clock { int stats_interval; struct clockcheck *sanity_check; struct interface uds_interface; + LIST_HEAD(clock_subscribers_head, clock_subscriber) subscribers; }; struct clock the_clock; @@ -110,9 +120,114 @@ static int cid_eq(struct ClockIdentity *a, struct ClockIdentity *b) return 0 == memcmp(a, b, sizeof(*a)); } +#ifndef LIST_FOREACH_SAFE +#define LIST_FOREACH_SAFE(var, head, field, tvar) \ + for ((var) = LIST_FIRST((head)); \ + (var) && ((tvar) = LIST_NEXT((var), field), 1); \ + (var) = (tvar)) +#endif + +static void remove_subscriber(struct clock_subscriber *s) +{ + LIST_REMOVE(s, list); + free(s); +} + +static void clock_update_subscription(struct clock *c, struct ptp_message *req, + uint8_t *bitmask) +{ + struct clock_subscriber *s; + int i, remove = 1; + + for (i = 0; i < EVENT_BITMASK_CNT; i++) { + if (bitmask[i]) { + remove = 0; + break; + } + } + + LIST_FOREACH(s, &c->subscribers, list) { + if (!memcmp(&s->targetPortIdentity, &req->header.sourcePortIdentity, + sizeof(struct PortIdentity))) { + /* Found, update the transport address and event + * mask. */ + if (!remove) { + s->addr = req->address; + memcpy(s->events, bitmask, EVENT_BITMASK_CNT); + } else { + remove_subscriber(s); + } + return; + } + } + if (remove) + return; + /* Not present yet, add the subscriber. */ + s = malloc(sizeof(*s)); + if (!s) { + pr_err("failed to allocate memory for a subscriber"); + return; + } + s->targetPortIdentity = req->header.sourcePortIdentity; + s->addr = req->address; + memcpy(s->events, bitmask, EVENT_BITMASK_CNT); + s->sequenceId = 0; + LIST_INSERT_HEAD(&c->subscribers, s, list); +} + +static void clock_get_subscription(struct clock *c, struct ptp_message *req, + uint8_t *bitmask) +{ + struct clock_subscriber *s; + + LIST_FOREACH(s, &c->subscribers, list) { + if (!memcmp(&s->targetPortIdentity, &req->header.sourcePortIdentity, + sizeof(struct PortIdentity))) { + memcpy(bitmask, s->events, EVENT_BITMASK_CNT); + return; + } + } + /* A client without entry means the client has no subscriptions. */ + memset(bitmask, 0, EVENT_BITMASK_CNT); +} + +static void clock_flush_subscriptions(struct clock *c) +{ + struct clock_subscriber *s, *tmp; + + LIST_FOREACH_SAFE(s, &c->subscribers, list, tmp) { + remove_subscriber(s); + } +} + +void clock_send_notification(struct clock *c, struct ptp_message *msg, + int msglen, enum notification event) +{ + unsigned int event_pos = event / 8; + uint8_t mask = 1 << (event % 8); + struct port *uds = c->port[c->nports]; + struct clock_subscriber *s; + + LIST_FOREACH(s, &c->subscribers, list) { + if (!(s->events[event_pos] & mask)) + continue; + /* send event */ + msg->header.sequenceId = htons(s->sequenceId); + s->sequenceId++; + msg->management.targetPortIdentity.clockIdentity = + s->targetPortIdentity.clockIdentity; + msg->management.targetPortIdentity.portNumber = + htons(s->targetPortIdentity.portNumber); + msg->address = s->addr; + port_forward_to(uds, msg); + } +} + void clock_destroy(struct clock *c) { int i; + + clock_flush_subscriptions(c); for (i = 0; i < c->nports; i++) { port_close(c->port[i]); close(c->fault_fd[i]); @@ -180,6 +295,7 @@ static int clock_management_get_response(struct clock *c, struct port *p, struct ptp_message *rsp; struct time_status_np *tsn; struct grandmaster_settings_np *gsn; + struct subscribe_events_np *sen; struct PortIdentity pid = port_identity(p); struct PTPText *text; @@ -288,6 +404,15 @@ static int clock_management_get_response(struct clock *c, struct port *p, datalen = sizeof(*gsn); respond = 1; break; + case SUBSCRIBE_EVENTS_NP: + if (p != c->port[c->nports]) { + /* Only the UDS port allowed. */ + break; + } + sen = (struct subscribe_events_np *)tlv->data; + clock_get_subscription(c, req, sen->bitmask); + respond = 1; + break; } if (respond) { if (datalen % 2) { @@ -309,6 +434,7 @@ static int clock_management_set(struct clock *c, struct port *p, int respond = 0; struct management_tlv *tlv; struct grandmaster_settings_np *gsn; + struct subscribe_events_np *sen; tlv = (struct management_tlv *) req->management.suffix; @@ -322,6 +448,11 @@ static int clock_management_set(struct clock *c, struct port *p, *changed = 1; respond = 1; break; + case SUBSCRIBE_EVENTS_NP: + sen = (struct subscribe_events_np *)tlv->data; + clock_update_subscription(c, req, sen->bitmask); + respond = 1; + break; } if (respond && !clock_management_get_response(c, p, id, req)) pr_err("failed to send management set response"); @@ -669,6 +800,8 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count, clock_sync_interval(c, 0); + LIST_INIT(&c->subscribers); + for (i = 0; i < count; i++) { c->port[i] = port_open(phc_index, timestamping, 1+i, &iface[i], c); if (!c->port[i]) { @@ -880,6 +1013,7 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) case PRIMARY_DOMAIN: case TIME_STATUS_NP: case GRANDMASTER_SETTINGS_NP: + case SUBSCRIBE_EVENTS_NP: clock_management_send_error(p, msg, NOT_SUPPORTED); break; default: diff --git a/clock.h b/clock.h index 804640b..8718f2d 100644 --- a/clock.h +++ b/clock.h @@ -23,6 +23,7 @@ #include "dm.h" #include "ds.h" #include "config.h" +#include "notification.h" #include "servo.h" #include "tlv.h" #include "tmv.h" @@ -133,6 +134,16 @@ void clock_install_fda(struct clock *c, struct port *p, struct fdarray fda); */ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg); +/** + * Send notification about an event to all subscribers. + * @param c The clock instance. + * @param msg The PTP message to send, in network byte order. + * @param msglen The length of the message in bytes. + * @param event The event that occured. + */ +void clock_send_notification(struct clock *c, struct ptp_message *msg, + int msglen, enum notification event); + /** * Obtain a clock's parent data set. * @param c The clock instance. diff --git a/notification.h b/notification.h new file mode 100644 index 0000000..57e7a78 --- /dev/null +++ b/notification.h @@ -0,0 +1,27 @@ +/** + * @file notification.h + * @brief Definitions for the notification framework. + * @note Copyright (C) 2014 Red Hat, Inc., Jiri Benc + * + * 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_NOTIFICATION_H +#define HAVE_NOTIFICATION_H + +enum notification { + NOTIFY_DUMMY, +}; + +#endif diff --git a/tlv.c b/tlv.c index b8cdd39..430410f 100644 --- a/tlv.c +++ b/tlv.c @@ -242,6 +242,10 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, pdsnp->neighborPropDelayThresh = ntohl(pdsnp->neighborPropDelayThresh); pdsnp->asCapable = ntohl(pdsnp->asCapable); break; + case SUBSCRIBE_EVENTS_NP: + if (data_len != sizeof(struct subscribe_events_np)) + goto bad_length; + break; case SAVE_IN_NON_VOLATILE_STORAGE: case RESET_NON_VOLATILE_STORAGE: case INITIALIZE: diff --git a/tlv.h b/tlv.h index 60c937d..a379baa 100644 --- a/tlv.h +++ b/tlv.h @@ -79,6 +79,7 @@ enum management_action { #define PRIMARY_DOMAIN 0x4002 #define TIME_STATUS_NP 0xC000 #define GRANDMASTER_SETTINGS_NP 0xC001 +#define SUBSCRIBE_EVENTS_NP 0xC003 /* Port management ID values */ #define NULL_MANAGEMENT 0x0000 @@ -196,6 +197,13 @@ struct port_ds_np { Integer32 asCapable; } PACKED; + +#define EVENT_BITMASK_CNT 64 + +struct subscribe_events_np { + uint8_t bitmask[EVENT_BITMASK_CNT]; +} PACKED; + enum clock_type { CLOCK_TYPE_ORDINARY = 0x8000, CLOCK_TYPE_BOUNDARY = 0x4000,