diff --git a/clock.c b/clock.c index 5b251c1..726999e 100644 --- a/clock.c +++ b/clock.c @@ -21,6 +21,7 @@ #include #include #include +#include #include "address.h" #include "bmc.h" @@ -40,11 +41,12 @@ #include "uds.h" #include "util.h" -#define CLK_N_PORTS (MAX_PORTS + 1) /* plus one for the UDS interface */ #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; @@ -80,10 +82,12 @@ struct clock { struct ClockIdentity ptl[PATH_TRACE_MAX]; struct foreign_clock *best; struct ClockIdentity best_id; - struct port *port[CLK_N_PORTS]; - struct pollfd pollfd[CLK_N_PORTS*N_CLOCK_PFD]; - int fault_fd[CLK_N_PORTS]; + LIST_HEAD(ports_head, port) ports; + struct port *uds_port; + struct pollfd *pollfd; + int pollfd_valid; int nports; /* does not include the UDS port */ + int last_port_number; int free_running; int freq_est_interval; int grand_master_capable; /* for 802.1AS only */ @@ -116,6 +120,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) { @@ -234,7 +240,7 @@ void clock_send_notification(struct clock *c, struct ptp_message *msg, { unsigned int event_pos = event / 8; uint8_t mask = 1 << (event % 8); - struct port *uds = c->port[c->nports]; + struct port *uds = c->uds_port; struct clock_subscriber *s; LIST_FOREACH(s, &c->subscribers, list) { @@ -254,14 +260,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]); - close(c->fault_fd[i]); + LIST_FOREACH_SAFE(p, &c->ports, list, tmp) { + clock_remove_port(c, p); } - port_close(c->port[i]); /*uds*/ + port_close(c->uds_port); + free(c->pollfd); if (c->clkid != CLOCK_REALTIME) { phc_close(c->clkid); } @@ -276,25 +282,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 set_tmo_lin(c->fault_fd[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 set_tmo_lin(c->fault_fd[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 set_tmo_log(c->fault_fd[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); @@ -433,7 +439,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p, respond = 1; break; case TLV_SUBSCRIBE_EVENTS_NP: - if (p != c->port[c->nports]) { + if (p != c->uds_port) { /* Only the UDS port allowed. */ break; } @@ -733,7 +739,7 @@ static int forwarding(struct clock *c, struct port *p) default: break; } - if (p == c->port[c->nports] && ps != PS_FAULTY) { /*uds*/ + if (p == c->uds_port && ps != PS_FAULTY) { return 1; } return 0; @@ -746,14 +752,49 @@ UInteger8 clock_class(struct clock *c) return c->dds.clockQuality.clockClass; } -struct clock *clock_create(int phc_index, struct interface *iface, int count, +static int clock_add_port(struct clock *c, int phc_index, + enum timestamp_type timestamping, + struct interface *iface) +{ + struct port *p; + + if (clock_resize_pollfd(c, c->nports + 1)) + return -1; + p = port_open(phc_index, timestamping, ++c->last_port_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 interfaces_head *ifaces, 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; + int 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 interface *iface; struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); @@ -848,45 +889,40 @@ 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->fault_fd[i] = timerfd_create(CLOCK_MONOTONIC, 0); - if (c->fault_fd[i] < 0) { - pr_err("timerfd_create failed: %m"); - return NULL; - } - c->pollfd[N_CLOCK_PFD * i + N_POLLFD].fd = c->fault_fd[i]; - c->pollfd[N_CLOCK_PFD * i + N_POLLFD].events = POLLIN|POLLPRI; - } + LIST_INIT(&c->ports); + c->last_port_number = 0; /* - * One extra port is for the UDS interface. + * Create the UDS interface. */ - c->port[i] = port_open(phc_index, timestamping, 0, udsif, c); - if (!c->port[i]) { + 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. */ + STAILQ_FOREACH(iface, ifaces, list) { + if (clock_add_port(c, phc_index, timestamping, iface)) { + pr_err("failed to open port %s", iface->name); + return NULL; + } + } - for (i = 0; i < c->nports; i++) - port_dispatch(c->port[i], EV_INITIALIZE, 0); + c->dds.numberPorts = c->nports; - port_dispatch(c->port[i], EV_INITIALIZE, 0); /*uds*/ + LIST_FOREACH(p, &c->ports, list) { + port_dispatch(p, EV_INITIALIZE, 0); + } + port_dispatch(c->uds_port, EV_INITIALIZE, 0); return c; } @@ -943,40 +979,84 @@ 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; - for (i = 0; i < c->nports + 1; i++) { - if (p == c->port[i]) - break; + struct pollfd *new_pollfd; + + /* 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; } - 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; +} + +static void clock_check_pollfd(struct clock *c) +{ + struct port *p; + struct pollfd *dest = c->pollfd; + + if (c->pollfd_valid) + return; + LIST_FOREACH(p, &c->ports, list) { + clock_fill_pollfd(dest, p); + dest += N_CLOCK_PFD; } + clock_fill_pollfd(dest, c->uds_port); + c->pollfd_valid = 1; +} + +void clock_fda_changed(struct clock *c) +{ + c->pollfd_valid = 0; +} + +static int clock_do_forward_mgmt(struct clock *c, + struct port *in, struct port *out, + struct ptp_message *msg, int *pre_sent) +{ + if (in == out || !forwarding(c, out)) + return 0; + if (!*pre_sent) { + /* delay calling msg_pre_send until + * actually forwarding */ + msg_pre_send(msg); + *pre_sent = 1; + } + return port_forward(out, msg); } 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 *fwd; + struct port *piter; + int pdulen = 0, msg_ready = 0; + if (forwarding(c, p) && msg->management.boundaryHops) { - for (i = 0; i < c->nports + 1; i++) { - fwd = c->port[i]; - if (fwd != p && forwarding(c, fwd)) { - /* delay calling msg_pre_send until - * actually forwarding */ - if (!msg_ready) { - msg_ready = 1; - pdulen = msg->header.messageLength; - msg->management.boundaryHops--; - msg_pre_send(msg); - } - if (port_forward(fwd, msg)) - pr_err("port %d: management forward failed", i + 1); - } + pdulen = msg->header.messageLength; + msg->management.boundaryHops--; + 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) { msg_post_recv(msg, pdulen); msg->management.boundaryHops++; @@ -986,7 +1066,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} @@ -1021,7 +1102,7 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) clock_management_send_error(p, msg, TLV_WRONG_LENGTH); return changed; } - if (p != c->port[c->nports]) { + if (p != c->uds_port) { /* Sorry, only allowed on the UDS port. */ clock_management_send_error(p, msg, TLV_NOT_SUPPORTED); return changed; @@ -1037,7 +1118,7 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) switch (mgt->id) { case TLV_PORT_PROPERTIES_NP: - if (p != c->port[c->nports]) { + if (p != c->uds_port) { /* Only the UDS port allowed. */ clock_management_send_error(p, msg, TLV_NOT_SUPPORTED); return 0; @@ -1082,8 +1163,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) @@ -1101,7 +1182,7 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) void clock_notify_event(struct clock *c, enum notification event) { - struct port *uds = c->port[c->nports]; + struct port *uds = c->uds_port; struct PortIdentity pid = port_identity(uds); struct ptp_message *msg; UInteger16 msg_len; @@ -1139,10 +1220,13 @@ 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); + clock_check_pollfd(c); + cnt = poll(c->pollfd, (c->nports + 1) * N_CLOCK_PFD, -1); if (cnt < 0) { if (EINTR == errno) { return 0; @@ -1154,39 +1238,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->port[i], 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; } @@ -1260,20 +1343,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 + 1; i++) { - if (p == c->port[i]) - break; - } - 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; @@ -1398,10 +1467,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) @@ -1430,10 +1500,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; @@ -1457,7 +1527,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..a2b46be 100644 --- a/clock.h +++ b/clock.h @@ -63,14 +63,13 @@ UInteger8 clock_class(struct clock *c); * * @param phc_index PTP hardware clock device to use. * Pass -1 to select CLOCK_REALTIME. - * @param interface An array of network interfaces. - * @param count The number of elements in @a interfaces. + * @param ifaces A queue of network interfaces. * @param timestamping The timestamping mode for this clock. * @param dds A pointer to a default data set for the clock. * @param servo The servo that this clock will use. * @return A pointer to the single global clock instance. */ -struct clock *clock_create(int phc_index, struct interface *iface, int count, +struct clock *clock_create(int phc_index, struct interfaces_head *ifaces, enum timestamp_type timestamping, struct default_ds *dds, enum servo_type servo); @@ -117,12 +116,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 +190,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/config.c b/config.c index 0983c62..1c03fee 100644 --- a/config.c +++ b/config.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include "config.h" #include "ether.h" @@ -162,40 +163,40 @@ static enum parser_result parse_pod_setting(const char *option, static enum parser_result parse_port_setting(const char *option, const char *value, struct config *cfg, - int p) + struct interface *iface) { enum parser_result r; int val; - r = parse_pod_setting(option, value, &cfg->iface[p].pod); + r = parse_pod_setting(option, value, &iface->pod); if (r != NOT_PARSED) return r; if (!strcmp(option, "network_transport")) { if (!strcasecmp("L2", value)) - cfg->iface[p].transport = TRANS_IEEE_802_3; + iface->transport = TRANS_IEEE_802_3; else if (!strcasecmp("UDPv4", value)) - cfg->iface[p].transport = TRANS_UDP_IPV4; + iface->transport = TRANS_UDP_IPV4; else if (!strcasecmp("UDPv6", value)) - cfg->iface[p].transport = TRANS_UDP_IPV6; + iface->transport = TRANS_UDP_IPV6; else return BAD_VALUE; } else if (!strcmp(option, "delay_mechanism")) { if (!strcasecmp("Auto", value)) - cfg->iface[p].dm = DM_AUTO; + iface->dm = DM_AUTO; else if (!strcasecmp("E2E", value)) - cfg->iface[p].dm = DM_E2E; + iface->dm = DM_E2E; else if (!strcasecmp("P2P", value)) - cfg->iface[p].dm = DM_P2P; + iface->dm = DM_P2P; else return BAD_VALUE; } else if (!strcmp(option, "delay_filter")) { if (!strcasecmp("moving_average", value)) - cfg->iface[p].delay_filter = FILTER_MOVING_AVERAGE; + iface->delay_filter = FILTER_MOVING_AVERAGE; else if (!strcasecmp("moving_median", value)) - cfg->iface[p].delay_filter = FILTER_MOVING_MEDIAN; + iface->delay_filter = FILTER_MOVING_MEDIAN; else return BAD_VALUE; @@ -203,7 +204,7 @@ static enum parser_result parse_port_setting(const char *option, r = get_ranged_int(value, &val, 1, INT_MAX); if (r != PARSED_OK) return r; - cfg->iface[p].delay_filter_length = val; + iface->delay_filter_length = val; } else return NOT_PARSED; @@ -616,7 +617,8 @@ int config_read(char *name, struct config *cfg) FILE *fp; char buf[1024], *line, *c; const char *option, *value; - int current_port = 0, line_num; + struct interface *current_port = NULL; + int line_num; fp = 0 == strncmp(name, "-", 2) ? stdin : fopen(name, "r"); @@ -652,8 +654,9 @@ int config_read(char *name, struct config *cfg) goto parse_error; } current_port = config_create_interface(port, cfg); - if (current_port < 0) + if (!current_port) goto parse_error; + config_init_interface(current_port, cfg); } continue; } @@ -665,7 +668,7 @@ int config_read(char *name, struct config *cfg) fprintf(stderr, "could not parse line %d in %s section\n", line_num, current_section == GLOBAL_SECTION ? - "global" : cfg->iface[current_port].name); + "global" : current_port->name); goto parse_error; } @@ -683,7 +686,7 @@ int config_read(char *name, struct config *cfg) fprintf(stderr, "unknown option %s at line %d in %s section\n", option, line_num, current_section == GLOBAL_SECTION ? - "global" : cfg->iface[current_port].name); + "global" : current_port->name); goto parse_error; case BAD_VALUE: fprintf(stderr, "%s is a bad value for option %s at line %d\n", @@ -717,36 +720,45 @@ parse_error: return -2; } -/* returns the number matching that interface, or -1 on failure */ -int config_create_interface(char *name, struct config *cfg) +struct interface *config_create_interface(char *name, struct config *cfg) { struct interface *iface; - int i; - - if (cfg->nports >= MAX_PORTS) { - fprintf(stderr, "more than %d ports specified\n", MAX_PORTS); - return -1; - } - - iface = &cfg->iface[cfg->nports]; /* only create each interface once (by name) */ - for(i = 0; i < cfg->nports; i++) { - if (0 == strncmp(name, cfg->iface[i].name, MAX_IFNAME_SIZE)) - return i; + STAILQ_FOREACH(iface, &cfg->interfaces, list) { + if (0 == strncmp(name, iface->name, MAX_IFNAME_SIZE)) + return iface; + } + + iface = calloc(1, sizeof(struct interface)); + if (!iface) { + fprintf(stderr, "cannot allocate memory for a port\n"); + return NULL; } strncpy(iface->name, name, MAX_IFNAME_SIZE); + STAILQ_INSERT_TAIL(&cfg->interfaces, iface, list); + return iface; +} + +void config_init_interface(struct interface *iface, struct config *cfg) +{ iface->dm = cfg->dm; iface->transport = cfg->transport; memcpy(&iface->pod, &cfg->pod, sizeof(cfg->pod)); - sk_get_ts_info(name, &iface->ts_info); + sk_get_ts_info(iface->name, &iface->ts_info); iface->delay_filter = cfg->dds.delay_filter; iface->delay_filter_length = cfg->dds.delay_filter_length; - - cfg->nports++; - - return i; +} + +void config_destroy(struct config *cfg) +{ + struct interface *iface; + + while ((iface = STAILQ_FIRST(&cfg->interfaces))) { + STAILQ_REMOVE_HEAD(&cfg->interfaces, list); + free(iface); + } } diff --git a/config.h b/config.h index be5a651..9b74f12 100644 --- a/config.h +++ b/config.h @@ -20,6 +20,8 @@ #ifndef HAVE_CONFIG_H #define HAVE_CONFIG_H +#include + #include "ds.h" #include "dm.h" #include "filter.h" @@ -27,11 +29,11 @@ #include "servo.h" #include "sk.h" -#define MAX_PORTS 8 #define MAX_IFNAME_SIZE 108 /* = UNIX_PATH_MAX */ /** Defines a network interface, with PTP options. */ struct interface { + STAILQ_ENTRY(interface) list; char name[MAX_IFNAME_SIZE + 1]; enum delay_mechanism dm; enum transport_type transport; @@ -62,8 +64,7 @@ struct config { int cfg_ignore; /* configured interfaces */ - struct interface iface[MAX_PORTS]; - int nports; + STAILQ_HEAD(interfaces_head, interface) interfaces; enum timestamp_type timestamping; enum transport_type transport; @@ -102,6 +103,8 @@ struct config { }; int config_read(char *name, struct config *cfg); -int config_create_interface(char *name, struct config *cfg); +struct interface *config_create_interface(char *name, struct config *cfg); +void config_init_interface(struct interface *iface, struct config *cfg); +void config_destroy(struct config *cfg); #endif diff --git a/port.c b/port.c index 080bc8e..18405c6 100644 --- a/port.c +++ b/port.c @@ -22,6 +22,7 @@ #include #include #include +#include #include "bmc.h" #include "clock.h" @@ -60,11 +61,13 @@ struct nrate_estimator { }; struct port { + LIST_ENTRY(port) list; char *name; struct clock *clock; struct transport *trp; enum timestamp_type timestamping; struct fdarray fda; + int fault_fd; struct foreign_clock *best; enum syfu_state syfu; struct ptp_message *last_syncfup; @@ -193,6 +196,16 @@ int fault_interval(struct port *port, enum fault_type ft, return 0; } +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 = { @@ -253,6 +266,17 @@ int set_tmo_random(int fd, int min, int span, int log_seconds) return timerfd_settime(fd, 0, &tmo, NULL); } +int port_set_fault_timer_log(struct port *port, + unsigned int scale, int log_seconds) +{ + return set_tmo_log(port->fault_fd, scale, log_seconds); +} + +int port_set_fault_timer_lin(struct port *port, int seconds) +{ + return set_tmo_lin(port->fault_fd, seconds); +} + static void fc_clear(struct foreign_clock *fc) { struct ptp_message *m; @@ -1343,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; @@ -1353,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) @@ -1401,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: @@ -1417,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; } /* @@ -1903,6 +1938,8 @@ void port_close(struct port *p) } transport_destroy(p->trp); filter_destroy(p->delay_filter); + if (p->fault_fd >= 0) + close(p->fault_fd); free(p); } @@ -2217,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; @@ -2408,18 +2450,15 @@ struct port *port_open(int phc_index, pr_err("port %d: PHC device mismatch", number); pr_err("port %d: /dev/ptp%d requested, but /dev/ptp%d attached", number, phc_index, interface->ts_info.phc_index); - free(p); - return NULL; + goto err_port; } p->pod = interface->pod; p->name = interface->name; p->clock = clock; p->trp = transport_create(interface->transport); - if (!p->trp) { - free(p); - return NULL; - } + if (!p->trp) + goto err_port; p->timestamping = timestamping; p->portIdentity.clockIdentity = clock_identity(clock); p->portIdentity.portNumber = number; @@ -2431,12 +2470,28 @@ struct port *port_open(int phc_index, interface->delay_filter_length); if (!p->delay_filter) { pr_err("Failed to create delay filter"); - transport_destroy(p->trp); - free(p); - return NULL; + goto err_transport; } 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); + if (p->fault_fd < 0) { + pr_err("timerfd_create failed: %m"); + goto err_filter; + } + } return p; + +err_filter: + filter_destroy(p->delay_filter); +err_transport: + transport_destroy(p->trp); +err_port: + free(p); + return NULL; } enum port_state port_state(struct port *port) diff --git a/port.h b/port.h index a1b8ad7..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,22 @@ 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. + * @return File descriptor or -1 if not applicable. + */ +int port_fault_fd(struct port *port); + /** * Utility function for setting or resetting a file descriptor timer. * @@ -244,6 +267,28 @@ int set_tmo_random(int fd, int min, int span, int log_seconds); */ int set_tmo_lin(int fd, int seconds); +/** + * Sets port's fault file descriptor timer. + * Passing both 'scale' and 'log_seconds' as zero disables the timer. + * + * @param fd A port instance. + * @param scale The multiplicative factor for the timer. + * @param log_seconds The exponential factor for the timer. + * @return Zero on success, non-zero otherwise. + */ +int port_set_fault_timer_log(struct port *port, + unsigned int scale, int log_seconds); + +/** + * Sets port's fault file descriptor timer. + * Passing 'seconds' as zero disables the timer. + * + * @param fd A port instance. + * @param seconds The timeout value for the timer. + * @return Zero on success, non-zero otherwise. + */ +int port_set_fault_timer_lin(struct port *port, int seconds); + /** * Returns a port's last fault type. * diff --git a/ptp4l.c b/ptp4l.c index 5080a79..83824f7 100644 --- a/ptp4l.c +++ b/ptp4l.c @@ -40,6 +40,8 @@ int assume_two_step = 0; static struct config cfg_settings = { + .interfaces = STAILQ_HEAD_INITIALIZER(cfg_settings.interfaces), + .dds = { .dds = { .flags = DDS_TWO_STEP_FLAG, @@ -164,9 +166,7 @@ int main(int argc, char *argv[]) { char *config = NULL, *req_phc = NULL, *progname; int c, i; - struct interface *iface = cfg_settings.iface; - char *ports[MAX_PORTS]; - int nports = 0; + struct interface *iface; int *cfg_ignore = &cfg_settings.cfg_ignore; enum delay_mechanism *dm = &cfg_settings.dm; enum transport_type *transport = &cfg_settings.transport; @@ -229,8 +229,8 @@ int main(int argc, char *argv[]) config = optarg; break; case 'i': - ports[nports] = optarg; - nports++; + if (!config_create_interface(optarg, &cfg_settings)) + return -1; break; case 'p': req_phc = optarg; @@ -291,14 +291,7 @@ int main(int argc, char *argv[]) print_set_syslog(cfg_settings.use_syslog); print_set_level(cfg_settings.print_level); - for (i = 0; i < nports; i++) { - if (config_create_interface(ports[i], &cfg_settings) < 0) { - fprintf(stderr, "too many interfaces\n"); - return -1; - } - } - - if (!cfg_settings.nports) { + if (STAILQ_EMPTY(&cfg_settings.interfaces)) { fprintf(stderr, "no interface specified\n"); usage(progname); return -1; @@ -338,18 +331,21 @@ int main(int argc, char *argv[]) break; } - /* check whether timestamping mode is supported. */ - for (i = 0; i < cfg_settings.nports; i++) { - if (iface[i].ts_info.valid && - ((iface[i].ts_info.so_timestamping & required_modes) != required_modes)) { + /* Init interface configs and check whether timestamping mode is + * supported. */ + STAILQ_FOREACH(iface, &cfg_settings.interfaces, list) { + config_init_interface(iface, &cfg_settings); + if (iface->ts_info.valid && + ((iface->ts_info.so_timestamping & required_modes) != required_modes)) { fprintf(stderr, "interface '%s' does not support " "requested timestamping mode.\n", - iface[i].name); + iface->name); return -1; } } /* determine PHC Clock index */ + iface = STAILQ_FIRST(&cfg_settings.interfaces); if (cfg_settings.dds.free_running) { phc_index = -1; } else if (*timestamping == TS_SOFTWARE || *timestamping == TS_LEGACY_HW) { @@ -359,8 +355,8 @@ int main(int argc, char *argv[]) fprintf(stderr, "bad ptp device string\n"); return -1; } - } else if (iface[0].ts_info.valid) { - phc_index = iface[0].ts_info.phc_index; + } else if (iface->ts_info.valid) { + phc_index = iface->ts_info.phc_index; } else { fprintf(stderr, "ptp device not specified and\n" "automatic determination is not\n" @@ -372,12 +368,12 @@ int main(int argc, char *argv[]) pr_info("selected /dev/ptp%d as PTP clock", phc_index); } - if (generate_clock_identity(&ds->clockIdentity, iface[0].name)) { + if (generate_clock_identity(&ds->clockIdentity, iface->name)) { fprintf(stderr, "failed to generate a clock identity\n"); return -1; } - clock = clock_create(phc_index, iface, cfg_settings.nports, + clock = clock_create(phc_index, &cfg_settings.interfaces, *timestamping, &cfg_settings.dds, cfg_settings.clock_servo); if (!clock) { @@ -391,5 +387,6 @@ int main(int argc, char *argv[]) } clock_destroy(clock); + config_destroy(&cfg_settings); return 0; }