From 08575133b3462563979c47aa439cc81156463c7f Mon Sep 17 00:00:00 2001 From: Jiri Benc Date: Thu, 14 Aug 2014 15:56:03 +0200 Subject: [PATCH] Dynamic port allocation Remove the limit of MAX_PORTS ports (default 8) and keep the ports in a linked list. This allows ptp4l to be used on large machines and in the future, it will allow dynamic adding and removing of ports while ptp4l is running. For this to work, pollfd needs to be dynamically allocated. Changed pollfd handling from clock_install_fda/clock_remove_fda to notification (clock_fda_changed), where the clock will rebuild pollfd by querying all its ports. Signed-off-by: Jiri Benc --- clock.c | 234 ++++++++++++++++++++++++++++++++++---------------------- clock.h | 15 +--- port.c | 40 ++++++++-- port.h | 16 ++++ 4 files changed, 194 insertions(+), 111 deletions(-) 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.