diff --git a/clock.c b/clock.c index b487eef..3c6e5ce 100644 --- a/clock.c +++ b/clock.c @@ -21,6 +21,7 @@ #include #include #include +#include #include "address.h" #include "bmc.h" @@ -43,7 +44,9 @@ #define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer */ #define POW2_41 ((double)(1ULL << 41)) -#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) +struct port { + LIST_ENTRY(port) list; +}; struct freq_estimator { tmv_t origin1; @@ -79,9 +82,9 @@ struct clock { struct ClockIdentity ptl[PATH_TRACE_MAX]; struct foreign_clock *best; struct ClockIdentity best_id; - struct port *port[MAX_PORTS]; + LIST_HEAD(ports_head, port) ports; struct port *uds_port; - struct pollfd pollfd[(MAX_PORTS + 1) * N_CLOCK_PFD]; + struct pollfd *pollfd; int nports; /* does not include the UDS port */ int free_running; int freq_est_interval; @@ -115,6 +118,8 @@ struct clock { struct clock the_clock; static void handle_state_decision_event(struct clock *c); +static int clock_resize_pollfd(struct clock *c, int new_nports); +static void clock_remove_port(struct clock *c, struct port *p); static int cid_eq(struct ClockIdentity *a, struct ClockIdentity *b) { @@ -253,12 +258,14 @@ void clock_send_notification(struct clock *c, struct ptp_message *msg, void clock_destroy(struct clock *c) { - int i; + struct port *p, *tmp; clock_flush_subscriptions(c); - for (i = 0; i < c->nports; i++) - port_close(c->port[i]); + LIST_FOREACH_SAFE(p, &c->ports, list, tmp) { + clock_remove_port(c, p); + } port_close(c->uds_port); + free(c->pollfd); if (c->clkid != CLOCK_REALTIME) { phc_close(c->clkid); } @@ -273,25 +280,25 @@ void clock_destroy(struct clock *c) msg_cleanup(); } -static int clock_fault_timeout(struct clock *c, int index, int set) +static int clock_fault_timeout(struct port *port, int set) { struct fault_interval i; if (!set) { - pr_debug("clearing fault on port %d", index + 1); - return port_set_fault_timer_lin(c->port[index], 0); + pr_debug("clearing fault on port %d", port_number(port)); + return port_set_fault_timer_lin(port, 0); } - fault_interval(c->port[index], last_fault_type(c->port[index]), &i); + fault_interval(port, last_fault_type(port), &i); if (i.type == FTMO_LINEAR_SECONDS) { pr_debug("waiting %d seconds to clear fault on port %d", - i.val, index + 1); - return port_set_fault_timer_lin(c->port[index], i.val); + i.val, port_number(port)); + return port_set_fault_timer_lin(port, i.val); } else if (i.type == FTMO_LOG2_SECONDS) { pr_debug("waiting 2^{%d} seconds to clear fault on port %d", - i.val, index + 1); - return port_set_fault_timer_log(c->port[index], 1, i.val); + i.val, port_number(port)); + return port_set_fault_timer_log(port, 1, i.val); } pr_err("Unsupported fault interval type %d", i.type); @@ -743,12 +750,45 @@ UInteger8 clock_class(struct clock *c) return c->dds.clockQuality.clockClass; } +static int clock_add_port(struct clock *c, int phc_index, + enum timestamp_type timestamping, int number, + struct interface *iface) +{ + struct port *p; + + if (clock_resize_pollfd(c, c->nports + 1)) + return -1; + p = port_open(phc_index, timestamping, number, iface, c); + if (!p) { + /* No need to shrink pollfd */ + return -1; + } + LIST_INSERT_HEAD(&c->ports, p, list); + c->nports++; + clock_fda_changed(c); + return 0; +} + +static void clock_remove_port(struct clock *c, struct port *p) +{ + /* Do not call clock_resize_pollfd, it's pointless to shrink + * the allocated memory at this point, clock_destroy will free + * it all anyway. This function is usable from other parts of + * the code, but even then we don't mind if pollfd is larger + * than necessary. */ + LIST_REMOVE(p, list); + c->nports--; + clock_fda_changed(c); + port_close(p); +} + struct clock *clock_create(int phc_index, struct interface *iface, int count, enum timestamp_type timestamping, struct default_ds *dds, enum servo_type servo) { int i, fadj = 0, max_adj = 0.0, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0; struct clock *c = &the_clock; + struct port *p; char phc[32]; struct interface *udsif = &c->uds_interface; struct timespec ts; @@ -845,39 +885,38 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count, c->dad.pds.observedParentClockPhaseChangeRate = 0x7fffffff; c->dad.ptl = c->ptl; - for (i = 0; i < ARRAY_SIZE(c->pollfd); i++) { - c->pollfd[i].fd = -1; - c->pollfd[i].events = 0; - } - 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]) { - pr_err("failed to open port %s", iface[i].name); - return NULL; - } - c->pollfd[N_CLOCK_PFD * i + N_POLLFD].fd = port_fault_fd(c->port[i]); - c->pollfd[N_CLOCK_PFD * i + N_POLLFD].events = POLLIN|POLLPRI; - } + LIST_INIT(&c->ports); /* * Create the UDS interface. */ + if (clock_resize_pollfd(c, 0)) { + pr_err("failed to allocate pollfd"); + return NULL; + } c->uds_port = port_open(phc_index, timestamping, 0, udsif, c); if (!c->uds_port) { pr_err("failed to open the UDS port"); return NULL; } + clock_fda_changed(c); - c->dds.numberPorts = c->nports = count; + /* Create the ports. */ + for (i = 0; i < count; i++) { + if (clock_add_port(c, phc_index, timestamping, i + 1, &iface[i])) { + pr_err("failed to open port %s", iface[i].name); + return NULL; + } + } - for (i = 0; i < c->nports; i++) - port_dispatch(c->port[i], EV_INITIALIZE, 0); + c->dds.numberPorts = c->nports; + LIST_FOREACH(p, &c->ports, list) { + port_dispatch(p, EV_INITIALIZE, 0); + } port_dispatch(c->uds_port, EV_INITIALIZE, 0); return c; @@ -935,21 +974,44 @@ struct ClockIdentity clock_identity(struct clock *c) return c->dds.clockIdentity; } -void clock_install_fda(struct clock *c, struct port *p, struct fdarray fda) +static int clock_resize_pollfd(struct clock *c, int new_nports) { - int i, j, k; + struct pollfd *new_pollfd; - for (i = 0; i < c->nports; i++) { - if (p == c->port[i]) - break; + /* Need to allocate one extra block of fds for uds */ + new_pollfd = realloc(c->pollfd, + (new_nports + 1) * N_CLOCK_PFD * + sizeof(struct pollfd)); + if (!new_pollfd) + return -1; + c->pollfd = new_pollfd; + return 0; +} + +static void clock_fill_pollfd(struct pollfd *dest, struct port *p) +{ + struct fdarray *fda; + int i; + + fda = port_fda(p); + for (i = 0; i < N_POLLFD; i++) { + dest[i].fd = fda->fd[i]; + dest[i].events = POLLIN|POLLPRI; } - if (p == c->uds_port) - i = c->nports; - for (j = 0; j < N_POLLFD; j++) { - k = N_CLOCK_PFD * i + j; - c->pollfd[k].fd = fda.fd[j]; - c->pollfd[k].events = POLLIN|POLLPRI; + dest[i].fd = port_fault_fd(p); + dest[i].events = POLLIN|POLLPRI; +} + +void clock_fda_changed(struct clock *c) +{ + struct port *p; + struct pollfd *dest = c->pollfd; + + LIST_FOREACH(p, &c->ports, list) { + clock_fill_pollfd(dest, p); + dest += N_CLOCK_PFD; } + clock_fill_pollfd(dest, c->uds_port); } static int clock_do_forward_mgmt(struct clock *c, @@ -969,14 +1031,17 @@ static int clock_do_forward_mgmt(struct clock *c, static void clock_forward_mgmt_msg(struct clock *c, struct port *p, struct ptp_message *msg) { - int i, pdulen = 0, msg_ready = 0; + struct port *piter; + int pdulen = 0, msg_ready = 0; if (forwarding(c, p) && msg->management.boundaryHops) { pdulen = msg->header.messageLength; msg->management.boundaryHops--; - for (i = 0; i < c->nports; i++) - if (clock_do_forward_mgmt(c, p, c->port[i], msg, &msg_ready)) - pr_err("port %d: management forward failed", i + 1); + LIST_FOREACH(piter, &c->ports, list) { + if (clock_do_forward_mgmt(c, p, piter, msg, &msg_ready)) + pr_err("port %d: management forward failed", + port_number(piter)); + } if (clock_do_forward_mgmt(c, p, c->uds_port, msg, &msg_ready)) pr_err("uds port: management forward failed"); if (msg_ready) { @@ -988,7 +1053,8 @@ static void clock_forward_mgmt_msg(struct clock *c, struct port *p, struct ptp_m int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) { - int changed = 0, i, res, answers; + int changed = 0, res, answers; + struct port *piter; struct management_tlv *mgt; struct ClockIdentity *tcid, wildcard = { {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff} @@ -1084,8 +1150,8 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) break; default: answers = 0; - for (i = 0; i < c->nports; i++) { - res = port_manage(c->port[i], p, msg); + LIST_FOREACH(piter, &c->ports, list) { + res = port_manage(piter, p, msg); if (res < 0) return changed; if (res > 0) @@ -1141,10 +1207,12 @@ struct PortIdentity clock_parent_identity(struct clock *c) int clock_poll(struct clock *c) { - int cnt, err, i, j, k, sde = 0; + int cnt, err, i, sde = 0; enum fsm_event event; + struct pollfd *cur; + struct port *p; - cnt = poll(c->pollfd, ARRAY_SIZE(c->pollfd), -1); + cnt = poll(c->pollfd, (c->nports + 1) * N_CLOCK_PFD, -1); if (cnt < 0) { if (EINTR == errno) { return 0; @@ -1156,39 +1224,38 @@ int clock_poll(struct clock *c) return 0; } - for (i = 0; i < c->nports; i++) { - + cur = c->pollfd; + LIST_FOREACH(p, &c->ports, list) { /* Let the ports handle their events. */ - for (j = err = 0; j < N_POLLFD && !err; j++) { - k = N_CLOCK_PFD * i + j; - if (c->pollfd[k].revents & (POLLIN|POLLPRI)) { - event = port_event(c->port[i], j); + for (i = err = 0; i < N_POLLFD && !err; i++) { + if (cur[i].revents & (POLLIN|POLLPRI)) { + event = port_event(p, i); if (EV_STATE_DECISION_EVENT == event) sde = 1; if (EV_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES == event) sde = 1; - err = port_dispatch(c->port[i], event, 0); + err = port_dispatch(p, event, 0); /* Clear any fault after a little while. */ - if (PS_FAULTY == port_state(c->port[i])) { - clock_fault_timeout(c, i, 1); + if (PS_FAULTY == port_state(p)) { + clock_fault_timeout(p, 1); break; } } } /* Check the fault timer. */ - k = N_CLOCK_PFD * i + N_POLLFD; - if (c->pollfd[k].revents & (POLLIN|POLLPRI)) { - clock_fault_timeout(c, i, 0); - port_dispatch(c->port[i], EV_FAULT_CLEARED, 0); + if (cur[N_POLLFD].revents & (POLLIN|POLLPRI)) { + clock_fault_timeout(p, 0); + port_dispatch(p, EV_FAULT_CLEARED, 0); } + + cur += N_CLOCK_PFD; } /* Check the UDS port. */ - for (j = 0; j < N_POLLFD; j++) { - k = N_CLOCK_PFD * i + j; - if (c->pollfd[k].revents & (POLLIN|POLLPRI)) { - event = port_event(c->uds_port, j); + for (i = 0; i < N_POLLFD; i++) { + if (cur[i].revents & (POLLIN|POLLPRI)) { + event = port_event(c->uds_port, i); if (EV_STATE_DECISION_EVENT == event) sde = 1; } @@ -1262,22 +1329,6 @@ void clock_peer_delay(struct clock *c, tmv_t ppd, double nrr) stats_add_value(c->stats.delay, tmv_to_nanoseconds(ppd)); } -void clock_remove_fda(struct clock *c, struct port *p, struct fdarray fda) -{ - int i, j, k; - for (i = 0; i < c->nports; i++) { - if (p == c->port[i]) - break; - } - if (p == c->uds_port) - i = c->nports; - for (j = 0; j < N_POLLFD; j++) { - k = N_CLOCK_PFD * i + j; - c->pollfd[k].fd = -1; - c->pollfd[k].events = 0; - } -} - int clock_slave_only(struct clock *c) { return c->dds.flags & DDS_SLAVE_ONLY; @@ -1402,10 +1453,11 @@ static void handle_state_decision_event(struct clock *c) { struct foreign_clock *best = NULL, *fc; struct ClockIdentity best_id; - int fresh_best = 0, i; + struct port *piter; + int fresh_best = 0; - for (i = 0; i < c->nports; i++) { - fc = port_compute_best(c->port[i]); + LIST_FOREACH(piter, &c->ports, list) { + fc = port_compute_best(piter); if (!fc) continue; if (!best || dscmp(&fc->dataset, &best->dataset) > 0) @@ -1434,10 +1486,10 @@ static void handle_state_decision_event(struct clock *c) c->best = best; c->best_id = best_id; - for (i = 0; i < c->nports; i++) { + LIST_FOREACH(piter, &c->ports, list) { enum port_state ps; enum fsm_event event; - ps = bmc_state_decision(c, c->port[i]); + ps = bmc_state_decision(c, piter); switch (ps) { case PS_LISTENING: event = EV_NONE; @@ -1461,7 +1513,7 @@ static void handle_state_decision_event(struct clock *c) event = EV_FAULT_DETECTED; break; } - port_dispatch(c->port[i], event, fresh_best); + port_dispatch(piter, event, fresh_best); } } diff --git a/clock.h b/clock.h index 92ec163..0702aca 100644 --- a/clock.h +++ b/clock.h @@ -117,12 +117,11 @@ int clock_gm_capable(struct clock *c); struct ClockIdentity clock_identity(struct clock *c); /** - * Install a port's file descriptor array into its controlling clock. + * Informs clock that a file descriptor of one of its ports changed. The + * clock will rebuild its array of file descriptors to poll. * @param c The clock instance. - * @param p The port installing the array. - * @param fda The port's open file decriptors for its sockets and timers. */ -void clock_install_fda(struct clock *c, struct port *p, struct fdarray fda); +void clock_fda_changed(struct clock *c); /** * Manage the clock according to a given message. @@ -192,14 +191,6 @@ void clock_peer_delay(struct clock *c, tmv_t ppd, double nrr); */ int clock_poll(struct clock *c); -/** - * Remove a port's file descriptor array from its controlling clock. - * @param c The clock instance. - * @param p The port removing the array. - * @param fda The port's file decriptor array. - */ -void clock_remove_fda(struct clock *c, struct port *p, struct fdarray fda); - /** * Obtain the slave-only flag from a clock's default data set. * @param c The clock instance. diff --git a/port.c b/port.c index bd1b178..18405c6 100644 --- a/port.c +++ b/port.c @@ -22,6 +22,7 @@ #include #include #include +#include #include "bmc.h" #include "clock.h" @@ -60,6 +61,7 @@ struct nrate_estimator { }; struct port { + LIST_ENTRY(port) list; char *name; struct clock *clock; struct transport *trp; @@ -199,6 +201,11 @@ int port_fault_fd(struct port *port) return port->fault_fd; } +struct fdarray *port_fda(struct port *port) +{ + return &port->fda; +} + int set_tmo_log(int fd, unsigned int scale, int log_seconds) { struct itimerspec tmo = { @@ -1360,6 +1367,14 @@ static void flush_peer_delay(struct port *p) } } +static void port_clear_fda(struct port *p, int count) +{ + int i; + + for (i = 0; i < count; i++) + p->fda.fd[i] = -1; +} + static void port_disable(struct port *p) { int i; @@ -1370,12 +1385,13 @@ static void port_disable(struct port *p) p->best = NULL; free_foreign_masters(p); - clock_remove_fda(p->clock, p, p->fda); transport_close(p->trp, &p->fda); for (i = 0; i < N_TIMER_FDS; i++) { close(p->fda.fd[FD_ANNOUNCE_TIMER + i]); } + port_clear_fda(p, N_TIMER_FDS); + clock_fda_changed(p->clock); } static int port_initialize(struct port *p) @@ -1418,7 +1434,7 @@ static int port_initialize(struct port *p) port_nrate_initialize(p); - clock_install_fda(p->clock, p, p->fda); + clock_fda_changed(p->clock); return 0; no_tmo: @@ -1434,16 +1450,18 @@ no_timers: static int port_renew_transport(struct port *p) { + int res; + if (!port_is_enabled(p)) { return 0; } - clock_remove_fda(p->clock, p, p->fda); transport_close(p->trp, &p->fda); - if (transport_open(p->trp, p->name, &p->fda, p->timestamping)) { - return -1; - } - clock_install_fda(p->clock, p, p->fda); - return 0; + port_clear_fda(p, FD_ANNOUNCE_TIMER); + res = transport_open(p->trp, p->name, &p->fda, p->timestamping); + /* Need to call clock_fda_changed even if transport_open failed in + * order to update clock to the now closed descriptors. */ + clock_fda_changed(p->clock); + return res; } /* @@ -2236,6 +2254,11 @@ struct PortIdentity port_identity(struct port *p) return p->portIdentity; } +int port_number(struct port *p) +{ + return portnum(p); +} + int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg) { struct management_tlv *mgt; @@ -2451,6 +2474,7 @@ struct port *port_open(int phc_index, } p->nrate.ratio = 1.0; + port_clear_fda(p, N_TIMER_FDS); p->fault_fd = -1; if (number) { p->fault_fd = timerfd_create(CLOCK_MONOTONIC, 0); diff --git a/port.h b/port.h index 20d0f4c..662eaef 100644 --- a/port.h +++ b/port.h @@ -119,6 +119,13 @@ int port_prepare_and_send(struct port *p, struct ptp_message *msg, int event); */ struct PortIdentity port_identity(struct port *p); +/** + * Obtain a port number. + * @param p A port instance. + * @return The port number of 'p'. + */ +int port_number(struct port *p); + /** * Manage a port according to a given message. * @param p A pointer previously obtained via port_open(). @@ -200,6 +207,15 @@ struct port *port_open(int phc_index, */ enum port_state port_state(struct port *port); +/** + * Return array of file descriptors for this port. The fault fd is not + * included. + * @param port A port instance + * @return Array of file descriptors. Unused descriptors are guranteed + * to be set to -1. + */ +struct fdarray *port_fda(struct port *port); + /** * Return file descriptor of the port. * @param port A port instance.