diff --git a/clock.c b/clock.c index 9bbcefa..faf2dea 100644 --- a/clock.c +++ b/clock.c @@ -17,11 +17,11 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include -#include #include #include #include #include +#include #include #include "address.h" diff --git a/clock.c.headers b/clock.c.headers deleted file mode 100644 index faf2dea..0000000 --- a/clock.c.headers +++ /dev/null @@ -1,1808 +0,0 @@ -/** - * @file clock.c - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include - -#include "address.h" -#include "bmc.h" -#include "clock.h" -#include "clockadj.h" -#include "clockcheck.h" -#include "foreign.h" -#include "filter.h" -#include "missing.h" -#include "msg.h" -#include "phc.h" -#include "port.h" -#include "servo.h" -#include "stats.h" -#include "print.h" -#include "rtnl.h" -#include "tlv.h" -#include "tsproc.h" -#include "uds.h" -#include "util.h" - -#define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer */ -#define POW2_41 ((double)(1ULL << 41)) - -struct port { - LIST_ENTRY(port) list; -}; - -struct freq_estimator { - tmv_t origin1; - tmv_t ingress1; - unsigned int max_count; - unsigned int count; -}; - -struct clock_stats { - struct stats *offset; - struct stats *freq; - struct stats *delay; - 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; - time_t expiration; -}; - -struct clock { - enum clock_type type; - struct config *config; - clockid_t clkid; - struct servo *servo; - enum servo_type servo_type; - int (*dscmp)(struct dataset *a, struct dataset *b); - struct defaultDS dds; - struct dataset default_dataset; - struct currentDS cur; - struct parent_ds dad; - struct timePropertiesDS tds; - struct ClockIdentity ptl[PATH_TRACE_MAX]; - struct foreign_clock *best; - struct ClockIdentity best_id; - 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 sde; - int free_running; - int freq_est_interval; - int grand_master_capable; /* for 802.1AS only */ - int utc_timescale; - int utc_offset_set; - int leap_set; - int kernel_leap; - int utc_offset; - int time_flags; /* grand master role */ - int time_source; /* grand master role */ - enum servo_state servo_state; - enum timestamp_type timestamping; - tmv_t master_offset; - tmv_t path_delay; - tmv_t ingress_ts; - tmv_t initial_delay; - struct tsproc *tsproc; - struct freq_estimator fest; - struct time_status_np status; - double master_local_rr; /* maintained when free_running */ - double nrr; - struct clock_description desc; - struct clock_stats stats; - int stats_interval; - struct clockcheck *sanity_check; - struct interface uds_interface; - LIST_HEAD(clock_subscribers_head, clock_subscriber) subscribers; -}; - -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 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, uint16_t duration) -{ - struct clock_subscriber *s; - int i, remove = 1; - struct timespec now; - - for (i = 0; i < EVENT_BITMASK_CNT; i++) { - if (bitmask[i]) { - remove = 0; - break; - } - } - - LIST_FOREACH(s, &c->subscribers, list) { - if (pid_eq(&s->targetPortIdentity, - &req->header.sourcePortIdentity)) { - /* Found, update the transport address and event - * mask. */ - if (!remove) { - s->addr = req->address; - memcpy(s->events, bitmask, EVENT_BITMASK_CNT); - clock_gettime(CLOCK_MONOTONIC, &now); - s->expiration = now.tv_sec + duration; - } 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); - clock_gettime(CLOCK_MONOTONIC, &now); - s->expiration = now.tv_sec + duration; - 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, uint16_t *duration) -{ - struct clock_subscriber *s; - struct timespec now; - - LIST_FOREACH(s, &c->subscribers, list) { - if (pid_eq(&s->targetPortIdentity, - &req->header.sourcePortIdentity)) { - memcpy(bitmask, s->events, EVENT_BITMASK_CNT); - clock_gettime(CLOCK_MONOTONIC, &now); - if (s->expiration < now.tv_sec) - *duration = 0; - else - *duration = s->expiration - now.tv_sec; - return; - } - } - /* A client without entry means the client has no subscriptions. */ - memset(bitmask, 0, EVENT_BITMASK_CNT); - *duration = 0; -} - -static void clock_flush_subscriptions(struct clock *c) -{ - struct clock_subscriber *s, *tmp; - - LIST_FOREACH_SAFE(s, &c->subscribers, list, tmp) { - remove_subscriber(s); - } -} - -static void clock_prune_subscriptions(struct clock *c) -{ - struct clock_subscriber *s, *tmp; - struct timespec now; - - clock_gettime(CLOCK_MONOTONIC, &now); - LIST_FOREACH_SAFE(s, &c->subscribers, list, tmp) { - if (s->expiration <= now.tv_sec) { - pr_info("subscriber %s timed out", - pid2str(&s->targetPortIdentity)); - remove_subscriber(s); - } - } -} - -void clock_send_notification(struct clock *c, struct ptp_message *msg, - enum notification event) -{ - unsigned int event_pos = event / 8; - uint8_t mask = 1 << (event % 8); - struct port *uds = c->uds_port; - 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) -{ - struct port *p, *tmp; - - clock_flush_subscriptions(c); - 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); - } - servo_destroy(c->servo); - tsproc_destroy(c->tsproc); - stats_destroy(c->stats.offset); - stats_destroy(c->stats.freq); - stats_destroy(c->stats.delay); - if (c->sanity_check) { - clockcheck_destroy(c->sanity_check); - } - memset(c, 0, sizeof(*c)); - msg_cleanup(); - tc_cleanup(); -} - -static int clock_fault_timeout(struct port *port, int set) -{ - struct fault_interval i; - - if (!set) { - pr_debug("clearing fault on port %d", port_number(port)); - return port_set_fault_timer_lin(port, 0); - } - - 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, 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, port_number(port)); - return port_set_fault_timer_log(port, 1, i.val); - } - - pr_err("Unsupported fault interval type %d", i.type); - return -1; -} - -static void clock_freq_est_reset(struct clock *c) -{ - c->fest.origin1 = tmv_zero(); - c->fest.ingress1 = tmv_zero(); - c->fest.count = 0; -} - -static void clock_management_send_error(struct port *p, - struct ptp_message *msg, int error_id) -{ - if (port_management_error(port_identity(p), p, msg, error_id)) - pr_err("failed to send management error status"); -} - -/* The 'p' and 'req' paremeters are needed for the GET actions that operate - * on per-client datasets. If such actions do not apply to the caller, it is - * allowed to pass both of them as NULL. - */ -static int clock_management_fill_response(struct clock *c, struct port *p, - struct ptp_message *req, - struct ptp_message *rsp, int id) -{ - struct grandmaster_settings_np *gsn; - struct management_tlv_datum *mtd; - struct subscribe_events_np *sen; - struct management_tlv *tlv; - struct time_status_np *tsn; - struct tlv_extra *extra; - struct PTPText *text; - int datalen = 0; - - extra = tlv_extra_alloc(); - if (!extra) { - pr_err("failed to allocate TLV descriptor"); - return 0; - } - extra->tlv = (struct TLV *) rsp->management.suffix; - - tlv = (struct management_tlv *) rsp->management.suffix; - tlv->type = TLV_MANAGEMENT; - tlv->id = id; - - switch (id) { - case TLV_USER_DESCRIPTION: - text = (struct PTPText *) tlv->data; - text->length = c->desc.userDescription.length; - memcpy(text->text, c->desc.userDescription.text, text->length); - datalen = 1 + text->length; - break; - case TLV_DEFAULT_DATA_SET: - memcpy(tlv->data, &c->dds, sizeof(c->dds)); - datalen = sizeof(c->dds); - break; - case TLV_CURRENT_DATA_SET: - memcpy(tlv->data, &c->cur, sizeof(c->cur)); - datalen = sizeof(c->cur); - break; - case TLV_PARENT_DATA_SET: - memcpy(tlv->data, &c->dad.pds, sizeof(c->dad.pds)); - datalen = sizeof(c->dad.pds); - break; - case TLV_TIME_PROPERTIES_DATA_SET: - memcpy(tlv->data, &c->tds, sizeof(c->tds)); - datalen = sizeof(c->tds); - break; - case TLV_PRIORITY1: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->dds.priority1; - datalen = sizeof(*mtd); - break; - case TLV_PRIORITY2: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->dds.priority2; - datalen = sizeof(*mtd); - break; - case TLV_DOMAIN: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->dds.domainNumber; - datalen = sizeof(*mtd); - break; - case TLV_SLAVE_ONLY: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->dds.flags & DDS_SLAVE_ONLY; - datalen = sizeof(*mtd); - break; - case TLV_CLOCK_ACCURACY: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->dds.clockQuality.clockAccuracy; - datalen = sizeof(*mtd); - break; - case TLV_TRACEABILITY_PROPERTIES: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->tds.flags & (TIME_TRACEABLE|FREQ_TRACEABLE); - datalen = sizeof(*mtd); - break; - case TLV_TIMESCALE_PROPERTIES: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = c->tds.flags & PTP_TIMESCALE; - datalen = sizeof(*mtd); - break; - case TLV_TIME_STATUS_NP: - tsn = (struct time_status_np *) tlv->data; - tsn->master_offset = tmv_to_nanoseconds(c->master_offset); - tsn->ingress_time = tmv_to_nanoseconds(c->ingress_ts); - tsn->cumulativeScaledRateOffset = - (Integer32) (c->status.cumulativeScaledRateOffset + - c->nrr * POW2_41 - POW2_41); - tsn->scaledLastGmPhaseChange = c->status.scaledLastGmPhaseChange; - tsn->gmTimeBaseIndicator = c->status.gmTimeBaseIndicator; - tsn->lastGmPhaseChange = c->status.lastGmPhaseChange; - if (cid_eq(&c->dad.pds.grandmasterIdentity, &c->dds.clockIdentity)) - tsn->gmPresent = 0; - else - tsn->gmPresent = 1; - tsn->gmIdentity = c->dad.pds.grandmasterIdentity; - datalen = sizeof(*tsn); - break; - case TLV_GRANDMASTER_SETTINGS_NP: - gsn = (struct grandmaster_settings_np *) tlv->data; - gsn->clockQuality = c->dds.clockQuality; - gsn->utc_offset = c->utc_offset; - gsn->time_flags = c->time_flags; - gsn->time_source = c->time_source; - datalen = sizeof(*gsn); - break; - case TLV_SUBSCRIBE_EVENTS_NP: - if (p != c->uds_port) { - /* Only the UDS port allowed. */ - break; - } - sen = (struct subscribe_events_np *)tlv->data; - clock_get_subscription(c, req, sen->bitmask, &sen->duration); - break; - default: - /* The caller should *not* respond to this message. */ - tlv_extra_recycle(extra); - return 0; - } - if (datalen % 2) { - tlv->data[datalen] = 0; - datalen++; - } - tlv->length = sizeof(tlv->id) + datalen; - rsp->header.messageLength += sizeof(*tlv) + datalen; - msg_tlv_attach(rsp, extra); - - /* The caller can respond to this message. */ - return 1; -} - -static int clock_management_get_response(struct clock *c, struct port *p, - int id, struct ptp_message *req) -{ - struct PortIdentity pid = port_identity(p); - struct ptp_message *rsp; - int respond; - - rsp = port_management_reply(pid, p, req); - if (!rsp) { - return 0; - } - respond = clock_management_fill_response(c, p, req, rsp, id); - if (respond) - port_prepare_and_send(p, rsp, TRANS_GENERAL); - msg_put(rsp); - return respond; -} - -static int clock_management_set(struct clock *c, struct port *p, - int id, struct ptp_message *req, int *changed) -{ - int respond = 0; - struct management_tlv *tlv; - struct management_tlv_datum *mtd; - struct grandmaster_settings_np *gsn; - struct subscribe_events_np *sen; - - tlv = (struct management_tlv *) req->management.suffix; - - switch (id) { - case TLV_PRIORITY1: - mtd = (struct management_tlv_datum *) tlv->data; - c->dds.priority1 = mtd->val; - *changed = 1; - respond = 1; - break; - case TLV_PRIORITY2: - mtd = (struct management_tlv_datum *) tlv->data; - c->dds.priority2 = mtd->val; - *changed = 1; - respond = 1; - break; - case TLV_GRANDMASTER_SETTINGS_NP: - gsn = (struct grandmaster_settings_np *) tlv->data; - c->dds.clockQuality = gsn->clockQuality; - c->utc_offset = gsn->utc_offset; - c->time_flags = gsn->time_flags; - c->time_source = gsn->time_source; - *changed = 1; - respond = 1; - break; - case TLV_SUBSCRIBE_EVENTS_NP: - sen = (struct subscribe_events_np *)tlv->data; - clock_update_subscription(c, req, sen->bitmask, - sen->duration); - respond = 1; - break; - } - if (respond && !clock_management_get_response(c, p, id, req)) - pr_err("failed to send management set response"); - return respond ? 1 : 0; -} - -static void clock_stats_update(struct clock_stats *s, - double offset, double freq) -{ - struct stats_result offset_stats, freq_stats, delay_stats; - - stats_add_value(s->offset, offset); - stats_add_value(s->freq, freq); - - if (stats_get_num_values(s->offset) < s->max_count) - return; - - stats_get_result(s->offset, &offset_stats); - stats_get_result(s->freq, &freq_stats); - - /* Path delay stats are updated separately, they may be empty. */ - if (!stats_get_result(s->delay, &delay_stats)) { - pr_info("rms %4.0f max %4.0f " - "freq %+6.0f +/- %3.0f " - "delay %5.0f +/- %3.0f", - offset_stats.rms, offset_stats.max_abs, - freq_stats.mean, freq_stats.stddev, - delay_stats.mean, delay_stats.stddev); - } else { - pr_info("rms %4.0f max %4.0f " - "freq %+6.0f +/- %3.0f", - offset_stats.rms, offset_stats.max_abs, - freq_stats.mean, freq_stats.stddev); - } - - stats_reset(s->offset); - stats_reset(s->freq); - stats_reset(s->delay); -} - -static enum servo_state clock_no_adjust(struct clock *c, tmv_t ingress, - tmv_t origin) -{ - double fui; - double ratio, freq; - struct freq_estimator *f = &c->fest; - enum servo_state state = SERVO_UNLOCKED; - /* - * The ratio of the local clock freqency to the master clock - * is estimated by: - * - * (ingress_2 - ingress_1) / (origin_2 - origin_1) - * - * Both of the origin time estimates include the path delay, - * but we assume that the path delay is in fact constant. - * By leaving out the path delay altogther, we can avoid the - * error caused by our imperfect path delay measurement. - */ - if (tmv_is_zero(f->ingress1)) { - f->ingress1 = ingress; - f->origin1 = origin; - return state; - } - f->count++; - if (f->count < f->max_count) { - return state; - } - if (tmv_cmp(ingress, f->ingress1) == 0) { - pr_warning("bad timestamps in rate ratio calculation"); - return state; - } - - ratio = tmv_dbl(tmv_sub(origin, f->origin1)) / - tmv_dbl(tmv_sub(ingress, f->ingress1)); - freq = (1.0 - ratio) * 1e9; - - if (c->stats.max_count > 1) { - clock_stats_update(&c->stats, tmv_dbl(c->master_offset), freq); - } else { - pr_info("master offset %10" PRId64 " s%d freq %+7.0f " - "path delay %9" PRId64, - tmv_to_nanoseconds(c->master_offset), state, freq, - tmv_to_nanoseconds(c->path_delay)); - } - - fui = 1.0 + (c->status.cumulativeScaledRateOffset + 0.0) / POW2_41; - - pr_debug("peer/local %.9f", c->nrr); - pr_debug("fup_info %.9f", fui); - pr_debug("product %.9f", fui * c->nrr); - pr_debug("sum-1 %.9f", fui + c->nrr - 1.0); - pr_debug("master/local %.9f", ratio); - pr_debug("diff %+.9f", ratio - (fui + c->nrr - 1.0)); - - f->ingress1 = ingress; - f->origin1 = origin; - f->count = 0; - - c->master_local_rr = ratio; - - return state; -} - -static void clock_update_grandmaster(struct clock *c) -{ - struct parentDS *pds = &c->dad.pds; - memset(&c->cur, 0, sizeof(c->cur)); - memset(c->ptl, 0, sizeof(c->ptl)); - pds->parentPortIdentity.clockIdentity = c->dds.clockIdentity; - pds->parentPortIdentity.portNumber = 0; - pds->grandmasterIdentity = c->dds.clockIdentity; - pds->grandmasterClockQuality = c->dds.clockQuality; - pds->grandmasterPriority1 = c->dds.priority1; - pds->grandmasterPriority2 = c->dds.priority2; - c->dad.path_length = 0; - c->tds.currentUtcOffset = c->utc_offset; - c->tds.flags = c->time_flags; - c->tds.timeSource = c->time_source; -} - -static void clock_update_slave(struct clock *c) -{ - struct parentDS *pds = &c->dad.pds; - struct ptp_message *msg = TAILQ_FIRST(&c->best->messages); - c->cur.stepsRemoved = 1 + c->best->dataset.stepsRemoved; - pds->parentPortIdentity = c->best->dataset.sender; - pds->grandmasterIdentity = msg->announce.grandmasterIdentity; - pds->grandmasterClockQuality = msg->announce.grandmasterClockQuality; - pds->grandmasterPriority1 = msg->announce.grandmasterPriority1; - pds->grandmasterPriority2 = msg->announce.grandmasterPriority2; - c->tds.currentUtcOffset = msg->announce.currentUtcOffset; - c->tds.flags = msg->header.flagField[1]; - c->tds.timeSource = msg->announce.timeSource; - if (!(c->tds.flags & PTP_TIMESCALE)) { - pr_warning("foreign master not using PTP timescale"); - } - if (c->tds.currentUtcOffset < c->utc_offset) { - pr_warning("running in a temporal vortex"); - } - if ((c->tds.flags & UTC_OFF_VALID && c->tds.flags & TIME_TRACEABLE) || - (c->tds.currentUtcOffset > c->utc_offset)) { - pr_info("updating UTC offset to %d", c->tds.currentUtcOffset); - c->utc_offset = c->tds.currentUtcOffset; - } -} - -static int clock_utc_correct(struct clock *c, tmv_t ingress) -{ - struct timespec offset; - int utc_offset, leap, clock_leap; - uint64_t ts; - - if (!c->utc_timescale) - return 0; - - utc_offset = c->utc_offset; - - if (c->tds.flags & LEAP_61) { - leap = 1; - } else if (c->tds.flags & LEAP_59) { - leap = -1; - } else { - leap = 0; - } - - /* Handle leap seconds. */ - if ((leap || c->leap_set) && c->clkid == CLOCK_REALTIME) { - /* If the clock will be stepped, the time stamp has to be the - target time. Ignore possible 1 second error in utc_offset. */ - if (c->servo_state == SERVO_UNLOCKED) { - ts = tmv_to_nanoseconds(tmv_sub(ingress, - c->master_offset)); - if (c->tds.flags & PTP_TIMESCALE) - ts -= utc_offset * NS_PER_SEC; - } else { - ts = tmv_to_nanoseconds(ingress); - } - - /* Suspend clock updates in the last second before midnight. */ - if (is_utc_ambiguous(ts)) { - pr_info("clock update suspended due to leap second"); - return -1; - } - - clock_leap = leap_second_status(ts, c->leap_set, - &leap, &utc_offset); - if (c->leap_set != clock_leap) { - if (c->kernel_leap) - sysclk_set_leap(clock_leap); - else - servo_leap(c->servo, clock_leap); - c->leap_set = clock_leap; - } - } - - /* Update TAI-UTC offset of the system clock if valid and traceable. */ - if (c->tds.flags & UTC_OFF_VALID && c->tds.flags & TIME_TRACEABLE && - c->utc_offset_set != utc_offset && c->clkid == CLOCK_REALTIME) { - sysclk_set_tai_offset(utc_offset); - c->utc_offset_set = utc_offset; - } - - if (!(c->tds.flags & PTP_TIMESCALE)) - return 0; - - offset.tv_sec = utc_offset; - offset.tv_nsec = 0; - /* Local clock is UTC, but master is TAI. */ - c->master_offset = tmv_add(c->master_offset, timespec_to_tmv(offset)); - return 0; -} - -static int forwarding(struct clock *c, struct port *p) -{ - enum port_state ps = port_state(p); - switch (ps) { - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_SLAVE: - case PS_UNCALIBRATED: - case PS_PRE_MASTER: - return 1; - default: - break; - } - if (p == c->uds_port && ps != PS_FAULTY) { - return 1; - } - return 0; -} - -/* public methods */ - -UInteger8 clock_class(struct clock *c) -{ - return c->dds.clockQuality.clockClass; -} - -struct config *clock_config(struct clock *c) -{ - return c->config; -} - -int (*clock_dscmp(struct clock *c))(struct dataset *a, struct dataset *b) -{ - return c->dscmp; -} - -struct currentDS *clock_current_dataset(struct clock *c) -{ - return &c->cur; -} - -static int clock_add_port(struct clock *c, int phc_index, - enum timestamp_type timestamping, - struct interface *iface) -{ - struct port *p, *piter, *lastp = NULL; - - 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_FOREACH(piter, &c->ports, list) { - lastp = piter; - } - if (lastp) { - LIST_INSERT_AFTER(lastp, p, list); - } else { - 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); -} - -int clock_required_modes(struct clock *c) -{ - int required_modes = 0; - - switch (c->timestamping) { - case TS_SOFTWARE: - required_modes |= SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - break; - case TS_LEGACY_HW: - required_modes |= SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_SYS_HARDWARE; - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - required_modes |= SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - break; - default: - break; - } - - return required_modes; -} - -/* - * If we do not have a slave or the rtnl query failed, then use our - * own interface name as the time stamping interface name. - */ -static void ensure_ts_label(struct interface *iface) -{ - if (iface->ts_label[0] == '\0') - strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE); -} - -struct clock *clock_create(enum clock_type type, struct config *config, - const char *phc_device) -{ - enum servo_type servo = config_get_int(config, NULL, "clock_servo"); - enum timestamp_type timestamping; - int fadj = 0, max_adj = 0, sw_ts; - int phc_index, required_modes = 0; - struct clock *c = &the_clock; - struct port *p; - unsigned char oui[OUI_LEN]; - char phc[32], *tmp; - struct interface *iface, *udsif = &c->uds_interface; - struct timespec ts; - int sfl; - - clock_gettime(CLOCK_REALTIME, &ts); - srandom(ts.tv_sec ^ ts.tv_nsec); - - if (c->nports) { - clock_destroy(c); - } - - switch (type) { - case CLOCK_TYPE_ORDINARY: - case CLOCK_TYPE_BOUNDARY: - case CLOCK_TYPE_P2P: - case CLOCK_TYPE_E2E: - c->type = type; - break; - case CLOCK_TYPE_MANAGEMENT: - return NULL; - } - - /* Initialize the defaultDS. */ - c->dds.clockQuality.clockClass = - config_get_int(config, NULL, "clockClass"); - c->dds.clockQuality.clockAccuracy = - config_get_int(config, NULL, "clockAccuracy"); - c->dds.clockQuality.offsetScaledLogVariance = - config_get_int(config, NULL, "offsetScaledLogVariance"); - - c->desc.productDescription.max_symbols = 64; - c->desc.revisionData.max_symbols = 32; - c->desc.userDescription.max_symbols = 128; - - tmp = config_get_string(config, NULL, "productDescription"); - if (count_char(tmp, ';') != 2 || - static_ptp_text_set(&c->desc.productDescription, tmp)) { - pr_err("invalid productDescription '%s'", tmp); - return NULL; - } - tmp = config_get_string(config, NULL, "revisionData"); - if (count_char(tmp, ';') != 2 || - static_ptp_text_set(&c->desc.revisionData, tmp)) { - pr_err("invalid revisionData '%s'", tmp); - return NULL; - } - tmp = config_get_string(config, NULL, "userDescription"); - if (static_ptp_text_set(&c->desc.userDescription, tmp)) { - pr_err("invalid userDescription '%s'", tmp); - return NULL; - } - tmp = config_get_string(config, NULL, "manufacturerIdentity"); - if (OUI_LEN != sscanf(tmp, "%hhx:%hhx:%hhx", &oui[0], &oui[1], &oui[2])) { - pr_err("invalid manufacturerIdentity '%s'", tmp); - return NULL; - } - memcpy(c->desc.manufacturerIdentity, oui, OUI_LEN); - - c->dds.domainNumber = config_get_int(config, NULL, "domainNumber"); - - if (config_get_int(config, NULL, "slaveOnly")) { - c->dds.flags |= DDS_SLAVE_ONLY; - } - if (!config_get_int(config, NULL, "gmCapable") && - c->dds.flags & DDS_SLAVE_ONLY) { - pr_err("Cannot mix 1588 slaveOnly with 802.1AS !gmCapable"); - return NULL; - } - if (!config_get_int(config, NULL, "gmCapable") || - c->dds.flags & DDS_SLAVE_ONLY) { - c->dds.clockQuality.clockClass = 255; - } - c->default_dataset.localPriority = - config_get_int(config, NULL, "G.8275.defaultDS.localPriority"); - - /* Harmonize the twoStepFlag with the time_stamping option. */ - if (config_harmonize_onestep(config)) { - return NULL; - } - if (config_get_int(config, NULL, "twoStepFlag")) { - c->dds.flags |= DDS_TWO_STEP_FLAG; - } - timestamping = config_get_int(config, NULL, "time_stamping"); - if (timestamping == TS_SOFTWARE) { - sw_ts = 1; - } else { - sw_ts = 0; - } - - c->dds.priority1 = config_get_int(config, NULL, "priority1"); - c->dds.priority2 = config_get_int(config, NULL, "priority2"); - - /* Check the time stamping mode on each interface. */ - c->timestamping = timestamping; - required_modes = clock_required_modes(c); - STAILQ_FOREACH(iface, &config->interfaces, list) { - rtnl_get_ts_device(iface->name, iface->ts_label); - ensure_ts_label(iface); - sk_get_ts_info(iface->ts_label, &iface->ts_info); - if (iface->ts_info.valid && - ((iface->ts_info.so_timestamping & required_modes) != required_modes)) { - pr_err("interface '%s' does not support " - "requested timestamping mode", iface->name); - return NULL; - } - } - - iface = STAILQ_FIRST(&config->interfaces); - - /* determine PHC Clock index */ - if (config_get_int(config, NULL, "free_running")) { - phc_index = -1; - } else if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) { - phc_index = -1; - } else if (phc_device) { - if (1 != sscanf(phc_device, "/dev/ptp%d", &phc_index)) { - pr_err("bad ptp device string"); - return NULL; - } - } else if (iface->ts_info.valid) { - phc_index = iface->ts_info.phc_index; - } else { - pr_err("PTP device not specified and automatic determination" - " is not supported. Please specify PTP device."); - return NULL; - } - if (phc_index >= 0) { - pr_info("selected /dev/ptp%d as PTP clock", phc_index); - } - - if (generate_clock_identity(&c->dds.clockIdentity, iface->name)) { - pr_err("failed to generate a clock identity"); - return NULL; - } - - /* Configure the UDS. */ - snprintf(udsif->name, sizeof(udsif->name), "%s", - config_get_string(config, NULL, "uds_address")); - if (config_set_section_int(config, udsif->name, - "announceReceiptTimeout", 0)) { - return NULL; - } - if (config_set_section_int(config, udsif->name, - "delay_mechanism", DM_AUTO)) { - return NULL; - } - if (config_set_section_int(config, udsif->name, - "network_transport", TRANS_UDS)) { - return NULL; - } - if (config_set_section_int(config, udsif->name, "delay_filter_length", 1)) { - return NULL; - } - - c->config = config; - c->free_running = config_get_int(config, NULL, "free_running"); - c->freq_est_interval = config_get_int(config, NULL, "freq_est_interval"); - c->grand_master_capable = config_get_int(config, NULL, "gmCapable"); - c->kernel_leap = config_get_int(config, NULL, "kernel_leap"); - c->utc_offset = config_get_int(config, NULL, "utc_offset"); - c->time_source = config_get_int(config, NULL, "timeSource"); - - if (c->free_running) { - c->clkid = CLOCK_INVALID; - if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) { - c->utc_timescale = 1; - } - } else if (phc_index >= 0) { - snprintf(phc, sizeof(phc), "/dev/ptp%d", phc_index); - c->clkid = phc_open(phc); - if (c->clkid == CLOCK_INVALID) { - pr_err("Failed to open %s: %m", phc); - return NULL; - } - max_adj = phc_max_adj(c->clkid); - if (!max_adj) { - pr_err("clock is not adjustable"); - return NULL; - } - clockadj_init(c->clkid); - } else { - c->clkid = CLOCK_REALTIME; - c->utc_timescale = 1; - clockadj_init(c->clkid); - max_adj = sysclk_max_freq(); - sysclk_set_leap(0); - } - c->utc_offset_set = 0; - c->leap_set = 0; - c->time_flags = c->utc_timescale ? 0 : PTP_TIMESCALE; - - if (c->clkid != CLOCK_INVALID) { - fadj = (int) clockadj_get_freq(c->clkid); - /* Due to a bug in older kernels, the reading may silently fail - and return 0. Set the frequency back to make sure fadj is - the actual frequency of the clock. */ - clockadj_set_freq(c->clkid, fadj); - } - c->servo = servo_create(c->config, servo, -fadj, max_adj, sw_ts); - if (!c->servo) { - pr_err("Failed to create clock servo"); - return NULL; - } - c->servo_state = SERVO_UNLOCKED; - c->servo_type = servo; - if (config_get_int(config, NULL, "dataset_comparison") == DS_CMP_G8275) { - c->dscmp = telecom_dscmp; - } else { - c->dscmp = dscmp; - } - c->tsproc = tsproc_create(config_get_int(config, NULL, "tsproc_mode"), - config_get_int(config, NULL, "delay_filter"), - config_get_int(config, NULL, "delay_filter_length")); - if (!c->tsproc) { - pr_err("Failed to create time stamp processor"); - return NULL; - } - c->initial_delay = dbl_tmv(config_get_int(config, NULL, "initial_delay")); - c->master_local_rr = 1.0; - c->nrr = 1.0; - c->stats_interval = config_get_int(config, NULL, "summary_interval"); - c->stats.offset = stats_create(); - c->stats.freq = stats_create(); - c->stats.delay = stats_create(); - if (!c->stats.offset || !c->stats.freq || !c->stats.delay) { - pr_err("failed to create stats"); - return NULL; - } - sfl = config_get_int(config, NULL, "sanity_freq_limit"); - if (sfl) { - c->sanity_check = clockcheck_create(sfl); - if (!c->sanity_check) { - pr_err("Failed to create clock sanity check"); - return NULL; - } - } - - /* Initialize the parentDS. */ - clock_update_grandmaster(c); - c->dad.pds.parentStats = 0; - c->dad.pds.observedParentOffsetScaledLogVariance = 0xffff; - c->dad.pds.observedParentClockPhaseChangeRate = 0x7fffffff; - c->dad.ptl = c->ptl; - - clock_sync_interval(c, 0); - - LIST_INIT(&c->subscribers); - LIST_INIT(&c->ports); - c->last_port_number = 0; - - if (clock_resize_pollfd(c, 0)) { - pr_err("failed to allocate pollfd"); - return NULL; - } - - /* Create the UDS interface. */ - 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); - - /* Create the ports. */ - STAILQ_FOREACH(iface, &config->interfaces, list) { - if (clock_add_port(c, phc_index, timestamping, iface)) { - pr_err("failed to open port %s", iface->name); - return NULL; - } - } - - 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; -} - -struct dataset *clock_best_foreign(struct clock *c) -{ - return c->best ? &c->best->dataset : NULL; -} - -struct port *clock_best_port(struct clock *c) -{ - return c->best ? c->best->port : NULL; -} - -struct dataset *clock_default_ds(struct clock *c) -{ - struct dataset *out = &c->default_dataset; - struct defaultDS *in = &c->dds; - - out->priority1 = in->priority1; - out->identity = in->clockIdentity; - out->quality = in->clockQuality; - out->priority2 = in->priority2; - out->stepsRemoved = 0; - out->sender.clockIdentity = in->clockIdentity; - out->sender.portNumber = 0; - out->receiver.clockIdentity = in->clockIdentity; - out->receiver.portNumber = 0; - - return out; -} - -UInteger8 clock_domain_number(struct clock *c) -{ - return c->dds.domainNumber; -} - -struct port *clock_first_port(struct clock *c) -{ - return LIST_FIRST(&c->ports); -} - -void clock_follow_up_info(struct clock *c, struct follow_up_info_tlv *f) -{ - c->status.cumulativeScaledRateOffset = f->cumulativeScaledRateOffset; - c->status.scaledLastGmPhaseChange = f->scaledLastGmPhaseChange; - c->status.gmTimeBaseIndicator = f->gmTimeBaseIndicator; - memcpy(&c->status.lastGmPhaseChange, &f->lastGmPhaseChange, - sizeof(c->status.lastGmPhaseChange)); -} - -int clock_free_running(struct clock *c) -{ - return c->free_running ? 1 : 0; -} - -int clock_gm_capable(struct clock *c) -{ - return c->grand_master_capable; -} - -struct ClockIdentity clock_identity(struct clock *c) -{ - return c->dds.clockIdentity; -} - -static int clock_resize_pollfd(struct clock *c, int new_nports) -{ - struct pollfd *new_pollfd; - - /* Need to allocate one whole 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; - } - 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; - - /* Don't forward any requests to the UDS port. */ - if (out == c->uds_port) { - switch (management_action(msg)) { - case GET: - case SET: - case COMMAND: - 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) -{ - struct port *piter; - int pdulen = 0, msg_ready = 0; - - if (forwarding(c, p) && msg->management.boundaryHops) { - 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++; - } - } -} - -tmv_t clock_ingress_time(struct clock *c) -{ - return c->ingress_ts; -} - -int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg) -{ - int changed = 0, res, answers; - struct port *piter; - struct management_tlv *mgt; - struct ClockIdentity *tcid, wildcard = { - {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff} - }; - - /* Forward this message out all eligible ports. */ - clock_forward_mgmt_msg(c, p, msg); - - /* Apply this message to the local clock and ports. */ - tcid = &msg->management.targetPortIdentity.clockIdentity; - if (!cid_eq(tcid, &wildcard) && !cid_eq(tcid, &c->dds.clockIdentity)) { - return changed; - } - if (msg_tlv_count(msg) != 1) { - return changed; - } - mgt = (struct management_tlv *) msg->management.suffix; - - /* - The correct length according to the management ID is checked - in tlv.c, but management TLVs with empty bodies are also - received successfully to support GETs and CMDs. At this - point the TLV either has the correct length or length 2. - */ - switch (management_action(msg)) { - case GET: - if (clock_management_get_response(c, p, mgt->id, msg)) - return changed; - break; - case SET: - if (mgt->length == 2 && mgt->id != TLV_NULL_MANAGEMENT) { - clock_management_send_error(p, msg, TLV_WRONG_LENGTH); - return changed; - } - if (p != c->uds_port) { - /* Sorry, only allowed on the UDS port. */ - clock_management_send_error(p, msg, TLV_NOT_SUPPORTED); - return changed; - } - if (clock_management_set(c, p, mgt->id, msg, &changed)) - return changed; - break; - case COMMAND: - break; - default: - return changed; - } - - switch (mgt->id) { - case TLV_PORT_PROPERTIES_NP: - if (p != c->uds_port) { - /* Only the UDS port allowed. */ - clock_management_send_error(p, msg, TLV_NOT_SUPPORTED); - return 0; - } - } - - switch (mgt->id) { - case TLV_USER_DESCRIPTION: - case TLV_SAVE_IN_NON_VOLATILE_STORAGE: - case TLV_RESET_NON_VOLATILE_STORAGE: - case TLV_INITIALIZE: - case TLV_FAULT_LOG: - case TLV_FAULT_LOG_RESET: - case TLV_DEFAULT_DATA_SET: - case TLV_CURRENT_DATA_SET: - case TLV_PARENT_DATA_SET: - case TLV_TIME_PROPERTIES_DATA_SET: - case TLV_PRIORITY1: - case TLV_PRIORITY2: - case TLV_DOMAIN: - case TLV_SLAVE_ONLY: - case TLV_TIME: - case TLV_CLOCK_ACCURACY: - case TLV_UTC_PROPERTIES: - case TLV_TRACEABILITY_PROPERTIES: - case TLV_TIMESCALE_PROPERTIES: - case TLV_PATH_TRACE_LIST: - case TLV_PATH_TRACE_ENABLE: - case TLV_GRANDMASTER_CLUSTER_TABLE: - case TLV_ACCEPTABLE_MASTER_TABLE: - case TLV_ACCEPTABLE_MASTER_MAX_TABLE_SIZE: - case TLV_ALTERNATE_TIME_OFFSET_ENABLE: - case TLV_ALTERNATE_TIME_OFFSET_NAME: - case TLV_ALTERNATE_TIME_OFFSET_MAX_KEY: - case TLV_ALTERNATE_TIME_OFFSET_PROPERTIES: - case TLV_TRANSPARENT_CLOCK_DEFAULT_DATA_SET: - case TLV_PRIMARY_DOMAIN: - case TLV_TIME_STATUS_NP: - case TLV_GRANDMASTER_SETTINGS_NP: - case TLV_SUBSCRIBE_EVENTS_NP: - clock_management_send_error(p, msg, TLV_NOT_SUPPORTED); - break; - default: - answers = 0; - LIST_FOREACH(piter, &c->ports, list) { - res = port_manage(piter, p, msg); - if (res < 0) - return changed; - if (res > 0) - answers++; - } - if (!answers) { - /* IEEE 1588 Interpretation #21 suggests to use - * TLV_WRONG_VALUE for ports that do not exist */ - clock_management_send_error(p, msg, TLV_WRONG_VALUE); - } - break; - } - return changed; -} - -void clock_notify_event(struct clock *c, enum notification event) -{ - struct port *uds = c->uds_port; - struct PortIdentity pid = port_identity(uds); - struct ptp_message *msg; - int id; - - switch (event) { - /* set id */ - default: - return; - } - /* targetPortIdentity and sequenceId will be filled by - * clock_send_notification */ - msg = port_management_notify(pid, uds); - if (!msg) - return; - if (!clock_management_fill_response(c, NULL, NULL, msg, id)) - goto err; - if (msg_pre_send(msg)) - goto err; - clock_send_notification(c, msg, event); -err: - msg_put(msg); -} - -struct parent_ds *clock_parent_ds(struct clock *c) -{ - return &c->dad; -} - -struct PortIdentity clock_parent_identity(struct clock *c) -{ - return c->dad.pds.parentPortIdentity; -} - -void clock_set_sde(struct clock *c, int sde) -{ - c->sde = sde; -} - -int clock_poll(struct clock *c) -{ - int cnt, i; - enum fsm_event event; - struct pollfd *cur; - struct port *p; - - clock_check_pollfd(c); - cnt = poll(c->pollfd, (c->nports + 1) * N_CLOCK_PFD, -1); - if (cnt < 0) { - if (EINTR == errno) { - return 0; - } else { - pr_emerg("poll failed"); - return -1; - } - } else if (!cnt) { - return 0; - } - - cur = c->pollfd; - - LIST_FOREACH(p, &c->ports, list) { - /* Let the ports handle their events. */ - for (i = 0; i < N_POLLFD; i++) { - if (cur[i].revents & (POLLIN|POLLPRI)) { - event = port_event(p, i); - if (EV_STATE_DECISION_EVENT == event) { - c->sde = 1; - } - if (EV_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES == event) { - c->sde = 1; - } - port_dispatch(p, event, 0); - /* Clear any fault after a little while. */ - if (PS_FAULTY == port_state(p)) { - clock_fault_timeout(p, 1); - break; - } - } - } - - /* - * When the fault timer expires we clear the fault, - * but only if the link is up. - */ - if (cur[N_POLLFD].revents & (POLLIN|POLLPRI)) { - clock_fault_timeout(p, 0); - if (port_link_status_get(p)) { - port_dispatch(p, EV_FAULT_CLEARED, 0); - } - } - - cur += N_CLOCK_PFD; - } - - /* Check the UDS port. */ - 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) { - c->sde = 1; - } - } - } - - if (c->sde) { - handle_state_decision_event(c); - c->sde = 0; - } - clock_prune_subscriptions(c); - return 0; -} - -void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx) -{ - tsproc_up_ts(c->tsproc, req, rx); - - if (tsproc_update_delay(c->tsproc, &c->path_delay)) - return; - - c->cur.meanPathDelay = tmv_to_TimeInterval(c->path_delay); - - if (c->stats.delay) - stats_add_value(c->stats.delay, tmv_dbl(c->path_delay)); -} - -void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx, - double nrr) -{ - c->path_delay = ppd; - c->nrr = nrr; - - tsproc_set_delay(c->tsproc, ppd); - tsproc_up_ts(c->tsproc, req, rx); - - if (c->stats.delay) - stats_add_value(c->stats.delay, tmv_dbl(ppd)); -} - -int clock_slave_only(struct clock *c) -{ - return c->dds.flags & DDS_SLAVE_ONLY; -} - -UInteger16 clock_steps_removed(struct clock *c) -{ - return c->cur.stepsRemoved; -} - -int clock_switch_phc(struct clock *c, int phc_index) -{ - struct servo *servo; - int fadj, max_adj; - clockid_t clkid; - char phc[32]; - - snprintf(phc, sizeof(phc), "/dev/ptp%d", phc_index); - clkid = phc_open(phc); - if (clkid == CLOCK_INVALID) { - pr_err("Switching PHC, failed to open %s: %m", phc); - return -1; - } - max_adj = phc_max_adj(clkid); - if (!max_adj) { - pr_err("Switching PHC, clock is not adjustable"); - phc_close(clkid); - return -1; - } - fadj = (int) clockadj_get_freq(clkid); - clockadj_set_freq(clkid, fadj); - servo = servo_create(c->config, c->servo_type, -fadj, max_adj, 0); - if (!servo) { - pr_err("Switching PHC, failed to create clock servo"); - phc_close(clkid); - return -1; - } - phc_close(c->clkid); - servo_destroy(c->servo); - c->clkid = clkid; - c->servo = servo; - c->servo_state = SERVO_UNLOCKED; - return 0; -} - -enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin) -{ - double adj, weight; - enum servo_state state = SERVO_UNLOCKED; - - c->ingress_ts = ingress; - - tsproc_down_ts(c->tsproc, origin, ingress); - - if (tsproc_update_offset(c->tsproc, &c->master_offset, &weight)) { - if (c->free_running) { - return clock_no_adjust(c, ingress, origin); - } else { - return state; - } - } - - if (clock_utc_correct(c, ingress)) { - return c->servo_state; - } - - c->cur.offsetFromMaster = tmv_to_TimeInterval(c->master_offset); - - if (c->free_running) { - return clock_no_adjust(c, ingress, origin); - } - - adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset), - tmv_to_nanoseconds(ingress), weight, &state); - c->servo_state = state; - - if (c->stats.max_count > 1) { - clock_stats_update(&c->stats, tmv_dbl(c->master_offset), adj); - } else { - pr_info("master offset %10" PRId64 " s%d freq %+7.0f " - "path delay %9" PRId64, - tmv_to_nanoseconds(c->master_offset), state, adj, - tmv_to_nanoseconds(c->path_delay)); - } - - tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c)); - - switch (state) { - case SERVO_UNLOCKED: - break; - case SERVO_JUMP: - clockadj_set_freq(c->clkid, -adj); - clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset)); - c->ingress_ts = tmv_zero(); - if (c->sanity_check) { - clockcheck_set_freq(c->sanity_check, -adj); - clockcheck_step(c->sanity_check, - -tmv_to_nanoseconds(c->master_offset)); - } - tsproc_reset(c->tsproc, 0); - break; - case SERVO_LOCKED: - clockadj_set_freq(c->clkid, -adj); - if (c->clkid == CLOCK_REALTIME) { - sysclk_set_sync(); - } - if (c->sanity_check) { - clockcheck_set_freq(c->sanity_check, -adj); - } - break; - } - return state; -} - -void clock_sync_interval(struct clock *c, int n) -{ - int shift; - - shift = c->freq_est_interval - n; - if (shift < 0) - shift = 0; - else if (shift >= sizeof(int) * 8) { - shift = sizeof(int) * 8 - 1; - pr_warning("freq_est_interval is too long"); - } - c->fest.max_count = (1 << shift); - - shift = c->stats_interval - n; - if (shift < 0) - shift = 0; - else if (shift >= sizeof(int) * 8) { - shift = sizeof(int) * 8 - 1; - pr_warning("summary_interval is too long"); - } - c->stats.max_count = (1 << shift); - - servo_sync_interval(c->servo, n < 0 ? 1.0 / (1 << -n) : 1 << n); -} - -struct timePropertiesDS *clock_time_properties(struct clock *c) -{ - return &c->tds; -} - -void clock_update_time_properties(struct clock *c, struct timePropertiesDS tds) -{ - c->tds = tds; -} - -static void handle_state_decision_event(struct clock *c) -{ - struct foreign_clock *best = NULL, *fc; - struct ClockIdentity best_id; - struct port *piter; - int fresh_best = 0; - - LIST_FOREACH(piter, &c->ports, list) { - fc = port_compute_best(piter); - if (!fc) - continue; - if (!best || c->dscmp(&fc->dataset, &best->dataset) > 0) - best = fc; - } - - if (best) { - best_id = best->dataset.identity; - } else { - best_id = c->dds.clockIdentity; - } - - if (cid_eq(&best_id, &c->dds.clockIdentity)) { - pr_notice("selected local clock %s as best master", - cid2str(&best_id)); - } else { - pr_notice("selected best master clock %s", - cid2str(&best_id)); - } - - if (!cid_eq(&best_id, &c->best_id)) { - clock_freq_est_reset(c); - tsproc_reset(c->tsproc, 1); - if (!tmv_is_zero(c->initial_delay)) - tsproc_set_delay(c->tsproc, c->initial_delay); - c->ingress_ts = tmv_zero(); - c->path_delay = c->initial_delay; - c->nrr = 1.0; - fresh_best = 1; - } - - c->best = best; - c->best_id = best_id; - - LIST_FOREACH(piter, &c->ports, list) { - enum port_state ps; - enum fsm_event event; - ps = bmc_state_decision(c, piter, c->dscmp); - switch (ps) { - case PS_LISTENING: - event = EV_NONE; - break; - case PS_GRAND_MASTER: - pr_notice("assuming the grand master role"); - clock_update_grandmaster(c); - event = EV_RS_GRAND_MASTER; - break; - case PS_MASTER: - event = EV_RS_MASTER; - break; - case PS_PASSIVE: - event = EV_RS_PASSIVE; - break; - case PS_SLAVE: - clock_update_slave(c); - event = EV_RS_SLAVE; - break; - default: - event = EV_FAULT_DETECTED; - break; - } - port_dispatch(piter, event, fresh_best); - } -} - -struct clock_description *clock_description(struct clock *c) -{ - return &c->desc; -} - -enum clock_type clock_type(struct clock *c) -{ - return c->type; -} - -void clock_check_ts(struct clock *c, uint64_t ts) -{ - if (c->sanity_check && clockcheck_sample(c->sanity_check, ts)) { - servo_reset(c->servo); - } -} - -double clock_rate_ratio(struct clock *c) -{ - if (c->free_running) { - return c->master_local_rr; - } - return servo_rate_ratio(c->servo); -} diff --git a/config.c b/config.c index 3530ce6..7914ba4 100644 --- a/config.c +++ b/config.c @@ -164,13 +164,6 @@ static struct config_enum delay_mech_enu[] = { { NULL, 0 }, }; -static struct config_enum hwts_filter_enu[] = { - { "normal", HWTS_FILTER_NORMAL }, - { "check", HWTS_FILTER_CHECK }, - { "full", HWTS_FILTER_FULL }, - { NULL, 0 }, -}; - static struct config_enum nw_trans_enu[] = { { "L2", TRANS_IEEE_802_3 }, { "UDPv4", TRANS_UDP_IPV4 }, @@ -222,7 +215,6 @@ struct config_item config_tab[] = { GLOB_ITEM_INT("G.8275.defaultDS.localPriority", 128, 1, UINT8_MAX), PORT_ITEM_INT("G.8275.portDS.localPriority", 128, 1, UINT8_MAX), GLOB_ITEM_INT("gmCapable", 1, 0, 1), - GLOB_ITEM_ENU("hwts_filter", HWTS_FILTER_NORMAL, hwts_filter_enu), PORT_ITEM_INT("hybrid_e2e", 0, 0, 1), PORT_ITEM_INT("ignore_transport_specific", 0, 0, 1), PORT_ITEM_INT("ingressLatency", 0, INT_MIN, INT_MAX), diff --git a/config.c.hwtsfilter b/config.c.hwtsfilter deleted file mode 100644 index 7914ba4..0000000 --- a/config.c.hwtsfilter +++ /dev/null @@ -1,1077 +0,0 @@ -/** - * @file config.c - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include - -#include "bmc.h" -#include "clock.h" -#include "config.h" -#include "ether.h" -#include "hash.h" -#include "print.h" -#include "util.h" - -enum config_section { - GLOBAL_SECTION, - UC_MTAB_SECTION, - PORT_SECTION, - UNKNOWN_SECTION, -}; - -enum config_type { - CFG_TYPE_INT, - CFG_TYPE_DOUBLE, - CFG_TYPE_ENUM, - CFG_TYPE_STRING, -}; - -struct config_enum { - const char *label; - int value; -}; - -typedef union { - int i; - double d; - char *s; -} any_t; - -#define CONFIG_LABEL_SIZE 32 - -#define CFG_ITEM_STATIC (1 << 0) /* statically allocated, not to be freed */ -#define CFG_ITEM_LOCKED (1 << 1) /* command line value, may not be changed */ -#define CFG_ITEM_PORT (1 << 2) /* item may appear in port sections */ -#define CFG_ITEM_DYNSTR (1 << 4) /* string value dynamically allocated */ - -struct config_item { - char label[CONFIG_LABEL_SIZE]; - enum config_type type; - struct config_enum *tab; - unsigned int flags; - any_t val; - any_t min; - any_t max; -}; - -#define N_CONFIG_ITEMS (sizeof(config_tab) / sizeof(config_tab[0])) - -#define CONFIG_ITEM_DBL(_label, _port, _default, _min, _max) { \ - .label = _label, \ - .type = CFG_TYPE_DOUBLE, \ - .flags = _port ? CFG_ITEM_PORT : 0, \ - .val.d = _default, \ - .min.d = _min, \ - .max.d = _max, \ -} -#define CONFIG_ITEM_ENUM(_label, _port, _default, _table) { \ - .label = _label, \ - .type = CFG_TYPE_ENUM, \ - .flags = _port ? CFG_ITEM_PORT : 0, \ - .tab = _table, \ - .val.i = _default, \ -} -#define CONFIG_ITEM_INT(_label, _port, _default, _min, _max) { \ - .label = _label, \ - .type = CFG_TYPE_INT, \ - .flags = _port ? CFG_ITEM_PORT : 0, \ - .val.i = _default, \ - .min.i = _min, \ - .max.i = _max, \ -} -#define CONFIG_ITEM_STRING(_label, _port, _default) { \ - .label = _label, \ - .type = CFG_TYPE_STRING, \ - .flags = _port ? CFG_ITEM_PORT : 0, \ - .val.s = _default, \ -} - -#define GLOB_ITEM_DBL(label, _default, min, max) \ - CONFIG_ITEM_DBL(label, 0, _default, min, max) - -#define GLOB_ITEM_ENU(label, _default, table) \ - CONFIG_ITEM_ENUM(label, 0, _default, table) - -#define GLOB_ITEM_INT(label, _default, min, max) \ - CONFIG_ITEM_INT(label, 0, _default, min, max) - -#define GLOB_ITEM_STR(label, _default) \ - CONFIG_ITEM_STRING(label, 0, _default) - -#define PORT_ITEM_DBL(label, _default, min, max) \ - CONFIG_ITEM_DBL(label, 1, _default, min, max) - -#define PORT_ITEM_ENU(label, _default, table) \ - CONFIG_ITEM_ENUM(label, 1, _default, table) - -#define PORT_ITEM_INT(label, _default, min, max) \ - CONFIG_ITEM_INT(label, 1, _default, min, max) - -#define PORT_ITEM_STR(label, _default) \ - CONFIG_ITEM_STRING(label, 1, _default) - -static struct config_enum clock_servo_enu[] = { - { "pi", CLOCK_SERVO_PI }, - { "linreg", CLOCK_SERVO_LINREG }, - { "ntpshm", CLOCK_SERVO_NTPSHM }, - { "nullf", CLOCK_SERVO_NULLF }, - { NULL, 0 }, -}; - -static struct config_enum clock_type_enu[] = { - { "OC", CLOCK_TYPE_ORDINARY }, - { "BC", CLOCK_TYPE_BOUNDARY }, - { "P2P_TC", CLOCK_TYPE_P2P }, - { "E2E_TC", CLOCK_TYPE_E2E }, - { NULL, 0 }, -}; - -static struct config_enum dataset_comp_enu[] = { - { "ieee1588", DS_CMP_IEEE1588 }, - { "G.8275.x", DS_CMP_G8275 }, - { NULL, 0 }, -}; - -static struct config_enum delay_filter_enu[] = { - { "moving_average", FILTER_MOVING_AVERAGE }, - { "moving_median", FILTER_MOVING_MEDIAN }, - { NULL, 0 }, -}; - -static struct config_enum delay_mech_enu[] = { - { "Auto", DM_AUTO }, - { "E2E", DM_E2E }, - { "P2P", DM_P2P }, - { NULL, 0 }, -}; - -static struct config_enum nw_trans_enu[] = { - { "L2", TRANS_IEEE_802_3 }, - { "UDPv4", TRANS_UDP_IPV4 }, - { "UDPv6", TRANS_UDP_IPV6 }, - { NULL, 0 }, -}; - -static struct config_enum timestamping_enu[] = { - { "hardware", TS_HARDWARE }, - { "software", TS_SOFTWARE }, - { "legacy", TS_LEGACY_HW }, - { "onestep", TS_ONESTEP }, - { "p2p1step", TS_P2P1STEP }, - { NULL, 0 }, -}; - -static struct config_enum tsproc_enu[] = { - { "filter", TSPROC_FILTER }, - { "raw", TSPROC_RAW }, - { "filter_weight", TSPROC_FILTER_WEIGHT }, - { "raw_weight", TSPROC_RAW_WEIGHT }, - { NULL, 0 }, -}; - -struct config_item config_tab[] = { - PORT_ITEM_INT("announceReceiptTimeout", 3, 2, UINT8_MAX), - GLOB_ITEM_INT("assume_two_step", 0, 0, 1), - PORT_ITEM_INT("boundary_clock_jbod", 0, 0, 1), - GLOB_ITEM_INT("check_fup_sync", 0, 0, 1), - GLOB_ITEM_INT("clockAccuracy", 0xfe, 0, UINT8_MAX), - GLOB_ITEM_INT("clockClass", 248, 0, UINT8_MAX), - GLOB_ITEM_ENU("clock_servo", CLOCK_SERVO_PI, clock_servo_enu), - GLOB_ITEM_ENU("clock_type", CLOCK_TYPE_ORDINARY, clock_type_enu), - GLOB_ITEM_ENU("dataset_comparison", DS_CMP_IEEE1588, dataset_comp_enu), - PORT_ITEM_INT("delayAsymmetry", 0, INT_MIN, INT_MAX), - PORT_ITEM_ENU("delay_filter", FILTER_MOVING_MEDIAN, delay_filter_enu), - PORT_ITEM_INT("delay_filter_length", 10, 1, INT_MAX), - PORT_ITEM_ENU("delay_mechanism", DM_E2E, delay_mech_enu), - GLOB_ITEM_INT("dscp_event", 0, 0, 63), - GLOB_ITEM_INT("dscp_general", 0, 0, 63), - GLOB_ITEM_INT("domainNumber", 0, 0, 127), - PORT_ITEM_INT("egressLatency", 0, INT_MIN, INT_MAX), - PORT_ITEM_INT("fault_badpeernet_interval", 16, INT32_MIN, INT32_MAX), - PORT_ITEM_INT("fault_reset_interval", 4, INT8_MIN, INT8_MAX), - GLOB_ITEM_DBL("first_step_threshold", 0.00002, 0.0, DBL_MAX), - PORT_ITEM_INT("follow_up_info", 0, 0, 1), - GLOB_ITEM_INT("free_running", 0, 0, 1), - PORT_ITEM_INT("freq_est_interval", 1, 0, INT_MAX), - GLOB_ITEM_INT("G.8275.defaultDS.localPriority", 128, 1, UINT8_MAX), - PORT_ITEM_INT("G.8275.portDS.localPriority", 128, 1, UINT8_MAX), - GLOB_ITEM_INT("gmCapable", 1, 0, 1), - PORT_ITEM_INT("hybrid_e2e", 0, 0, 1), - PORT_ITEM_INT("ignore_transport_specific", 0, 0, 1), - PORT_ITEM_INT("ingressLatency", 0, INT_MIN, INT_MAX), - PORT_ITEM_INT("inhibit_multicast_service", 0, 0, 1), - GLOB_ITEM_INT("initial_delay", 0, 0, INT_MAX), - GLOB_ITEM_INT("kernel_leap", 1, 0, 1), - PORT_ITEM_INT("logAnnounceInterval", 1, INT8_MIN, INT8_MAX), - PORT_ITEM_INT("logMinDelayReqInterval", 0, INT8_MIN, INT8_MAX), - PORT_ITEM_INT("logMinPdelayReqInterval", 0, INT8_MIN, INT8_MAX), - PORT_ITEM_INT("logSyncInterval", 0, INT8_MIN, INT8_MAX), - GLOB_ITEM_INT("logging_level", LOG_INFO, PRINT_LEVEL_MIN, PRINT_LEVEL_MAX), - PORT_ITEM_INT("masterOnly", 0, 0, 1), - GLOB_ITEM_STR("message_tag", NULL), - GLOB_ITEM_STR("manufacturerIdentity", "00:00:00"), - GLOB_ITEM_INT("max_frequency", 900000000, 0, INT_MAX), - PORT_ITEM_INT("min_neighbor_prop_delay", -20000000, INT_MIN, -1), - PORT_ITEM_INT("neighborPropDelayThresh", 20000000, 0, INT_MAX), - PORT_ITEM_INT("net_sync_monitor", 0, 0, 1), - PORT_ITEM_ENU("network_transport", TRANS_UDP_IPV4, nw_trans_enu), - GLOB_ITEM_INT("ntpshm_segment", 0, INT_MIN, INT_MAX), - GLOB_ITEM_INT("offsetScaledLogVariance", 0xffff, 0, UINT16_MAX), - PORT_ITEM_INT("path_trace_enabled", 0, 0, 1), - GLOB_ITEM_DBL("pi_integral_const", 0.0, 0.0, DBL_MAX), - GLOB_ITEM_DBL("pi_integral_exponent", 0.4, -DBL_MAX, DBL_MAX), - GLOB_ITEM_DBL("pi_integral_norm_max", 0.3, DBL_MIN, 2.0), - GLOB_ITEM_DBL("pi_integral_scale", 0.0, 0.0, DBL_MAX), - GLOB_ITEM_DBL("pi_proportional_const", 0.0, 0.0, DBL_MAX), - GLOB_ITEM_DBL("pi_proportional_exponent", -0.3, -DBL_MAX, DBL_MAX), - GLOB_ITEM_DBL("pi_proportional_norm_max", 0.7, DBL_MIN, 1.0), - GLOB_ITEM_DBL("pi_proportional_scale", 0.0, 0.0, DBL_MAX), - GLOB_ITEM_INT("priority1", 128, 0, UINT8_MAX), - GLOB_ITEM_INT("priority2", 128, 0, UINT8_MAX), - GLOB_ITEM_STR("productDescription", ";;"), - PORT_ITEM_STR("ptp_dst_mac", "01:1B:19:00:00:00"), - PORT_ITEM_STR("p2p_dst_mac", "01:80:C2:00:00:0E"), - GLOB_ITEM_STR("revisionData", ";;"), - GLOB_ITEM_INT("sanity_freq_limit", 200000000, 0, INT_MAX), - GLOB_ITEM_INT("slaveOnly", 0, 0, 1), - GLOB_ITEM_DBL("step_threshold", 0.0, 0.0, DBL_MAX), - GLOB_ITEM_INT("summary_interval", 0, INT_MIN, INT_MAX), - PORT_ITEM_INT("syncReceiptTimeout", 0, 0, UINT8_MAX), - GLOB_ITEM_INT("tc_spanning_tree", 0, 0, 1), - GLOB_ITEM_INT("timeSource", INTERNAL_OSCILLATOR, 0x10, 0xfe), - GLOB_ITEM_ENU("time_stamping", TS_HARDWARE, timestamping_enu), - PORT_ITEM_INT("transportSpecific", 0, 0, 0x0F), - PORT_ITEM_ENU("tsproc_mode", TSPROC_FILTER, tsproc_enu), - GLOB_ITEM_INT("twoStepFlag", 1, 0, 1), - GLOB_ITEM_INT("tx_timestamp_timeout", 1, 1, INT_MAX), - PORT_ITEM_INT("udp_ttl", 1, 1, 255), - PORT_ITEM_INT("udp6_scope", 0x0E, 0x00, 0x0F), - GLOB_ITEM_STR("uds_address", "/var/run/ptp4l"), - PORT_ITEM_INT("unicast_listen", 0, 0, 1), - PORT_ITEM_INT("unicast_master_table", 0, 0, INT_MAX), - PORT_ITEM_INT("unicast_req_duration", 3600, 10, INT_MAX), - GLOB_ITEM_INT("use_syslog", 1, 0, 1), - GLOB_ITEM_STR("userDescription", ""), - GLOB_ITEM_INT("utc_offset", CURRENT_UTC_OFFSET, 0, INT_MAX), - GLOB_ITEM_INT("verbose", 0, 0, 1), -}; - -static struct unicast_master_table *current_uc_mtab; - -static enum parser_result -parse_fault_interval(struct config *cfg, const char *section, - const char *option, const char *value); - -static struct config_item *config_section_item(struct config *cfg, - const char *section, - const char *name) -{ - char buf[CONFIG_LABEL_SIZE + MAX_IFNAME_SIZE]; - - snprintf(buf, sizeof(buf), "%s.%s", section, name); - return hash_lookup(cfg->htab, buf); -} - -static struct config_item *config_global_item(struct config *cfg, - const char *name) -{ - return config_section_item(cfg, "global", name); -} - -static struct config_item *config_find_item(struct config *cfg, - const char *section, - const char *name) -{ - struct config_item *ci; - if (section) { - ci = config_section_item(cfg, section, name); - if (ci) { - return ci; - } - } - return config_global_item(cfg, name); -} - -static struct config_item *config_item_alloc(struct config *cfg, - const char *section, - const char *name, - enum config_type type) -{ - struct config_item *ci; - char buf[CONFIG_LABEL_SIZE + MAX_IFNAME_SIZE]; - - ci = calloc(1, sizeof(*ci)); - if (!ci) { - fprintf(stderr, "low memory\n"); - return NULL; - } - strncpy(ci->label, name, CONFIG_LABEL_SIZE - 1); - ci->type = type; - - snprintf(buf, sizeof(buf), "%s.%s", section, ci->label); - if (hash_insert(cfg->htab, buf, ci)) { - fprintf(stderr, "low memory or duplicate item %s\n", name); - free(ci); - return NULL; - } - - return ci; -} - -static void config_item_free(void *ptr) -{ - struct config_item *ci = ptr; - if (ci->type == CFG_TYPE_STRING && ci->flags & CFG_ITEM_DYNSTR) - free(ci->val.s); - if (ci->flags & CFG_ITEM_STATIC) - return; - free(ci); -} - -static int config_switch_unicast_mtab(struct config *cfg, int idx, int line_num) -{ - struct unicast_master_table *table; - - if (idx < 1) { - fprintf(stderr, "line %d: table_id %d is out of range. " - "Must be in the range %d to %d\n", - line_num, idx, 1, INT_MAX); - return -1; - } - STAILQ_FOREACH(table, &cfg->unicast_master_tables, list) { - if (table->table_index == idx) { - fprintf(stderr, "line %d: table_id %d already taken\n", - line_num, idx); - return -1; - } - } - table = calloc(1, sizeof(*table)); - if (!table) { - fprintf(stderr, "low memory\n"); - return -1; - } - STAILQ_INIT(&table->addrs); - table->table_index = idx; - memset(&table->peer_addr.portIdentity, 0xff, - sizeof(table->peer_addr.portIdentity)); - STAILQ_INSERT_TAIL(&cfg->unicast_master_tables, table, list); - current_uc_mtab = table; - return 0; -} - -static int config_unicast_mtab_address(enum transport_type type, char *address, - int line_num) -{ - struct unicast_master_address *item; - - if (!current_uc_mtab) { - fprintf(stderr, "line %d: missing table_id\n", line_num); - return -1; - } - item = calloc(1, sizeof(*item)); - if (!item) { - fprintf(stderr, "low memory\n"); - return -1; - } - if (str2addr(type, address, &item->address)) { - fprintf(stderr, "line %d: bad address\n", line_num); - free(item); - return -1; - } - memset(&item->portIdentity, 0xff, sizeof(item->portIdentity)); - item->type = type; - STAILQ_INSERT_TAIL(¤t_uc_mtab->addrs, item, list); - current_uc_mtab->count++; - - return 0; -} - -static int config_unicast_mtab_peer(char *address, int line_num) -{ - if (!current_uc_mtab) { - fprintf(stderr, "line %d: missing table_id\n", line_num); - return -1; - } - if (current_uc_mtab->peer_name) { - free(current_uc_mtab->peer_name); - } - current_uc_mtab->peer_name = strdup(address); - if (!current_uc_mtab->peer_name) { - fprintf(stderr, "low memory\n"); - return -1; - } - return 0; -} - -static int config_unicast_mtab_query_interval(int lqi, int line_num) -{ - if (!current_uc_mtab) { - fprintf(stderr, "line %d: missing table_id\n", line_num); - return -1; - } - if (lqi < INT8_MIN || lqi > INT8_MAX) { - fprintf(stderr, "line %d: logQueryInterval %d out of range\n", - line_num, lqi); - return -1; - } - current_uc_mtab->logQueryInterval = lqi; - return 0; -} - -static enum parser_result parse_section_line(char *s, enum config_section *section) -{ - if (!strcasecmp(s, "[global]")) { - *section = GLOBAL_SECTION; - } else if (!strcasecmp(s, "[unicast_master_table]")) { - *section = UC_MTAB_SECTION; - current_uc_mtab = NULL; - } else if (s[0] == '[') { - char c; - *section = PORT_SECTION; - /* Replace square brackets with white space. */ - while (0 != (c = *s)) { - if (c == '[' || c == ']') - *s = ' '; - s++; - } - } else - return NOT_PARSED; - return PARSED_OK; -} - -static enum parser_result parse_item(struct config *cfg, - int commandline, - const char *section, - const char *option, - const char *value) -{ - enum parser_result r; - struct config_item *cgi, *dst; - struct config_enum *cte; - double df; - int val; - - r = parse_fault_interval(cfg, section, option, value); - if (r != NOT_PARSED) - return r; - - r = BAD_VALUE; - - /* If there is no default value, then the option is bogus. */ - cgi = config_global_item(cfg, option); - if (!cgi) { - return NOT_PARSED; - } - - switch (cgi->type) { - case CFG_TYPE_INT: - r = get_ranged_int(value, &val, cgi->min.i, cgi->max.i); - break; - case CFG_TYPE_DOUBLE: - r = get_ranged_double(value, &df, cgi->min.d, cgi->max.d); - break; - case CFG_TYPE_ENUM: - for (cte = cgi->tab; cte->label; cte++) { - if (!strcasecmp(cte->label, value)) { - val = cte->value; - r = PARSED_OK; - break; - } - } - break; - case CFG_TYPE_STRING: - r = PARSED_OK; - break; - } - if (r != PARSED_OK) { - return r; - } - - if (section) { - if (!(cgi->flags & CFG_ITEM_PORT)) { - return NOT_PARSED; - } - /* Create or update this port specific item. */ - dst = config_section_item(cfg, section, option); - if (!dst) { - dst = config_item_alloc(cfg, section, option, cgi->type); - if (!dst) { - return NOT_PARSED; - } - } - } else if (!commandline && cgi->flags & CFG_ITEM_LOCKED) { - /* This global option was set on the command line. */ - return PARSED_OK; - } else { - /* Update the global default value. */ - dst = cgi; - } - - switch (dst->type) { - case CFG_TYPE_INT: - case CFG_TYPE_ENUM: - dst->val.i = val; - break; - case CFG_TYPE_DOUBLE: - dst->val.d = df; - break; - case CFG_TYPE_STRING: - if (dst->flags & CFG_ITEM_DYNSTR) { - free(dst->val.s); - } - dst->val.s = strdup(value); - if (!dst->val.s) { - pr_err("low memory"); - return NOT_PARSED; - } - dst->flags |= CFG_ITEM_DYNSTR; - break; - } - - if (commandline) { - dst->flags |= CFG_ITEM_LOCKED; - } - return PARSED_OK; -} - -static enum parser_result parse_fault_interval(struct config *cfg, - const char *section, - const char *option, - const char *value) -{ - int i, val; - const char *str, *fault_options[2] = { - "fault_badpeernet_interval", - "fault_reset_interval", - }; - int fault_values[2] = { - 0, FRI_ASAP, - }; - - if (strcasecmp("ASAP", value)) { - return NOT_PARSED; - } - for (i = 0; i < 2; i++) { - str = fault_options[i]; - val = fault_values[i]; - if (!strcmp(option, str)) { - if (config_set_section_int(cfg, section, str, val)) { - pr_err("bug: failed to set option %s!", option); - exit(-1); - } - return PARSED_OK; - } - } - return NOT_PARSED; -} - -static int parse_unicast_mtab_line(struct config *cfg, char *line, int line_num) -{ - char address[64 + 1] = {0}, transport[16 + 1] = {0}; - enum transport_type type = TRANS_UDS; - struct config_enum *cte; - int cnt, lqi, table_id; - - cnt = sscanf(line, " table_id %d", &table_id); - if (cnt == 1) { - return config_switch_unicast_mtab(cfg, table_id, line_num); - } - cnt = sscanf(line, " logQueryInterval %d", &lqi); - if (cnt == 1) { - return config_unicast_mtab_query_interval(lqi, line_num); - } - cnt = sscanf(line, " peer_address %64s", address); - if (cnt == 1) { - return config_unicast_mtab_peer(address, line_num); - } - cnt = sscanf(line, " %16s %64s", transport, address); - if (cnt != 2) { - fprintf(stderr, "bad master table at line %d\n", line_num); - return -1; - } - for (cte = nw_trans_enu; cte->label; cte++) { - if (!strcasecmp(cte->label, transport)) { - type = cte->value; - break; - } - } - return config_unicast_mtab_address(type, address, line_num); -} - -static enum parser_result parse_setting_line(char *line, - const char **option, - const char **value) -{ - *option = line; - - while (!isspace(line[0])) { - if (line[0] == '\0') - return NOT_PARSED; - line++; - } - - while (isspace(line[0])) { - line[0] = '\0'; - line++; - } - - *value = line; - - return PARSED_OK; -} - -static void check_deprecated_options(const char **option) -{ - const char *new_option = NULL; - - if (!strcmp(*option, "pi_offset_const")) { - new_option = "step_threshold"; - } else if (!strcmp(*option, "pi_f_offset_const")) { - new_option = "first_step_threshold"; - } else if (!strcmp(*option, "pi_max_frequency")) { - new_option = "max_frequency"; - } - - if (new_option) { - fprintf(stderr, "option %s is deprecated, please use %s instead\n", - *option, new_option); - *option = new_option; - } -} - -static struct option *config_alloc_longopts(void) -{ - struct config_item *ci; - struct option *opts; - int i; - - opts = calloc(1, (1 + N_CONFIG_ITEMS) * sizeof(*opts)); - if (!opts) { - return NULL; - } - for (i = 0; i < N_CONFIG_ITEMS; i++) { - ci = &config_tab[i]; - opts[i].name = ci->label; - opts[i].has_arg = required_argument; - } - - return opts; -} - -int config_read(char *name, struct config *cfg) -{ - enum config_section current_section = UNKNOWN_SECTION; - enum parser_result parser_res; - FILE *fp; - char buf[1024], *line, *c; - const char *option, *value; - struct interface *current_port = NULL; - int line_num; - - fp = 0 == strncmp(name, "-", 2) ? stdin : fopen(name, "r"); - - if (!fp) { - fprintf(stderr, "failed to open configuration file %s: %m\n", name); - return -1; - } - - for (line_num = 1; fgets(buf, sizeof(buf), fp); line_num++) { - c = buf; - - /* skip whitespace characters */ - while (isspace(*c)) - c++; - - /* ignore empty lines and comments */ - if (*c == '#' || *c == '\n' || *c == '\0') - continue; - - line = c; - - /* remove trailing whitespace characters and \n */ - c += strlen(line) - 1; - while (c > line && (*c == '\n' || isspace(*c))) - *c-- = '\0'; - - if (parse_section_line(line, ¤t_section) == PARSED_OK) { - if (current_section == PORT_SECTION) { - char port[17]; - if (1 != sscanf(line, " %16s", port)) { - fprintf(stderr, "could not parse port name on line %d\n", - line_num); - goto parse_error; - } - current_port = config_create_interface(port, cfg); - if (!current_port) - goto parse_error; - } - continue; - } - - if (current_section == UC_MTAB_SECTION) { - if (parse_unicast_mtab_line(cfg, line, line_num)) { - goto parse_error; - } - continue; - } - - if (current_section == UNKNOWN_SECTION) { - fprintf(stderr, "line %d is not in a section\n", line_num); - goto parse_error; - } - - if (parse_setting_line(line, &option, &value)) { - fprintf(stderr, "could not parse line %d in %s section\n", - line_num, current_section == GLOBAL_SECTION ? - "global" : current_port->name); - goto parse_error; - } - - check_deprecated_options(&option); - - parser_res = parse_item(cfg, 0, current_section == GLOBAL_SECTION ? - NULL : current_port->name, option, value); - - switch (parser_res) { - case PARSED_OK: - break; - case NOT_PARSED: - fprintf(stderr, "unknown option %s at line %d in %s section\n", - option, line_num, - current_section == GLOBAL_SECTION ? "global" : - current_port->name); - goto parse_error; - case BAD_VALUE: - fprintf(stderr, "%s is a bad value for option %s at line %d\n", - value, option, line_num); - goto parse_error; - case MALFORMED: - fprintf(stderr, "%s is a malformed value for option %s at line %d\n", - value, option, line_num); - goto parse_error; - case OUT_OF_RANGE: - fprintf(stderr, "%s is an out of range value for option %s at line %d\n", - value, option, line_num); - goto parse_error; - } - } - - fclose(fp); - return 0; - -parse_error: - fprintf(stderr, "failed to parse configuration file %s\n", name); - fclose(fp); - return -2; -} - -struct interface *config_create_interface(char *name, struct config *cfg) -{ - struct interface *iface; - - /* only create each interface once (by name) */ - 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); - cfg->n_interfaces++; - - return iface; -} - -struct config *config_create(void) -{ - char buf[CONFIG_LABEL_SIZE + 8]; - struct config_item *ci; - struct config *cfg; - int i; - - cfg = calloc(1, sizeof(*cfg)); - if (!cfg) { - return NULL; - } - STAILQ_INIT(&cfg->interfaces); - STAILQ_INIT(&cfg->unicast_master_tables); - - cfg->opts = config_alloc_longopts(); - if (!cfg->opts) { - free(cfg); - return NULL; - } - - cfg->htab = hash_create(); - if (!cfg->htab) { - free(cfg->opts); - free(cfg); - return NULL; - } - - /* Populate the hash table with global defaults. */ - for (i = 0; i < N_CONFIG_ITEMS; i++) { - ci = &config_tab[i]; - ci->flags |= CFG_ITEM_STATIC; - snprintf(buf, sizeof(buf), "global.%s", ci->label); - if (hash_insert(cfg->htab, buf, ci)) { - fprintf(stderr, "duplicate item %s\n", ci->label); - goto fail; - } - } - - /* Perform a Built In Self Test.*/ - for (i = 0; i < N_CONFIG_ITEMS; i++) { - ci = &config_tab[i]; - ci = config_global_item(cfg, ci->label); - if (ci != &config_tab[i]) { - fprintf(stderr, "config BIST failed at %s\n", - config_tab[i].label); - goto fail; - } - } - return cfg; -fail: - hash_destroy(cfg->htab, NULL); - free(cfg->opts); - free(cfg); - return NULL; -} - -void config_destroy(struct config *cfg) -{ - struct unicast_master_address *address; - struct unicast_master_table *table; - struct interface *iface; - - while ((iface = STAILQ_FIRST(&cfg->interfaces))) { - STAILQ_REMOVE_HEAD(&cfg->interfaces, list); - free(iface); - } - while ((table = STAILQ_FIRST(&cfg->unicast_master_tables))) { - while ((address = STAILQ_FIRST(&table->addrs))) { - STAILQ_REMOVE_HEAD(&table->addrs, list); - free(address); - } - if (table->peer_name) { - free(table->peer_name); - } - STAILQ_REMOVE_HEAD(&cfg->unicast_master_tables, list); - free(table); - } - hash_destroy(cfg->htab, config_item_free); - free(cfg->opts); - free(cfg); -} - -double config_get_double(struct config *cfg, const char *section, - const char *option) -{ - struct config_item *ci = config_find_item(cfg, section, option); - - if (!ci || ci->type != CFG_TYPE_DOUBLE) { - pr_err("bug: config option %s missing or invalid!", option); - exit(-1); - } - pr_debug("config item %s.%s is %f", section, option, ci->val.d); - return ci->val.d; -} - -int config_get_int(struct config *cfg, const char *section, const char *option) -{ - struct config_item *ci = config_find_item(cfg, section, option); - - if (!ci) { - pr_err("bug: config option %s missing!", option); - exit(-1); - } - switch (ci->type) { - case CFG_TYPE_DOUBLE: - case CFG_TYPE_STRING: - pr_err("bug: config option %s type mismatch!", option); - exit(-1); - case CFG_TYPE_INT: - case CFG_TYPE_ENUM: - break; - } - pr_debug("config item %s.%s is %d", section, option, ci->val.i); - return ci->val.i; -} - -char *config_get_string(struct config *cfg, const char *section, - const char *option) -{ - struct config_item *ci = config_find_item(cfg, section, option); - - if (!ci || ci->type != CFG_TYPE_STRING) { - pr_err("bug: config option %s missing or invalid!", option); - exit(-1); - } - pr_debug("config item %s.%s is '%s'", section, option, ci->val.s); - return ci->val.s; -} - -int config_harmonize_onestep(struct config *cfg) -{ - enum timestamp_type tstype = config_get_int(cfg, NULL, "time_stamping"); - int two_step_flag = config_get_int(cfg, NULL, "twoStepFlag"); - - switch (tstype) { - case TS_SOFTWARE: - case TS_LEGACY_HW: - if (!two_step_flag) { - pr_err("one step is only possible " - "with hardware time stamping"); - return -1; - } - break; - case TS_HARDWARE: - if (!two_step_flag) { - pr_debug("upgrading to one step time stamping " - "in order to match the twoStepFlag"); - if (config_set_int(cfg, "time_stamping", TS_ONESTEP)) { - return -1; - } - } - break; - case TS_ONESTEP: - case TS_P2P1STEP: - if (two_step_flag) { - pr_debug("one step mode implies twoStepFlag=0, " - "clearing twoStepFlag to match"); - if (config_set_int(cfg, "twoStepFlag", 0)) { - return -1; - } - } - break; - } - - return 0; -} - -int config_parse_option(struct config *cfg, const char *opt, const char *val) -{ - enum parser_result result; - - result = parse_item(cfg, 1, NULL, opt, val); - - switch (result) { - case PARSED_OK: - return 0; - case NOT_PARSED: - fprintf(stderr, "unknown option %s\n", opt); - break; - case BAD_VALUE: - fprintf(stderr, "%s is a bad value for option %s\n", val, opt); - break; - case MALFORMED: - fprintf(stderr, "%s is a malformed value for option %s\n", - val, opt); - break; - case OUT_OF_RANGE: - fprintf(stderr, "%s is an out of range value for option %s\n", - val, opt); - break; - } - return -1; -} - -int config_set_double(struct config *cfg, const char *option, double val) -{ - struct config_item *ci = config_find_item(cfg, NULL, option); - - if (!ci || ci->type != CFG_TYPE_DOUBLE) { - pr_err("bug: config option %s missing or invalid!", option); - return -1; - } - ci->flags |= CFG_ITEM_LOCKED; - ci->val.d = val; - pr_debug("locked item global.%s as %f", option, ci->val.d); - return 0; -} - -int config_set_section_int(struct config *cfg, const char *section, - const char *option, int val) -{ - struct config_item *cgi, *dst; - - cgi = config_find_item(cfg, NULL, option); - if (!cgi) { - pr_err("bug: config option %s missing!", option); - return -1; - } - switch (cgi->type) { - case CFG_TYPE_DOUBLE: - case CFG_TYPE_STRING: - pr_err("bug: config option %s type mismatch!", option); - return -1; - case CFG_TYPE_INT: - case CFG_TYPE_ENUM: - break; - } - if (!section) { - cgi->flags |= CFG_ITEM_LOCKED; - cgi->val.i = val; - pr_debug("locked item global.%s as %d", option, cgi->val.i); - return 0; - } - /* Create or update this port specific item. */ - dst = config_section_item(cfg, section, option); - if (!dst) { - dst = config_item_alloc(cfg, section, option, cgi->type); - if (!dst) { - return -1; - } - } - dst->val.i = val; - pr_debug("section item %s.%s now %d", section, option, dst->val.i); - return 0; -} - -int config_set_string(struct config *cfg, const char *option, - const char *val) -{ - struct config_item *ci = config_find_item(cfg, NULL, option); - - if (!ci || ci->type != CFG_TYPE_STRING) { - pr_err("bug: config option %s missing or invalid!", option); - return -1; - } - ci->flags |= CFG_ITEM_LOCKED; - if (ci->flags & CFG_ITEM_DYNSTR) { - free(ci->val.s); - } - ci->val.s = strdup(val); - if (!ci->val.s) { - pr_err("low memory"); - return -1; - } - ci->flags |= CFG_ITEM_DYNSTR; - pr_debug("locked item global.%s as '%s'", option, ci->val.s); - return 0; -} diff --git a/missing.h b/missing.h index 8f92079..2f7adb9 100644 --- a/missing.h +++ b/missing.h @@ -118,22 +118,6 @@ enum { #define IFLA_BOND_MAX (__IFLA_BOND_MAX - 1) #endif /*IFLA_BOND_MAX*/ -#ifndef NLA_TYPE_MAX -enum { - NLA_UNSPEC, - NLA_U8, - NLA_U16, - NLA_U32, - NLA_U64, - NLA_STRING, - NLA_FLAG, - NLA_MSECS, - NLA_NESTED, - __NLA_TYPE_MAX, -}; -#define NLA_TYPE_MAX (__NLA_TYPE_MAX - 1) -#endif /*NLA_TYPE_MAX*/ - #ifdef __UCLIBC__ #if (_XOPEN_SOURCE >= 600 || _POSIX_C_SOURCE >= 200112L) && \ diff --git a/missing.h.team b/missing.h.team deleted file mode 100644 index 2f7adb9..0000000 --- a/missing.h.team +++ /dev/null @@ -1,158 +0,0 @@ -/** - * @file missing.h - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ - -/* - * When glibc offers the syscall, this will go away. - */ -#ifndef HAVE_MISSING_H -#define HAVE_MISSING_H - -#include -#include -#include -#include - -#ifndef ADJ_TAI -#define ADJ_TAI 0x0080 -#endif - -#ifndef ADJ_NANO -#define ADJ_NANO 0x2000 -#endif - -#ifndef ADJ_SETOFFSET -#define ADJ_SETOFFSET 0x0100 -#endif - -#ifndef CLOCK_INVALID -#define CLOCK_INVALID -1 -#endif - -#define CLOCKFD 3 -#define FD_TO_CLOCKID(fd) ((clockid_t) ((((unsigned int) ~fd) << 3) | CLOCKFD)) -#define CLOCKID_TO_FD(clk) ((unsigned int) ~((clk) >> 3)) - -#ifndef HAVE_ONESTEP_SYNC -enum _missing_hwtstamp_tx_types { - HWTSTAMP_TX_ONESTEP_SYNC = 2, -}; -#endif - -#ifndef HAVE_ONESTEP_P2P -enum { - HWTSTAMP_TX_ONESTEP_P2P = 3, -}; -#endif - -#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 - -#ifndef SIOCGHWTSTAMP -#define SIOCGHWTSTAMP 0x89b1 -#endif - -#ifndef SO_SELECT_ERR_QUEUE -#define SO_SELECT_ERR_QUEUE 45 -#endif - -#ifndef HAVE_CLOCK_ADJTIME -static inline int clock_adjtime(clockid_t id, struct timex *tx) -{ - return syscall(__NR_clock_adjtime, id, tx); -} -#endif - -#ifndef IFLA_BOND_MAX -enum { - IFLA_BOND_UNSPEC, - IFLA_BOND_MODE, - IFLA_BOND_ACTIVE_SLAVE, - IFLA_BOND_MIIMON, - IFLA_BOND_UPDELAY, - IFLA_BOND_DOWNDELAY, - IFLA_BOND_USE_CARRIER, - IFLA_BOND_ARP_INTERVAL, - IFLA_BOND_ARP_IP_TARGET, - IFLA_BOND_ARP_VALIDATE, - IFLA_BOND_ARP_ALL_TARGETS, - IFLA_BOND_PRIMARY, - IFLA_BOND_PRIMARY_RESELECT, - IFLA_BOND_FAIL_OVER_MAC, - IFLA_BOND_XMIT_HASH_POLICY, - IFLA_BOND_RESEND_IGMP, - IFLA_BOND_NUM_PEER_NOTIF, - IFLA_BOND_ALL_SLAVES_ACTIVE, - IFLA_BOND_MIN_LINKS, - IFLA_BOND_LP_INTERVAL, - IFLA_BOND_PACKETS_PER_SLAVE, - IFLA_BOND_AD_LACP_RATE, - IFLA_BOND_AD_SELECT, - IFLA_BOND_AD_INFO, - IFLA_BOND_AD_ACTOR_SYS_PRIO, - IFLA_BOND_AD_USER_PORT_KEY, - IFLA_BOND_AD_ACTOR_SYSTEM, - IFLA_BOND_TLB_DYNAMIC_LB, - __IFLA_BOND_MAX, -}; - -#define IFLA_BOND_MAX (__IFLA_BOND_MAX - 1) -#endif /*IFLA_BOND_MAX*/ - -#ifdef __UCLIBC__ - -#if (_XOPEN_SOURCE >= 600 || _POSIX_C_SOURCE >= 200112L) && \ - defined __UCLIBC_HAS_THREADS_NATIVE__ - -#include - -#else - -#define TFD_TIMER_ABSTIME (1 << 0) - -static inline int clock_nanosleep(clockid_t clock_id, int flags, - const struct timespec *request, - struct timespec *remain) -{ - return syscall(__NR_clock_nanosleep, clock_id, flags, request, remain); -} - -static inline int timerfd_create(int clockid, int flags) -{ - return syscall(__NR_timerfd_create, clockid, flags); -} - -static inline int timerfd_settime(int fd, int flags, - const struct itimerspec *new_value, - struct itimerspec *old_value) -{ - return syscall(__NR_timerfd_settime, fd, flags, new_value, old_value); -} -#endif - -#else /*__UCLIBC__*/ - -#include - -#endif - -#endif diff --git a/phc2sys.8 b/phc2sys.8 index b3a3de3..45cb0e3 100644 --- a/phc2sys.8 +++ b/phc2sys.8 @@ -108,9 +108,9 @@ together with the option, the master clock is used only to correct the offset by whole number of seconds, which cannot be fixed with PPS alone. Not compatible with the .B \-a -option. This option does not support bonded interface (e.g. bond0, team0). If +option. This option does not support bonded interface (e.g. bond0). If .B ptp4l -has a port on an active-backup bond or team interface, the +has a port on an active-backup bond interface, the .B \-a option can be used to track the active interface. .TP diff --git a/phc2sys.8.team b/phc2sys.8.team deleted file mode 100644 index 45cb0e3..0000000 --- a/phc2sys.8.team +++ /dev/null @@ -1,507 +0,0 @@ -.TH PHC2SYS 8 "April 2018" "linuxptp" -.SH NAME -phc2sys \- synchronize two or more clocks - -.SH SYNOPSIS -.B phc2sys \-a -[ -.B \-r -] [ -.B \-r -] [ -.BI \-f " config-file" -] [ options ] [ -.I long-options -] -.br -.B phc2sys -[ -.BI \-f " config-file" -] [ -.BI \-d " pps-device" -] [ -.BI \-s " device" -] [ -.BI \-c " device" -] [ -.BI \-O " offset" -] [ -.BI \-w -] [ options ] [ -.I long-options -] -.I .\|.\|. - - -.SH DESCRIPTION -.B phc2sys -is a program which synchronizes two or more clocks in the system. Typically, -it is used to synchronize the system clock to a PTP hardware clock (PHC), -which itself is synchronized by the -.BR ptp4l (8) -program. - -With the -.B \-a -option, the clocks to synchronize are fetched from the running -.B ptp4l -daemon and the direction of synchronization automatically follows changes of -the PTP port states. - -Manual configuration is also possible. When using manual configuration, two -synchronization modes are supported, one uses a pulse per second (PPS) -signal provided by the source clock and the other mode reads time from the -source clock directly. Some clocks can be used in both modes, the mode which -will synchronize the slave clock with better accuracy depends on hardware -and driver implementation. - -.SH OPTIONS -.TP -.BI \-a -Read the clocks to synchronize from running -.B ptp4l -and follow changes in the port states, adjusting the synchronization -direction automatically. The system clock (CLOCK_REALTIME) is not -synchronized, unless the -.B \-r -option is also specified. -.TP -.BI \-r -Only valid together with the -.B \-a -option. Instructs -.B phc2sys -to also synchronize the system clock (CLOCK_REALTIME). By default, the -system clock is not considered as a possible time source. If you want the -system clock to be eligible to become a time source, specify the -.B \-r -option twice. -.TP -.BI \-f " config" -Read configuration from the specified file. No configuration file is read by -default. -.TP -.BI \-d " pps-device" -Specify the PPS device of the master clock (e.g. /dev/pps0). With this option -the PPS synchronization mode is used instead of the direct mode. As the PPS -signal does not specify time and only marks start of a second, the slave clock -should be already close to the correct time before -.B phc2sys -is started or the -.B \-s -option should be used too. With the -.B \-s -option the PPS signal of the master clock is enabled automatically, otherwise -it has to be enabled before -.B phc2sys -is started (e.g. by running \f(CWecho 1 > /sys/class/ptp/ptp0/pps_enable\fP). -This option can be used only with the system clock as the slave clock. Not -compatible with the -.B \-a -option. -.TP -.BI \-s " device" -Specify the master clock by device (e.g. /dev/ptp0) or interface (e.g. eth0) or -by name (e.g. CLOCK_REALTIME for the system clock). When this option is used -together with the -.B \-d -option, the master clock is used only to correct the offset by whole number of -seconds, which cannot be fixed with PPS alone. Not compatible with the -.B \-a -option. This option does not support bonded interface (e.g. bond0). If -.B ptp4l -has a port on an active-backup bond interface, the -.B \-a -option can be used to track the active interface. -.TP -.BI \-i " interface" -Performs the exact same function as -.B \-s -for compatibility reasons. Previously enabled specifying master clock by network -interface. However, this can now be done using -.B \-s -and this option is no longer necessary. As such it has been deprecated, and -should no longer be used. -.TP -.BI \-c " device" -Specify the slave clock by device (e.g. /dev/ptp1) or interface (e.g. eth1) or -by name. The default is CLOCK_REALTIME (the system clock). Not compatible -with the -.B \-a -option. -.TP -.BI \-E " servo" -Specify which clock servo should be used. Valid values are pi for a PI -controller, linreg for an adaptive controller using linear regression, and -ntpshm for the NTP SHM reference clock to allow another process to synchronize -the local clock. -The default is pi. -.TP -.BI \-P " kp" -Specify the proportional constant of the PI controller. The default is 0.7. -.TP -.BI \-I " ki" -Specify the integral constant of the PI controller. The default is 0.3. -.TP -.BI \-S " step" -Specify the step threshold of the servo. It is the maximum offset that -the servo corrects by changing the clock frequency instead of stepping the -clock. The clock is stepped on start regardless of the option if the offset is -larger than 20 microseconds (unless the -.BI \-F -option is used). It's specified in seconds. The value of 0.0 disables stepping -after the start. The default is 0.0. -.TP -.BI \-F " step" -Specify the step threshold applied only on the first update. It is the maximum -offset that is corrected by changing the clock frequency. It's specified in -seconds. The value of 0.0 disables stepping on start. The default is 0.00002 -(20 microseconds). -.TP -.BI \-R " update-rate" -Specify the slave clock update rate when running in the direct synchronization -mode. The default is 1 per second. -.TP -.BI \-N " phc-num" -Specify the number of master clock readings per one slave clock update. Only -the fastest reading is used to update the slave clock, this is useful to -minimize the error caused by random delays in scheduling and bus utilization. -The default is 5. -.TP -.BI \-O " offset" -Specify the offset between the slave and master times in seconds. Not -compatible with the -.B \-a -option. See -.SM -.B TIME SCALE USAGE -below. -.TP -.BI \-L " freq-limit" -The maximum allowed frequency offset between uncorrected clock and the system -monotonic clock in parts per billion (ppb). This is used as a sanity check of -the synchronized clock. When a larger offset is measured, a warning message -will be printed and the servo will be reset. When set to 0, the sanity check is -disabled. The default is 200000000 (20%). -.TP -.BI \-M " segment" -The number of the SHM segment used by ntpshm servo. -The default is 0. -.TP -.BI \-u " summary-updates" -Specify the number of clock updates included in summary statistics. The -statistics include offset root mean square (RMS), maximum absolute offset, -frequency offset mean and standard deviation, and mean of the delay in clock -readings and standard deviation. The units are nanoseconds and parts per -billion (ppb). If zero, the individual samples are printed instead of the -statistics. The messages are printed at the LOG_INFO level. -The default is 0 (disabled). -.TP -.B \-w -Wait until ptp4l is in a synchronized state. If the -.B \-O -option is not used, also keep the offset between the slave and master -times updated according to the currentUtcOffset value obtained from ptp4l and -the direction of the clock synchronization. Not compatible with the -.B \-a -option. -.TP -.BI \-n " domain-number" -Specify the domain number used by ptp4l. The default is 0. -.TP -.B \-x -When a leap second is announced, don't apply it in the kernel by stepping the -clock, but let the servo correct the one-second offset slowly by changing the -clock frequency (unless the -.B \-S -option is used). -.TP -.BI \-z " uds-address" -Specifies the address of the server's UNIX domain socket. -The default is /var/run/ptp4l. -.TP -.BI \-l " print-level" -Set the maximum syslog level of messages which should be printed or sent to -the system logger. The default is 6 (LOG_INFO). -.TP -.BI \-t " message-tag" -Specify the tag which is added to all messages printed to the standard output -or system log. The default is an empty string. -.TP -.B \-m -Print messages to the standard output. -.TP -.B \-q -Don't send messages to the system logger. -.TP -.BI \-h -Display a help message. -.TP -.B \-v -Prints the software version and exits. - -.SH LONG OPTIONS - -Each and every configuration file option (see below in section -.BR FILE\ OPTIONS) -may also appear -as a "long" style command line argument. For example, the transportSpecific -option may be set using either of these two forms: - -.RS -\f(CW\-\-transportSpecific 1 \-\-transportSpecific=1\fP -.RE - -Option values given on the command line override values in the global -section of the configuration file (which, in turn overrides default -values). - -.SH CONFIGURATION FILE - -The configuration file is divided into sections. Each section starts with a -line containing its name enclosed in brackets and it follows with settings. -Each setting is placed on a separate line, it contains the name of the -option and the value separated by whitespace characters. Empty lines and lines -starting with # are ignored. - -The global section (indicated as -.BR [global] ) -sets the program options. This is the only used option. - -.SH FILE OPTIONS - -.TP -.B domainNumber -Specify the domain number used by phc2sys. The default is 0. Same as option -.B \-n -(see above). - -.TP -.B kernel_leap -When a leap second is announced, let the kernel apply it by stepping the -clock instead of correcting the one-second offset with servo, which would -correct the one-second offset slowly by changing the clock frequency -(unless the step_threshold option is set to correct such offset by -stepping). Relevant only with software time stamping. The default is 1 -(enabled). Same as option -.B \-x -(see above). - -The maximum logging level of messages which should be printed. -The default is 6 (LOG_INFO). Same as option -.B \-l -(see above). - -.TP -.B logging_level -The maximum logging level of messages which should be printed. -The default is 6 (LOG_INFO). Same as option -.B \-l -(see above). - -.TP -.B message_tag -The tag which is added to all messages printed to the standard output -or system log. The default is an empty string (which cannot be set in -the configuration file as the option requires an argument). -Same as option -.B \-t -(see above). - -.TP -.B sanity_freq_limit -The maximum allowed frequency offset between uncorrected clock and the -system monotonic clock in parts per billion (ppb). This is used as a -sanity check of the synchronized clock. When a larger offset is measured, -a warning message will be printed and the servo will be reset. When set -to 0, the sanity check is disabled. The default is 200000000 (20%). -Same as option -.B \-L -(see above). - -.TP -.B clock_servo -The servo which is used to synchronize the local clock. Valid values -are "pi" for a PI controller, "linreg" for an adaptive controller using -linear regression, "ntpshm" for the NTP SHM reference clock to allow -another process to synchronize the local clock (the SHM segment number -is set to the domain number), and "nullf" for a servo that always dials -frequency offset zero (for use in SyncE nodes). The default is "pi." -Same as option -.B \-E -(see above). - -.TP -.B transportSpecific -The transport specific field. Must be in the range 0 to 255. -The default is 0. - -.TP -.B use_syslog -Print messages to the system log if enabled. The default is 1 (enabled). -Related to option -.B \-q -(see above). - -.TP -.B verbose -Print messages to the standard output if enabled. The default is 0 (disabled). -Related to option -.B \-m -(see above). - -.TP -.B pi_proportional_const -Specifies the proportional constant of the PI controller. -Same as option -.B \-P -(see above). - -.TP -.B pi_integral_const -Specifies the integral constant of the PI controller. -Same as option -.B \-I -(see above). - -.TP -.B step_threshold -Specifies the step threshold of the servo. It is the maximum offset that -the servo corrects by changing the clock frequency instead of stepping -the clock. The clock is stepped on start regardless of the option if the -offset is larger than 20 microseconds (unless the -F option is used). -It's specified in seconds. The value of 0.0 disables stepping after -the start. The default is 0.0. -Same as option -.B \-S -(see above). - -.TP -.B first_step_threshold -Specify the step threshold applied only on the first update. It is the -maximum offset that is corrected by adjusting clock. It's specified in -seconds. The value of 0.0 disables stepping on start. The default is -0.00002 (20 microseconds). -Same as option -.B \-F -(see above). - -.TP -.B ntpshm_segment -The number of the SHM segment used by ntpshm servo. The default is 0. -Same as option -.B \-M -(see above). - -.TP -.B uds_address -Specifies the address of the server's UNIX domain socket. The default -is /var/run/ptp4 -Same as option -.B \-z -(see above). - -.SH TIME SCALE USAGE - -.B Ptp4l -uses either PTP time scale or UTC (Coordinated Universal Time) time -scale. PTP time scale is continuous and shifted against UTC by a few tens of -seconds as PTP time scale does not apply leap seconds. - -In hardware time stamping mode, -.B ptp4l -announces use of PTP time scale and PHC -is used for the stamps. That means PHC must follow PTP time scale while system -clock follows UTC. Time offset between these two is maintained by -.BR phc2sys . - -.B Phc2sys -acquires the offset value either by reading it from ptp4l when -.B \-a -or -.B \-w -is in effect or from command line when -.B \-O -is supplied. Failure to maintain the correct offset can result in local system -clock being off some seconds to domain master system clock when in slave mode, -or incorect PTP time announced to the network in case the host is the domain -master. - -.SH EXAMPLES - -Synchronize time automatically according to the current -.B ptp4l -state, synchronize the system clock to the remote master. - -.RS -\f(CWphc2sys \-a \-r\fP -.RE - -Same as above, but when the host becomes the domain master, synchronize time -in the domain to its system clock. - -.RS -\f(CWphc2sys \-a \-rr\fP -.RE - -Same as above, in an IEEE 802.1AS domain. - -.RS -\f(CWphc2sys \-a \-rr --transportSpecific=1\fP -.RE - -The host is a domain master, PTP clock is synchronized to system clock and the -time offset is obtained from -.BR ptp4l . -.B Phc2sys -waits for -.B ptp4l -to get at least one port in master or slave mode before starting the -synchronization. - -.RS -\f(CWphc2sys \-c /dev/ptp0 \-s CLOCK_REALTIME \-w\fP -.RE - -Same as above, time offset is provided on command line and -.B phc2sys -does not wait for -.BR ptp4l . - -.RS -\f(CWphc2sys \-c /dev/ptp0 \-s CLOCK_REALTIME \-O 35\fP -.RE - -The host is in slave mode, system clock is synchronized from PTP clock, -.B phc2sys -waits for -.B ptp4l -and the offset is set automatically. - -.RS -\f(CWphc2sys \-s /dev/ptp0 \-w\fP -.RE - -Same as above, PTP clock id is read from the network interface, the offset is -provided on command line -.B phc2sys -does not wait. - -.RS -\f(CWphc2sys \-s eth0 \-O \-35\fP -.RE - -.SH WARNING - -Be cautious when the same configuration file is used for both ptp4l and phc2sys. -Keep in mind, that values specified in the configuration file take precedence -over their default values. If a certain option, which is common to ptp4l and -phc2sys, is specified to a non-default value in the configuration file -(p.e., for ptp4l), then this non-default value applies also for phc2sys. This -might be not what is expected. - -It is recommended to use seperate configuration files for ptp4l and -phc2sys in order to avoid any unexpected behavior. - -.SH SEE ALSO -.BR ptp4l (8) diff --git a/phc2sys.c b/phc2sys.c index 65ff1cb..15f8d75 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -74,7 +74,7 @@ struct clock { LIST_ENTRY(clock) dst_list; clockid_t clkid; int phc_index; - int sysoff_method; + int sysoff_supported; int is_utc; int dest_only; int state; @@ -255,8 +255,9 @@ static struct clock *clock_add(struct node *node, char *device) c->servo = servo_add(node, c); if (clkid != CLOCK_INVALID && clkid != CLOCK_REALTIME) - c->sysoff_method = sysoff_probe(CLOCKID_TO_FD(clkid), - node->phc_readings); + c->sysoff_supported = (SYSOFF_SUPPORTED == + sysoff_probe(CLOCKID_TO_FD(clkid), + node->phc_readings)); LIST_INSERT_HEAD(&node->clocks, c, list); return c; @@ -783,23 +784,12 @@ static int do_loop(struct node *node, int subscriptions) continue; if (clock->clkid == CLOCK_REALTIME && - node->master->sysoff_method >= 0) { + node->master->sysoff_supported) { /* use sysoff */ if (sysoff_measure(CLOCKID_TO_FD(node->master->clkid), - node->master->sysoff_method, node->phc_readings, - &offset, &ts, &delay) < 0) + &offset, &ts, &delay)) return -1; - } else if (node->master->clkid == CLOCK_REALTIME && - clock->sysoff_method >= 0) { - /* use reversed sysoff */ - if (sysoff_measure(CLOCKID_TO_FD(clock->clkid), - clock->sysoff_method, - node->phc_readings, - &offset, &ts, &delay) < 0) - return -1; - offset = -offset; - ts += offset; } else { /* use phc */ if (!read_phc(node->master->clkid, clock->clkid, diff --git a/phc2sys.c.sysoff b/phc2sys.c.sysoff deleted file mode 100644 index 15f8d75..0000000 --- a/phc2sys.c.sysoff +++ /dev/null @@ -1,1687 +0,0 @@ -/** - * @file phc2sys.c - * @brief Utility program to synchronize two clocks via a PPS. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "clockadj.h" -#include "clockcheck.h" -#include "ds.h" -#include "fsm.h" -#include "missing.h" -#include "notification.h" -#include "ntpshm.h" -#include "phc.h" -#include "pi.h" -#include "pmc_common.h" -#include "print.h" -#include "servo.h" -#include "sk.h" -#include "stats.h" -#include "sysoff.h" -#include "tlv.h" -#include "uds.h" -#include "util.h" -#include "version.h" - -#define KP 0.7 -#define KI 0.3 -#define NS_PER_SEC 1000000000LL - -#define PHC_PPS_OFFSET_LIMIT 10000000 -#define PMC_UPDATE_INTERVAL (60 * NS_PER_SEC) -#define PMC_SUBSCRIBE_DURATION 180 /* 3 minutes */ -/* Note that PMC_SUBSCRIBE_DURATION has to be longer than - * PMC_UPDATE_INTERVAL otherwise subscription will time out before it is - * renewed. - */ - -struct clock { - LIST_ENTRY(clock) list; - LIST_ENTRY(clock) dst_list; - clockid_t clkid; - int phc_index; - int sysoff_supported; - int is_utc; - int dest_only; - int state; - int new_state; - int sync_offset; - int leap_set; - int utc_offset_set; - struct servo *servo; - enum servo_state servo_state; - char *device; - const char *source_label; - struct stats *offset_stats; - struct stats *freq_stats; - struct stats *delay_stats; - struct clockcheck *sanity_check; -}; - -struct port { - LIST_ENTRY(port) list; - unsigned int number; - int state; - struct clock *clock; -}; - -struct node { - unsigned int stats_max_count; - int sanity_freq_limit; - enum servo_type servo_type; - int phc_readings; - double phc_interval; - int sync_offset; - int forced_sync_offset; - int utc_offset_traceable; - int leap; - int kernel_leap; - struct pmc *pmc; - int pmc_ds_requested; - uint64_t pmc_last_update; - int state_changed; - int clock_identity_set; - struct ClockIdentity clock_identity; - LIST_HEAD(port_head, port) ports; - LIST_HEAD(clock_head, clock) clocks; - LIST_HEAD(dst_clock_head, clock) dst_clocks; - struct clock *master; -}; - -static struct config *phc2sys_config; - -static int update_pmc(struct node *node, int subscribe); -static int clock_handle_leap(struct node *node, struct clock *clock, - int64_t offset, uint64_t ts); -static int run_pmc_get_utc_offset(struct node *node, int timeout); -static void run_pmc_events(struct node *node); - -static int normalize_state(int state); -static int run_pmc_port_properties(struct node *node, int timeout, - unsigned int port, - int *state, int *tstamping, char *iface); - -static clockid_t clock_open(char *device, int *phc_index) -{ - struct sk_ts_info ts_info; - char phc_device[19]; - int clkid; - - /* check if device is CLOCK_REALTIME */ - if (!strcasecmp(device, "CLOCK_REALTIME")) - return CLOCK_REALTIME; - - /* check if device is valid phc device */ - clkid = phc_open(device); - if (clkid != CLOCK_INVALID) - return clkid; - - /* check if device is a valid ethernet device */ - if (sk_get_ts_info(device, &ts_info) || !ts_info.valid) { - fprintf(stderr, "unknown clock %s: %m\n", device); - return CLOCK_INVALID; - } - - if (ts_info.phc_index < 0) { - fprintf(stderr, "interface %s does not have a PHC\n", device); - return CLOCK_INVALID; - } - - sprintf(phc_device, "/dev/ptp%d", ts_info.phc_index); - clkid = phc_open(phc_device); - if (clkid == CLOCK_INVALID) - fprintf(stderr, "cannot open %s: %m\n", device); - *phc_index = ts_info.phc_index; - return clkid; -} - -static struct servo *servo_add(struct node *node, struct clock *clock) -{ - double ppb; - int max_ppb; - struct servo *servo; - - clockadj_init(clock->clkid); - ppb = clockadj_get_freq(clock->clkid); - /* The reading may silently fail and return 0, reset the frequency to - make sure ppb is the actual frequency of the clock. */ - clockadj_set_freq(clock->clkid, ppb); - if (clock->clkid == CLOCK_REALTIME) { - sysclk_set_leap(0); - max_ppb = sysclk_max_freq(); - } else { - max_ppb = phc_max_adj(clock->clkid); - if (!max_ppb) { - pr_err("clock is not adjustable"); - return NULL; - } - } - - servo = servo_create(phc2sys_config, node->servo_type, - -ppb, max_ppb, 0); - if (!servo) { - pr_err("Failed to create servo"); - return NULL; - } - - servo_sync_interval(servo, node->phc_interval); - - return servo; -} - -static struct clock *clock_add(struct node *node, char *device) -{ - struct clock *c; - clockid_t clkid = CLOCK_INVALID; - int phc_index = -1; - - if (device) { - clkid = clock_open(device, &phc_index); - if (clkid == CLOCK_INVALID) - return NULL; - } - - c = calloc(1, sizeof(*c)); - if (!c) { - pr_err("failed to allocate memory for a clock"); - return NULL; - } - c->clkid = clkid; - c->phc_index = phc_index; - c->servo_state = SERVO_UNLOCKED; - c->device = device ? strdup(device) : NULL; - - if (c->clkid == CLOCK_REALTIME) { - c->source_label = "sys"; - c->is_utc = 1; - } else { - c->source_label = "phc"; - } - - if (node->stats_max_count > 0) { - c->offset_stats = stats_create(); - c->freq_stats = stats_create(); - c->delay_stats = stats_create(); - if (!c->offset_stats || - !c->freq_stats || - !c->delay_stats) { - pr_err("failed to create stats"); - return NULL; - } - } - if (node->sanity_freq_limit) { - c->sanity_check = clockcheck_create(node->sanity_freq_limit); - if (!c->sanity_check) { - pr_err("failed to create clock check"); - return NULL; - } - } - - if (clkid != CLOCK_INVALID) - c->servo = servo_add(node, c); - - if (clkid != CLOCK_INVALID && clkid != CLOCK_REALTIME) - c->sysoff_supported = (SYSOFF_SUPPORTED == - sysoff_probe(CLOCKID_TO_FD(clkid), - node->phc_readings)); - - LIST_INSERT_HEAD(&node->clocks, c, list); - return c; -} - -static void clock_cleanup(struct node *node) -{ - struct clock *c, *tmp; - - LIST_FOREACH_SAFE(c, &node->clocks, list, tmp) { - if (c->servo) { - servo_destroy(c->servo); - } - if (c->sanity_check) { - clockcheck_destroy(c->sanity_check); - } - if (c->delay_stats) { - stats_destroy(c->delay_stats); - } - if (c->freq_stats) { - stats_destroy(c->freq_stats); - } - if (c->offset_stats) { - stats_destroy(c->offset_stats); - } - if (c->device) { - free(c->device); - } - free(c); - } -} - -static void port_cleanup(struct node *node) -{ - struct port *p, *tmp; - - LIST_FOREACH_SAFE(p, &node->ports, list, tmp) { - free(p); - } -} - -static struct port *port_get(struct node *node, unsigned int number) -{ - struct port *p; - - LIST_FOREACH(p, &node->ports, list) { - if (p->number == number) - return p; - } - return NULL; -} - -static struct port *port_add(struct node *node, unsigned int number, - char *device) -{ - struct port *p; - struct clock *c = NULL, *tmp; - - p = port_get(node, number); - if (p) - return p; - /* port is a new one, look whether we have the device already on - * a different port */ - LIST_FOREACH(tmp, &node->clocks, list) { - if (!strcmp(tmp->device, device)) { - c = tmp; - break; - } - } - if (!c) { - c = clock_add(node, device); - if (!c) - return NULL; - } - p = malloc(sizeof(*p)); - if (!p) { - pr_err("failed to allocate memory for a port"); - return NULL; - } - p->number = number; - p->clock = c; - LIST_INSERT_HEAD(&node->ports, p, list); - return p; -} - -static void clock_reinit(struct node *node, struct clock *clock, int new_state) -{ - int phc_index = -1, phc_switched = 0; - int state, timestamping, ret = -1; - struct port *p; - struct servo *servo; - struct sk_ts_info ts_info; - char iface[IFNAMSIZ]; - clockid_t clkid = CLOCK_INVALID; - - LIST_FOREACH(p, &node->ports, list) { - if (p->clock == clock) { - ret = run_pmc_port_properties(node, 1000, p->number, - &state, ×tamping, - iface); - if (ret > 0) - p->state = normalize_state(state); - } - } - - if (ret > 0 && timestamping != TS_SOFTWARE) { - /* Check if device changed */ - if (strcmp(clock->device, iface)) { - free(clock->device); - clock->device = strdup(iface); - } - /* Check if phc index changed */ - if (!sk_get_ts_info(clock->device, &ts_info) && - clock->phc_index != ts_info.phc_index) { - clkid = clock_open(clock->device, &phc_index); - if (clkid == CLOCK_INVALID) - return; - - phc_close(clock->clkid); - clock->clkid = clkid; - clock->phc_index = phc_index; - - servo = servo_add(node, clock); - if (servo) { - servo_destroy(clock->servo); - clock->servo = servo; - } - - phc_switched = 1; - } - } - - if (new_state == PS_MASTER || phc_switched) { - servo_reset(clock->servo); - clock->servo_state = SERVO_UNLOCKED; - - if (clock->offset_stats) { - stats_reset(clock->offset_stats); - stats_reset(clock->freq_stats); - stats_reset(clock->delay_stats); - } - } -} - -static struct clock *find_dst_clock(struct node *node, int phc_index) { - struct clock *c = NULL; - LIST_FOREACH(c, &node->dst_clocks, dst_list) { - if (c->phc_index == phc_index) { - break; - } - } - return c; -} - -static void reconfigure(struct node *node) -{ - struct clock *c, *rt = NULL, *src = NULL, *last = NULL, *dup = NULL; - int src_cnt = 0, dst_cnt = 0; - - pr_info("reconfiguring after port state change"); - node->state_changed = 0; - - while (node->dst_clocks.lh_first != NULL) { - LIST_REMOVE(node->dst_clocks.lh_first, dst_list); - } - - LIST_FOREACH(c, &node->clocks, list) { - if (c->clkid == CLOCK_REALTIME) { - rt = c; - continue; - } - - if (c->new_state) { - clock_reinit(node, c, c->new_state); - c->state = c->new_state; - c->new_state = 0; - } - - switch (c->state) { - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_PASSIVE: - dup = find_dst_clock(node, c->phc_index); - if (!dup) { - pr_info("selecting %s for synchronization", - c->device); - dst_cnt++; - LIST_INSERT_HEAD(&node->dst_clocks, - c, dst_list); - } else { - pr_info("skipping %s: %s has the same clock " - "and is already selected", - c->device, dup->device); - } - break; - case PS_UNCALIBRATED: - src_cnt++; - break; - case PS_SLAVE: - src = c; - src_cnt++; - break; - } - last = c; - } - if (dst_cnt > 1 && !src) { - if (!rt || rt->dest_only) { - node->master = last; - /* Reset to original state in next reconfiguration. */ - node->master->new_state = node->master->state; - node->master->state = PS_SLAVE; - if (rt) - rt->state = PS_SLAVE; - pr_info("no source, selecting %s as the default clock", - last->device); - return; - } - } - if (src_cnt > 1) { - pr_info("multiple master clocks available, postponing sync..."); - node->master = NULL; - return; - } - if (src_cnt > 0 && !src) { - pr_info("master clock not ready, waiting..."); - node->master = NULL; - return; - } - if (!src_cnt && !dst_cnt) { - pr_info("no PHC ready, waiting..."); - node->master = NULL; - return; - } - if ((!src_cnt && (!rt || rt->dest_only)) || - (!dst_cnt && !rt)) { - pr_info("nothing to synchronize"); - node->master = NULL; - return; - } - if (!src_cnt) { - src = rt; - rt->state = PS_SLAVE; - } else if (rt) { - if (rt->state != PS_MASTER) { - rt->state = PS_MASTER; - clock_reinit(node, rt, rt->state); - } - LIST_INSERT_HEAD(&node->dst_clocks, rt, dst_list); - pr_info("selecting %s for synchronization", rt->device); - } - node->master = src; - pr_info("selecting %s as the master clock", src->device); -} - -static int read_phc(clockid_t clkid, clockid_t sysclk, int readings, - int64_t *offset, uint64_t *ts, int64_t *delay) -{ - struct timespec tdst1, tdst2, tsrc; - int i; - int64_t interval, best_interval = INT64_MAX; - - /* Pick the quickest clkid reading. */ - for (i = 0; i < readings; i++) { - if (clock_gettime(sysclk, &tdst1) || - clock_gettime(clkid, &tsrc) || - clock_gettime(sysclk, &tdst2)) { - pr_err("failed to read clock: %m"); - return 0; - } - - interval = (tdst2.tv_sec - tdst1.tv_sec) * NS_PER_SEC + - tdst2.tv_nsec - tdst1.tv_nsec; - - if (best_interval > interval) { - best_interval = interval; - *offset = (tdst1.tv_sec - tsrc.tv_sec) * NS_PER_SEC + - tdst1.tv_nsec - tsrc.tv_nsec + interval / 2; - *ts = tdst2.tv_sec * NS_PER_SEC + tdst2.tv_nsec; - } - } - *delay = best_interval; - - return 1; -} - -static int64_t get_sync_offset(struct node *node, struct clock *dst) -{ - int direction = node->forced_sync_offset; - - if (!direction) - direction = dst->is_utc - node->master->is_utc; - return (int64_t)dst->sync_offset * NS_PER_SEC * direction; -} - -static void update_clock_stats(struct clock *clock, unsigned int max_count, - int64_t offset, double freq, int64_t delay) -{ - struct stats_result offset_stats, freq_stats, delay_stats; - - stats_add_value(clock->offset_stats, offset); - stats_add_value(clock->freq_stats, freq); - if (delay >= 0) - stats_add_value(clock->delay_stats, delay); - - if (stats_get_num_values(clock->offset_stats) < max_count) - return; - - stats_get_result(clock->offset_stats, &offset_stats); - stats_get_result(clock->freq_stats, &freq_stats); - - if (!stats_get_result(clock->delay_stats, &delay_stats)) { - pr_info("%s " - "rms %4.0f max %4.0f " - "freq %+6.0f +/- %3.0f " - "delay %5.0f +/- %3.0f", - clock->device, - offset_stats.rms, offset_stats.max_abs, - freq_stats.mean, freq_stats.stddev, - delay_stats.mean, delay_stats.stddev); - } else { - pr_info("%s " - "rms %4.0f max %4.0f " - "freq %+6.0f +/- %3.0f", - clock->device, - offset_stats.rms, offset_stats.max_abs, - freq_stats.mean, freq_stats.stddev); - } - - stats_reset(clock->offset_stats); - stats_reset(clock->freq_stats); - stats_reset(clock->delay_stats); -} - -static void update_clock(struct node *node, struct clock *clock, - int64_t offset, uint64_t ts, int64_t delay) -{ - enum servo_state state; - double ppb; - - if (clock_handle_leap(node, clock, offset, ts)) - return; - - offset += get_sync_offset(node, clock); - - if (clock->sanity_check && clockcheck_sample(clock->sanity_check, ts)) - servo_reset(clock->servo); - - ppb = servo_sample(clock->servo, offset, ts, 1.0, &state); - clock->servo_state = state; - - switch (state) { - case SERVO_UNLOCKED: - break; - case SERVO_JUMP: - clockadj_step(clock->clkid, -offset); - if (clock->sanity_check) - clockcheck_step(clock->sanity_check, -offset); - /* Fall through. */ - case SERVO_LOCKED: - clockadj_set_freq(clock->clkid, -ppb); - if (clock->clkid == CLOCK_REALTIME) - sysclk_set_sync(); - if (clock->sanity_check) - clockcheck_set_freq(clock->sanity_check, -ppb); - break; - } - - if (clock->offset_stats) { - update_clock_stats(clock, node->stats_max_count, offset, ppb, delay); - } else { - if (delay >= 0) { - pr_info("%s %s offset %9" PRId64 " s%d freq %+7.0f " - "delay %6" PRId64, - clock->device, node->master->source_label, - offset, state, ppb, delay); - } else { - pr_info("%s %s offset %9" PRId64 " s%d freq %+7.0f", - clock->device, node->master->source_label, - offset, state, ppb); - } - } -} - -static void enable_pps_output(clockid_t src) -{ - int enable = 1; - - if (!phc_has_pps(src)) - return; - if (ioctl(CLOCKID_TO_FD(src), PTP_ENABLE_PPS, enable) < 0) - pr_warning("failed to enable PPS output"); -} - -static int read_pps(int fd, int64_t *offset, uint64_t *ts) -{ - struct pps_fdata pfd; - - pfd.timeout.sec = 10; - pfd.timeout.nsec = 0; - pfd.timeout.flags = ~PPS_TIME_INVALID; - if (ioctl(fd, PPS_FETCH, &pfd)) { - pr_err("failed to fetch PPS: %m"); - return 0; - } - - *ts = pfd.info.assert_tu.sec * NS_PER_SEC; - *ts += pfd.info.assert_tu.nsec; - - *offset = *ts % NS_PER_SEC; - if (*offset > NS_PER_SEC / 2) - *offset -= NS_PER_SEC; - - return 1; -} - -static int do_pps_loop(struct node *node, struct clock *clock, int fd) -{ - int64_t pps_offset, phc_offset, phc_delay; - uint64_t pps_ts, phc_ts; - clockid_t src = node->master->clkid; - - node->master->source_label = "pps"; - - if (src == CLOCK_INVALID) { - /* The sync offset can't be applied with PPS alone. */ - node->sync_offset = 0; - } else { - enable_pps_output(node->master->clkid); - } - - while (is_running()) { - if (!read_pps(fd, &pps_offset, &pps_ts)) { - continue; - } - - /* If a PHC is available, use it to get the whole number - of seconds in the offset and PPS for the rest. */ - if (src != CLOCK_INVALID) { - if (!read_phc(src, clock->clkid, node->phc_readings, - &phc_offset, &phc_ts, &phc_delay)) - return -1; - - /* Convert the time stamp to the PHC time. */ - phc_ts -= phc_offset; - - /* Check if it is close to the start of the second. */ - if (phc_ts % NS_PER_SEC > PHC_PPS_OFFSET_LIMIT) { - pr_warning("PPS is not in sync with PHC" - " (0.%09lld)", phc_ts % NS_PER_SEC); - continue; - } - - phc_ts = phc_ts / NS_PER_SEC * NS_PER_SEC; - pps_offset = pps_ts - phc_ts; - } - - if (update_pmc(node, 0) < 0) - continue; - update_clock(node, clock, pps_offset, pps_ts, -1); - } - close(fd); - return 0; -} - -static int update_needed(struct clock *c) -{ - switch (c->state) { - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_PASSIVE: - return 1; - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - return 0; -} - -static int do_loop(struct node *node, int subscriptions) -{ - struct timespec interval; - struct clock *clock; - uint64_t ts; - int64_t offset, delay; - - interval.tv_sec = node->phc_interval; - interval.tv_nsec = (node->phc_interval - interval.tv_sec) * 1e9; - - while (is_running()) { - clock_nanosleep(CLOCK_MONOTONIC, 0, &interval, NULL); - if (update_pmc(node, subscriptions) < 0) - continue; - - if (subscriptions) { - run_pmc_events(node); - if (node->state_changed) { - /* force getting offset, as it may have - * changed after the port state change */ - if (run_pmc_get_utc_offset(node, 1000) <= 0) { - pr_err("failed to get UTC offset"); - continue; - } - reconfigure(node); - } - } - if (!node->master) - continue; - - LIST_FOREACH(clock, &node->dst_clocks, dst_list) { - if (!update_needed(clock)) - continue; - - /* don't try to synchronize the clock to itself */ - if (clock->clkid == node->master->clkid || - (clock->phc_index >= 0 && - clock->phc_index == node->master->phc_index) || - !strcmp(clock->device, node->master->device)) - continue; - - if (clock->clkid == CLOCK_REALTIME && - node->master->sysoff_supported) { - /* use sysoff */ - if (sysoff_measure(CLOCKID_TO_FD(node->master->clkid), - node->phc_readings, - &offset, &ts, &delay)) - return -1; - } else { - /* use phc */ - if (!read_phc(node->master->clkid, clock->clkid, - node->phc_readings, - &offset, &ts, &delay)) - continue; - } - update_clock(node, clock, offset, ts, delay); - } - } - return 0; -} - -static int check_clock_identity(struct node *node, struct ptp_message *msg) -{ - if (!node->clock_identity_set) - return 1; - return cid_eq(&node->clock_identity, - &msg->header.sourcePortIdentity.clockIdentity); -} - -static int is_msg_mgt(struct ptp_message *msg) -{ - struct TLV *tlv; - - if (msg_type(msg) != MANAGEMENT) - return 0; - if (management_action(msg) != RESPONSE) - return 0; - if (msg_tlv_count(msg) != 1) - return 0; - tlv = (struct TLV *) msg->management.suffix; - if (tlv->type == TLV_MANAGEMENT) - return 1; - if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS) - return -1; - return 0; -} - -static int get_mgt_id(struct ptp_message *msg) -{ - struct management_tlv *mgt = (struct management_tlv *) msg->management.suffix; - return mgt->id; -} - -static void *get_mgt_data(struct ptp_message *msg) -{ - struct management_tlv *mgt = (struct management_tlv *) msg->management.suffix; - return mgt->data; -} - -static int get_mgt_err_id(struct ptp_message *msg) -{ - struct management_error_status *mgt; - - mgt = (struct management_error_status *)msg->management.suffix; - return mgt->id; -} - -static int normalize_state(int state) -{ - if (state != PS_MASTER && state != PS_SLAVE && - state != PS_PRE_MASTER && state != PS_UNCALIBRATED) { - /* treat any other state as "not a master nor a slave" */ - state = PS_DISABLED; - } - return state; -} - -static int clock_compute_state(struct node *node, struct clock *clock) -{ - struct port *p; - int state = PS_DISABLED; - - LIST_FOREACH(p, &node->ports, list) { - if (p->clock != clock) - continue; - /* PS_SLAVE takes the highest precedence, PS_UNCALIBRATED - * after that, PS_MASTER is third, PS_PRE_MASTER fourth and - * all of that overrides PS_DISABLED, which corresponds - * nicely with the numerical values */ - if (p->state > state) - state = p->state; - } - return state; -} - -static int recv_subscribed(struct node *node, struct ptp_message *msg, - int excluded) -{ - int mgt_id, state; - struct portDS *pds; - struct port *port; - struct clock *clock; - - mgt_id = get_mgt_id(msg); - if (mgt_id == excluded) - return 0; - switch (mgt_id) { - case TLV_PORT_DATA_SET: - pds = get_mgt_data(msg); - port = port_get(node, pds->portIdentity.portNumber); - if (!port) { - pr_info("received data for unknown port %s", - pid2str(&pds->portIdentity)); - return 1; - } - state = normalize_state(pds->portState); - if (port->state != state) { - pr_info("port %s changed state", - pid2str(&pds->portIdentity)); - port->state = state; - clock = port->clock; - state = clock_compute_state(node, clock); - if (clock->state != state || clock->new_state) { - clock->new_state = state; - node->state_changed = 1; - } - } - return 1; - } - return 0; -} - -static void send_subscription(struct node *node) -{ - struct subscribe_events_np sen; - - memset(&sen, 0, sizeof(sen)); - sen.duration = PMC_SUBSCRIBE_DURATION; - sen.bitmask[0] = 1 << NOTIFY_PORT_STATE; - pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen)); -} - -static int init_pmc(struct config *cfg, struct node *node) -{ - char uds_local[MAX_IFNAME_SIZE + 1]; - - snprintf(uds_local, sizeof(uds_local), "/var/run/phc2sys.%d", - getpid()); - node->pmc = pmc_create(cfg, TRANS_UDS, uds_local, 0, - config_get_int(cfg, NULL, "domainNumber"), - config_get_int(cfg, NULL, "transportSpecific") << 4, 1); - if (!node->pmc) { - pr_err("failed to create pmc"); - return -1; - } - - return 0; -} - -/* Return values: - * 1: success - * 0: timeout - * -1: error reported by the other side - * -2: local error, fatal - */ -static int run_pmc(struct node *node, int timeout, int ds_id, - struct ptp_message **msg) -{ -#define N_FD 1 - struct pollfd pollfd[N_FD]; - int cnt, res; - - while (1) { - pollfd[0].fd = pmc_get_transport_fd(node->pmc); - pollfd[0].events = POLLIN|POLLPRI; - if (!node->pmc_ds_requested && ds_id >= 0) - pollfd[0].events |= POLLOUT; - - cnt = poll(pollfd, N_FD, timeout); - if (cnt < 0) { - pr_err("poll failed"); - return -2; - } - if (!cnt) { - /* Request the data set again in the next run. */ - node->pmc_ds_requested = 0; - return 0; - } - - /* Send a new request if there are no pending messages. */ - if ((pollfd[0].revents & POLLOUT) && - !(pollfd[0].revents & (POLLIN|POLLPRI))) { - switch (ds_id) { - case TLV_SUBSCRIBE_EVENTS_NP: - send_subscription(node); - break; - default: - pmc_send_get_action(node->pmc, ds_id); - break; - } - node->pmc_ds_requested = 1; - } - - if (!(pollfd[0].revents & (POLLIN|POLLPRI))) - continue; - - *msg = pmc_recv(node->pmc); - - if (!*msg) - continue; - - if (!check_clock_identity(node, *msg)) { - msg_put(*msg); - *msg = NULL; - continue; - } - - res = is_msg_mgt(*msg); - if (res < 0 && get_mgt_err_id(*msg) == ds_id) { - node->pmc_ds_requested = 0; - return -1; - } - if (res <= 0 || recv_subscribed(node, *msg, ds_id) || - get_mgt_id(*msg) != ds_id) { - msg_put(*msg); - *msg = NULL; - continue; - } - node->pmc_ds_requested = 0; - return 1; - } -} - -static int run_pmc_wait_sync(struct node *node, int timeout) -{ - struct ptp_message *msg; - int res; - void *data; - Enumeration8 portState; - - while (1) { - res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg); - if (res <= 0) - return res; - - data = get_mgt_data(msg); - portState = ((struct portDS *)data)->portState; - msg_put(msg); - - switch (portState) { - case PS_MASTER: - case PS_SLAVE: - return 1; - } - /* try to get more data sets (for other ports) */ - node->pmc_ds_requested = 1; - } -} - -static int run_pmc_get_utc_offset(struct node *node, int timeout) -{ - struct ptp_message *msg; - int res; - struct timePropertiesDS *tds; - - res = run_pmc(node, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg); - if (res <= 0) - return res; - - tds = (struct timePropertiesDS *)get_mgt_data(msg); - if (tds->flags & PTP_TIMESCALE) { - node->sync_offset = tds->currentUtcOffset; - if (tds->flags & LEAP_61) - node->leap = 1; - else if (tds->flags & LEAP_59) - node->leap = -1; - else - node->leap = 0; - node->utc_offset_traceable = tds->flags & UTC_OFF_VALID && - tds->flags & TIME_TRACEABLE; - } else { - node->sync_offset = 0; - node->leap = 0; - node->utc_offset_traceable = 0; - } - msg_put(msg); - return 1; -} - -static int run_pmc_get_number_ports(struct node *node, int timeout) -{ - struct ptp_message *msg; - int res; - struct defaultDS *dds; - - res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); - if (res <= 0) - return res; - - dds = (struct defaultDS *)get_mgt_data(msg); - res = dds->numberPorts; - msg_put(msg); - return res; -} - -static int run_pmc_subscribe(struct node *node, int timeout) -{ - struct ptp_message *msg; - int res; - - res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg); - if (res <= 0) - return res; - msg_put(msg); - return 1; -} - -static void run_pmc_events(struct node *node) -{ - struct ptp_message *msg; - - run_pmc(node, 0, -1, &msg); -} - -static int run_pmc_port_properties(struct node *node, int timeout, - unsigned int port, - int *state, int *tstamping, char *iface) -{ - struct ptp_message *msg; - int res, len; - struct port_properties_np *ppn; - - pmc_target_port(node->pmc, port); - while (1) { - res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg); - if (res <= 0) - goto out; - - ppn = get_mgt_data(msg); - if (ppn->portIdentity.portNumber != port) { - msg_put(msg); - continue; - } - - *state = ppn->port_state; - *tstamping = ppn->timestamping; - len = ppn->interface.length; - if (len > IFNAMSIZ - 1) - len = IFNAMSIZ - 1; - memcpy(iface, ppn->interface.text, len); - iface[len] = '\0'; - - msg_put(msg); - res = 1; - break; - } -out: - pmc_target_all(node->pmc); - return res; -} - -static int run_pmc_clock_identity(struct node *node, int timeout) -{ - struct ptp_message *msg; - struct defaultDS *dds; - int res; - - res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); - if (res <= 0) - return res; - - dds = (struct defaultDS *)get_mgt_data(msg); - memcpy(&node->clock_identity, &dds->clockIdentity, - sizeof(struct ClockIdentity)); - node->clock_identity_set = 1; - msg_put(msg); - return 1; -} - -static void close_pmc(struct node *node) -{ - pmc_destroy(node->pmc); - node->pmc = NULL; -} - -static int auto_init_ports(struct node *node, int add_rt) -{ - struct port *port; - struct clock *clock; - int number_ports, res; - unsigned int i; - int state, timestamping; - char iface[IFNAMSIZ]; - - while (1) { - if (!is_running()) - return -1; - res = run_pmc_clock_identity(node, 1000); - if (res < 0) - return -1; - if (res > 0) - break; - /* res == 0, timeout */ - pr_notice("Waiting for ptp4l..."); - } - - number_ports = run_pmc_get_number_ports(node, 1000); - if (number_ports <= 0) { - pr_err("failed to get number of ports"); - return -1; - } - - res = run_pmc_subscribe(node, 1000); - if (res <= 0) { - pr_err("failed to subscribe"); - return -1; - } - - for (i = 1; i <= number_ports; i++) { - res = run_pmc_port_properties(node, 1000, i, &state, - ×tamping, iface); - if (res == -1) { - /* port does not exist, ignore the port */ - continue; - } - if (res <= 0) { - pr_err("failed to get port properties"); - return -1; - } - if (timestamping == TS_SOFTWARE) { - /* ignore ports with software time stamping */ - continue; - } - port = port_add(node, i, iface); - if (!port) - return -1; - port->state = normalize_state(state); - } - if (LIST_EMPTY(&node->clocks)) { - pr_err("no suitable ports available"); - return -1; - } - LIST_FOREACH(clock, &node->clocks, list) { - clock->new_state = clock_compute_state(node, clock); - } - node->state_changed = 1; - - if (add_rt) { - clock = clock_add(node, "CLOCK_REALTIME"); - if (!clock) - return -1; - if (add_rt == 1) - clock->dest_only = 1; - } - - /* get initial offset */ - if (run_pmc_get_utc_offset(node, 1000) <= 0) { - pr_err("failed to get UTC offset"); - return -1; - } - return 0; -} - -/* Returns: -1 in case of error, 0 otherwise */ -static int update_pmc(struct node *node, int subscribe) -{ - struct timespec tp; - uint64_t ts; - - if (clock_gettime(CLOCK_MONOTONIC, &tp)) { - pr_err("failed to read clock: %m"); - return -1; - } - ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec; - - if (node->pmc && - !(ts > node->pmc_last_update && - ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) { - if (subscribe) - run_pmc_subscribe(node, 0); - if (run_pmc_get_utc_offset(node, 0) > 0) - node->pmc_last_update = ts; - } - - return 0; -} - -/* Returns: non-zero to skip clock update */ -static int clock_handle_leap(struct node *node, struct clock *clock, - int64_t offset, uint64_t ts) -{ - int clock_leap, node_leap = node->leap; - - clock->sync_offset = node->sync_offset; - - if ((node_leap || clock->leap_set) && - clock->is_utc != node->master->is_utc) { - /* If the master clock is in UTC, get a time stamp from it, as - it is the clock which will include the leap second. */ - if (node->master->is_utc) { - struct timespec tp; - if (clock_gettime(node->master->clkid, &tp)) { - pr_err("failed to read clock: %m"); - return -1; - } - ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec; - } - - /* If the clock will be stepped, the time stamp has to be the - new time. Ignore possible 1 second error in UTC offset. */ - if (clock->is_utc && clock->servo_state == SERVO_UNLOCKED) - ts -= offset + get_sync_offset(node, clock); - - /* Suspend clock updates in the last second before midnight. */ - if (is_utc_ambiguous(ts)) { - pr_info("clock update suspended due to leap second"); - return 1; - } - - clock_leap = leap_second_status(ts, clock->leap_set, - &node_leap, - &clock->sync_offset); - - if (clock->leap_set != clock_leap) { - /* Only the system clock can leap. */ - if (clock->clkid == CLOCK_REALTIME && - node->kernel_leap) - sysclk_set_leap(clock_leap); - else - servo_leap(clock->servo, clock_leap); - clock->leap_set = clock_leap; - } - } - - if (node->utc_offset_traceable && - clock->utc_offset_set != clock->sync_offset) { - if (clock->clkid == CLOCK_REALTIME) - sysclk_set_tai_offset(clock->sync_offset); - clock->utc_offset_set = clock->sync_offset; - } - - return 0; -} - -static void usage(char *progname) -{ - fprintf(stderr, - "\n" - "usage: %s [options]\n\n" - "\n" - " automatic configuration:\n" - " -a turn on autoconfiguration\n" - " -r synchronize system (realtime) clock\n" - " repeat -r to consider it also as a time source\n" - " manual configuration:\n" - " -c [dev|name] slave clock (CLOCK_REALTIME)\n" - " -d [dev] master PPS device\n" - " -s [dev|name] master clock\n" - " -O [offset] slave-master time offset (0)\n" - " -w wait for ptp4l\n" - " common options:\n" - " -f [file] configuration file\n" - " -E [pi|linreg] clock servo (pi)\n" - " -P [kp] proportional constant (0.7)\n" - " -I [ki] integration constant (0.3)\n" - " -S [step] step threshold (disabled)\n" - " -F [step] step threshold only on start (0.00002)\n" - " -R [rate] slave clock update rate in HZ (1.0)\n" - " -N [num] number of master clock readings per update (5)\n" - " -L [limit] sanity frequency limit in ppb (200000000)\n" - " -M [num] NTP SHM segment number (0)\n" - " -u [num] number of clock updates in summary stats (0)\n" - " -n [num] domain number (0)\n" - " -x apply leap seconds by servo instead of kernel\n" - " -z [path] server address for UDS (/var/run/ptp4l)\n" - " -l [num] set the logging level to 'num' (6)\n" - " -t [tag] add tag to log messages\n" - " -m print messages to stdout\n" - " -q do not print messages to the syslog\n" - " -v prints the software version and exits\n" - " -h prints this message and exits\n" - "\n", - progname); -} - -int main(int argc, char *argv[]) -{ - char *config = NULL, *dst_name = NULL, *progname, *src_name = NULL; - struct clock *src, *dst; - struct config *cfg; - struct option *opts; - int autocfg = 0, c, domain_number = 0, index, ntpshm_segment; - int pps_fd = -1, print_level = LOG_INFO, r = -1, rt = 0, wait_sync = 0; - double phc_rate, tmp; - struct node node = { - .phc_readings = 5, - .phc_interval = 1.0, - }; - - handle_term_signals(); - - cfg = phc2sys_config = config_create(); - if (!cfg) { - return -1; - } - - opts = config_long_options(cfg); - - config_set_double(cfg, "pi_proportional_const", KP); - config_set_double(cfg, "pi_integral_const", KI); - - /* Process the command line arguments. */ - progname = strrchr(argv[0], '/'); - progname = progname ? 1+progname : argv[0]; - while (EOF != (c = getopt_long(argc, argv, - "arc:d:f:s:E:P:I:S:F:R:N:O:L:M:i:u:wn:xz:l:t:mqvh", - opts, &index))) { - switch (c) { - case 0: - if (config_parse_option(cfg, opts[index].name, optarg)) { - goto bad_usage; - } - break; - case 'a': - autocfg = 1; - break; - case 'r': - rt++; - break; - case 'c': - dst_name = strdup(optarg); - break; - case 'd': - pps_fd = open(optarg, O_RDONLY); - if (pps_fd < 0) { - fprintf(stderr, - "cannot open '%s': %m\n", optarg); - goto end; - } - break; - case 'f': - config = optarg; - break; - case 'i': - fprintf(stderr, - "'-i' has been deprecated. please use '-s' instead.\n"); - /* fallthrough */ - case 's': - src_name = strdup(optarg); - break; - case 'E': - if (!strcasecmp(optarg, "pi")) { - config_set_int(cfg, "clock_servo", - CLOCK_SERVO_PI); - } else if (!strcasecmp(optarg, "linreg")) { - config_set_int(cfg, "clock_servo", - CLOCK_SERVO_LINREG); - } else if (!strcasecmp(optarg, "ntpshm")) { - config_set_int(cfg, "clock_servo", - CLOCK_SERVO_NTPSHM); - } else { - fprintf(stderr, - "invalid servo name %s\n", optarg); - goto end; - } - break; - case 'P': - if (get_arg_val_d(c, optarg, &tmp, 0.0, DBL_MAX) || - config_set_double(cfg, "pi_proportional_const", tmp)) - goto end; - break; - case 'I': - if (get_arg_val_d(c, optarg, &tmp, 0.0, DBL_MAX) || - config_set_double(cfg, "pi_integral_const", tmp)) - goto end; - break; - case 'S': - if (get_arg_val_d(c, optarg, &tmp, 0.0, DBL_MAX) || - config_set_double(cfg, "step_threshold", tmp)) - goto end; - break; - case 'F': - if (get_arg_val_d(c, optarg, &tmp, 0.0, DBL_MAX) || - config_set_double(cfg, "first_step_threshold", tmp)) - goto end; - break; - case 'R': - if (get_arg_val_d(c, optarg, &phc_rate, 1e-9, DBL_MAX)) - goto end; - node.phc_interval = 1.0 / phc_rate; - break; - case 'N': - if (get_arg_val_i(c, optarg, &node.phc_readings, 1, INT_MAX)) - goto end; - break; - case 'O': - if (get_arg_val_i(c, optarg, &node.sync_offset, - INT_MIN, INT_MAX)) - goto end; - node.forced_sync_offset = -1; - break; - case 'L': - if (get_arg_val_i(c, optarg, &node.sanity_freq_limit, 0, INT_MAX) || - config_set_int(cfg, "sanity_freq_limit", node.sanity_freq_limit)) { - goto end; - } - break; - case 'M': - if (get_arg_val_i(c, optarg, &ntpshm_segment, INT_MIN, INT_MAX) || - config_set_int(cfg, "ntpshm_segment", ntpshm_segment)) - goto end; - break; - case 'u': - if (get_arg_val_ui(c, optarg, &node.stats_max_count, - 0, UINT_MAX)) - goto end; - break; - case 'w': - wait_sync = 1; - break; - case 'n': - if (get_arg_val_i(c, optarg, &domain_number, 0, 255) || - config_set_int(cfg, "domainNumber", domain_number)) { - goto end; - } - break; - case 'x': - if (config_set_int(cfg, "kernel_leap", 0)) { - goto end; - } - break; - case 'z': - if (strlen(optarg) > MAX_IFNAME_SIZE) { - fprintf(stderr, "path %s too long, max is %d\n", - optarg, MAX_IFNAME_SIZE); - goto end; - } - if (config_set_string(cfg, "uds_address", optarg)) { - goto end; - } - break; - case 'l': - if (get_arg_val_i(c, optarg, &print_level, - PRINT_LEVEL_MIN, PRINT_LEVEL_MAX) || - config_set_int(cfg, "logging_level", print_level)) { - goto end; - } - break; - case 't': - if (config_set_string(cfg, "message_tag", optarg)) { - goto end; - } - break; - case 'm': - if (config_set_int(cfg, "verbose", 1)) { - goto end; - } - break; - case 'q': - if (config_set_int(cfg, "use_syslog", 0)) { - goto end; - } - break; - case 'v': - version_show(stdout); - config_destroy(cfg); - return 0; - case 'h': - usage(progname); - config_destroy(cfg); - return 0; - default: - goto bad_usage; - } - } - - if (config && (c = config_read(config, cfg))) { - return c; - } - - if (autocfg && (src_name || dst_name || pps_fd >= 0 || wait_sync || node.forced_sync_offset)) { - fprintf(stderr, - "autoconfiguration cannot be mixed with manual config options.\n"); - goto bad_usage; - } - if (!autocfg && pps_fd < 0 && !src_name) { - fprintf(stderr, - "autoconfiguration or valid source clock must be selected.\n"); - goto bad_usage; - } - - if (!autocfg && !wait_sync && !node.forced_sync_offset) { - fprintf(stderr, - "time offset must be specified using -w or -O\n"); - goto bad_usage; - } - - if (node.servo_type == CLOCK_SERVO_NTPSHM) { - node.kernel_leap = 0; - node.sanity_freq_limit = 0; - } - - print_set_progname(progname); - print_set_tag(config_get_string(cfg, NULL, "message_tag")); - print_set_verbose(config_get_int(cfg, NULL, "verbose")); - print_set_syslog(config_get_int(cfg, NULL, "use_syslog")); - print_set_level(config_get_int(cfg, NULL, "logging_level")); - - node.servo_type = config_get_int(cfg, NULL, "clock_servo"); - if (node.servo_type == CLOCK_SERVO_NTPSHM) { - config_set_int(cfg, "kernel_leap", 0); - config_set_int(cfg, "sanity_freq_limit", 0); - } - node.kernel_leap = config_get_int(cfg, NULL, "kernel_leap"); - node.sanity_freq_limit = config_get_int(cfg, NULL, "sanity_freq_limit"); - - if (autocfg) { - if (init_pmc(cfg, &node)) - goto end; - if (auto_init_ports(&node, rt) < 0) - goto end; - r = do_loop(&node, 1); - goto end; - } - - src = clock_add(&node, src_name); - free(src_name); - if (!src) { - fprintf(stderr, - "valid source clock must be selected.\n"); - goto bad_usage; - } - src->state = PS_SLAVE; - node.master = src; - - dst = clock_add(&node, dst_name ? dst_name : "CLOCK_REALTIME"); - free(dst_name); - if (!dst) { - fprintf(stderr, - "valid destination clock must be selected.\n"); - goto bad_usage; - } - dst->state = PS_MASTER; - LIST_INSERT_HEAD(&node.dst_clocks, dst, dst_list); - - if (pps_fd >= 0 && dst->clkid != CLOCK_REALTIME) { - fprintf(stderr, - "cannot use a pps device unless destination is CLOCK_REALTIME\n"); - goto bad_usage; - } - - r = -1; - - if (wait_sync) { - if (init_pmc(cfg, &node)) - goto end; - - while (is_running()) { - r = run_pmc_wait_sync(&node, 1000); - if (r < 0) - goto end; - if (r > 0) - break; - else - pr_notice("Waiting for ptp4l..."); - } - - if (!node.forced_sync_offset) { - r = run_pmc_get_utc_offset(&node, 1000); - if (r <= 0) { - pr_err("failed to get UTC offset"); - goto end; - } - } - - if (node.forced_sync_offset || - (src->clkid != CLOCK_REALTIME && dst->clkid != CLOCK_REALTIME) || - src->clkid == CLOCK_INVALID) - close_pmc(&node); - } - - if (pps_fd >= 0) { - /* only one destination clock allowed with PPS until we - * implement a mean to specify PTP port to PPS mapping */ - servo_sync_interval(dst->servo, 1.0); - r = do_pps_loop(&node, dst, pps_fd); - } else { - r = do_loop(&node, 0); - } - -end: - if (node.pmc) - close_pmc(&node); - clock_cleanup(&node); - port_cleanup(&node); - config_destroy(cfg); - msg_cleanup(); - return r; -bad_usage: - usage(progname); - config_destroy(cfg); - return -1; -} diff --git a/phc_ctl.c b/phc_ctl.c index b9a9cf4..4a78a19 100644 --- a/phc_ctl.c +++ b/phc_ctl.c @@ -367,12 +367,10 @@ static int do_cmp(clockid_t clkid, int cmdc, char *cmdv[]) struct timespec ts, rta, rtb; int64_t sys_offset, delay = 0, offset; uint64_t sys_ts; - int method; - method = sysoff_probe(CLOCKID_TO_FD(clkid), 9); - - if (method >= 0 && sysoff_measure(CLOCKID_TO_FD(clkid), method, 9, - &sys_offset, &sys_ts, &delay) >= 0) { + if (SYSOFF_SUPPORTED == + sysoff_measure(CLOCKID_TO_FD(clkid), + 9, &sys_offset, &sys_ts, &delay)) { pr_notice( "offset from CLOCK_REALTIME is %"PRId64"ns\n", sys_offset); return 0; diff --git a/phc_ctl.c.sysoff b/phc_ctl.c.sysoff deleted file mode 100644 index 4a78a19..0000000 --- a/phc_ctl.c.sysoff +++ /dev/null @@ -1,572 +0,0 @@ -/* - * @file phc_ctl.c - * @brief Utility program to directly control and debug a PHC device. - * @note Copyright (C) 2014 Jacob Keller - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "clockadj.h" -#include "missing.h" -#include "phc.h" -#include "print.h" -#include "sk.h" -#include "sysoff.h" -#include "util.h" -#include "version.h" - -#define NSEC2SEC 1000000000.0 - -/* trap the alarm signal so that pause() will wake up on receipt */ -static void handle_alarm(int s) -{ - return; -} - -static void double_to_timespec(double d, struct timespec *ts) -{ - double fraction, whole; - - fraction = modf(d, &whole); - - /* cast the whole value to a time_t to store as seconds */ - ts->tv_sec = (time_t)whole; - /* tv_nsec is a long, so we multiply the nanoseconds per second double - * value by our fractional component. This results in a correct - * timespec from the double representing seconds. - */ - ts->tv_nsec = (long)(NSEC2SEC * fraction); -} - -static int install_handler(int signum, void(*handler)(int)) -{ - struct sigaction action; - sigset_t mask; - - /* Unblock the signal */ - sigemptyset(&mask); - sigaddset(&mask, signum); - sigprocmask(SIG_UNBLOCK, &mask, NULL); - - /* Install the signal handler */ - action.sa_handler = handler; - action.sa_flags = 0; - sigemptyset(&action.sa_mask); - sigaction(signum, &action, NULL); - - return 0; -} - -static int64_t calculate_offset(struct timespec *ts1, - struct timespec *rt, - struct timespec *ts2) -{ - int64_t interval; - int64_t offset; - -#define NSEC_PER_SEC 1000000000ULL - /* calculate interval between clock realtime */ - interval = (ts2->tv_sec - ts1->tv_sec) * NSEC_PER_SEC; - interval += ts2->tv_nsec - ts1->tv_nsec; - - /* assume PHC read occured half way between CLOCK_REALTIME reads */ - - offset = (rt->tv_sec - ts1->tv_sec) * NSEC_PER_SEC; - offset += (rt->tv_nsec - ts1->tv_nsec) - (interval / 2); - - return offset; -} - -static clockid_t clock_open(char *device) -{ - struct sk_ts_info ts_info; - char phc_device[19]; - int clkid; - - /* check if device is CLOCK_REALTIME */ - if (!strcasecmp(device, "CLOCK_REALTIME")) - return CLOCK_REALTIME; - - /* check if device is valid phc device */ - clkid = phc_open(device); - if (clkid != CLOCK_INVALID) - return clkid; - - /* check if device is a valid ethernet device */ - if (sk_get_ts_info(device, &ts_info) || !ts_info.valid) { - pr_err("unknown clock %s: %m", device); - return CLOCK_INVALID; - } - - if (ts_info.phc_index < 0) { - pr_err("interface %s does not have a PHC", device); - return CLOCK_INVALID; - } - - sprintf(phc_device, "/dev/ptp%d", ts_info.phc_index); - clkid = phc_open(phc_device); - if (clkid == CLOCK_INVALID) - pr_err("cannot open %s for %s: %m", phc_device, device); - return clkid; -} - -static void usage(const char *progname) -{ - fprintf(stderr, - "\n" - "usage: %s [options] -- [command]\n\n" - " device ethernet or ptp clock device" - "\n" - " options\n" - " -l [num] set the logging level to 'num'\n" - " -q do not print messages to the syslog\n" - " -Q do not print messages to stdout\n" - " -v prints the software version and exits\n" - " -h prints this message and exits\n" - "\n" - " commands\n" - " specify commands with arguments. Can specify multiple\n" - " commands to be executed in order. Seconds are read as\n" - " double precision floating point values.\n" - " set [seconds] set PHC time (defaults to time on CLOCK_REALTIME)\n" - " get get PHC time\n" - " adj adjust PHC time by offset\n" - " freq [ppb] adjust PHC frequency (default returns current offset)\n" - " cmp compare PHC offset to CLOCK_REALTIME\n" - " caps display device capabilities (default if no command given)\n" - " wait pause between commands\n" - "\n", - progname); -} - -typedef int (*cmd_func_t)(clockid_t, int, char *[]); -struct cmd_t { - const char *name; - const cmd_func_t function; -}; - -static cmd_func_t get_command_function(const char *name); -static inline int name_is_a_command(const char *name); - -static int do_set(clockid_t clkid, int cmdc, char *cmdv[]) -{ - struct timespec ts; - double time_arg = 0; - int args_to_eat = 0; - - enum parser_result r; - - memset(&ts, 0, sizeof(ts)); - - /* if we have no more arguments, or the next argument is the ";" - * separator, then we run set as default parameter mode */ - if (cmdc < 1 || name_is_a_command(cmdv[0])) { - clock_gettime(CLOCK_REALTIME, &ts); - - /* since we aren't using the options, we can simply ensure - * that we don't eat any arguments - */ - args_to_eat = 0; - } else { - /* parse the double */ - r = get_ranged_double(cmdv[0], &time_arg, 0.0, DBL_MAX); - switch (r) { - case PARSED_OK: - break; - case MALFORMED: - pr_err("set: '%s' is not a valid double", cmdv[0]); - return -2; - case OUT_OF_RANGE: - pr_err("set: '%s' is out of range", cmdv[0]); - return -2; - default: - pr_err("set: couldn't process '%s'", cmdv[0]); - return -2; - } - - double_to_timespec(time_arg, &ts); - - /* in order for processing to work, we need to ensure the - * run_cmds loop eats the optional set argument - */ - args_to_eat = 1; - } - - if (clock_settime(clkid, &ts)) { - pr_err("set: failed to set clock time: %s", - strerror(errno)); - return -1; - } else { - pr_notice("set clock time to %ld.%09ld or %s", - ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec)); - } - - return args_to_eat; -} - -static int do_get(clockid_t clkid, int cmdc, char *cmdv[]) -{ - struct timespec ts; - - memset(&ts, 0, sizeof(ts)); - if (clock_gettime(clkid, &ts)) { - pr_err("get: failed to get clock time: %s", - strerror(errno)); - - return -1; - } else { - pr_notice("clock time is %ld.%09lu or %s", - ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec)); - } - - /* get operation does not require any arguments */ - return 0; -} - -static int do_adj(clockid_t clkid, int cmdc, char *cmdv[]) -{ - double time_arg; - int64_t nsecs; - enum parser_result r; - - if (cmdc < 1 || name_is_a_command(cmdv[0])) { - pr_err("adj: missing required time argument"); - return -2; - } - - /* parse the double time offset argument */ - r = get_ranged_double(cmdv[0], &time_arg, -DBL_MAX, DBL_MAX); - switch (r) { - case PARSED_OK: - break; - case MALFORMED: - pr_err("adj: '%s' is not a valid double", cmdv[0]); - return -2; - case OUT_OF_RANGE: - pr_err("adj: '%s' is out of range.", cmdv[0]); - return -2; - default: - pr_err("adj: couldn't process '%s'", cmdv[0]); - return -2; - } - - nsecs = (int64_t)(NSEC2SEC * time_arg); - - clockadj_init(clkid); - clockadj_step(clkid, nsecs); - - pr_notice("adjusted clock by %lf seconds", time_arg); - - /* adjustment always consumes one argument */ - return 1; -} - -static int do_freq(clockid_t clkid, int cmdc, char *cmdv[]) -{ - double ppb; - enum parser_result r; - - clockadj_init(clkid); - - if (cmdc < 1 || name_is_a_command(cmdv[0])) { - ppb = clockadj_get_freq(clkid); - pr_err("clock frequency offset is %lfppb", ppb); - - /* no argument was used */ - return 0; - } - - /* parse the double ppb argument */ - r = get_ranged_double(cmdv[0], &ppb, -NSEC2SEC, NSEC2SEC); - switch (r) { - case PARSED_OK: - break; - case MALFORMED: - pr_err("freq: '%s' is not a valid double", cmdv[0]); - return -2; - case OUT_OF_RANGE: - pr_err("freq: '%s' is out of range.", cmdv[0]); - return -2; - default: - pr_err("freq: couldn't process '%s'", cmdv[0]); - return -2; - } - - clockadj_set_freq(clkid, ppb); - pr_err("adjusted clock frequency offset to %lfppb", ppb); - - /* consumed one argument to determine the frequency adjustment value */ - return 1; -} - -static int do_caps(clockid_t clkid, int cmdc, char *cmdv[]) -{ - struct ptp_clock_caps caps; - - if (clkid == CLOCK_REALTIME) { - pr_warning("CLOCK_REALTIME is not a PHC device."); - return 0; - } - - if (ioctl(CLOCKID_TO_FD(clkid), PTP_CLOCK_GETCAPS, &caps)) { - pr_err("get capabilities failed: %s", - strerror(errno)); - return -1; - } - - pr_notice("\n" - "capabilities:\n" - " %d maximum frequency adjustment (ppb)\n" - " %d programable alarms\n" - " %d external time stamp channels\n" - " %d programmable periodic signals\n" - " %s pulse per second support", - caps.max_adj, - caps.n_alarm, - caps.n_ext_ts, - caps.n_per_out, - caps.pps ? "has" : "doesn't have"); - return 0; -} - -static int do_cmp(clockid_t clkid, int cmdc, char *cmdv[]) -{ - struct timespec ts, rta, rtb; - int64_t sys_offset, delay = 0, offset; - uint64_t sys_ts; - - if (SYSOFF_SUPPORTED == - sysoff_measure(CLOCKID_TO_FD(clkid), - 9, &sys_offset, &sys_ts, &delay)) { - pr_notice( "offset from CLOCK_REALTIME is %"PRId64"ns\n", - sys_offset); - return 0; - } - - memset(&ts, 0, sizeof(ts)); - memset(&ts, 0, sizeof(rta)); - memset(&ts, 0, sizeof(rtb)); - if (clock_gettime(CLOCK_REALTIME, &rta) || - clock_gettime(clkid, &ts) || - clock_gettime(CLOCK_REALTIME, &rtb)) { - pr_err("cmp: failed clock reads: %s\n", - strerror(errno)); - return -1; - } - - offset = calculate_offset(&rta, &ts, &rtb); - pr_notice( "offset from CLOCK_REALTIME is approximately %"PRId64"ns\n", - offset); - - return 0; -} - -static int do_wait(clockid_t clkid, int cmdc, char *cmdv[]) -{ - double time_arg; - struct timespec ts; - struct itimerval timer; - enum parser_result r; - - if (cmdc < 1 || name_is_a_command(cmdv[0])) { - pr_err("wait: requires sleep duration argument\n"); - return -2; - } - - memset(&timer, 0, sizeof(timer)); - - /* parse the double time offset argument */ - r = get_ranged_double(cmdv[0], &time_arg, 0.0, DBL_MAX); - switch (r) { - case PARSED_OK: - break; - case MALFORMED: - pr_err("wait: '%s' is not a valid double", cmdv[0]); - return -2; - case OUT_OF_RANGE: - pr_err("wait: '%s' is out of range.", cmdv[0]); - return -2; - default: - pr_err("wait: couldn't process '%s'", cmdv[0]); - return -2; - } - - double_to_timespec(time_arg, &ts); - timer.it_value.tv_sec = ts.tv_sec; - timer.it_value.tv_usec = ts.tv_nsec / 1000; - setitimer(ITIMER_REAL, &timer, NULL); - pause(); - - /* the SIGALRM is already trapped during initialization, so we will - * wake up here once the alarm is handled. - */ - pr_notice( "process slept for %lf seconds\n", time_arg); - - return 1; -} - -static const struct cmd_t all_commands[] = { - { "set", &do_set }, - { "get", &do_get }, - { "adj", &do_adj }, - { "freq", &do_freq }, - { "cmp", &do_cmp }, - { "caps", &do_caps }, - { "wait", &do_wait }, - { 0, 0 } -}; - -static cmd_func_t get_command_function(const char *name) -{ - int i; - cmd_func_t cmd = NULL; - - for (i = 0; all_commands[i].name != NULL; i++) { - if (!strncmp(name, - all_commands[i].name, - strlen(all_commands[i].name))) - cmd = all_commands[i].function; - } - - return cmd; -} - -static inline int name_is_a_command(const char *name) -{ - return get_command_function(name) != NULL; -} - -static int run_cmds(clockid_t clkid, int cmdc, char *cmdv[]) -{ - int i = 0, result = 0; - cmd_func_t action = NULL; - - while (i < cmdc) { - char *arg = cmdv[i]; - - /* increment now to remove the command argument */ - i++; - - action = get_command_function(arg); - if (action) - result = action(clkid, cmdc - i, &cmdv[i]); - else - pr_err("unknown command %s.", arg); - - /* result is how many arguments were used up by the command, - * not including the ";". We will increment the loop counter - * to avoid processing the arguments as commands. - */ - if (result < 0) - return result; - else - i += result; - } - - return 0; -} - -int main(int argc, char *argv[]) -{ - const char *progname; - char **cmdv, *default_cmdv[] = { "caps" }; - int c, result, cmdc; - int print_level = LOG_INFO, verbose = 1, use_syslog = 1; - clockid_t clkid; - - install_handler(SIGALRM, handle_alarm); - - /* Process the command line arguments. */ - progname = strrchr(argv[0], '/'); - progname = progname ? 1+progname : argv[0]; - while (EOF != (c = getopt(argc, argv, - "l:qQvh"))) { - switch (c) { - case 'l': - if (get_arg_val_i(c, optarg, &print_level, - PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) - return -1; - break; - case 'q': - use_syslog = 0; - break; - case 'Q': - verbose = 0; - break; - case 'v': - version_show(stdout); - return 0; - case 'h': - usage(progname); - return 0; - default: - usage(progname); - return -1; - } - } - - print_set_progname(progname); - print_set_verbose(verbose); - print_set_syslog(use_syslog); - print_set_level(print_level); - - if ((argc - optind) < 1) { - usage(progname); - return -1; - } - - if ((argc - optind) == 1) { - cmdv = default_cmdv; - cmdc = 1; - } else { - cmdv = &argv[optind+1]; - cmdc = argc - optind - 1; - } - - clkid = clock_open(argv[optind]); - if (clkid == CLOCK_INVALID) - return -1; - - /* pass the remaining arguments to the run_cmds loop */ - result = run_cmds(clkid, cmdc, cmdv); - if (result < -1) { - /* show usage when command fails */ - usage(progname); - return result; - } - - return 0; -} diff --git a/pmc_common.c b/pmc_common.c index 4d48e3a..4a160f6 100644 --- a/pmc_common.c +++ b/pmc_common.c @@ -546,7 +546,6 @@ int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize) } extra = msg_tlv_append(msg, sizeof(*mgt) + datasize); if (!extra) { - msg_put(msg); return -ENOMEM; } mgt = (struct management_tlv *) extra->tlv; diff --git a/pmc_common.c.msgput b/pmc_common.c.msgput deleted file mode 100644 index 4a160f6..0000000 --- a/pmc_common.c.msgput +++ /dev/null @@ -1,656 +0,0 @@ -/** - * @file pmc_common.c - * @note Copyright (C) 2012 Richard Cochran - * @note Copyright (C) 2013 Miroslav Lichvar - * - * 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. - */ -#include -#include -#include - -#include "print.h" -#include "tlv.h" -#include "transport.h" -#include "util.h" -#include "pmc_common.h" - -/* - Field Len Type - -------------------------------------------------------- - clockType 2 - physicalLayerProtocol 1 PTPText - physicalAddressLength 2 UInteger16 - physicalAddress 0 - protocolAddress 4 Enumeration16 + UInteger16 - manufacturerIdentity 3 - reserved 1 - productDescription 1 PTPText - revisionData 1 PTPText - userDescription 1 PTPText - profileIdentity 6 - -------------------------------------------------------- - TOTAL 22 -*/ -#define EMPTY_CLOCK_DESCRIPTION 22 -/* Includes one extra byte to make length even. */ -#define EMPTY_PTP_TEXT 2 - -static void do_get_action(struct pmc *pmc, int action, int index, char *str); -static void do_set_action(struct pmc *pmc, int action, int index, char *str); -static void not_supported(struct pmc *pmc, int action, int index, char *str); -static void null_management(struct pmc *pmc, int action, int index, char *str); - -static const char *action_string[] = { - "GET", - "SET", - "RESPONSE", - "COMMAND", - "ACKNOWLEDGE", -}; - -struct management_id { - char name[64]; - int code; - void (*func)(struct pmc *pmc, int action, int index, char *str); -}; - -struct management_id idtab[] = { -/* Clock management ID values */ - { "USER_DESCRIPTION", TLV_USER_DESCRIPTION, do_get_action }, - { "SAVE_IN_NON_VOLATILE_STORAGE", TLV_SAVE_IN_NON_VOLATILE_STORAGE, not_supported }, - { "RESET_NON_VOLATILE_STORAGE", TLV_RESET_NON_VOLATILE_STORAGE, not_supported }, - { "INITIALIZE", TLV_INITIALIZE, not_supported }, - { "FAULT_LOG", TLV_FAULT_LOG, not_supported }, - { "FAULT_LOG_RESET", TLV_FAULT_LOG_RESET, not_supported }, - { "DEFAULT_DATA_SET", TLV_DEFAULT_DATA_SET, do_get_action }, - { "CURRENT_DATA_SET", TLV_CURRENT_DATA_SET, do_get_action }, - { "PARENT_DATA_SET", TLV_PARENT_DATA_SET, do_get_action }, - { "TIME_PROPERTIES_DATA_SET", TLV_TIME_PROPERTIES_DATA_SET, do_get_action }, - { "PRIORITY1", TLV_PRIORITY1, do_set_action }, - { "PRIORITY2", TLV_PRIORITY2, do_set_action }, - { "DOMAIN", TLV_DOMAIN, do_get_action }, - { "SLAVE_ONLY", TLV_SLAVE_ONLY, do_get_action }, - { "TIME", TLV_TIME, not_supported }, - { "CLOCK_ACCURACY", TLV_CLOCK_ACCURACY, do_get_action }, - { "UTC_PROPERTIES", TLV_UTC_PROPERTIES, not_supported }, - { "TRACEABILITY_PROPERTIES", TLV_TRACEABILITY_PROPERTIES, do_get_action }, - { "TIMESCALE_PROPERTIES", TLV_TIMESCALE_PROPERTIES, do_get_action }, - { "PATH_TRACE_LIST", TLV_PATH_TRACE_LIST, not_supported }, - { "PATH_TRACE_ENABLE", TLV_PATH_TRACE_ENABLE, not_supported }, - { "GRANDMASTER_CLUSTER_TABLE", TLV_GRANDMASTER_CLUSTER_TABLE, not_supported }, - { "ACCEPTABLE_MASTER_TABLE", TLV_ACCEPTABLE_MASTER_TABLE, not_supported }, - { "ACCEPTABLE_MASTER_MAX_TABLE_SIZE", TLV_ACCEPTABLE_MASTER_MAX_TABLE_SIZE, not_supported }, - { "ALTERNATE_TIME_OFFSET_ENABLE", TLV_ALTERNATE_TIME_OFFSET_ENABLE, not_supported }, - { "ALTERNATE_TIME_OFFSET_NAME", TLV_ALTERNATE_TIME_OFFSET_NAME, not_supported }, - { "ALTERNATE_TIME_OFFSET_MAX_KEY", TLV_ALTERNATE_TIME_OFFSET_MAX_KEY, not_supported }, - { "ALTERNATE_TIME_OFFSET_PROPERTIES", TLV_ALTERNATE_TIME_OFFSET_PROPERTIES, not_supported }, - { "TRANSPARENT_CLOCK_DEFAULT_DATA_SET", TLV_TRANSPARENT_CLOCK_DEFAULT_DATA_SET, not_supported }, - { "PRIMARY_DOMAIN", TLV_PRIMARY_DOMAIN, not_supported }, - { "TIME_STATUS_NP", TLV_TIME_STATUS_NP, do_get_action }, - { "GRANDMASTER_SETTINGS_NP", TLV_GRANDMASTER_SETTINGS_NP, do_set_action }, -/* Port management ID values */ - { "NULL_MANAGEMENT", TLV_NULL_MANAGEMENT, null_management }, - { "CLOCK_DESCRIPTION", TLV_CLOCK_DESCRIPTION, do_get_action }, - { "PORT_DATA_SET", TLV_PORT_DATA_SET, do_get_action }, - { "LOG_ANNOUNCE_INTERVAL", TLV_LOG_ANNOUNCE_INTERVAL, do_get_action }, - { "ANNOUNCE_RECEIPT_TIMEOUT", TLV_ANNOUNCE_RECEIPT_TIMEOUT, do_get_action }, - { "LOG_SYNC_INTERVAL", TLV_LOG_SYNC_INTERVAL, do_get_action }, - { "VERSION_NUMBER", TLV_VERSION_NUMBER, do_get_action }, - { "ENABLE_PORT", TLV_ENABLE_PORT, not_supported }, - { "DISABLE_PORT", TLV_DISABLE_PORT, not_supported }, - { "UNICAST_NEGOTIATION_ENABLE", TLV_UNICAST_NEGOTIATION_ENABLE, not_supported }, - { "UNICAST_MASTER_TABLE", TLV_UNICAST_MASTER_TABLE, not_supported }, - { "UNICAST_MASTER_MAX_TABLE_SIZE", TLV_UNICAST_MASTER_MAX_TABLE_SIZE, not_supported }, - { "ACCEPTABLE_MASTER_TABLE_ENABLED", TLV_ACCEPTABLE_MASTER_TABLE_ENABLED, not_supported }, - { "ALTERNATE_MASTER", TLV_ALTERNATE_MASTER, not_supported }, - { "TRANSPARENT_CLOCK_PORT_DATA_SET", TLV_TRANSPARENT_CLOCK_PORT_DATA_SET, not_supported }, - { "DELAY_MECHANISM", TLV_DELAY_MECHANISM, do_get_action }, - { "LOG_MIN_PDELAY_REQ_INTERVAL", TLV_LOG_MIN_PDELAY_REQ_INTERVAL, do_get_action }, - { "PORT_DATA_SET_NP", TLV_PORT_DATA_SET_NP, do_set_action }, -}; - -static void do_get_action(struct pmc *pmc, int action, int index, char *str) -{ - if (action == GET) - pmc_send_get_action(pmc, idtab[index].code); - else - fprintf(stderr, "%s only allows GET\n", idtab[index].name); -} - -static void do_set_action(struct pmc *pmc, int action, int index, char *str) -{ - struct grandmaster_settings_np gsn; - struct management_tlv_datum mtd; - struct port_ds_np pnp; - int cnt, code = idtab[index].code; - int leap_61, leap_59, utc_off_valid; - int ptp_timescale, time_traceable, freq_traceable; - - switch (action) { - case GET: - pmc_send_get_action(pmc, code); - return; - case SET: - break; - case RESPONSE: - case COMMAND: - case ACKNOWLEDGE: - default: - fprintf(stderr, "%s only allows GET or SET\n", - idtab[index].name); - return; - } - switch (code) { - case TLV_PRIORITY1: - case TLV_PRIORITY2: - cnt = sscanf(str, " %*s %*s %hhu", &mtd.val); - if (cnt != 1) { - fprintf(stderr, "%s SET needs 1 value\n", - idtab[index].name); - break; - } - pmc_send_set_action(pmc, code, &mtd, sizeof(mtd)); - break; - case TLV_GRANDMASTER_SETTINGS_NP: - cnt = sscanf(str, " %*s %*s " - "clockClass %hhu " - "clockAccuracy %hhx " - "offsetScaledLogVariance %hx " - "currentUtcOffset %hd " - "leap61 %d " - "leap59 %d " - "currentUtcOffsetValid %d " - "ptpTimescale %d " - "timeTraceable %d " - "frequencyTraceable %d " - "timeSource %hhx ", - &gsn.clockQuality.clockClass, - &gsn.clockQuality.clockAccuracy, - &gsn.clockQuality.offsetScaledLogVariance, - &gsn.utc_offset, - &leap_61, - &leap_59, - &utc_off_valid, - &ptp_timescale, - &time_traceable, - &freq_traceable, - &gsn.time_source); - if (cnt != 11) { - fprintf(stderr, "%s SET needs 11 values\n", - idtab[index].name); - break; - } - gsn.time_flags = 0; - if (leap_61) - gsn.time_flags |= LEAP_61; - if (leap_59) - gsn.time_flags |= LEAP_59; - if (utc_off_valid) - gsn.time_flags |= UTC_OFF_VALID; - if (ptp_timescale) - gsn.time_flags |= PTP_TIMESCALE; - if (time_traceable) - gsn.time_flags |= TIME_TRACEABLE; - if (freq_traceable) - gsn.time_flags |= FREQ_TRACEABLE; - pmc_send_set_action(pmc, code, &gsn, sizeof(gsn)); - break; - case TLV_PORT_DATA_SET_NP: - cnt = sscanf(str, " %*s %*s " - "neighborPropDelayThresh %u " - "asCapable %d ", - &pnp.neighborPropDelayThresh, - &pnp.asCapable); - if (cnt != 2) { - fprintf(stderr, "%s SET needs 2 values\n", - idtab[index].name); - break; - } - pmc_send_set_action(pmc, code, &pnp, sizeof(pnp)); - break; - } -} - -static void not_supported(struct pmc *pmc, int action, int index, char *str) -{ - fprintf(stdout, "sorry, %s not supported yet\n", idtab[index].name); -} - -static void null_management(struct pmc *pmc, int action, int index, char *str) -{ - if (action == GET) - pmc_send_get_action(pmc, idtab[index].code); - else - puts("non-get actions still todo"); -} - -static int parse_action(char *s) -{ - int len = strlen(s); - if (0 == strncasecmp(s, "GET", len)) - return GET; - else if (0 == strncasecmp(s, "SET", len)) - return SET; - else if (0 == strncasecmp(s, "CMD", len)) - return COMMAND; - else if (0 == strncasecmp(s, "COMMAND", len)) - return COMMAND; - return BAD_ACTION; -} - -static int parse_id(char *s) -{ - int i, index = BAD_ID, len = strlen(s); - /* check for exact match */ - for (i = 0; i < ARRAY_SIZE(idtab); i++) { - if (strcasecmp(s, idtab[i].name) == 0) { - return i; - } - } - /* look for a unique prefix match */ - for (i = 0; i < ARRAY_SIZE(idtab); i++) { - if (0 == strncasecmp(s, idtab[i].name, len)) { - if (index == BAD_ID) - index = i; - else - return AMBIGUOUS_ID; - } - } - return index; -} - -static int parse_target(struct pmc *pmc, const char *str) -{ - struct PortIdentity pid; - - if (str[0] == '*') { - memset(&pid, 0xff, sizeof(pid)); - } else if (str2pid(str, &pid)) { - return -1; - } - - return pmc_target(pmc, &pid); -} - -static void print_help(FILE *fp) -{ - int i; - fprintf(fp, "\n"); - for (i = 0; i < ARRAY_SIZE(idtab); i++) { - if (idtab[i].func != not_supported) - fprintf(fp, "\t[action] %s\n", idtab[i].name); - } - fprintf(fp, "\n"); - fprintf(fp, "\tThe [action] can be GET, SET, CMD, or COMMAND\n"); - fprintf(fp, "\tCommands are case insensitive and may be abbreviated.\n"); - fprintf(fp, "\n"); - fprintf(fp, "\tTARGET [portIdentity]\n"); - fprintf(fp, "\tTARGET *\n"); - fprintf(fp, "\n"); -} - -struct pmc { - UInteger16 sequence_id; - UInteger8 boundary_hops; - UInteger8 domain_number; - UInteger8 transport_specific; - struct PortIdentity port_identity; - struct PortIdentity target; - - struct transport *transport; - struct fdarray fdarray; - int zero_length_gets; -}; - -struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type, - const char *iface_name, UInteger8 boundary_hops, - UInteger8 domain_number, UInteger8 transport_specific, - int zero_datalen) -{ - struct interface iface; - struct pmc *pmc; - - memset(&iface, 0, sizeof(iface)); - - pmc = calloc(1, sizeof *pmc); - if (!pmc) - return NULL; - - if (transport_type != TRANS_UDS && - generate_clock_identity(&pmc->port_identity.clockIdentity, - iface_name)) { - pr_err("failed to generate a clock identity"); - goto failed; - } - pmc->port_identity.portNumber = 1; - pmc_target_all(pmc); - - pmc->boundary_hops = boundary_hops; - pmc->domain_number = domain_number; - pmc->transport_specific = transport_specific; - - pmc->transport = transport_create(cfg, transport_type); - if (!pmc->transport) { - pr_err("failed to create transport"); - goto failed; - } - - strncpy(iface.name, iface_name, MAX_IFNAME_SIZE); - if (iface.ts_label[0] == '\0') { - strncpy(iface.ts_label, iface.name, MAX_IFNAME_SIZE); - } - - if (transport_open(pmc->transport, &iface, - &pmc->fdarray, TS_SOFTWARE)) { - pr_err("failed to open transport"); - goto failed; - } - pmc->zero_length_gets = zero_datalen ? 1 : 0; - - return pmc; - -failed: - if (pmc->transport) - transport_destroy(pmc->transport); - free(pmc); - return NULL; -} - -void pmc_destroy(struct pmc *pmc) -{ - transport_close(pmc->transport, &pmc->fdarray); - transport_destroy(pmc->transport); - free(pmc); -} - -static struct ptp_message *pmc_message(struct pmc *pmc, uint8_t action) -{ - struct ptp_message *msg; - int pdulen; - - msg = msg_allocate(); - if (!msg) - return NULL; - - pdulen = sizeof(struct management_msg); - msg->hwts.type = TS_SOFTWARE; - - msg->header.tsmt = MANAGEMENT | pmc->transport_specific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = pdulen; - msg->header.domainNumber = pmc->domain_number; - msg->header.sourcePortIdentity = pmc->port_identity; - msg->header.sequenceId = pmc->sequence_id++; - msg->header.control = CTL_MANAGEMENT; - msg->header.logMessageInterval = 0x7f; - - msg->management.targetPortIdentity = pmc->target; - msg->management.startingBoundaryHops = pmc->boundary_hops; - msg->management.boundaryHops = pmc->boundary_hops; - msg->management.flags = action; - - return msg; -} - -static int pmc_send(struct pmc *pmc, struct ptp_message *msg) -{ - int err; - - err = msg_pre_send(msg); - if (err) { - pr_err("msg_pre_send failed"); - return -1; - } - return transport_send(pmc->transport, &pmc->fdarray, - TRANS_GENERAL, msg); -} - -static int pmc_tlv_datalen(struct pmc *pmc, int id) -{ - int len = 0; - - if (pmc->zero_length_gets) - return len; - - switch (id) { - case TLV_USER_DESCRIPTION: - len += EMPTY_PTP_TEXT; - break; - case TLV_DEFAULT_DATA_SET: - len += sizeof(struct defaultDS); - break; - case TLV_CURRENT_DATA_SET: - len += sizeof(struct currentDS); - break; - case TLV_PARENT_DATA_SET: - len += sizeof(struct parentDS); - break; - case TLV_TIME_PROPERTIES_DATA_SET: - len += sizeof(struct timePropertiesDS); - break; - case TLV_PRIORITY1: - case TLV_PRIORITY2: - case TLV_DOMAIN: - case TLV_SLAVE_ONLY: - case TLV_CLOCK_ACCURACY: - case TLV_TRACEABILITY_PROPERTIES: - case TLV_TIMESCALE_PROPERTIES: - len += sizeof(struct management_tlv_datum); - break; - case TLV_TIME_STATUS_NP: - len += sizeof(struct time_status_np); - break; - case TLV_GRANDMASTER_SETTINGS_NP: - len += sizeof(struct grandmaster_settings_np); - break; - case TLV_NULL_MANAGEMENT: - break; - case TLV_CLOCK_DESCRIPTION: - len += EMPTY_CLOCK_DESCRIPTION; - break; - case TLV_PORT_DATA_SET: - len += sizeof(struct portDS); - break; - case TLV_PORT_DATA_SET_NP: - len += sizeof(struct port_ds_np); - break; - case TLV_LOG_ANNOUNCE_INTERVAL: - case TLV_ANNOUNCE_RECEIPT_TIMEOUT: - case TLV_LOG_SYNC_INTERVAL: - case TLV_VERSION_NUMBER: - case TLV_DELAY_MECHANISM: - case TLV_LOG_MIN_PDELAY_REQ_INTERVAL: - len += sizeof(struct management_tlv_datum); - break; - } - return len; -} - -int pmc_get_transport_fd(struct pmc *pmc) -{ - return pmc->fdarray.fd[FD_GENERAL]; -} - -int pmc_send_get_action(struct pmc *pmc, int id) -{ - int datalen, pdulen; - struct ptp_message *msg; - struct management_tlv *mgt; - struct tlv_extra *extra; - - msg = pmc_message(pmc, GET); - if (!msg) { - return -1; - } - mgt = (struct management_tlv *) msg->management.suffix; - mgt->type = TLV_MANAGEMENT; - datalen = pmc_tlv_datalen(pmc, id); - mgt->length = 2 + datalen; - mgt->id = id; - pdulen = msg->header.messageLength + sizeof(*mgt) + datalen; - msg->header.messageLength = pdulen; - - extra = tlv_extra_alloc(); - if (!extra) { - pr_err("failed to allocate TLV descriptor"); - msg_put(msg); - return -ENOMEM; - } - extra->tlv = (struct TLV *) msg->management.suffix; - msg_tlv_attach(msg, extra); - - if (id == TLV_CLOCK_DESCRIPTION && !pmc->zero_length_gets) { - /* - * Make sure the tlv_extra pointers dereferenced in - * mgt_pre_send() do point to something. - */ - struct mgmt_clock_description *cd = &extra->cd; - uint8_t *buf = mgt->data; - cd->clockType = (UInteger16 *) buf; - buf += sizeof(*cd->clockType); - cd->physicalLayerProtocol = (struct PTPText *) buf; - buf += sizeof(struct PTPText) + cd->physicalLayerProtocol->length; - cd->physicalAddress = (struct PhysicalAddress *) buf; - buf += sizeof(struct PhysicalAddress) + 0; - cd->protocolAddress = (struct PortAddress *) buf; - } - - pmc_send(pmc, msg); - msg_put(msg); - - return 0; -} - -int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize) -{ - struct management_tlv *mgt; - struct ptp_message *msg; - struct tlv_extra *extra; - - msg = pmc_message(pmc, SET); - if (!msg) { - return -1; - } - extra = msg_tlv_append(msg, sizeof(*mgt) + datasize); - if (!extra) { - return -ENOMEM; - } - mgt = (struct management_tlv *) extra->tlv; - mgt->type = TLV_MANAGEMENT; - mgt->length = 2 + datasize; - mgt->id = id; - memcpy(mgt->data, data, datasize); - pmc_send(pmc, msg); - msg_put(msg); - - return 0; -} - -struct ptp_message *pmc_recv(struct pmc *pmc) -{ - struct ptp_message *msg; - int cnt, err; - - msg = msg_allocate(); - if (!msg) { - pr_err("low memory"); - return NULL; - } - msg->hwts.type = TS_SOFTWARE; - cnt = transport_recv(pmc->transport, pmc_get_transport_fd(pmc), msg); - if (cnt <= 0) { - pr_err("recv message failed"); - goto failed; - } - err = msg_post_recv(msg, cnt); - if (err) { - switch (err) { - case -EBADMSG: - pr_err("bad message"); - break; - case -EPROTO: - pr_debug("ignoring message"); - break; - } - goto failed; - } - if (msg_sots_missing(msg)) { - pr_err("received %s without timestamp", - msg_type_string(msg_type(msg))); - goto failed; - } - - return msg; -failed: - msg_put(msg); - return NULL; -} - -int pmc_target(struct pmc *pmc, struct PortIdentity *pid) -{ - pmc->target = *pid; - return 0; -} - -void pmc_target_port(struct pmc *pmc, UInteger16 portNumber) -{ - pmc->target.portNumber = portNumber; -} - -void pmc_target_all(struct pmc *pmc) -{ - memset(&pmc->target, 0xff, sizeof(pmc->target)); -} - -const char *pmc_action_string(int action) -{ - return action_string[action]; -} - -int pmc_do_command(struct pmc *pmc, char *str) -{ - int action, id; - char action_str[10+1] = {0}, id_str[64+1] = {0}; - - if (0 == strncasecmp(str, "HELP", strlen(str))) { - print_help(stdout); - return 0; - } - - if (2 != sscanf(str, " %10s %64s", action_str, id_str)) - return -1; - - if (0 == strncasecmp(action_str, "TARGET", strlen(action_str))) - return parse_target(pmc, id_str); - - action = parse_action(action_str); - id = parse_id(id_str); - - if (action == BAD_ACTION || id == BAD_ID) - return -1; - - if (id == AMBIGUOUS_ID) { - fprintf(stdout, "id %s is too ambiguous\n", id_str); - return 0; - } - - fprintf(stdout, "sending: %s %s\n", - action_string[action], idtab[id].name); - - idtab[id].func(pmc, action, id, str); - - return 0; -} diff --git a/port.c b/port.c index a019974..5e0aed7 100644 --- a/port.c +++ b/port.c @@ -2411,7 +2411,7 @@ void port_link_status(void *ctx, int linkup, int ts_index) sk_get_ts_info(p->iface->ts_label, &p->iface->ts_info); /* Only switch phc with HW time stamping mode */ - if (p->iface->ts_info.valid && p->iface->ts_info.phc_index >= 0) { + if (p->phc_index >= 0 && p->iface->ts_info.valid) { required_modes = clock_required_modes(p->clock); if ((p->iface->ts_info.so_timestamping & required_modes) != required_modes) { pr_err("interface '%s' does not support requested " @@ -2512,7 +2512,7 @@ static enum fsm_event bc_event(struct port *p, int fd_index) msg->hwts.type = p->timestamping; cnt = transport_recv(p->trp, fd, msg); - if (cnt < 0) { + if (cnt <= 0) { pr_err("port %hu: recv message failed", portnum(p)); msg_put(msg); return EV_FAULT_DETECTED; diff --git a/port.c.team b/port.c.team deleted file mode 100644 index 5e0aed7..0000000 --- a/port.c.team +++ /dev/null @@ -1,3011 +0,0 @@ -/** - * @file port.c - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include - -#include "bmc.h" -#include "clock.h" -#include "filter.h" -#include "missing.h" -#include "msg.h" -#include "phc.h" -#include "port.h" -#include "port_private.h" -#include "print.h" -#include "rtnl.h" -#include "sk.h" -#include "tc.h" -#include "tlv.h" -#include "tmv.h" -#include "tsproc.h" -#include "unicast_client.h" -#include "unicast_service.h" -#include "util.h" - -#define ALLOWED_LOST_RESPONSES 3 -#define ANNOUNCE_SPAN 1 - -enum syfu_event { - SYNC_MISMATCH, - SYNC_MATCH, - FUP_MISMATCH, - FUP_MATCH, -}; - -static int port_capable(struct port *p); -static int port_is_ieee8021as(struct port *p); -static void port_nrate_initialize(struct port *p); - -static int announce_compare(struct ptp_message *m1, struct ptp_message *m2) -{ - struct announce_msg *a = &m1->announce, *b = &m2->announce; - int len = - sizeof(a->grandmasterPriority1) + - sizeof(a->grandmasterClockQuality) + - sizeof(a->grandmasterPriority2) + - sizeof(a->grandmasterIdentity) + - sizeof(a->stepsRemoved); - - return memcmp(&a->grandmasterPriority1, &b->grandmasterPriority1, len); -} - -static void announce_to_dataset(struct ptp_message *m, struct port *p, - struct dataset *out) -{ - struct announce_msg *a = &m->announce; - out->priority1 = a->grandmasterPriority1; - out->identity = a->grandmasterIdentity; - out->quality = a->grandmasterClockQuality; - out->priority2 = a->grandmasterPriority2; - out->localPriority = p->localPriority; - out->stepsRemoved = a->stepsRemoved; - out->sender = m->header.sourcePortIdentity; - out->receiver = p->portIdentity; -} - -int clear_fault_asap(struct fault_interval *faint) -{ - switch (faint->type) { - case FTMO_LINEAR_SECONDS: - return faint->val == 0 ? 1 : 0; - case FTMO_LOG2_SECONDS: - return faint->val == FRI_ASAP ? 1 : 0; - case FTMO_CNT: - return 0; - } - return 0; -} - -static void extract_address(struct ptp_message *m, struct PortAddress *paddr) -{ - int len = 0; - - switch (paddr->networkProtocol) { - case TRANS_UDP_IPV4: - len = sizeof(m->address.sin.sin_addr.s_addr); - memcpy(paddr->address, &m->address.sin.sin_addr.s_addr, len); - break; - case TRANS_UDP_IPV6: - len = sizeof(m->address.sin6.sin6_addr.s6_addr); - memcpy(paddr->address, &m->address.sin6.sin6_addr.s6_addr, len); - break; - case TRANS_IEEE_802_3: - len = MAC_LEN; - memcpy(paddr->address, &m->address.sll.sll_addr, len); - break; - default: - return; - } - paddr->addressLength = len; -} - -static int msg_current(struct ptp_message *m, struct timespec now) -{ - int64_t t1, t2, tmo; - - t1 = m->ts.host.tv_sec * NSEC2SEC + m->ts.host.tv_nsec; - t2 = now.tv_sec * NSEC2SEC + now.tv_nsec; - - if (m->header.logMessageInterval < -63) { - tmo = 0; - } else if (m->header.logMessageInterval > 31) { - tmo = INT64_MAX; - } else if (m->header.logMessageInterval < 0) { - tmo = 4LL * NSEC2SEC / (1 << -m->header.logMessageInterval); - } else { - tmo = 4LL * (1 << m->header.logMessageInterval) * NSEC2SEC; - } - - return t2 - t1 < tmo; -} - -static int msg_source_equal(struct ptp_message *m1, struct foreign_clock *fc) -{ - struct PortIdentity *id1, *id2; - id1 = &m1->header.sourcePortIdentity; - id2 = &fc->dataset.sender; - return 0 == memcmp(id1, id2, sizeof(*id1)); -} - -int source_pid_eq(struct ptp_message *m1, struct ptp_message *m2) -{ - return pid_eq(&m1->header.sourcePortIdentity, - &m2->header.sourcePortIdentity); -} - -enum fault_type last_fault_type(struct port *port) -{ - return port->last_fault_type; -} - -void fault_interval(struct port *port, enum fault_type ft, - struct fault_interval *i) -{ - i->type = port->flt_interval_pertype[ft].type; - i->val = port->flt_interval_pertype[ft].val; -} - -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 = { - {0, 0}, {0, 0} - }; - uint64_t ns; - int i; - - if (log_seconds < 0) { - - log_seconds *= -1; - for (i = 1, ns = scale * 500000000ULL; i < log_seconds; i++) { - ns >>= 1; - } - tmo.it_value.tv_nsec = ns; - - while (tmo.it_value.tv_nsec >= NS_PER_SEC) { - tmo.it_value.tv_nsec -= NS_PER_SEC; - tmo.it_value.tv_sec++; - } - - } else - tmo.it_value.tv_sec = scale * (1 << log_seconds); - - return timerfd_settime(fd, 0, &tmo, NULL); -} - -int set_tmo_lin(int fd, int seconds) -{ - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - - tmo.it_value.tv_sec = seconds; - return timerfd_settime(fd, 0, &tmo, NULL); -} - -int set_tmo_random(int fd, int min, int span, int log_seconds) -{ - uint64_t value_ns, min_ns, span_ns; - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - - if (log_seconds >= 0) { - min_ns = min * NS_PER_SEC << log_seconds; - span_ns = span * NS_PER_SEC << log_seconds; - } else { - min_ns = min * NS_PER_SEC >> -log_seconds; - span_ns = span * NS_PER_SEC >> -log_seconds; - } - - value_ns = min_ns + (span_ns * (random() % (1 << 15) + 1) >> 15); - - tmo.it_value.tv_sec = value_ns / NS_PER_SEC; - tmo.it_value.tv_nsec = value_ns % NS_PER_SEC; - - 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); -} - -void fc_clear(struct foreign_clock *fc) -{ - struct ptp_message *m; - - while (fc->n_messages) { - m = TAILQ_LAST(&fc->messages, messages); - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } -} - -static void fc_prune(struct foreign_clock *fc) -{ - struct timespec now; - struct ptp_message *m; - - clock_gettime(CLOCK_MONOTONIC, &now); - - while (fc->n_messages > FOREIGN_MASTER_THRESHOLD) { - m = TAILQ_LAST(&fc->messages, messages); - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } - - while (!TAILQ_EMPTY(&fc->messages)) { - m = TAILQ_LAST(&fc->messages, messages); - if (msg_current(m, now)) - break; - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } -} - -static int delay_req_current(struct ptp_message *m, struct timespec now) -{ - int64_t t1, t2, tmo = 5 * NSEC2SEC; - - t1 = m->ts.host.tv_sec * NSEC2SEC + m->ts.host.tv_nsec; - t2 = now.tv_sec * NSEC2SEC + now.tv_nsec; - - return t2 - t1 < tmo; -} - -void delay_req_prune(struct port *p) -{ - struct timespec now; - struct ptp_message *m; - clock_gettime(CLOCK_MONOTONIC, &now); - - while (!TAILQ_EMPTY(&p->delay_req)) { - m = TAILQ_LAST(&p->delay_req, delay_req); - if (delay_req_current(m, now)) { - break; - } - TAILQ_REMOVE(&p->delay_req, m, list); - msg_put(m); - } -} - -void ts_add(tmv_t *ts, Integer64 correction) -{ - if (!correction) { - return; - } - *ts = tmv_add(*ts, correction_to_tmv(correction)); -} - -/* - * Returns non-zero if the announce message is different than last. - */ -static int add_foreign_master(struct port *p, struct ptp_message *m) -{ - struct foreign_clock *fc; - struct ptp_message *tmp; - int broke_threshold = 0, diff = 0; - - LIST_FOREACH(fc, &p->foreign_masters, list) { - if (msg_source_equal(m, fc)) { - break; - } - } - if (!fc) { - pr_notice("port %hu: new foreign master %s", portnum(p), - pid2str(&m->header.sourcePortIdentity)); - - fc = malloc(sizeof(*fc)); - if (!fc) { - pr_err("low memory, failed to add foreign master"); - return 0; - } - memset(fc, 0, sizeof(*fc)); - TAILQ_INIT(&fc->messages); - LIST_INSERT_HEAD(&p->foreign_masters, fc, list); - fc->port = p; - fc->dataset.sender = m->header.sourcePortIdentity; - /* We do not count this first message, see 9.5.3(b) */ - return 0; - } - - /* - * If this message breaks the threshold, that is an important change. - */ - fc_prune(fc); - if (FOREIGN_MASTER_THRESHOLD - 1 == fc->n_messages) { - broke_threshold = 1; - } - - /* - * Okay, go ahead and add this announcement. - */ - msg_get(m); - fc->n_messages++; - TAILQ_INSERT_HEAD(&fc->messages, m, list); - - /* - * Test if this announcement contains changed information. - */ - if (fc->n_messages > 1) { - tmp = TAILQ_NEXT(m, list); - diff = announce_compare(m, tmp); - } - - return broke_threshold || diff; -} - -static int follow_up_info_append(struct ptp_message *m) -{ - struct follow_up_info_tlv *fui; - struct tlv_extra *extra; - - extra = msg_tlv_append(m, sizeof(*fui)); - if (!extra) { - return -1; - } - fui = (struct follow_up_info_tlv *) extra->tlv; - fui->type = TLV_ORGANIZATION_EXTENSION; - fui->length = sizeof(*fui) - sizeof(fui->type) - sizeof(fui->length); - memcpy(fui->id, ieee8021_id, sizeof(ieee8021_id)); - fui->subtype[2] = 1; - - return 0; -} - -static int net_sync_resp_append(struct port *p, struct ptp_message *m) -{ - struct timePropertiesDS *tp = clock_time_properties(p->clock); - struct ClockIdentity cid = clock_identity(p->clock), pid; - struct currentDS *cds = clock_current_dataset(p->clock); - struct parent_ds *dad = clock_parent_ds(p->clock); - struct port *best = clock_best_port(p->clock); - struct nsm_resp_tlv_head *head; - struct Timestamp last_sync; - struct PortAddress *paddr; - struct ptp_message *tmp; - struct tlv_extra *extra; - unsigned char *ptr; - int tlv_len; - - uint8_t buf[sizeof(*paddr) + sizeof(struct sockaddr_storage)]; - - last_sync = tmv_to_Timestamp(clock_ingress_time(p->clock)); - pid = dad->pds.parentPortIdentity.clockIdentity; - paddr = (struct PortAddress *)buf; - - if (best && !cid_eq(&cid, &pid)) { - /* Extract the parent's protocol address. */ - paddr->networkProtocol = transport_type(best->trp); - paddr->addressLength = - transport_protocol_addr(best->trp, paddr->address); - if (best->best) { - tmp = TAILQ_FIRST(&best->best->messages); - extract_address(tmp, paddr); - } - } else { - /* We are our own parent. */ - paddr->networkProtocol = transport_type(p->trp); - paddr->addressLength = - transport_protocol_addr(p->trp, paddr->address); - } - - tlv_len = sizeof(*head) + sizeof(*extra->foot) + paddr->addressLength; - - extra = msg_tlv_append(m, tlv_len); - if (!extra) { - return -1; - } - - head = (struct nsm_resp_tlv_head *) extra->tlv; - head->type = TLV_PTPMON_RESP; - head->length = tlv_len - sizeof(head->type) - sizeof(head->length); - head->port_state = p->state == PS_GRAND_MASTER ? PS_MASTER : p->state; - head->parent_addr.networkProtocol = paddr->networkProtocol; - head->parent_addr.addressLength = paddr->addressLength; - memcpy(head->parent_addr.address, paddr->address, paddr->addressLength); - - ptr = (unsigned char *) head; - ptr += sizeof(*head) + paddr->addressLength; - extra->foot = (struct nsm_resp_tlv_foot *) ptr; - - memcpy(&extra->foot->parent, &dad->pds, sizeof(extra->foot->parent)); - memcpy(&extra->foot->current, cds, sizeof(extra->foot->current)); - memcpy(&extra->foot->timeprop, tp, sizeof(extra->foot->timeprop)); - memcpy(&extra->foot->lastsync, &last_sync, sizeof(extra->foot->lastsync)); - - return 0; -} - -static struct follow_up_info_tlv *follow_up_info_extract(struct ptp_message *m) -{ - struct follow_up_info_tlv *f; - struct tlv_extra *extra; - - TAILQ_FOREACH(extra, &m->tlv_list, list) { - f = (struct follow_up_info_tlv *) extra->tlv; - if (f->type == TLV_ORGANIZATION_EXTENSION && - f->length == sizeof(*f) - sizeof(f->type) - sizeof(f->length) && -// memcmp(f->id, ieee8021_id, sizeof(ieee8021_id)) && - !f->subtype[0] && !f->subtype[1] && f->subtype[2] == 1) { - return f; - } - } - return NULL; -} - -static void free_foreign_masters(struct port *p) -{ - struct foreign_clock *fc; - while ((fc = LIST_FIRST(&p->foreign_masters)) != NULL) { - LIST_REMOVE(fc, list); - fc_clear(fc); - free(fc); - } -} - -static int fup_sync_ok(struct ptp_message *fup, struct ptp_message *sync) -{ - /* - * NB - If the sk_check_fupsync option is not enabled, then - * both of these time stamps will be zero. - */ - if (tmv_cmp(fup->hwts.sw, sync->hwts.sw) < 0) { - return 0; - } - return 1; -} - -static int incapable_ignore(struct port *p, struct ptp_message *m) -{ - if (port_capable(p)) { - return 0; - } - if (msg_type(m) == ANNOUNCE || msg_type(m) == SYNC) { - return 1; - } - return 0; -} - -static int path_trace_append(struct port *p, struct ptp_message *m, - struct parent_ds *dad) -{ - int length = 1 + dad->path_length, ptt_len, tlv_len; - struct path_trace_tlv *ptt; - struct tlv_extra *extra; - - if (length > PATH_TRACE_MAX) { - return -1; - } - - ptt_len = length * sizeof(struct ClockIdentity); - tlv_len = ptt_len + sizeof(ptt->type) + sizeof(ptt->length); - - extra = msg_tlv_append(m, tlv_len); - if (!extra) { - return -1; - } - ptt = (struct path_trace_tlv *) extra->tlv; - ptt->type = TLV_PATH_TRACE; - ptt->length = ptt_len; - memcpy(ptt->cid, dad->ptl, ptt->length); - ptt->cid[length - 1] = clock_identity(p->clock); - - return 0; -} - -static int path_trace_ignore(struct port *p, struct ptp_message *m) -{ - struct path_trace_tlv *ptt; - struct ClockIdentity cid; - struct tlv_extra *extra; - int i, cnt; - - if (!p->path_trace_enabled) { - return 0; - } - if (msg_type(m) != ANNOUNCE) { - return 0; - } - TAILQ_FOREACH(extra, &m->tlv_list, list) { - ptt = (struct path_trace_tlv *) extra->tlv; - if (ptt->type != TLV_PATH_TRACE) { - continue; - } - cnt = path_length(ptt); - cid = clock_identity(p->clock); - for (i = 0; i < cnt; i++) { - if (cid_eq(&ptt->cid[i], &cid)) { - return 1; - } - } - } - return 0; -} - -static int peer_prepare_and_send(struct port *p, struct ptp_message *msg, - enum transport_event event) -{ - int cnt; - if (msg_pre_send(msg)) { - return -1; - } - if (msg_unicast(msg)) { - cnt = transport_sendto(p->trp, &p->fda, event, msg); - } else { - cnt = transport_peer(p->trp, &p->fda, event, msg); - } - if (cnt <= 0) { - return -1; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, p->tx_timestamp_offset); - } - return 0; -} - -static int port_capable(struct port *p) -{ - if (!port_is_ieee8021as(p)) { - /* Normal 1588 ports are always capable. */ - goto capable; - } - - if (tmv_to_nanoseconds(p->peer_delay) > p->neighborPropDelayThresh) { - if (p->asCapable) - pr_debug("port %hu: peer_delay (%" PRId64 ") > neighborPropDelayThresh " - "(%" PRId32 "), resetting asCapable", portnum(p), - tmv_to_nanoseconds(p->peer_delay), - p->neighborPropDelayThresh); - goto not_capable; - } - - if (tmv_to_nanoseconds(p->peer_delay) < p->min_neighbor_prop_delay) { - if (p->asCapable) - pr_debug("port %hu: peer_delay (%" PRId64 ") < min_neighbor_prop_delay " - "(%" PRId32 "), resetting asCapable", portnum(p), - tmv_to_nanoseconds(p->peer_delay), - p->min_neighbor_prop_delay); - goto not_capable; - } - - if (p->pdr_missing > ALLOWED_LOST_RESPONSES) { - if (p->asCapable) - pr_debug("port %hu: missed %d peer delay resp, " - "resetting asCapable", portnum(p), p->pdr_missing); - goto not_capable; - } - - if (p->multiple_seq_pdr_count) { - if (p->asCapable) - pr_debug("port %hu: multiple sequential peer delay resp, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - - if (!p->peer_portid_valid) { - if (p->asCapable) - pr_debug("port %hu: invalid peer port id, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - - if (!p->nrate.ratio_valid) { - if (p->asCapable) - pr_debug("port %hu: invalid nrate, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - -capable: - if (!p->asCapable) - pr_debug("port %hu: setting asCapable", portnum(p)); - p->asCapable = 1; - return 1; - -not_capable: - if (p->asCapable) - port_nrate_initialize(p); - p->asCapable = 0; - return 0; -} - -int port_clr_tmo(int fd) -{ - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - return timerfd_settime(fd, 0, &tmo, NULL); -} - -static int port_ignore(struct port *p, struct ptp_message *m) -{ - struct ClockIdentity c1, c2; - - if (incapable_ignore(p, m)) { - return 1; - } - if (path_trace_ignore(p, m)) { - return 1; - } - if (p->match_transport_specific && - msg_transport_specific(m) != p->transportSpecific) { - return 1; - } - if (pid_eq(&m->header.sourcePortIdentity, &p->portIdentity)) { - return 1; - } - if (m->header.domainNumber != clock_domain_number(p->clock)) { - return 1; - } - - c1 = clock_identity(p->clock); - c2 = m->header.sourcePortIdentity.clockIdentity; - - if (cid_eq(&c1, &c2)) { - return 1; - } - return 0; -} - -static int port_nsm_reply(struct port *p, struct ptp_message *m) -{ - struct tlv_extra *extra; - - if (!p->net_sync_monitor) { - return 0; - } - if (!p->hybrid_e2e) { - return 0; - } - if (!msg_unicast(m)) { - return 0; - } - TAILQ_FOREACH(extra, &m->tlv_list, list) { - if (extra->tlv->type == TLV_PTPMON_REQ) { - return 1; - } - } - return 0; -} - -/* - * Test whether a 802.1AS port may transmit a sync message. - */ -static int port_sync_incapable(struct port *p) -{ - struct ClockIdentity cid; - struct PortIdentity pid; - - if (!port_is_ieee8021as(p)) { - return 0; - } - if (clock_gm_capable(p->clock)) { - return 0; - } - cid = clock_identity(p->clock); - pid = clock_parent_identity(p->clock); - if (cid_eq(&cid, &pid.clockIdentity)) { - /* - * We are the GM, but without gmCapable set. - */ - return 1; - } - return 0; -} - -static int port_is_ieee8021as(struct port *p) -{ - return p->follow_up_info ? 1 : 0; -} - -static void port_management_send_error(struct port *p, struct port *ingress, - struct ptp_message *msg, int error_id) -{ - if (port_management_error(p->portIdentity, ingress, msg, error_id)) - pr_err("port %hu: management error failed", portnum(p)); -} - -static const Octet profile_id_drr[] = {0x00, 0x1B, 0x19, 0x00, 0x01, 0x00}; -static const Octet profile_id_p2p[] = {0x00, 0x1B, 0x19, 0x00, 0x02, 0x00}; - -static int port_management_fill_response(struct port *target, - struct ptp_message *rsp, int id) -{ - struct mgmt_clock_description *cd; - struct management_tlv_datum *mtd; - struct clock_description *desc; - struct port_properties_np *ppn; - struct management_tlv *tlv; - struct port_ds_np *pdsnp; - struct tlv_extra *extra; - struct portDS *pds; - uint16_t u16; - uint8_t *buf; - int datalen; - - extra = tlv_extra_alloc(); - if (!extra) { - pr_err("failed to allocate TLV descriptor"); - return 0; - } - extra->tlv = (struct TLV *) rsp->management.suffix; - - tlv = (struct management_tlv *) rsp->management.suffix; - tlv->type = TLV_MANAGEMENT; - tlv->id = id; - - switch (id) { - case TLV_NULL_MANAGEMENT: - datalen = 0; - break; - case TLV_CLOCK_DESCRIPTION: - cd = &extra->cd; - buf = tlv->data; - cd->clockType = (UInteger16 *) buf; - buf += sizeof(*cd->clockType); - *cd->clockType = clock_type(target->clock); - cd->physicalLayerProtocol = (struct PTPText *) buf; - switch(transport_type(target->trp)) { - case TRANS_UDP_IPV4: - case TRANS_UDP_IPV6: - case TRANS_IEEE_802_3: - ptp_text_set(cd->physicalLayerProtocol, "IEEE 802.3"); - break; - default: - ptp_text_set(cd->physicalLayerProtocol, NULL); - break; - } - buf += sizeof(struct PTPText) + cd->physicalLayerProtocol->length; - - cd->physicalAddress = (struct PhysicalAddress *) buf; - u16 = transport_physical_addr(target->trp, - cd->physicalAddress->address); - memcpy(&cd->physicalAddress->length, &u16, 2); - buf += sizeof(struct PhysicalAddress) + u16; - - cd->protocolAddress = (struct PortAddress *) buf; - u16 = transport_type(target->trp); - memcpy(&cd->protocolAddress->networkProtocol, &u16, 2); - u16 = transport_protocol_addr(target->trp, - cd->protocolAddress->address); - memcpy(&cd->protocolAddress->addressLength, &u16, 2); - buf += sizeof(struct PortAddress) + u16; - - desc = clock_description(target->clock); - cd->manufacturerIdentity = buf; - memcpy(cd->manufacturerIdentity, - desc->manufacturerIdentity, OUI_LEN); - buf += OUI_LEN; - *(buf++) = 0; /* reserved */ - - cd->productDescription = (struct PTPText *) buf; - ptp_text_copy(cd->productDescription, &desc->productDescription); - buf += sizeof(struct PTPText) + cd->productDescription->length; - - cd->revisionData = (struct PTPText *) buf; - ptp_text_copy(cd->revisionData, &desc->revisionData); - buf += sizeof(struct PTPText) + cd->revisionData->length; - - cd->userDescription = (struct PTPText *) buf; - ptp_text_copy(cd->userDescription, &desc->userDescription); - buf += sizeof(struct PTPText) + cd->userDescription->length; - - if (target->delayMechanism == DM_P2P) { - memcpy(buf, profile_id_p2p, PROFILE_ID_LEN); - } else { - memcpy(buf, profile_id_drr, PROFILE_ID_LEN); - } - buf += PROFILE_ID_LEN; - datalen = buf - tlv->data; - break; - case TLV_PORT_DATA_SET: - pds = (struct portDS *) tlv->data; - pds->portIdentity = target->portIdentity; - if (target->state == PS_GRAND_MASTER) { - pds->portState = PS_MASTER; - } else { - pds->portState = target->state; - } - pds->logMinDelayReqInterval = target->logMinDelayReqInterval; - pds->peerMeanPathDelay = target->peerMeanPathDelay; - pds->logAnnounceInterval = target->logAnnounceInterval; - pds->announceReceiptTimeout = target->announceReceiptTimeout; - pds->logSyncInterval = target->logSyncInterval; - if (target->delayMechanism) { - pds->delayMechanism = target->delayMechanism; - } else { - pds->delayMechanism = DM_E2E; - } - pds->logMinPdelayReqInterval = target->logMinPdelayReqInterval; - pds->versionNumber = target->versionNumber; - datalen = sizeof(*pds); - break; - case TLV_LOG_ANNOUNCE_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logAnnounceInterval; - datalen = sizeof(*mtd); - break; - case TLV_ANNOUNCE_RECEIPT_TIMEOUT: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->announceReceiptTimeout; - datalen = sizeof(*mtd); - break; - case TLV_LOG_SYNC_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logSyncInterval; - datalen = sizeof(*mtd); - break; - case TLV_VERSION_NUMBER: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->versionNumber; - datalen = sizeof(*mtd); - break; - case TLV_DELAY_MECHANISM: - mtd = (struct management_tlv_datum *) tlv->data; - if (target->delayMechanism) - mtd->val = target->delayMechanism; - else - mtd->val = DM_E2E; - datalen = sizeof(*mtd); - break; - case TLV_LOG_MIN_PDELAY_REQ_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logMinPdelayReqInterval; - datalen = sizeof(*mtd); - break; - case TLV_PORT_DATA_SET_NP: - pdsnp = (struct port_ds_np *) tlv->data; - pdsnp->neighborPropDelayThresh = target->neighborPropDelayThresh; - pdsnp->asCapable = target->asCapable; - datalen = sizeof(*pdsnp); - break; - case TLV_PORT_PROPERTIES_NP: - ppn = (struct port_properties_np *)tlv->data; - ppn->portIdentity = target->portIdentity; - if (target->state == PS_GRAND_MASTER) - ppn->port_state = PS_MASTER; - else - ppn->port_state = target->state; - ppn->timestamping = target->timestamping; - ptp_text_set(&ppn->interface, target->iface->ts_label); - datalen = sizeof(*ppn) + ppn->interface.length; - break; - default: - /* The caller should *not* respond to this message. */ - tlv_extra_recycle(extra); - return 0; - } - - if (datalen % 2) { - tlv->data[datalen] = 0; - datalen++; - } - tlv->length = sizeof(tlv->id) + datalen; - rsp->header.messageLength += sizeof(*tlv) + datalen; - msg_tlv_attach(rsp, extra); - - /* The caller can respond to this message. */ - return 1; -} - -static int port_management_get_response(struct port *target, - struct port *ingress, int id, - struct ptp_message *req) -{ - struct PortIdentity pid = port_identity(target); - struct ptp_message *rsp; - int respond; - - rsp = port_management_reply(pid, ingress, req); - if (!rsp) { - return 0; - } - respond = port_management_fill_response(target, rsp, id); - if (respond) - port_prepare_and_send(ingress, rsp, TRANS_GENERAL); - msg_put(rsp); - return respond; -} - -static int port_management_set(struct port *target, - struct port *ingress, int id, - struct ptp_message *req) -{ - int respond = 0; - struct management_tlv *tlv; - struct port_ds_np *pdsnp; - - tlv = (struct management_tlv *) req->management.suffix; - - switch (id) { - case TLV_PORT_DATA_SET_NP: - pdsnp = (struct port_ds_np *) tlv->data; - target->neighborPropDelayThresh = pdsnp->neighborPropDelayThresh; - respond = 1; - break; - } - if (respond && !port_management_get_response(target, ingress, id, req)) - pr_err("port %hu: failed to send management set response", portnum(target)); - return respond ? 1 : 0; -} - -static void port_nrate_calculate(struct port *p, tmv_t origin, tmv_t ingress) -{ - struct nrate_estimator *n = &p->nrate; - - /* - * We experienced a successful exchanges of peer delay request - * and response, reset pdr_missing for this port. - */ - p->pdr_missing = 0; - - if (tmv_is_zero(n->ingress1)) { - n->ingress1 = ingress; - n->origin1 = origin; - return; - } - n->count++; - if (n->count < n->max_count) { - return; - } - if (tmv_cmp(ingress, n->ingress1) == 0) { - pr_warning("bad timestamps in nrate calculation"); - return; - } - n->ratio = - tmv_dbl(tmv_sub(origin, n->origin1)) / - tmv_dbl(tmv_sub(ingress, n->ingress1)); - n->ingress1 = ingress; - n->origin1 = origin; - n->count = 0; - n->ratio_valid = 1; -} - -static void port_nrate_initialize(struct port *p) -{ - int shift = p->freq_est_interval - p->logMinPdelayReqInterval; - - if (shift < 0) - shift = 0; - else if (shift >= sizeof(int) * 8) { - shift = sizeof(int) * 8 - 1; - pr_warning("freq_est_interval is too long"); - } - - /* We start in the 'incapable' state. */ - p->pdr_missing = ALLOWED_LOST_RESPONSES + 1; - p->asCapable = 0; - - p->peer_portid_valid = 0; - - p->nrate.origin1 = tmv_zero(); - p->nrate.ingress1 = tmv_zero(); - p->nrate.max_count = (1 << shift); - p->nrate.count = 0; - p->nrate.ratio = 1.0; - p->nrate.ratio_valid = 0; -} - -int port_set_announce_tmo(struct port *p) -{ - return set_tmo_random(p->fda.fd[FD_ANNOUNCE_TIMER], - p->announceReceiptTimeout, - p->announce_span, p->logAnnounceInterval); -} - -int port_set_delay_tmo(struct port *p) -{ - if (p->delayMechanism == DM_P2P) { - return set_tmo_log(p->fda.fd[FD_DELAY_TIMER], 1, - p->logMinPdelayReqInterval); - } else { - return set_tmo_random(p->fda.fd[FD_DELAY_TIMER], 0, 2, - p->logMinDelayReqInterval); - } -} - -static int port_set_manno_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, p->logAnnounceInterval); -} - -int port_set_qualification_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_QUALIFICATION_TIMER], - 1+clock_steps_removed(p->clock), p->logAnnounceInterval); -} - -static int port_set_sync_rx_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_SYNC_RX_TIMER], - p->syncReceiptTimeout, p->logSyncInterval); -} - -static int port_set_sync_tx_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_SYNC_TX_TIMER], 1, p->logSyncInterval); -} - -void port_show_transition(struct port *p, enum port_state next, - enum fsm_event event) -{ - if (event == EV_FAULT_DETECTED) { - pr_notice("port %hu: %s to %s on %s (%s)", portnum(p), - ps_str[p->state], ps_str[next], ev_str[event], - ft_str(last_fault_type(p))); - } else { - pr_notice("port %hu: %s to %s on %s", portnum(p), - ps_str[p->state], ps_str[next], ev_str[event]); - } -} - -static void port_slave_priority_warning(struct port *p) -{ - UInteger16 n = portnum(p); - pr_warning("port %hu: master state recommended in slave only mode", n); - pr_warning("port %hu: defaultDS.priority1 probably misconfigured", n); -} - -static void port_synchronize(struct port *p, - tmv_t ingress_ts, - struct timestamp origin_ts, - Integer64 correction1, Integer64 correction2) -{ - enum servo_state state; - tmv_t t1, t1c, t2, c1, c2; - - port_set_sync_rx_tmo(p); - - t1 = timestamp_to_tmv(origin_ts); - t2 = ingress_ts; - c1 = correction_to_tmv(correction1); - c2 = correction_to_tmv(correction2); - t1c = tmv_add(t1, tmv_add(c1, c2)); - - state = clock_synchronize(p->clock, t2, t1c); - switch (state) { - case SERVO_UNLOCKED: - port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0); - break; - case SERVO_JUMP: - port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0); - flush_delay_req(p); - if (p->peer_delay_req) { - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; - } - break; - case SERVO_LOCKED: - port_dispatch(p, EV_MASTER_CLOCK_SELECTED, 0); - break; - } -} - -/* - * Handle out of order packets. The network stack might - * provide the follow up _before_ the sync message. After all, - * they can arrive on two different ports. In addition, time - * stamping in PHY devices might delay the event packets. - */ -static void port_syfufsm(struct port *p, enum syfu_event event, - struct ptp_message *m) -{ - struct ptp_message *syn, *fup; - - switch (p->syfu) { - case SF_EMPTY: - switch (event) { - case SYNC_MISMATCH: - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_SYNC; - break; - case FUP_MISMATCH: - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_FUP; - break; - case SYNC_MATCH: - break; - case FUP_MATCH: - break; - } - break; - - case SF_HAVE_SYNC: - switch (event) { - case SYNC_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - break; - case SYNC_MATCH: - break; - case FUP_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_FUP; - break; - case FUP_MATCH: - syn = p->last_syncfup; - port_synchronize(p, syn->hwts.ts, m->ts.pdu, - syn->header.correction, - m->header.correction); - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - break; - } - break; - - case SF_HAVE_FUP: - switch (event) { - case SYNC_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_SYNC; - break; - case SYNC_MATCH: - fup = p->last_syncfup; - port_synchronize(p, m->hwts.ts, fup->ts.pdu, - m->header.correction, - fup->header.correction); - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - break; - case FUP_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - break; - case FUP_MATCH: - break; - } - break; - } -} - -static int port_pdelay_request(struct port *p) -{ - struct ptp_message *msg; - int err; - - /* If multiple pdelay resp were not detected the counter can be reset */ - if (!p->multiple_pdr_detected) { - p->multiple_seq_pdr_count = 0; - } - p->multiple_pdr_detected = 0; - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = PDELAY_REQ | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct pdelay_req_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.correction = -p->asymmetry; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.delayreq++; - msg->header.control = CTL_OTHER; - msg->header.logMessageInterval = port_is_ieee8021as(p) ? - p->logMinPdelayReqInterval : 0x7f; - - if (unicast_client_enabled(p) && p->unicast_master_table->peer_name) { - msg->address = p->unicast_master_table->peer_addr.address; - msg->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, msg, TRANS_EVENT); - if (err) { - pr_err("port %hu: send peer delay request failed", portnum(p)); - goto out; - } - if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted peer delay request"); - goto out; - } - - if (p->peer_delay_req) { - if (port_capable(p)) { - p->pdr_missing++; - } - msg_put(p->peer_delay_req); - } - p->peer_delay_req = msg; - return 0; -out: - msg_put(msg); - return -1; -} - -int port_delay_request(struct port *p) -{ - struct ptp_message *msg; - - /* Time to send a new request, forget current pdelay resp and fup */ - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - p->peer_delay_resp = NULL; - } - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - p->peer_delay_fup = NULL; - } - - if (p->delayMechanism == DM_P2P) { - return port_pdelay_request(p); - } - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = DELAY_REQ | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct delay_req_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.correction = -p->asymmetry; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.delayreq++; - msg->header.control = CTL_DELAY_REQ; - msg->header.logMessageInterval = 0x7f; - - if (p->hybrid_e2e) { - struct ptp_message *dst = TAILQ_FIRST(&p->best->messages); - msg->address = dst->address; - msg->header.flagField[0] |= UNICAST; - } - - if (port_prepare_and_send(p, msg, TRANS_EVENT)) { - pr_err("port %hu: send delay request failed", portnum(p)); - goto out; - } - if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted delay request"); - goto out; - } - - TAILQ_INSERT_HEAD(&p->delay_req, msg, list); - - return 0; -out: - msg_put(msg); - return -1; -} - -int port_tx_announce(struct port *p, struct address *dst) -{ - struct timePropertiesDS *tp = clock_time_properties(p->clock); - struct parent_ds *dad = clock_parent_ds(p->clock); - struct ptp_message *msg; - int err; - - if (p->inhibit_multicast_service && !dst) { - return 0; - } - if (!port_capable(p)) { - return 0; - } - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = ANNOUNCE | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct announce_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.announce++; - msg->header.control = CTL_OTHER; - msg->header.logMessageInterval = p->logAnnounceInterval; - - msg->header.flagField[1] = tp->flags; - - if (dst) { - msg->address = *dst; - msg->header.flagField[0] |= UNICAST; - } - msg->announce.currentUtcOffset = tp->currentUtcOffset; - msg->announce.grandmasterPriority1 = dad->pds.grandmasterPriority1; - msg->announce.grandmasterClockQuality = dad->pds.grandmasterClockQuality; - msg->announce.grandmasterPriority2 = dad->pds.grandmasterPriority2; - msg->announce.grandmasterIdentity = dad->pds.grandmasterIdentity; - msg->announce.stepsRemoved = clock_steps_removed(p->clock); - msg->announce.timeSource = tp->timeSource; - - if (p->path_trace_enabled && path_trace_append(p, msg, dad)) { - pr_err("port %hu: append path trace failed", portnum(p)); - } - - err = port_prepare_and_send(p, msg, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send announce failed", portnum(p)); - } - msg_put(msg); - return err; -} - -int port_tx_sync(struct port *p, struct address *dst) -{ - struct ptp_message *msg, *fup; - int err, event; - - switch (p->timestamping) { - case TS_SOFTWARE: - case TS_LEGACY_HW: - case TS_HARDWARE: - event = TRANS_EVENT; - break; - case TS_ONESTEP: - event = TRANS_ONESTEP; - break; - case TS_P2P1STEP: - event = TRANS_P2P1STEP; - break; - default: - return -1; - } - - if (p->inhibit_multicast_service && !dst) { - return 0; - } - if (!port_capable(p)) { - return 0; - } - if (port_sync_incapable(p)) { - return 0; - } - msg = msg_allocate(); - if (!msg) { - return -1; - } - fup = msg_allocate(); - if (!fup) { - msg_put(msg); - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = SYNC | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct sync_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.sync++; - msg->header.control = CTL_SYNC; - msg->header.logMessageInterval = p->logSyncInterval; - - if (p->timestamping != TS_ONESTEP && p->timestamping != TS_P2P1STEP) { - msg->header.flagField[0] |= TWO_STEP; - } - - if (dst) { - msg->address = *dst; - msg->header.flagField[0] |= UNICAST; - msg->header.logMessageInterval = 0x7f; - } - err = port_prepare_and_send(p, msg, event); - if (err) { - pr_err("port %hu: send sync failed", portnum(p)); - goto out; - } - if (p->timestamping == TS_ONESTEP || p->timestamping == TS_P2P1STEP) { - goto out; - } else if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted sync"); - err = -1; - goto out; - } - - /* - * Send the follow up message right away. - */ - fup->hwts.type = p->timestamping; - - fup->header.tsmt = FOLLOW_UP | p->transportSpecific; - fup->header.ver = PTP_VERSION; - fup->header.messageLength = sizeof(struct follow_up_msg); - fup->header.domainNumber = clock_domain_number(p->clock); - fup->header.sourcePortIdentity = p->portIdentity; - fup->header.sequenceId = p->seqnum.sync - 1; - fup->header.control = CTL_FOLLOW_UP; - fup->header.logMessageInterval = p->logSyncInterval; - - fup->follow_up.preciseOriginTimestamp = tmv_to_Timestamp(msg->hwts.ts); - - if (dst) { - fup->address = *dst; - fup->header.flagField[0] |= UNICAST; - } - if (p->follow_up_info && follow_up_info_append(fup)) { - pr_err("port %hu: append fup info failed", portnum(p)); - err = -1; - goto out; - } - - err = port_prepare_and_send(p, fup, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send follow up failed", portnum(p)); - } -out: - msg_put(msg); - msg_put(fup); - return err; -} - -/* - * port initialize and disable - */ -int port_is_enabled(struct port *p) -{ - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - return 0; - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - return 1; -} - -void flush_last_sync(struct port *p) -{ - if (p->syfu != SF_EMPTY) { - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - } -} - -void flush_delay_req(struct port *p) -{ - struct ptp_message *m; - while ((m = TAILQ_FIRST(&p->delay_req)) != NULL) { - TAILQ_REMOVE(&p->delay_req, m, list); - msg_put(m); - } -} - -static void flush_peer_delay(struct port *p) -{ - if (p->peer_delay_req) { - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; - } - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - p->peer_delay_resp = NULL; - } - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - p->peer_delay_fup = NULL; - } -} - -static void port_clear_fda(struct port *p, int count) -{ - int i; - - for (i = 0; i < count; i++) - p->fda.fd[i] = -1; -} - -void port_disable(struct port *p) -{ - int i; - - tc_flush(p); - flush_last_sync(p); - flush_delay_req(p); - flush_peer_delay(p); - - p->best = NULL; - free_foreign_masters(p); - transport_close(p->trp, &p->fda); - - for (i = 0; i < N_TIMER_FDS; i++) { - close(p->fda.fd[FD_FIRST_TIMER + i]); - } - - /* Keep rtnl socket to get link status info. */ - port_clear_fda(p, FD_RTNL); - clock_fda_changed(p->clock); -} - -int port_initialize(struct port *p) -{ - struct config *cfg = clock_config(p->clock); - int fd[N_TIMER_FDS], i; - - p->multiple_seq_pdr_count = 0; - p->multiple_pdr_detected = 0; - p->last_fault_type = FT_UNSPECIFIED; - p->logMinDelayReqInterval = config_get_int(cfg, p->name, "logMinDelayReqInterval"); - p->peerMeanPathDelay = 0; - p->logAnnounceInterval = config_get_int(cfg, p->name, "logAnnounceInterval"); - p->announceReceiptTimeout = config_get_int(cfg, p->name, "announceReceiptTimeout"); - p->syncReceiptTimeout = config_get_int(cfg, p->name, "syncReceiptTimeout"); - p->transportSpecific = config_get_int(cfg, p->name, "transportSpecific"); - p->transportSpecific <<= 4; - p->match_transport_specific = !config_get_int(cfg, p->name, "ignore_transport_specific"); - p->master_only = config_get_int(cfg, p->name, "masterOnly"); - p->localPriority = config_get_int(cfg, p->name, "G.8275.portDS.localPriority"); - p->logSyncInterval = config_get_int(cfg, p->name, "logSyncInterval"); - p->logMinPdelayReqInterval = config_get_int(cfg, p->name, "logMinPdelayReqInterval"); - p->neighborPropDelayThresh = config_get_int(cfg, p->name, "neighborPropDelayThresh"); - p->min_neighbor_prop_delay = config_get_int(cfg, p->name, "min_neighbor_prop_delay"); - - for (i = 0; i < N_TIMER_FDS; i++) { - fd[i] = -1; - } - for (i = 0; i < N_TIMER_FDS; i++) { - fd[i] = timerfd_create(CLOCK_MONOTONIC, 0); - if (fd[i] < 0) { - pr_err("timerfd_create: %s", strerror(errno)); - goto no_timers; - } - } - if (transport_open(p->trp, p->iface, &p->fda, p->timestamping)) - goto no_tropen; - - for (i = 0; i < N_TIMER_FDS; i++) { - p->fda.fd[FD_FIRST_TIMER + i] = fd[i]; - } - - if (port_set_announce_tmo(p)) { - goto no_tmo; - } - if (unicast_client_enabled(p) && unicast_client_set_tmo(p)) { - goto no_tmo; - } - - /* No need to open rtnl socket on UDS port. */ - if (transport_type(p->trp) != TRANS_UDS) { - if (p->fda.fd[FD_RTNL] == -1) - p->fda.fd[FD_RTNL] = rtnl_open(); - if (p->fda.fd[FD_RTNL] >= 0) - rtnl_link_query(p->fda.fd[FD_RTNL], p->iface->name); - } - - port_nrate_initialize(p); - - clock_fda_changed(p->clock); - return 0; - -no_tmo: - transport_close(p->trp, &p->fda); -no_tropen: -no_timers: - for (i = 0; i < N_TIMER_FDS; i++) { - if (fd[i] >= 0) - close(fd[i]); - } - return -1; -} - -static int port_renew_transport(struct port *p) -{ - int res; - - if (!port_is_enabled(p)) { - return 0; - } - transport_close(p->trp, &p->fda); - port_clear_fda(p, FD_FIRST_TIMER); - res = transport_open(p->trp, p->iface, &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; -} - -/* - * Returns non-zero if the announce message is different than last. - */ -static int update_current_master(struct port *p, struct ptp_message *m) -{ - struct foreign_clock *fc = p->best; - struct ptp_message *tmp; - struct parent_ds *dad; - struct path_trace_tlv *ptt; - struct timePropertiesDS tds; - - if (!msg_source_equal(m, fc)) - return add_foreign_master(p, m); - - if (p->state != PS_PASSIVE) { - tds.currentUtcOffset = m->announce.currentUtcOffset; - tds.flags = m->header.flagField[1]; - tds.timeSource = m->announce.timeSource; - clock_update_time_properties(p->clock, tds); - } - if (p->path_trace_enabled) { - ptt = (struct path_trace_tlv *) m->announce.suffix; - dad = clock_parent_ds(p->clock); - memcpy(dad->ptl, ptt->cid, ptt->length); - dad->path_length = path_length(ptt); - } - port_set_announce_tmo(p); - fc_prune(fc); - msg_get(m); - fc->n_messages++; - TAILQ_INSERT_HEAD(&fc->messages, m, list); - if (fc->n_messages > 1) { - tmp = TAILQ_NEXT(m, list); - return announce_compare(m, tmp); - } - return 0; -} - -struct dataset *port_best_foreign(struct port *port) -{ - return port->best ? &port->best->dataset : NULL; -} - -/* message processing routines */ - -/* - * Returns non-zero if the announce message is both qualified and different. - */ -int process_announce(struct port *p, struct ptp_message *m) -{ - int result = 0; - - /* Do not qualify announce messages with stepsRemoved >= 255, see - * IEEE1588-2008 section 9.3.2.5 (d) - */ - if (m->announce.stepsRemoved >= 255) { - return result; - } - - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - break; - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - result = add_foreign_master(p, m); - break; - case PS_PASSIVE: - case PS_UNCALIBRATED: - case PS_SLAVE: - result = update_current_master(p, m); - break; - } - return result; -} - -static int process_delay_req(struct port *p, struct ptp_message *m) -{ - int err, nsm, saved_seqnum_sync; - struct ptp_message *msg; - - nsm = port_nsm_reply(p, m); - - if (!nsm && p->state != PS_MASTER && p->state != PS_GRAND_MASTER) { - return 0; - } - - if (p->delayMechanism == DM_P2P) { - pr_warning("port %hu: delay request on P2P port", portnum(p)); - return 0; - } - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = DELAY_RESP | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct delay_resp_msg); - msg->header.domainNumber = m->header.domainNumber; - msg->header.correction = m->header.correction; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = m->header.sequenceId; - msg->header.control = CTL_DELAY_RESP; - msg->header.logMessageInterval = p->logMinDelayReqInterval; - - msg->delay_resp.receiveTimestamp = tmv_to_Timestamp(m->hwts.ts); - - msg->delay_resp.requestingPortIdentity = m->header.sourcePortIdentity; - - if (p->hybrid_e2e && msg_unicast(m)) { - msg->address = m->address; - msg->header.flagField[0] |= UNICAST; - msg->header.logMessageInterval = 0x7f; - } - if (nsm && net_sync_resp_append(p, msg)) { - pr_err("port %hu: append NSM failed", portnum(p)); - err = -1; - goto out; - } - err = port_prepare_and_send(p, msg, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send delay response failed", portnum(p)); - goto out; - } - if (nsm) { - saved_seqnum_sync = p->seqnum.sync; - p->seqnum.sync = m->header.sequenceId; - err = port_tx_sync(p, &m->address); - p->seqnum.sync = saved_seqnum_sync; - } -out: - msg_put(msg); - return err; -} - -void process_delay_resp(struct port *p, struct ptp_message *m) -{ - struct delay_resp_msg *rsp = &m->delay_resp; - struct PortIdentity master; - struct ptp_message *req; - tmv_t c3, t3, t4, t4c; - - master = clock_parent_identity(p->clock); - - if (p->state != PS_UNCALIBRATED && p->state != PS_SLAVE) { - return; - } - if (!pid_eq(&rsp->requestingPortIdentity, &p->portIdentity)) { - return; - } - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - TAILQ_FOREACH(req, &p->delay_req, list) { - if (rsp->hdr.sequenceId == ntohs(req->delay_req.hdr.sequenceId)) { - break; - } - } - if (!req) { - return; - } - - c3 = correction_to_tmv(m->header.correction); - t3 = req->hwts.ts; - t4 = timestamp_to_tmv(m->ts.pdu); - t4c = tmv_sub(t4, c3); - - clock_path_delay(p->clock, t3, t4c); - - TAILQ_REMOVE(&p->delay_req, req, list); - msg_put(req); - - if (p->logMinDelayReqInterval == rsp->hdr.logMessageInterval) { - return; - } - if (msg_unicast(m)) { - /* Unicast responses have logMinDelayReqInterval set to 0x7F. */ - return; - } - if (rsp->hdr.logMessageInterval < -10 || - rsp->hdr.logMessageInterval > 22) { - pl_info(300, "port %hu: ignore bogus delay request interval 2^%d", - portnum(p), rsp->hdr.logMessageInterval); - return; - } - p->logMinDelayReqInterval = rsp->hdr.logMessageInterval; - pr_notice("port %hu: minimum delay request interval 2^%d", - portnum(p), p->logMinDelayReqInterval); - port_set_delay_tmo(p); -} - -void process_follow_up(struct port *p, struct ptp_message *m) -{ - enum syfu_event event; - struct PortIdentity master; - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - return; - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - master = clock_parent_identity(p->clock); - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - - if (p->follow_up_info) { - struct follow_up_info_tlv *fui = follow_up_info_extract(m); - if (!fui) - return; - clock_follow_up_info(p->clock, fui); - } - - if (p->syfu == SF_HAVE_SYNC && - p->last_syncfup->header.sequenceId == m->header.sequenceId) { - event = FUP_MATCH; - } else { - event = FUP_MISMATCH; - } - port_syfufsm(p, event, m); -} - -int process_pdelay_req(struct port *p, struct ptp_message *m) -{ - struct ptp_message *rsp, *fup; - enum transport_event event; - int err; - - switch (p->timestamping) { - case TS_SOFTWARE: - case TS_LEGACY_HW: - case TS_HARDWARE: - case TS_ONESTEP: - event = TRANS_EVENT; - break; - case TS_P2P1STEP: - event = TRANS_P2P1STEP; - break; - default: - return -1; - } - - if (p->delayMechanism == DM_E2E) { - pr_warning("port %hu: pdelay_req on E2E port", portnum(p)); - return 0; - } - if (p->delayMechanism == DM_AUTO) { - pr_info("port %hu: peer detected, switch to P2P", portnum(p)); - p->delayMechanism = DM_P2P; - port_set_delay_tmo(p); - } - if (p->peer_portid_valid) { - if (!pid_eq(&p->peer_portid, &m->header.sourcePortIdentity)) { - pr_err("port %hu: received pdelay_req msg with " - "unexpected peer port id %s", - portnum(p), - pid2str(&m->header.sourcePortIdentity)); - p->peer_portid_valid = 0; - port_capable(p); - } - } else { - p->peer_portid_valid = 1; - p->peer_portid = m->header.sourcePortIdentity; - pr_debug("port %hu: peer port id set to %s", portnum(p), - pid2str(&p->peer_portid)); - } - - rsp = msg_allocate(); - if (!rsp) { - return -1; - } - - fup = msg_allocate(); - if (!fup) { - msg_put(rsp); - return -1; - } - - rsp->hwts.type = p->timestamping; - - rsp->header.tsmt = PDELAY_RESP | p->transportSpecific; - rsp->header.ver = PTP_VERSION; - rsp->header.messageLength = sizeof(struct pdelay_resp_msg); - rsp->header.domainNumber = m->header.domainNumber; - rsp->header.sourcePortIdentity = p->portIdentity; - rsp->header.sequenceId = m->header.sequenceId; - rsp->header.control = CTL_OTHER; - rsp->header.logMessageInterval = 0x7f; - - /* - * NB - We do not have any fraction nanoseconds for the correction - * fields, neither in the response or the follow up. - */ - if (p->timestamping == TS_P2P1STEP) { - rsp->header.correction = m->header.correction; - rsp->header.correction += p->tx_timestamp_offset; - rsp->header.correction += p->rx_timestamp_offset; - } else { - rsp->header.flagField[0] |= TWO_STEP; - rsp->pdelay_resp.requestReceiptTimestamp = - tmv_to_Timestamp(m->hwts.ts); - } - rsp->pdelay_resp.requestingPortIdentity = m->header.sourcePortIdentity; - - if (msg_unicast(m)) { - rsp->address = m->address; - rsp->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, rsp, event); - if (err) { - pr_err("port %hu: send peer delay response failed", portnum(p)); - goto out; - } - if (p->timestamping == TS_P2P1STEP) { - goto out; - } else if (msg_sots_missing(rsp)) { - pr_err("missing timestamp on transmitted peer delay response"); - err = -1; - goto out; - } - - /* - * Send the follow up message right away. - */ - fup->hwts.type = p->timestamping; - - fup->header.tsmt = PDELAY_RESP_FOLLOW_UP | p->transportSpecific; - fup->header.ver = PTP_VERSION; - fup->header.messageLength = sizeof(struct pdelay_resp_fup_msg); - fup->header.domainNumber = m->header.domainNumber; - fup->header.correction = m->header.correction; - fup->header.sourcePortIdentity = p->portIdentity; - fup->header.sequenceId = m->header.sequenceId; - fup->header.control = CTL_OTHER; - fup->header.logMessageInterval = 0x7f; - - fup->pdelay_resp_fup.requestingPortIdentity = m->header.sourcePortIdentity; - - fup->pdelay_resp_fup.responseOriginTimestamp = - tmv_to_Timestamp(rsp->hwts.ts); - - if (msg_unicast(m)) { - fup->address = m->address; - fup->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, fup, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send pdelay_resp_fup failed", portnum(p)); - } -out: - msg_put(rsp); - msg_put(fup); - return err; -} - -static void port_peer_delay(struct port *p) -{ - tmv_t c1, c2, t1, t2, t3, t3c, t4; - struct ptp_message *req = p->peer_delay_req; - struct ptp_message *rsp = p->peer_delay_resp; - struct ptp_message *fup = p->peer_delay_fup; - - /* Check for response, validate port and sequence number. */ - - if (!rsp) - return; - - if (!pid_eq(&rsp->pdelay_resp.requestingPortIdentity, &p->portIdentity)) - return; - - if (rsp->header.sequenceId != ntohs(req->header.sequenceId)) - return; - - t1 = req->hwts.ts; - t4 = rsp->hwts.ts; - c1 = correction_to_tmv(rsp->header.correction + p->asymmetry); - - /* Process one-step response immediately. */ - if (one_step(rsp)) { - t2 = tmv_zero(); - t3 = tmv_zero(); - c2 = tmv_zero(); - goto calc; - } - - /* Check for follow up, validate port and sequence number. */ - - if (!fup) - return; - - if (!pid_eq(&fup->pdelay_resp_fup.requestingPortIdentity, &p->portIdentity)) - return; - - if (fup->header.sequenceId != rsp->header.sequenceId) - return; - - if (!source_pid_eq(fup, rsp)) - return; - - /* Process follow up response. */ - t2 = timestamp_to_tmv(rsp->ts.pdu); - t3 = timestamp_to_tmv(fup->ts.pdu); - c2 = correction_to_tmv(fup->header.correction); -calc: - t3c = tmv_add(t3, tmv_add(c1, c2)); - - if (p->follow_up_info) - port_nrate_calculate(p, t3c, t4); - - tsproc_set_clock_rate_ratio(p->tsproc, p->nrate.ratio * - clock_rate_ratio(p->clock)); - tsproc_up_ts(p->tsproc, t1, t2); - tsproc_down_ts(p->tsproc, t3c, t4); - if (tsproc_update_delay(p->tsproc, &p->peer_delay)) - return; - - p->peerMeanPathDelay = tmv_to_TimeInterval(p->peer_delay); - - if (p->state == PS_UNCALIBRATED || p->state == PS_SLAVE) { - clock_peer_delay(p->clock, p->peer_delay, t1, t2, - p->nrate.ratio); - } - - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; -} - -int process_pdelay_resp(struct port *p, struct ptp_message *m) -{ - if (p->peer_delay_resp) { - if (!source_pid_eq(p->peer_delay_resp, m)) { - pr_err("port %hu: multiple peer responses", portnum(p)); - if (!p->multiple_pdr_detected) { - p->multiple_pdr_detected = 1; - p->multiple_seq_pdr_count++; - } - if (p->multiple_seq_pdr_count >= 3) { - p->last_fault_type = FT_BAD_PEER_NETWORK; - return -1; - } - } - } - if (!p->peer_delay_req) { - pr_err("port %hu: rogue peer delay response", portnum(p)); - return -1; - } - if (p->peer_portid_valid) { - if (!pid_eq(&p->peer_portid, &m->header.sourcePortIdentity)) { - pr_err("port %hu: received pdelay_resp msg with " - "unexpected peer port id %s", - portnum(p), - pid2str(&m->header.sourcePortIdentity)); - p->peer_portid_valid = 0; - port_capable(p); - } - } else { - p->peer_portid_valid = 1; - p->peer_portid = m->header.sourcePortIdentity; - pr_debug("port %hu: peer port id set to %s", portnum(p), - pid2str(&p->peer_portid)); - } - - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - } - msg_get(m); - p->peer_delay_resp = m; - port_peer_delay(p); - return 0; -} - -void process_pdelay_resp_fup(struct port *p, struct ptp_message *m) -{ - if (!p->peer_delay_req) { - return; - } - - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - } - - msg_get(m); - p->peer_delay_fup = m; - port_peer_delay(p); -} - -void process_sync(struct port *p, struct ptp_message *m) -{ - enum syfu_event event; - struct PortIdentity master; - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - return; - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - master = clock_parent_identity(p->clock); - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - - if (!msg_unicast(m) && - m->header.logMessageInterval != p->log_sync_interval) { - p->log_sync_interval = m->header.logMessageInterval; - clock_sync_interval(p->clock, p->log_sync_interval); - } - - m->header.correction += p->asymmetry; - - if (one_step(m)) { - port_synchronize(p, m->hwts.ts, m->ts.pdu, - m->header.correction, 0); - flush_last_sync(p); - return; - } - - if (p->syfu == SF_HAVE_FUP && - fup_sync_ok(p->last_syncfup, m) && - p->last_syncfup->header.sequenceId == m->header.sequenceId) { - event = SYNC_MATCH; - } else { - event = SYNC_MISMATCH; - } - port_syfufsm(p, event, m); -} - -/* public methods */ - -void port_close(struct port *p) -{ - if (port_is_enabled(p)) { - port_disable(p); - } - - if (p->fda.fd[FD_RTNL] >= 0) { - rtnl_close(p->fda.fd[FD_RTNL]); - } - - unicast_service_cleanup(p); - transport_destroy(p->trp); - tsproc_destroy(p->tsproc); - if (p->fault_fd >= 0) { - close(p->fault_fd); - } - free(p); -} - -struct foreign_clock *port_compute_best(struct port *p) -{ - int (*dscmp)(struct dataset *a, struct dataset *b); - struct foreign_clock *fc; - struct ptp_message *tmp; - - dscmp = clock_dscmp(p->clock); - p->best = NULL; - - if (p->master_only) - return p->best; - - LIST_FOREACH(fc, &p->foreign_masters, list) { - tmp = TAILQ_FIRST(&fc->messages); - if (!tmp) - continue; - - announce_to_dataset(tmp, p, &fc->dataset); - - fc_prune(fc); - - if (fc->n_messages < FOREIGN_MASTER_THRESHOLD) - continue; - - if (!p->best) - p->best = fc; - else if (dscmp(&fc->dataset, &p->best->dataset) > 0) - p->best = fc; - else - fc_clear(fc); - } - - return p->best; -} - -static void port_e2e_transition(struct port *p, enum port_state next) -{ - port_clr_tmo(p->fda.fd[FD_ANNOUNCE_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_RX_TIMER]); - port_clr_tmo(p->fda.fd[FD_DELAY_TIMER]); - port_clr_tmo(p->fda.fd[FD_QUALIFICATION_TIMER]); - port_clr_tmo(p->fda.fd[FD_MANNO_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_TX_TIMER]); - /* Leave FD_UNICAST_REQ_TIMER running. */ - - switch (next) { - case PS_INITIALIZING: - break; - case PS_FAULTY: - case PS_DISABLED: - port_disable(p); - break; - case PS_LISTENING: - port_set_announce_tmo(p); - break; - case PS_PRE_MASTER: - port_set_qualification_tmo(p); - break; - case PS_MASTER: - case PS_GRAND_MASTER: - set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, -10); /*~1ms*/ - port_set_sync_tx_tmo(p); - break; - case PS_PASSIVE: - port_set_announce_tmo(p); - break; - case PS_UNCALIBRATED: - flush_last_sync(p); - flush_delay_req(p); - /* fall through */ - case PS_SLAVE: - port_set_announce_tmo(p); - port_set_delay_tmo(p); - break; - }; -} - -static void port_p2p_transition(struct port *p, enum port_state next) -{ - port_clr_tmo(p->fda.fd[FD_ANNOUNCE_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_RX_TIMER]); - /* Leave FD_DELAY_TIMER running. */ - port_clr_tmo(p->fda.fd[FD_QUALIFICATION_TIMER]); - port_clr_tmo(p->fda.fd[FD_MANNO_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_TX_TIMER]); - /* Leave FD_UNICAST_REQ_TIMER running. */ - - switch (next) { - case PS_INITIALIZING: - break; - case PS_FAULTY: - case PS_DISABLED: - port_disable(p); - break; - case PS_LISTENING: - port_set_announce_tmo(p); - port_set_delay_tmo(p); - break; - case PS_PRE_MASTER: - port_set_qualification_tmo(p); - break; - case PS_MASTER: - case PS_GRAND_MASTER: - set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, -10); /*~1ms*/ - port_set_sync_tx_tmo(p); - break; - case PS_PASSIVE: - port_set_announce_tmo(p); - break; - case PS_UNCALIBRATED: - flush_last_sync(p); - flush_peer_delay(p); - /* fall through */ - case PS_SLAVE: - port_set_announce_tmo(p); - break; - }; -} - -void port_dispatch(struct port *p, enum fsm_event event, int mdiff) -{ - p->dispatch(p, event, mdiff); -} - -static void bc_dispatch(struct port *p, enum fsm_event event, int mdiff) -{ - if (clock_slave_only(p->clock)) { - if (event == EV_RS_MASTER || event == EV_RS_GRAND_MASTER) { - port_slave_priority_warning(p); - } - } - - if (!port_state_update(p, event, mdiff)) { - return; - } - - if (p->delayMechanism == DM_P2P) { - port_p2p_transition(p, p->state); - } else { - port_e2e_transition(p, p->state); - } - - if (p->jbod && p->state == PS_UNCALIBRATED) { - if (clock_switch_phc(p->clock, p->phc_index)) { - p->last_fault_type = FT_SWITCH_PHC; - port_dispatch(p, EV_FAULT_DETECTED, 0); - return; - } - clock_sync_interval(p->clock, p->log_sync_interval); - } -} - -void port_link_status(void *ctx, int linkup, int ts_index) -{ - struct port *p = ctx; - int link_state; - char ts_label[MAX_IFNAME_SIZE + 1] = {0}; - int required_modes; - - link_state = linkup ? LINK_UP : LINK_DOWN; - if (p->link_status & link_state) { - p->link_status = link_state; - } else { - p->link_status = link_state | LINK_STATE_CHANGED; - pr_notice("port %hu: link %s", portnum(p), linkup ? "up" : "down"); - } - - /* ts_label changed */ - if (if_indextoname(ts_index, ts_label) && strcmp(p->iface->ts_label, ts_label)) { - strncpy(p->iface->ts_label, ts_label, MAX_IFNAME_SIZE); - p->link_status |= TS_LABEL_CHANGED; - pr_notice("port %hu: ts label changed to %s", portnum(p), ts_label); - } - - /* Both link down/up and change ts_label may change phc index. */ - if (p->link_status & LINK_UP && - (p->link_status & LINK_STATE_CHANGED || p->link_status & TS_LABEL_CHANGED)) { - sk_get_ts_info(p->iface->ts_label, &p->iface->ts_info); - - /* Only switch phc with HW time stamping mode */ - if (p->phc_index >= 0 && p->iface->ts_info.valid) { - required_modes = clock_required_modes(p->clock); - if ((p->iface->ts_info.so_timestamping & required_modes) != required_modes) { - pr_err("interface '%s' does not support requested " - "timestamping mode, set link status down by force.", - p->iface->ts_label); - p->link_status = LINK_DOWN | LINK_STATE_CHANGED; - } else if (p->phc_index != p->iface->ts_info.phc_index) { - p->phc_index = p->iface->ts_info.phc_index; - - if (clock_switch_phc(p->clock, p->phc_index)) { - p->last_fault_type = FT_SWITCH_PHC; - port_dispatch(p, EV_FAULT_DETECTED, 0); - return; - } - clock_sync_interval(p->clock, p->log_sync_interval); - } - } - } - - /* - * A port going down can affect the BMCA result. - * Force a state decision event. - */ - if (p->link_status & LINK_DOWN) - clock_set_sde(p->clock, 1); -} - -enum fsm_event port_event(struct port *p, int fd_index) -{ - return p->event(p, fd_index); -} - -static enum fsm_event bc_event(struct port *p, int fd_index) -{ - enum fsm_event event = EV_NONE; - struct ptp_message *msg; - int cnt, fd = p->fda.fd[fd_index], err; - - switch (fd_index) { - case FD_ANNOUNCE_TIMER: - case FD_SYNC_RX_TIMER: - pr_debug("port %hu: %s timeout", portnum(p), - fd_index == FD_SYNC_RX_TIMER ? "rx sync" : "announce"); - if (p->best) - fc_clear(p->best); - port_set_announce_tmo(p); - delay_req_prune(p); - if (clock_slave_only(p->clock) && p->delayMechanism != DM_P2P && - port_renew_transport(p)) { - return EV_FAULT_DETECTED; - } - return EV_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES; - - case FD_DELAY_TIMER: - pr_debug("port %hu: delay timeout", portnum(p)); - port_set_delay_tmo(p); - delay_req_prune(p); - return port_delay_request(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_QUALIFICATION_TIMER: - pr_debug("port %hu: qualification timeout", portnum(p)); - return EV_QUALIFICATION_TIMEOUT_EXPIRES; - - case FD_MANNO_TIMER: - pr_debug("port %hu: master tx announce timeout", portnum(p)); - port_set_manno_tmo(p); - return port_tx_announce(p, NULL) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_SYNC_TX_TIMER: - pr_debug("port %hu: master sync timeout", portnum(p)); - port_set_sync_tx_tmo(p); - return port_tx_sync(p, NULL) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_UNICAST_SRV_TIMER: - pr_debug("port %hu: unicast service timeout", portnum(p)); - return unicast_service_timer(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_UNICAST_REQ_TIMER: - pr_debug("port %hu: unicast request timeout", portnum(p)); - return unicast_client_timer(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_RTNL: - pr_debug("port %hu: received link status notification", portnum(p)); - rtnl_link_status(fd, p->name, port_link_status, p); - if (p->link_status == (LINK_UP | LINK_STATE_CHANGED)) - return EV_FAULT_CLEARED; - else if ((p->link_status == (LINK_DOWN | LINK_STATE_CHANGED)) || - (p->link_status & TS_LABEL_CHANGED)) - return EV_FAULT_DETECTED; - else - return EV_NONE; - } - - msg = msg_allocate(); - if (!msg) - return EV_FAULT_DETECTED; - - msg->hwts.type = p->timestamping; - - cnt = transport_recv(p->trp, fd, msg); - if (cnt <= 0) { - pr_err("port %hu: recv message failed", portnum(p)); - msg_put(msg); - return EV_FAULT_DETECTED; - } - err = msg_post_recv(msg, cnt); - if (err) { - switch (err) { - case -EBADMSG: - pr_err("port %hu: bad message", portnum(p)); - break; - case -EPROTO: - pr_debug("port %hu: ignoring message", portnum(p)); - break; - } - msg_put(msg); - return EV_NONE; - } - if (port_ignore(p, msg)) { - msg_put(msg); - return EV_NONE; - } - if (msg_sots_missing(msg) && - !(p->timestamping == TS_P2P1STEP && msg_type(msg) == PDELAY_REQ)) { - pr_err("port %hu: received %s without timestamp", - portnum(p), msg_type_string(msg_type(msg))); - msg_put(msg); - return EV_NONE; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, -p->rx_timestamp_offset); - clock_check_ts(p->clock, tmv_to_nanoseconds(msg->hwts.ts)); - } - - switch (msg_type(msg)) { - case SYNC: - process_sync(p, msg); - break; - case DELAY_REQ: - if (process_delay_req(p, msg)) - event = EV_FAULT_DETECTED; - break; - case PDELAY_REQ: - if (process_pdelay_req(p, msg)) - event = EV_FAULT_DETECTED; - break; - case PDELAY_RESP: - if (process_pdelay_resp(p, msg)) - event = EV_FAULT_DETECTED; - break; - case FOLLOW_UP: - process_follow_up(p, msg); - break; - case DELAY_RESP: - process_delay_resp(p, msg); - break; - case PDELAY_RESP_FOLLOW_UP: - process_pdelay_resp_fup(p, msg); - break; - case ANNOUNCE: - if (process_announce(p, msg)) - event = EV_STATE_DECISION_EVENT; - break; - case SIGNALING: - if (process_signaling(p, msg)) { - event = EV_FAULT_DETECTED; - } - break; - case MANAGEMENT: - if (clock_manage(p->clock, p, msg)) - event = EV_STATE_DECISION_EVENT; - break; - } - - msg_put(msg); - return event; -} - -int port_forward(struct port *p, struct ptp_message *msg) -{ - int cnt; - cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, msg); - return cnt <= 0 ? -1 : 0; -} - -int port_forward_to(struct port *p, struct ptp_message *msg) -{ - int cnt; - cnt = transport_sendto(p->trp, &p->fda, TRANS_GENERAL, msg); - return cnt <= 0 ? -1 : 0; -} - -int port_prepare_and_send(struct port *p, struct ptp_message *msg, - enum transport_event event) -{ - int cnt; - - if (msg_pre_send(msg)) { - return -1; - } - if (msg_unicast(msg)) { - cnt = transport_sendto(p->trp, &p->fda, event, msg); - } else { - cnt = transport_send(p->trp, &p->fda, event, msg); - } - if (cnt <= 0) { - return -1; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, p->tx_timestamp_offset); - } - return 0; -} - -struct PortIdentity port_identity(struct port *p) -{ - return p->portIdentity; -} - -int port_number(struct port *p) -{ - return portnum(p); -} - -int port_link_status_get(struct port *p) -{ - return !!(p->link_status & LINK_UP); -} - -int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg) -{ - struct management_tlv *mgt; - UInteger16 target = msg->management.targetPortIdentity.portNumber; - - if (target != portnum(p) && target != 0xffff) { - return 0; - } - mgt = (struct management_tlv *) msg->management.suffix; - - switch (management_action(msg)) { - case GET: - if (port_management_get_response(p, ingress, mgt->id, msg)) - return 1; - break; - case SET: - if (port_management_set(p, ingress, mgt->id, msg)) - return 1; - break; - case COMMAND: - break; - default: - return -1; - } - - switch (mgt->id) { - case TLV_NULL_MANAGEMENT: - case TLV_CLOCK_DESCRIPTION: - case TLV_PORT_DATA_SET: - case TLV_LOG_ANNOUNCE_INTERVAL: - case TLV_ANNOUNCE_RECEIPT_TIMEOUT: - case TLV_LOG_SYNC_INTERVAL: - case TLV_VERSION_NUMBER: - case TLV_ENABLE_PORT: - case TLV_DISABLE_PORT: - case TLV_UNICAST_NEGOTIATION_ENABLE: - case TLV_UNICAST_MASTER_TABLE: - case TLV_UNICAST_MASTER_MAX_TABLE_SIZE: - case TLV_ACCEPTABLE_MASTER_TABLE_ENABLED: - case TLV_ALTERNATE_MASTER: - case TLV_TRANSPARENT_CLOCK_PORT_DATA_SET: - case TLV_DELAY_MECHANISM: - case TLV_LOG_MIN_PDELAY_REQ_INTERVAL: - port_management_send_error(p, ingress, msg, TLV_NOT_SUPPORTED); - break; - default: - port_management_send_error(p, ingress, msg, TLV_NO_SUCH_ID); - return -1; - } - return 1; -} - -int port_management_error(struct PortIdentity pid, struct port *ingress, - struct ptp_message *req, Enumeration16 error_id) -{ - struct management_error_status *mes; - struct management_tlv *mgt; - struct ptp_message *msg; - struct tlv_extra *extra; - int err = 0; - - mgt = (struct management_tlv *) req->management.suffix; - msg = port_management_reply(pid, ingress, req); - if (!msg) { - return -1; - } - - extra = msg_tlv_append(msg, sizeof(*mes)); - if (!extra) { - msg_put(msg); - return -ENOMEM; - } - mes = (struct management_error_status *) extra->tlv; - mes->type = TLV_MANAGEMENT_ERROR_STATUS; - mes->length = 8; - mes->error = error_id; - mes->id = mgt->id; - - err = port_prepare_and_send(ingress, msg, TRANS_GENERAL); - msg_put(msg); - return err; -} - -static struct ptp_message * -port_management_construct(struct PortIdentity pid, struct port *ingress, - UInteger16 sequenceId, - struct PortIdentity *targetPortIdentity, - UInteger8 boundaryHops, uint8_t action) -{ - struct ptp_message *msg; - - msg = msg_allocate(); - if (!msg) - return NULL; - - msg->hwts.type = ingress->timestamping; - - msg->header.tsmt = MANAGEMENT | ingress->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct management_msg); - msg->header.domainNumber = clock_domain_number(ingress->clock); - msg->header.sourcePortIdentity = pid; - msg->header.sequenceId = sequenceId; - msg->header.control = CTL_MANAGEMENT; - msg->header.logMessageInterval = 0x7f; - - if (targetPortIdentity) - msg->management.targetPortIdentity = *targetPortIdentity; - msg->management.startingBoundaryHops = boundaryHops; - msg->management.boundaryHops = boundaryHops; - - switch (action) { - case GET: case SET: - msg->management.flags = RESPONSE; - break; - case COMMAND: - msg->management.flags = ACKNOWLEDGE; - break; - } - return msg; -} - -struct ptp_message *port_management_reply(struct PortIdentity pid, - struct port *ingress, - struct ptp_message *req) -{ - UInteger8 boundaryHops; - - boundaryHops = req->management.startingBoundaryHops - - req->management.boundaryHops; - return port_management_construct(pid, ingress, - req->header.sequenceId, - &req->header.sourcePortIdentity, - boundaryHops, - management_action(req)); -} - -struct ptp_message *port_management_notify(struct PortIdentity pid, - struct port *port) -{ - return port_management_construct(pid, port, 0, NULL, 1, GET); -} - -void port_notify_event(struct port *p, enum notification event) -{ - struct PortIdentity pid = port_identity(p); - struct ptp_message *msg; - int id; - - switch (event) { - case NOTIFY_PORT_STATE: - id = TLV_PORT_DATA_SET; - break; - default: - return; - } - /* targetPortIdentity and sequenceId will be filled by - * clock_send_notification */ - msg = port_management_notify(pid, p); - if (!msg) - return; - if (!port_management_fill_response(p, msg, id)) - goto err; - if (msg_pre_send(msg)) - goto err; - clock_send_notification(p->clock, msg, event); -err: - msg_put(msg); -} - -struct port *port_open(int phc_index, - enum timestamp_type timestamping, - int number, - struct interface *interface, - struct clock *clock) -{ - enum clock_type type = clock_type(clock); - struct config *cfg = clock_config(clock); - struct port *p = malloc(sizeof(*p)); - enum transport_type transport; - int i; - - if (!p) { - return NULL; - } - - memset(p, 0, sizeof(*p)); - TAILQ_INIT(&p->tc_transmitted); - - switch (type) { - case CLOCK_TYPE_ORDINARY: - case CLOCK_TYPE_BOUNDARY: - p->dispatch = bc_dispatch; - p->event = bc_event; - break; - case CLOCK_TYPE_P2P: - p->dispatch = p2p_dispatch; - p->event = p2p_event; - break; - case CLOCK_TYPE_E2E: - p->dispatch = e2e_dispatch; - p->event = e2e_event; - break; - case CLOCK_TYPE_MANAGEMENT: - goto err_port; - } - - p->state_machine = clock_slave_only(clock) ? ptp_slave_fsm : ptp_fsm; - p->phc_index = phc_index; - p->jbod = config_get_int(cfg, interface->name, "boundary_clock_jbod"); - transport = config_get_int(cfg, interface->name, "network_transport"); - - if (transport == TRANS_UDS) { - ; /* UDS cannot have a PHC. */ - } else if (!interface->ts_info.valid) { - pr_warning("port %d: get_ts_info not supported", number); - } else if (phc_index >= 0 && phc_index != interface->ts_info.phc_index) { - if (p->jbod) { - pr_warning("port %d: just a bunch of devices", number); - p->phc_index = interface->ts_info.phc_index; - } else { - pr_err("port %d: PHC device mismatch", number); - pr_err("port %d: /dev/ptp%d requested, ptp%d attached", - number, phc_index, interface->ts_info.phc_index); - goto err_port; - } - } - - p->name = interface->name; - p->iface = interface; - p->asymmetry = config_get_int(cfg, p->name, "delayAsymmetry"); - p->asymmetry <<= 16; - p->announce_span = transport == TRANS_UDS ? 0 : ANNOUNCE_SPAN; - p->follow_up_info = config_get_int(cfg, p->name, "follow_up_info"); - p->freq_est_interval = config_get_int(cfg, p->name, "freq_est_interval"); - p->net_sync_monitor = config_get_int(cfg, p->name, "net_sync_monitor"); - p->path_trace_enabled = config_get_int(cfg, p->name, "path_trace_enabled"); - p->tc_spanning_tree = config_get_int(cfg, p->name, "tc_spanning_tree"); - p->rx_timestamp_offset = config_get_int(cfg, p->name, "ingressLatency"); - p->rx_timestamp_offset <<= 16; - p->tx_timestamp_offset = config_get_int(cfg, p->name, "egressLatency"); - p->tx_timestamp_offset <<= 16; - p->link_status = LINK_UP; - p->clock = clock; - p->trp = transport_create(cfg, transport); - if (!p->trp) { - goto err_port; - } - p->timestamping = timestamping; - p->portIdentity.clockIdentity = clock_identity(clock); - p->portIdentity.portNumber = number; - p->state = PS_INITIALIZING; - p->delayMechanism = config_get_int(cfg, p->name, "delay_mechanism"); - p->versionNumber = PTP_VERSION; - - if (number && unicast_client_claim_table(p)) { - goto err_port; - } - if (unicast_client_enabled(p) && - config_set_section_int(cfg, p->name, "hybrid_e2e", 1)) { - goto err_port; - } - if (number && unicast_service_initialize(p)) { - goto err_port; - } - p->hybrid_e2e = config_get_int(cfg, p->name, "hybrid_e2e"); - - if (number && type == CLOCK_TYPE_P2P && p->delayMechanism != DM_P2P) { - pr_err("port %d: P2P TC needs P2P ports", number); - goto err_port; - } - if (number && type == CLOCK_TYPE_E2E && p->delayMechanism != DM_E2E) { - pr_err("port %d: E2E TC needs E2E ports", number); - goto err_port; - } - if (p->hybrid_e2e && p->delayMechanism != DM_E2E) { - pr_warning("port %d: hybrid_e2e only works with E2E", number); - } - if (p->net_sync_monitor && !p->hybrid_e2e) { - pr_warning("port %d: net_sync_monitor needs hybrid_e2e", number); - } - - /* Set fault timeouts to a default value */ - for (i = 0; i < FT_CNT; i++) { - p->flt_interval_pertype[i].type = FTMO_LOG2_SECONDS; - p->flt_interval_pertype[i].val = 4; - } - p->flt_interval_pertype[FT_BAD_PEER_NETWORK].type = FTMO_LINEAR_SECONDS; - p->flt_interval_pertype[FT_BAD_PEER_NETWORK].val = - config_get_int(cfg, p->name, "fault_badpeernet_interval"); - - p->flt_interval_pertype[FT_UNSPECIFIED].val = - config_get_int(cfg, p->name, "fault_reset_interval"); - - p->tsproc = tsproc_create(config_get_int(cfg, p->name, "tsproc_mode"), - config_get_int(cfg, p->name, "delay_filter"), - config_get_int(cfg, p->name, "delay_filter_length")); - if (!p->tsproc) { - pr_err("Failed to create time stamp processor"); - goto err_transport; - } - p->nrate.ratio = 1.0; - - port_clear_fda(p, N_POLLFD); - 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_tsproc; - } - } - return p; - -err_tsproc: - tsproc_destroy(p->tsproc); -err_transport: - transport_destroy(p->trp); -err_port: - free(p); - return NULL; -} - -enum port_state port_state(struct port *port) -{ - return port->state; -} - -int port_state_update(struct port *p, enum fsm_event event, int mdiff) -{ - enum port_state next = p->state_machine(p->state, event, mdiff); - - if (PS_FAULTY == next) { - struct fault_interval i; - fault_interval(p, last_fault_type(p), &i); - if (clear_fault_asap(&i)) { - pr_notice("port %hu: clearing fault immediately", portnum(p)); - next = p->state_machine(next, EV_FAULT_CLEARED, 0); - } - } - - if (PS_INITIALIZING == next) { - /* - * This is a special case. Since we initialize the - * port immediately, we can skip right to listening - * state if all goes well. - */ - if (port_is_enabled(p)) { - port_disable(p); - } - if (port_initialize(p)) { - event = EV_FAULT_DETECTED; - } else { - event = EV_INIT_COMPLETE; - } - next = p->state_machine(next, event, 0); - } - - if (next != p->state) { - port_show_transition(p, next, event); - p->state = next; - port_notify_event(p, NOTIFY_PORT_STATE); - unicast_client_state_changed(p); - return 1; - } - - return 0; -} diff --git a/port.c.zerolength b/port.c.zerolength deleted file mode 100644 index c945c13..0000000 --- a/port.c.zerolength +++ /dev/null @@ -1,3011 +0,0 @@ -/** - * @file port.c - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include - -#include "bmc.h" -#include "clock.h" -#include "filter.h" -#include "missing.h" -#include "msg.h" -#include "phc.h" -#include "port.h" -#include "port_private.h" -#include "print.h" -#include "rtnl.h" -#include "sk.h" -#include "tc.h" -#include "tlv.h" -#include "tmv.h" -#include "tsproc.h" -#include "unicast_client.h" -#include "unicast_service.h" -#include "util.h" - -#define ALLOWED_LOST_RESPONSES 3 -#define ANNOUNCE_SPAN 1 - -enum syfu_event { - SYNC_MISMATCH, - SYNC_MATCH, - FUP_MISMATCH, - FUP_MATCH, -}; - -static int port_capable(struct port *p); -static int port_is_ieee8021as(struct port *p); -static void port_nrate_initialize(struct port *p); - -static int announce_compare(struct ptp_message *m1, struct ptp_message *m2) -{ - struct announce_msg *a = &m1->announce, *b = &m2->announce; - int len = - sizeof(a->grandmasterPriority1) + - sizeof(a->grandmasterClockQuality) + - sizeof(a->grandmasterPriority2) + - sizeof(a->grandmasterIdentity) + - sizeof(a->stepsRemoved); - - return memcmp(&a->grandmasterPriority1, &b->grandmasterPriority1, len); -} - -static void announce_to_dataset(struct ptp_message *m, struct port *p, - struct dataset *out) -{ - struct announce_msg *a = &m->announce; - out->priority1 = a->grandmasterPriority1; - out->identity = a->grandmasterIdentity; - out->quality = a->grandmasterClockQuality; - out->priority2 = a->grandmasterPriority2; - out->localPriority = p->localPriority; - out->stepsRemoved = a->stepsRemoved; - out->sender = m->header.sourcePortIdentity; - out->receiver = p->portIdentity; -} - -int clear_fault_asap(struct fault_interval *faint) -{ - switch (faint->type) { - case FTMO_LINEAR_SECONDS: - return faint->val == 0 ? 1 : 0; - case FTMO_LOG2_SECONDS: - return faint->val == FRI_ASAP ? 1 : 0; - case FTMO_CNT: - return 0; - } - return 0; -} - -static void extract_address(struct ptp_message *m, struct PortAddress *paddr) -{ - int len = 0; - - switch (paddr->networkProtocol) { - case TRANS_UDP_IPV4: - len = sizeof(m->address.sin.sin_addr.s_addr); - memcpy(paddr->address, &m->address.sin.sin_addr.s_addr, len); - break; - case TRANS_UDP_IPV6: - len = sizeof(m->address.sin6.sin6_addr.s6_addr); - memcpy(paddr->address, &m->address.sin6.sin6_addr.s6_addr, len); - break; - case TRANS_IEEE_802_3: - len = MAC_LEN; - memcpy(paddr->address, &m->address.sll.sll_addr, len); - break; - default: - return; - } - paddr->addressLength = len; -} - -static int msg_current(struct ptp_message *m, struct timespec now) -{ - int64_t t1, t2, tmo; - - t1 = m->ts.host.tv_sec * NSEC2SEC + m->ts.host.tv_nsec; - t2 = now.tv_sec * NSEC2SEC + now.tv_nsec; - - if (m->header.logMessageInterval < -63) { - tmo = 0; - } else if (m->header.logMessageInterval > 31) { - tmo = INT64_MAX; - } else if (m->header.logMessageInterval < 0) { - tmo = 4LL * NSEC2SEC / (1 << -m->header.logMessageInterval); - } else { - tmo = 4LL * (1 << m->header.logMessageInterval) * NSEC2SEC; - } - - return t2 - t1 < tmo; -} - -static int msg_source_equal(struct ptp_message *m1, struct foreign_clock *fc) -{ - struct PortIdentity *id1, *id2; - id1 = &m1->header.sourcePortIdentity; - id2 = &fc->dataset.sender; - return 0 == memcmp(id1, id2, sizeof(*id1)); -} - -int source_pid_eq(struct ptp_message *m1, struct ptp_message *m2) -{ - return pid_eq(&m1->header.sourcePortIdentity, - &m2->header.sourcePortIdentity); -} - -enum fault_type last_fault_type(struct port *port) -{ - return port->last_fault_type; -} - -void fault_interval(struct port *port, enum fault_type ft, - struct fault_interval *i) -{ - i->type = port->flt_interval_pertype[ft].type; - i->val = port->flt_interval_pertype[ft].val; -} - -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 = { - {0, 0}, {0, 0} - }; - uint64_t ns; - int i; - - if (log_seconds < 0) { - - log_seconds *= -1; - for (i = 1, ns = scale * 500000000ULL; i < log_seconds; i++) { - ns >>= 1; - } - tmo.it_value.tv_nsec = ns; - - while (tmo.it_value.tv_nsec >= NS_PER_SEC) { - tmo.it_value.tv_nsec -= NS_PER_SEC; - tmo.it_value.tv_sec++; - } - - } else - tmo.it_value.tv_sec = scale * (1 << log_seconds); - - return timerfd_settime(fd, 0, &tmo, NULL); -} - -int set_tmo_lin(int fd, int seconds) -{ - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - - tmo.it_value.tv_sec = seconds; - return timerfd_settime(fd, 0, &tmo, NULL); -} - -int set_tmo_random(int fd, int min, int span, int log_seconds) -{ - uint64_t value_ns, min_ns, span_ns; - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - - if (log_seconds >= 0) { - min_ns = min * NS_PER_SEC << log_seconds; - span_ns = span * NS_PER_SEC << log_seconds; - } else { - min_ns = min * NS_PER_SEC >> -log_seconds; - span_ns = span * NS_PER_SEC >> -log_seconds; - } - - value_ns = min_ns + (span_ns * (random() % (1 << 15) + 1) >> 15); - - tmo.it_value.tv_sec = value_ns / NS_PER_SEC; - tmo.it_value.tv_nsec = value_ns % NS_PER_SEC; - - 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); -} - -void fc_clear(struct foreign_clock *fc) -{ - struct ptp_message *m; - - while (fc->n_messages) { - m = TAILQ_LAST(&fc->messages, messages); - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } -} - -static void fc_prune(struct foreign_clock *fc) -{ - struct timespec now; - struct ptp_message *m; - - clock_gettime(CLOCK_MONOTONIC, &now); - - while (fc->n_messages > FOREIGN_MASTER_THRESHOLD) { - m = TAILQ_LAST(&fc->messages, messages); - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } - - while (!TAILQ_EMPTY(&fc->messages)) { - m = TAILQ_LAST(&fc->messages, messages); - if (msg_current(m, now)) - break; - TAILQ_REMOVE(&fc->messages, m, list); - fc->n_messages--; - msg_put(m); - } -} - -static int delay_req_current(struct ptp_message *m, struct timespec now) -{ - int64_t t1, t2, tmo = 5 * NSEC2SEC; - - t1 = m->ts.host.tv_sec * NSEC2SEC + m->ts.host.tv_nsec; - t2 = now.tv_sec * NSEC2SEC + now.tv_nsec; - - return t2 - t1 < tmo; -} - -void delay_req_prune(struct port *p) -{ - struct timespec now; - struct ptp_message *m; - clock_gettime(CLOCK_MONOTONIC, &now); - - while (!TAILQ_EMPTY(&p->delay_req)) { - m = TAILQ_LAST(&p->delay_req, delay_req); - if (delay_req_current(m, now)) { - break; - } - TAILQ_REMOVE(&p->delay_req, m, list); - msg_put(m); - } -} - -void ts_add(tmv_t *ts, Integer64 correction) -{ - if (!correction) { - return; - } - *ts = tmv_add(*ts, correction_to_tmv(correction)); -} - -/* - * Returns non-zero if the announce message is different than last. - */ -static int add_foreign_master(struct port *p, struct ptp_message *m) -{ - struct foreign_clock *fc; - struct ptp_message *tmp; - int broke_threshold = 0, diff = 0; - - LIST_FOREACH(fc, &p->foreign_masters, list) { - if (msg_source_equal(m, fc)) { - break; - } - } - if (!fc) { - pr_notice("port %hu: new foreign master %s", portnum(p), - pid2str(&m->header.sourcePortIdentity)); - - fc = malloc(sizeof(*fc)); - if (!fc) { - pr_err("low memory, failed to add foreign master"); - return 0; - } - memset(fc, 0, sizeof(*fc)); - TAILQ_INIT(&fc->messages); - LIST_INSERT_HEAD(&p->foreign_masters, fc, list); - fc->port = p; - fc->dataset.sender = m->header.sourcePortIdentity; - /* We do not count this first message, see 9.5.3(b) */ - return 0; - } - - /* - * If this message breaks the threshold, that is an important change. - */ - fc_prune(fc); - if (FOREIGN_MASTER_THRESHOLD - 1 == fc->n_messages) { - broke_threshold = 1; - } - - /* - * Okay, go ahead and add this announcement. - */ - msg_get(m); - fc->n_messages++; - TAILQ_INSERT_HEAD(&fc->messages, m, list); - - /* - * Test if this announcement contains changed information. - */ - if (fc->n_messages > 1) { - tmp = TAILQ_NEXT(m, list); - diff = announce_compare(m, tmp); - } - - return broke_threshold || diff; -} - -static int follow_up_info_append(struct ptp_message *m) -{ - struct follow_up_info_tlv *fui; - struct tlv_extra *extra; - - extra = msg_tlv_append(m, sizeof(*fui)); - if (!extra) { - return -1; - } - fui = (struct follow_up_info_tlv *) extra->tlv; - fui->type = TLV_ORGANIZATION_EXTENSION; - fui->length = sizeof(*fui) - sizeof(fui->type) - sizeof(fui->length); - memcpy(fui->id, ieee8021_id, sizeof(ieee8021_id)); - fui->subtype[2] = 1; - - return 0; -} - -static int net_sync_resp_append(struct port *p, struct ptp_message *m) -{ - struct timePropertiesDS *tp = clock_time_properties(p->clock); - struct ClockIdentity cid = clock_identity(p->clock), pid; - struct currentDS *cds = clock_current_dataset(p->clock); - struct parent_ds *dad = clock_parent_ds(p->clock); - struct port *best = clock_best_port(p->clock); - struct nsm_resp_tlv_head *head; - struct Timestamp last_sync; - struct PortAddress *paddr; - struct ptp_message *tmp; - struct tlv_extra *extra; - unsigned char *ptr; - int tlv_len; - - uint8_t buf[sizeof(*paddr) + sizeof(struct sockaddr_storage)]; - - last_sync = tmv_to_Timestamp(clock_ingress_time(p->clock)); - pid = dad->pds.parentPortIdentity.clockIdentity; - paddr = (struct PortAddress *)buf; - - if (best && !cid_eq(&cid, &pid)) { - /* Extract the parent's protocol address. */ - paddr->networkProtocol = transport_type(best->trp); - paddr->addressLength = - transport_protocol_addr(best->trp, paddr->address); - if (best->best) { - tmp = TAILQ_FIRST(&best->best->messages); - extract_address(tmp, paddr); - } - } else { - /* We are our own parent. */ - paddr->networkProtocol = transport_type(p->trp); - paddr->addressLength = - transport_protocol_addr(p->trp, paddr->address); - } - - tlv_len = sizeof(*head) + sizeof(*extra->foot) + paddr->addressLength; - - extra = msg_tlv_append(m, tlv_len); - if (!extra) { - return -1; - } - - head = (struct nsm_resp_tlv_head *) extra->tlv; - head->type = TLV_PTPMON_RESP; - head->length = tlv_len - sizeof(head->type) - sizeof(head->length); - head->port_state = p->state == PS_GRAND_MASTER ? PS_MASTER : p->state; - head->parent_addr.networkProtocol = paddr->networkProtocol; - head->parent_addr.addressLength = paddr->addressLength; - memcpy(head->parent_addr.address, paddr->address, paddr->addressLength); - - ptr = (unsigned char *) head; - ptr += sizeof(*head) + paddr->addressLength; - extra->foot = (struct nsm_resp_tlv_foot *) ptr; - - memcpy(&extra->foot->parent, &dad->pds, sizeof(extra->foot->parent)); - memcpy(&extra->foot->current, cds, sizeof(extra->foot->current)); - memcpy(&extra->foot->timeprop, tp, sizeof(extra->foot->timeprop)); - memcpy(&extra->foot->lastsync, &last_sync, sizeof(extra->foot->lastsync)); - - return 0; -} - -static struct follow_up_info_tlv *follow_up_info_extract(struct ptp_message *m) -{ - struct follow_up_info_tlv *f; - struct tlv_extra *extra; - - TAILQ_FOREACH(extra, &m->tlv_list, list) { - f = (struct follow_up_info_tlv *) extra->tlv; - if (f->type == TLV_ORGANIZATION_EXTENSION && - f->length == sizeof(*f) - sizeof(f->type) - sizeof(f->length) && -// memcmp(f->id, ieee8021_id, sizeof(ieee8021_id)) && - !f->subtype[0] && !f->subtype[1] && f->subtype[2] == 1) { - return f; - } - } - return NULL; -} - -static void free_foreign_masters(struct port *p) -{ - struct foreign_clock *fc; - while ((fc = LIST_FIRST(&p->foreign_masters)) != NULL) { - LIST_REMOVE(fc, list); - fc_clear(fc); - free(fc); - } -} - -static int fup_sync_ok(struct ptp_message *fup, struct ptp_message *sync) -{ - /* - * NB - If the sk_check_fupsync option is not enabled, then - * both of these time stamps will be zero. - */ - if (tmv_cmp(fup->hwts.sw, sync->hwts.sw) < 0) { - return 0; - } - return 1; -} - -static int incapable_ignore(struct port *p, struct ptp_message *m) -{ - if (port_capable(p)) { - return 0; - } - if (msg_type(m) == ANNOUNCE || msg_type(m) == SYNC) { - return 1; - } - return 0; -} - -static int path_trace_append(struct port *p, struct ptp_message *m, - struct parent_ds *dad) -{ - int length = 1 + dad->path_length, ptt_len, tlv_len; - struct path_trace_tlv *ptt; - struct tlv_extra *extra; - - if (length > PATH_TRACE_MAX) { - return -1; - } - - ptt_len = length * sizeof(struct ClockIdentity); - tlv_len = ptt_len + sizeof(ptt->type) + sizeof(ptt->length); - - extra = msg_tlv_append(m, tlv_len); - if (!extra) { - return -1; - } - ptt = (struct path_trace_tlv *) extra->tlv; - ptt->type = TLV_PATH_TRACE; - ptt->length = ptt_len; - memcpy(ptt->cid, dad->ptl, ptt->length); - ptt->cid[length - 1] = clock_identity(p->clock); - - return 0; -} - -static int path_trace_ignore(struct port *p, struct ptp_message *m) -{ - struct path_trace_tlv *ptt; - struct ClockIdentity cid; - struct tlv_extra *extra; - int i, cnt; - - if (!p->path_trace_enabled) { - return 0; - } - if (msg_type(m) != ANNOUNCE) { - return 0; - } - TAILQ_FOREACH(extra, &m->tlv_list, list) { - ptt = (struct path_trace_tlv *) extra->tlv; - if (ptt->type != TLV_PATH_TRACE) { - continue; - } - cnt = path_length(ptt); - cid = clock_identity(p->clock); - for (i = 0; i < cnt; i++) { - if (cid_eq(&ptt->cid[i], &cid)) { - return 1; - } - } - } - return 0; -} - -static int peer_prepare_and_send(struct port *p, struct ptp_message *msg, - enum transport_event event) -{ - int cnt; - if (msg_pre_send(msg)) { - return -1; - } - if (msg_unicast(msg)) { - cnt = transport_sendto(p->trp, &p->fda, event, msg); - } else { - cnt = transport_peer(p->trp, &p->fda, event, msg); - } - if (cnt <= 0) { - return -1; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, p->tx_timestamp_offset); - } - return 0; -} - -static int port_capable(struct port *p) -{ - if (!port_is_ieee8021as(p)) { - /* Normal 1588 ports are always capable. */ - goto capable; - } - - if (tmv_to_nanoseconds(p->peer_delay) > p->neighborPropDelayThresh) { - if (p->asCapable) - pr_debug("port %hu: peer_delay (%" PRId64 ") > neighborPropDelayThresh " - "(%" PRId32 "), resetting asCapable", portnum(p), - tmv_to_nanoseconds(p->peer_delay), - p->neighborPropDelayThresh); - goto not_capable; - } - - if (tmv_to_nanoseconds(p->peer_delay) < p->min_neighbor_prop_delay) { - if (p->asCapable) - pr_debug("port %hu: peer_delay (%" PRId64 ") < min_neighbor_prop_delay " - "(%" PRId32 "), resetting asCapable", portnum(p), - tmv_to_nanoseconds(p->peer_delay), - p->min_neighbor_prop_delay); - goto not_capable; - } - - if (p->pdr_missing > ALLOWED_LOST_RESPONSES) { - if (p->asCapable) - pr_debug("port %hu: missed %d peer delay resp, " - "resetting asCapable", portnum(p), p->pdr_missing); - goto not_capable; - } - - if (p->multiple_seq_pdr_count) { - if (p->asCapable) - pr_debug("port %hu: multiple sequential peer delay resp, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - - if (!p->peer_portid_valid) { - if (p->asCapable) - pr_debug("port %hu: invalid peer port id, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - - if (!p->nrate.ratio_valid) { - if (p->asCapable) - pr_debug("port %hu: invalid nrate, " - "resetting asCapable", portnum(p)); - goto not_capable; - } - -capable: - if (!p->asCapable) - pr_debug("port %hu: setting asCapable", portnum(p)); - p->asCapable = 1; - return 1; - -not_capable: - if (p->asCapable) - port_nrate_initialize(p); - p->asCapable = 0; - return 0; -} - -int port_clr_tmo(int fd) -{ - struct itimerspec tmo = { - {0, 0}, {0, 0} - }; - return timerfd_settime(fd, 0, &tmo, NULL); -} - -static int port_ignore(struct port *p, struct ptp_message *m) -{ - struct ClockIdentity c1, c2; - - if (incapable_ignore(p, m)) { - return 1; - } - if (path_trace_ignore(p, m)) { - return 1; - } - if (p->match_transport_specific && - msg_transport_specific(m) != p->transportSpecific) { - return 1; - } - if (pid_eq(&m->header.sourcePortIdentity, &p->portIdentity)) { - return 1; - } - if (m->header.domainNumber != clock_domain_number(p->clock)) { - return 1; - } - - c1 = clock_identity(p->clock); - c2 = m->header.sourcePortIdentity.clockIdentity; - - if (cid_eq(&c1, &c2)) { - return 1; - } - return 0; -} - -static int port_nsm_reply(struct port *p, struct ptp_message *m) -{ - struct tlv_extra *extra; - - if (!p->net_sync_monitor) { - return 0; - } - if (!p->hybrid_e2e) { - return 0; - } - if (!msg_unicast(m)) { - return 0; - } - TAILQ_FOREACH(extra, &m->tlv_list, list) { - if (extra->tlv->type == TLV_PTPMON_REQ) { - return 1; - } - } - return 0; -} - -/* - * Test whether a 802.1AS port may transmit a sync message. - */ -static int port_sync_incapable(struct port *p) -{ - struct ClockIdentity cid; - struct PortIdentity pid; - - if (!port_is_ieee8021as(p)) { - return 0; - } - if (clock_gm_capable(p->clock)) { - return 0; - } - cid = clock_identity(p->clock); - pid = clock_parent_identity(p->clock); - if (cid_eq(&cid, &pid.clockIdentity)) { - /* - * We are the GM, but without gmCapable set. - */ - return 1; - } - return 0; -} - -static int port_is_ieee8021as(struct port *p) -{ - return p->follow_up_info ? 1 : 0; -} - -static void port_management_send_error(struct port *p, struct port *ingress, - struct ptp_message *msg, int error_id) -{ - if (port_management_error(p->portIdentity, ingress, msg, error_id)) - pr_err("port %hu: management error failed", portnum(p)); -} - -static const Octet profile_id_drr[] = {0x00, 0x1B, 0x19, 0x00, 0x01, 0x00}; -static const Octet profile_id_p2p[] = {0x00, 0x1B, 0x19, 0x00, 0x02, 0x00}; - -static int port_management_fill_response(struct port *target, - struct ptp_message *rsp, int id) -{ - struct mgmt_clock_description *cd; - struct management_tlv_datum *mtd; - struct clock_description *desc; - struct port_properties_np *ppn; - struct management_tlv *tlv; - struct port_ds_np *pdsnp; - struct tlv_extra *extra; - struct portDS *pds; - uint16_t u16; - uint8_t *buf; - int datalen; - - extra = tlv_extra_alloc(); - if (!extra) { - pr_err("failed to allocate TLV descriptor"); - return 0; - } - extra->tlv = (struct TLV *) rsp->management.suffix; - - tlv = (struct management_tlv *) rsp->management.suffix; - tlv->type = TLV_MANAGEMENT; - tlv->id = id; - - switch (id) { - case TLV_NULL_MANAGEMENT: - datalen = 0; - break; - case TLV_CLOCK_DESCRIPTION: - cd = &extra->cd; - buf = tlv->data; - cd->clockType = (UInteger16 *) buf; - buf += sizeof(*cd->clockType); - *cd->clockType = clock_type(target->clock); - cd->physicalLayerProtocol = (struct PTPText *) buf; - switch(transport_type(target->trp)) { - case TRANS_UDP_IPV4: - case TRANS_UDP_IPV6: - case TRANS_IEEE_802_3: - ptp_text_set(cd->physicalLayerProtocol, "IEEE 802.3"); - break; - default: - ptp_text_set(cd->physicalLayerProtocol, NULL); - break; - } - buf += sizeof(struct PTPText) + cd->physicalLayerProtocol->length; - - cd->physicalAddress = (struct PhysicalAddress *) buf; - u16 = transport_physical_addr(target->trp, - cd->physicalAddress->address); - memcpy(&cd->physicalAddress->length, &u16, 2); - buf += sizeof(struct PhysicalAddress) + u16; - - cd->protocolAddress = (struct PortAddress *) buf; - u16 = transport_type(target->trp); - memcpy(&cd->protocolAddress->networkProtocol, &u16, 2); - u16 = transport_protocol_addr(target->trp, - cd->protocolAddress->address); - memcpy(&cd->protocolAddress->addressLength, &u16, 2); - buf += sizeof(struct PortAddress) + u16; - - desc = clock_description(target->clock); - cd->manufacturerIdentity = buf; - memcpy(cd->manufacturerIdentity, - desc->manufacturerIdentity, OUI_LEN); - buf += OUI_LEN; - *(buf++) = 0; /* reserved */ - - cd->productDescription = (struct PTPText *) buf; - ptp_text_copy(cd->productDescription, &desc->productDescription); - buf += sizeof(struct PTPText) + cd->productDescription->length; - - cd->revisionData = (struct PTPText *) buf; - ptp_text_copy(cd->revisionData, &desc->revisionData); - buf += sizeof(struct PTPText) + cd->revisionData->length; - - cd->userDescription = (struct PTPText *) buf; - ptp_text_copy(cd->userDescription, &desc->userDescription); - buf += sizeof(struct PTPText) + cd->userDescription->length; - - if (target->delayMechanism == DM_P2P) { - memcpy(buf, profile_id_p2p, PROFILE_ID_LEN); - } else { - memcpy(buf, profile_id_drr, PROFILE_ID_LEN); - } - buf += PROFILE_ID_LEN; - datalen = buf - tlv->data; - break; - case TLV_PORT_DATA_SET: - pds = (struct portDS *) tlv->data; - pds->portIdentity = target->portIdentity; - if (target->state == PS_GRAND_MASTER) { - pds->portState = PS_MASTER; - } else { - pds->portState = target->state; - } - pds->logMinDelayReqInterval = target->logMinDelayReqInterval; - pds->peerMeanPathDelay = target->peerMeanPathDelay; - pds->logAnnounceInterval = target->logAnnounceInterval; - pds->announceReceiptTimeout = target->announceReceiptTimeout; - pds->logSyncInterval = target->logSyncInterval; - if (target->delayMechanism) { - pds->delayMechanism = target->delayMechanism; - } else { - pds->delayMechanism = DM_E2E; - } - pds->logMinPdelayReqInterval = target->logMinPdelayReqInterval; - pds->versionNumber = target->versionNumber; - datalen = sizeof(*pds); - break; - case TLV_LOG_ANNOUNCE_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logAnnounceInterval; - datalen = sizeof(*mtd); - break; - case TLV_ANNOUNCE_RECEIPT_TIMEOUT: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->announceReceiptTimeout; - datalen = sizeof(*mtd); - break; - case TLV_LOG_SYNC_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logSyncInterval; - datalen = sizeof(*mtd); - break; - case TLV_VERSION_NUMBER: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->versionNumber; - datalen = sizeof(*mtd); - break; - case TLV_DELAY_MECHANISM: - mtd = (struct management_tlv_datum *) tlv->data; - if (target->delayMechanism) - mtd->val = target->delayMechanism; - else - mtd->val = DM_E2E; - datalen = sizeof(*mtd); - break; - case TLV_LOG_MIN_PDELAY_REQ_INTERVAL: - mtd = (struct management_tlv_datum *) tlv->data; - mtd->val = target->logMinPdelayReqInterval; - datalen = sizeof(*mtd); - break; - case TLV_PORT_DATA_SET_NP: - pdsnp = (struct port_ds_np *) tlv->data; - pdsnp->neighborPropDelayThresh = target->neighborPropDelayThresh; - pdsnp->asCapable = target->asCapable; - datalen = sizeof(*pdsnp); - break; - case TLV_PORT_PROPERTIES_NP: - ppn = (struct port_properties_np *)tlv->data; - ppn->portIdentity = target->portIdentity; - if (target->state == PS_GRAND_MASTER) - ppn->port_state = PS_MASTER; - else - ppn->port_state = target->state; - ppn->timestamping = target->timestamping; - ptp_text_set(&ppn->interface, target->iface->ts_label); - datalen = sizeof(*ppn) + ppn->interface.length; - break; - default: - /* The caller should *not* respond to this message. */ - tlv_extra_recycle(extra); - return 0; - } - - if (datalen % 2) { - tlv->data[datalen] = 0; - datalen++; - } - tlv->length = sizeof(tlv->id) + datalen; - rsp->header.messageLength += sizeof(*tlv) + datalen; - msg_tlv_attach(rsp, extra); - - /* The caller can respond to this message. */ - return 1; -} - -static int port_management_get_response(struct port *target, - struct port *ingress, int id, - struct ptp_message *req) -{ - struct PortIdentity pid = port_identity(target); - struct ptp_message *rsp; - int respond; - - rsp = port_management_reply(pid, ingress, req); - if (!rsp) { - return 0; - } - respond = port_management_fill_response(target, rsp, id); - if (respond) - port_prepare_and_send(ingress, rsp, TRANS_GENERAL); - msg_put(rsp); - return respond; -} - -static int port_management_set(struct port *target, - struct port *ingress, int id, - struct ptp_message *req) -{ - int respond = 0; - struct management_tlv *tlv; - struct port_ds_np *pdsnp; - - tlv = (struct management_tlv *) req->management.suffix; - - switch (id) { - case TLV_PORT_DATA_SET_NP: - pdsnp = (struct port_ds_np *) tlv->data; - target->neighborPropDelayThresh = pdsnp->neighborPropDelayThresh; - respond = 1; - break; - } - if (respond && !port_management_get_response(target, ingress, id, req)) - pr_err("port %hu: failed to send management set response", portnum(target)); - return respond ? 1 : 0; -} - -static void port_nrate_calculate(struct port *p, tmv_t origin, tmv_t ingress) -{ - struct nrate_estimator *n = &p->nrate; - - /* - * We experienced a successful exchanges of peer delay request - * and response, reset pdr_missing for this port. - */ - p->pdr_missing = 0; - - if (tmv_is_zero(n->ingress1)) { - n->ingress1 = ingress; - n->origin1 = origin; - return; - } - n->count++; - if (n->count < n->max_count) { - return; - } - if (tmv_cmp(ingress, n->ingress1) == 0) { - pr_warning("bad timestamps in nrate calculation"); - return; - } - n->ratio = - tmv_dbl(tmv_sub(origin, n->origin1)) / - tmv_dbl(tmv_sub(ingress, n->ingress1)); - n->ingress1 = ingress; - n->origin1 = origin; - n->count = 0; - n->ratio_valid = 1; -} - -static void port_nrate_initialize(struct port *p) -{ - int shift = p->freq_est_interval - p->logMinPdelayReqInterval; - - if (shift < 0) - shift = 0; - else if (shift >= sizeof(int) * 8) { - shift = sizeof(int) * 8 - 1; - pr_warning("freq_est_interval is too long"); - } - - /* We start in the 'incapable' state. */ - p->pdr_missing = ALLOWED_LOST_RESPONSES + 1; - p->asCapable = 0; - - p->peer_portid_valid = 0; - - p->nrate.origin1 = tmv_zero(); - p->nrate.ingress1 = tmv_zero(); - p->nrate.max_count = (1 << shift); - p->nrate.count = 0; - p->nrate.ratio = 1.0; - p->nrate.ratio_valid = 0; -} - -int port_set_announce_tmo(struct port *p) -{ - return set_tmo_random(p->fda.fd[FD_ANNOUNCE_TIMER], - p->announceReceiptTimeout, - p->announce_span, p->logAnnounceInterval); -} - -int port_set_delay_tmo(struct port *p) -{ - if (p->delayMechanism == DM_P2P) { - return set_tmo_log(p->fda.fd[FD_DELAY_TIMER], 1, - p->logMinPdelayReqInterval); - } else { - return set_tmo_random(p->fda.fd[FD_DELAY_TIMER], 0, 2, - p->logMinDelayReqInterval); - } -} - -static int port_set_manno_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, p->logAnnounceInterval); -} - -int port_set_qualification_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_QUALIFICATION_TIMER], - 1+clock_steps_removed(p->clock), p->logAnnounceInterval); -} - -static int port_set_sync_rx_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_SYNC_RX_TIMER], - p->syncReceiptTimeout, p->logSyncInterval); -} - -static int port_set_sync_tx_tmo(struct port *p) -{ - return set_tmo_log(p->fda.fd[FD_SYNC_TX_TIMER], 1, p->logSyncInterval); -} - -void port_show_transition(struct port *p, enum port_state next, - enum fsm_event event) -{ - if (event == EV_FAULT_DETECTED) { - pr_notice("port %hu: %s to %s on %s (%s)", portnum(p), - ps_str[p->state], ps_str[next], ev_str[event], - ft_str(last_fault_type(p))); - } else { - pr_notice("port %hu: %s to %s on %s", portnum(p), - ps_str[p->state], ps_str[next], ev_str[event]); - } -} - -static void port_slave_priority_warning(struct port *p) -{ - UInteger16 n = portnum(p); - pr_warning("port %hu: master state recommended in slave only mode", n); - pr_warning("port %hu: defaultDS.priority1 probably misconfigured", n); -} - -static void port_synchronize(struct port *p, - tmv_t ingress_ts, - struct timestamp origin_ts, - Integer64 correction1, Integer64 correction2) -{ - enum servo_state state; - tmv_t t1, t1c, t2, c1, c2; - - port_set_sync_rx_tmo(p); - - t1 = timestamp_to_tmv(origin_ts); - t2 = ingress_ts; - c1 = correction_to_tmv(correction1); - c2 = correction_to_tmv(correction2); - t1c = tmv_add(t1, tmv_add(c1, c2)); - - state = clock_synchronize(p->clock, t2, t1c); - switch (state) { - case SERVO_UNLOCKED: - port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0); - break; - case SERVO_JUMP: - port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0); - flush_delay_req(p); - if (p->peer_delay_req) { - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; - } - break; - case SERVO_LOCKED: - port_dispatch(p, EV_MASTER_CLOCK_SELECTED, 0); - break; - } -} - -/* - * Handle out of order packets. The network stack might - * provide the follow up _before_ the sync message. After all, - * they can arrive on two different ports. In addition, time - * stamping in PHY devices might delay the event packets. - */ -static void port_syfufsm(struct port *p, enum syfu_event event, - struct ptp_message *m) -{ - struct ptp_message *syn, *fup; - - switch (p->syfu) { - case SF_EMPTY: - switch (event) { - case SYNC_MISMATCH: - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_SYNC; - break; - case FUP_MISMATCH: - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_FUP; - break; - case SYNC_MATCH: - break; - case FUP_MATCH: - break; - } - break; - - case SF_HAVE_SYNC: - switch (event) { - case SYNC_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - break; - case SYNC_MATCH: - break; - case FUP_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_FUP; - break; - case FUP_MATCH: - syn = p->last_syncfup; - port_synchronize(p, syn->hwts.ts, m->ts.pdu, - syn->header.correction, - m->header.correction); - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - break; - } - break; - - case SF_HAVE_FUP: - switch (event) { - case SYNC_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - p->syfu = SF_HAVE_SYNC; - break; - case SYNC_MATCH: - fup = p->last_syncfup; - port_synchronize(p, m->hwts.ts, fup->ts.pdu, - m->header.correction, - fup->header.correction); - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - break; - case FUP_MISMATCH: - msg_put(p->last_syncfup); - msg_get(m); - p->last_syncfup = m; - break; - case FUP_MATCH: - break; - } - break; - } -} - -static int port_pdelay_request(struct port *p) -{ - struct ptp_message *msg; - int err; - - /* If multiple pdelay resp were not detected the counter can be reset */ - if (!p->multiple_pdr_detected) { - p->multiple_seq_pdr_count = 0; - } - p->multiple_pdr_detected = 0; - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = PDELAY_REQ | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct pdelay_req_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.correction = -p->asymmetry; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.delayreq++; - msg->header.control = CTL_OTHER; - msg->header.logMessageInterval = port_is_ieee8021as(p) ? - p->logMinPdelayReqInterval : 0x7f; - - if (unicast_client_enabled(p) && p->unicast_master_table->peer_name) { - msg->address = p->unicast_master_table->peer_addr.address; - msg->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, msg, TRANS_EVENT); - if (err) { - pr_err("port %hu: send peer delay request failed", portnum(p)); - goto out; - } - if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted peer delay request"); - goto out; - } - - if (p->peer_delay_req) { - if (port_capable(p)) { - p->pdr_missing++; - } - msg_put(p->peer_delay_req); - } - p->peer_delay_req = msg; - return 0; -out: - msg_put(msg); - return -1; -} - -int port_delay_request(struct port *p) -{ - struct ptp_message *msg; - - /* Time to send a new request, forget current pdelay resp and fup */ - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - p->peer_delay_resp = NULL; - } - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - p->peer_delay_fup = NULL; - } - - if (p->delayMechanism == DM_P2P) { - return port_pdelay_request(p); - } - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = DELAY_REQ | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct delay_req_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.correction = -p->asymmetry; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.delayreq++; - msg->header.control = CTL_DELAY_REQ; - msg->header.logMessageInterval = 0x7f; - - if (p->hybrid_e2e) { - struct ptp_message *dst = TAILQ_FIRST(&p->best->messages); - msg->address = dst->address; - msg->header.flagField[0] |= UNICAST; - } - - if (port_prepare_and_send(p, msg, TRANS_EVENT)) { - pr_err("port %hu: send delay request failed", portnum(p)); - goto out; - } - if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted delay request"); - goto out; - } - - TAILQ_INSERT_HEAD(&p->delay_req, msg, list); - - return 0; -out: - msg_put(msg); - return -1; -} - -int port_tx_announce(struct port *p, struct address *dst) -{ - struct timePropertiesDS *tp = clock_time_properties(p->clock); - struct parent_ds *dad = clock_parent_ds(p->clock); - struct ptp_message *msg; - int err; - - if (p->inhibit_multicast_service && !dst) { - return 0; - } - if (!port_capable(p)) { - return 0; - } - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = ANNOUNCE | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct announce_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.announce++; - msg->header.control = CTL_OTHER; - msg->header.logMessageInterval = p->logAnnounceInterval; - - msg->header.flagField[1] = tp->flags; - - if (dst) { - msg->address = *dst; - msg->header.flagField[0] |= UNICAST; - } - msg->announce.currentUtcOffset = tp->currentUtcOffset; - msg->announce.grandmasterPriority1 = dad->pds.grandmasterPriority1; - msg->announce.grandmasterClockQuality = dad->pds.grandmasterClockQuality; - msg->announce.grandmasterPriority2 = dad->pds.grandmasterPriority2; - msg->announce.grandmasterIdentity = dad->pds.grandmasterIdentity; - msg->announce.stepsRemoved = clock_steps_removed(p->clock); - msg->announce.timeSource = tp->timeSource; - - if (p->path_trace_enabled && path_trace_append(p, msg, dad)) { - pr_err("port %hu: append path trace failed", portnum(p)); - } - - err = port_prepare_and_send(p, msg, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send announce failed", portnum(p)); - } - msg_put(msg); - return err; -} - -int port_tx_sync(struct port *p, struct address *dst) -{ - struct ptp_message *msg, *fup; - int err, event; - - switch (p->timestamping) { - case TS_SOFTWARE: - case TS_LEGACY_HW: - case TS_HARDWARE: - event = TRANS_EVENT; - break; - case TS_ONESTEP: - event = TRANS_ONESTEP; - break; - case TS_P2P1STEP: - event = TRANS_P2P1STEP; - break; - default: - return -1; - } - - if (p->inhibit_multicast_service && !dst) { - return 0; - } - if (!port_capable(p)) { - return 0; - } - if (port_sync_incapable(p)) { - return 0; - } - msg = msg_allocate(); - if (!msg) { - return -1; - } - fup = msg_allocate(); - if (!fup) { - msg_put(msg); - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = SYNC | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct sync_msg); - msg->header.domainNumber = clock_domain_number(p->clock); - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = p->seqnum.sync++; - msg->header.control = CTL_SYNC; - msg->header.logMessageInterval = p->logSyncInterval; - - if (p->timestamping != TS_ONESTEP && p->timestamping != TS_P2P1STEP) { - msg->header.flagField[0] |= TWO_STEP; - } - - if (dst) { - msg->address = *dst; - msg->header.flagField[0] |= UNICAST; - msg->header.logMessageInterval = 0x7f; - } - err = port_prepare_and_send(p, msg, event); - if (err) { - pr_err("port %hu: send sync failed", portnum(p)); - goto out; - } - if (p->timestamping == TS_ONESTEP || p->timestamping == TS_P2P1STEP) { - goto out; - } else if (msg_sots_missing(msg)) { - pr_err("missing timestamp on transmitted sync"); - err = -1; - goto out; - } - - /* - * Send the follow up message right away. - */ - fup->hwts.type = p->timestamping; - - fup->header.tsmt = FOLLOW_UP | p->transportSpecific; - fup->header.ver = PTP_VERSION; - fup->header.messageLength = sizeof(struct follow_up_msg); - fup->header.domainNumber = clock_domain_number(p->clock); - fup->header.sourcePortIdentity = p->portIdentity; - fup->header.sequenceId = p->seqnum.sync - 1; - fup->header.control = CTL_FOLLOW_UP; - fup->header.logMessageInterval = p->logSyncInterval; - - fup->follow_up.preciseOriginTimestamp = tmv_to_Timestamp(msg->hwts.ts); - - if (dst) { - fup->address = *dst; - fup->header.flagField[0] |= UNICAST; - } - if (p->follow_up_info && follow_up_info_append(fup)) { - pr_err("port %hu: append fup info failed", portnum(p)); - err = -1; - goto out; - } - - err = port_prepare_and_send(p, fup, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send follow up failed", portnum(p)); - } -out: - msg_put(msg); - msg_put(fup); - return err; -} - -/* - * port initialize and disable - */ -int port_is_enabled(struct port *p) -{ - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - return 0; - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - return 1; -} - -void flush_last_sync(struct port *p) -{ - if (p->syfu != SF_EMPTY) { - msg_put(p->last_syncfup); - p->syfu = SF_EMPTY; - } -} - -void flush_delay_req(struct port *p) -{ - struct ptp_message *m; - while ((m = TAILQ_FIRST(&p->delay_req)) != NULL) { - TAILQ_REMOVE(&p->delay_req, m, list); - msg_put(m); - } -} - -static void flush_peer_delay(struct port *p) -{ - if (p->peer_delay_req) { - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; - } - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - p->peer_delay_resp = NULL; - } - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - p->peer_delay_fup = NULL; - } -} - -static void port_clear_fda(struct port *p, int count) -{ - int i; - - for (i = 0; i < count; i++) - p->fda.fd[i] = -1; -} - -void port_disable(struct port *p) -{ - int i; - - tc_flush(p); - flush_last_sync(p); - flush_delay_req(p); - flush_peer_delay(p); - - p->best = NULL; - free_foreign_masters(p); - transport_close(p->trp, &p->fda); - - for (i = 0; i < N_TIMER_FDS; i++) { - close(p->fda.fd[FD_FIRST_TIMER + i]); - } - - /* Keep rtnl socket to get link status info. */ - port_clear_fda(p, FD_RTNL); - clock_fda_changed(p->clock); -} - -int port_initialize(struct port *p) -{ - struct config *cfg = clock_config(p->clock); - int fd[N_TIMER_FDS], i; - - p->multiple_seq_pdr_count = 0; - p->multiple_pdr_detected = 0; - p->last_fault_type = FT_UNSPECIFIED; - p->logMinDelayReqInterval = config_get_int(cfg, p->name, "logMinDelayReqInterval"); - p->peerMeanPathDelay = 0; - p->logAnnounceInterval = config_get_int(cfg, p->name, "logAnnounceInterval"); - p->announceReceiptTimeout = config_get_int(cfg, p->name, "announceReceiptTimeout"); - p->syncReceiptTimeout = config_get_int(cfg, p->name, "syncReceiptTimeout"); - p->transportSpecific = config_get_int(cfg, p->name, "transportSpecific"); - p->transportSpecific <<= 4; - p->match_transport_specific = !config_get_int(cfg, p->name, "ignore_transport_specific"); - p->master_only = config_get_int(cfg, p->name, "masterOnly"); - p->localPriority = config_get_int(cfg, p->name, "G.8275.portDS.localPriority"); - p->logSyncInterval = config_get_int(cfg, p->name, "logSyncInterval"); - p->logMinPdelayReqInterval = config_get_int(cfg, p->name, "logMinPdelayReqInterval"); - p->neighborPropDelayThresh = config_get_int(cfg, p->name, "neighborPropDelayThresh"); - p->min_neighbor_prop_delay = config_get_int(cfg, p->name, "min_neighbor_prop_delay"); - - for (i = 0; i < N_TIMER_FDS; i++) { - fd[i] = -1; - } - for (i = 0; i < N_TIMER_FDS; i++) { - fd[i] = timerfd_create(CLOCK_MONOTONIC, 0); - if (fd[i] < 0) { - pr_err("timerfd_create: %s", strerror(errno)); - goto no_timers; - } - } - if (transport_open(p->trp, p->iface, &p->fda, p->timestamping)) - goto no_tropen; - - for (i = 0; i < N_TIMER_FDS; i++) { - p->fda.fd[FD_FIRST_TIMER + i] = fd[i]; - } - - if (port_set_announce_tmo(p)) { - goto no_tmo; - } - if (unicast_client_enabled(p) && unicast_client_set_tmo(p)) { - goto no_tmo; - } - - /* No need to open rtnl socket on UDS port. */ - if (transport_type(p->trp) != TRANS_UDS) { - if (p->fda.fd[FD_RTNL] == -1) - p->fda.fd[FD_RTNL] = rtnl_open(); - if (p->fda.fd[FD_RTNL] >= 0) - rtnl_link_query(p->fda.fd[FD_RTNL], p->iface->name); - } - - port_nrate_initialize(p); - - clock_fda_changed(p->clock); - return 0; - -no_tmo: - transport_close(p->trp, &p->fda); -no_tropen: -no_timers: - for (i = 0; i < N_TIMER_FDS; i++) { - if (fd[i] >= 0) - close(fd[i]); - } - return -1; -} - -static int port_renew_transport(struct port *p) -{ - int res; - - if (!port_is_enabled(p)) { - return 0; - } - transport_close(p->trp, &p->fda); - port_clear_fda(p, FD_FIRST_TIMER); - res = transport_open(p->trp, p->iface, &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; -} - -/* - * Returns non-zero if the announce message is different than last. - */ -static int update_current_master(struct port *p, struct ptp_message *m) -{ - struct foreign_clock *fc = p->best; - struct ptp_message *tmp; - struct parent_ds *dad; - struct path_trace_tlv *ptt; - struct timePropertiesDS tds; - - if (!msg_source_equal(m, fc)) - return add_foreign_master(p, m); - - if (p->state != PS_PASSIVE) { - tds.currentUtcOffset = m->announce.currentUtcOffset; - tds.flags = m->header.flagField[1]; - tds.timeSource = m->announce.timeSource; - clock_update_time_properties(p->clock, tds); - } - if (p->path_trace_enabled) { - ptt = (struct path_trace_tlv *) m->announce.suffix; - dad = clock_parent_ds(p->clock); - memcpy(dad->ptl, ptt->cid, ptt->length); - dad->path_length = path_length(ptt); - } - port_set_announce_tmo(p); - fc_prune(fc); - msg_get(m); - fc->n_messages++; - TAILQ_INSERT_HEAD(&fc->messages, m, list); - if (fc->n_messages > 1) { - tmp = TAILQ_NEXT(m, list); - return announce_compare(m, tmp); - } - return 0; -} - -struct dataset *port_best_foreign(struct port *port) -{ - return port->best ? &port->best->dataset : NULL; -} - -/* message processing routines */ - -/* - * Returns non-zero if the announce message is both qualified and different. - */ -int process_announce(struct port *p, struct ptp_message *m) -{ - int result = 0; - - /* Do not qualify announce messages with stepsRemoved >= 255, see - * IEEE1588-2008 section 9.3.2.5 (d) - */ - if (m->announce.stepsRemoved >= 255) { - return result; - } - - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - break; - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - result = add_foreign_master(p, m); - break; - case PS_PASSIVE: - case PS_UNCALIBRATED: - case PS_SLAVE: - result = update_current_master(p, m); - break; - } - return result; -} - -static int process_delay_req(struct port *p, struct ptp_message *m) -{ - int err, nsm, saved_seqnum_sync; - struct ptp_message *msg; - - nsm = port_nsm_reply(p, m); - - if (!nsm && p->state != PS_MASTER && p->state != PS_GRAND_MASTER) { - return 0; - } - - if (p->delayMechanism == DM_P2P) { - pr_warning("port %hu: delay request on P2P port", portnum(p)); - return 0; - } - - msg = msg_allocate(); - if (!msg) { - return -1; - } - - msg->hwts.type = p->timestamping; - - msg->header.tsmt = DELAY_RESP | p->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct delay_resp_msg); - msg->header.domainNumber = m->header.domainNumber; - msg->header.correction = m->header.correction; - msg->header.sourcePortIdentity = p->portIdentity; - msg->header.sequenceId = m->header.sequenceId; - msg->header.control = CTL_DELAY_RESP; - msg->header.logMessageInterval = p->logMinDelayReqInterval; - - msg->delay_resp.receiveTimestamp = tmv_to_Timestamp(m->hwts.ts); - - msg->delay_resp.requestingPortIdentity = m->header.sourcePortIdentity; - - if (p->hybrid_e2e && msg_unicast(m)) { - msg->address = m->address; - msg->header.flagField[0] |= UNICAST; - msg->header.logMessageInterval = 0x7f; - } - if (nsm && net_sync_resp_append(p, msg)) { - pr_err("port %hu: append NSM failed", portnum(p)); - err = -1; - goto out; - } - err = port_prepare_and_send(p, msg, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send delay response failed", portnum(p)); - goto out; - } - if (nsm) { - saved_seqnum_sync = p->seqnum.sync; - p->seqnum.sync = m->header.sequenceId; - err = port_tx_sync(p, &m->address); - p->seqnum.sync = saved_seqnum_sync; - } -out: - msg_put(msg); - return err; -} - -void process_delay_resp(struct port *p, struct ptp_message *m) -{ - struct delay_resp_msg *rsp = &m->delay_resp; - struct PortIdentity master; - struct ptp_message *req; - tmv_t c3, t3, t4, t4c; - - master = clock_parent_identity(p->clock); - - if (p->state != PS_UNCALIBRATED && p->state != PS_SLAVE) { - return; - } - if (!pid_eq(&rsp->requestingPortIdentity, &p->portIdentity)) { - return; - } - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - TAILQ_FOREACH(req, &p->delay_req, list) { - if (rsp->hdr.sequenceId == ntohs(req->delay_req.hdr.sequenceId)) { - break; - } - } - if (!req) { - return; - } - - c3 = correction_to_tmv(m->header.correction); - t3 = req->hwts.ts; - t4 = timestamp_to_tmv(m->ts.pdu); - t4c = tmv_sub(t4, c3); - - clock_path_delay(p->clock, t3, t4c); - - TAILQ_REMOVE(&p->delay_req, req, list); - msg_put(req); - - if (p->logMinDelayReqInterval == rsp->hdr.logMessageInterval) { - return; - } - if (msg_unicast(m)) { - /* Unicast responses have logMinDelayReqInterval set to 0x7F. */ - return; - } - if (rsp->hdr.logMessageInterval < -10 || - rsp->hdr.logMessageInterval > 22) { - pl_info(300, "port %hu: ignore bogus delay request interval 2^%d", - portnum(p), rsp->hdr.logMessageInterval); - return; - } - p->logMinDelayReqInterval = rsp->hdr.logMessageInterval; - pr_notice("port %hu: minimum delay request interval 2^%d", - portnum(p), p->logMinDelayReqInterval); - port_set_delay_tmo(p); -} - -void process_follow_up(struct port *p, struct ptp_message *m) -{ - enum syfu_event event; - struct PortIdentity master; - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - return; - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - master = clock_parent_identity(p->clock); - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - - if (p->follow_up_info) { - struct follow_up_info_tlv *fui = follow_up_info_extract(m); - if (!fui) - return; - clock_follow_up_info(p->clock, fui); - } - - if (p->syfu == SF_HAVE_SYNC && - p->last_syncfup->header.sequenceId == m->header.sequenceId) { - event = FUP_MATCH; - } else { - event = FUP_MISMATCH; - } - port_syfufsm(p, event, m); -} - -int process_pdelay_req(struct port *p, struct ptp_message *m) -{ - struct ptp_message *rsp, *fup; - enum transport_event event; - int err; - - switch (p->timestamping) { - case TS_SOFTWARE: - case TS_LEGACY_HW: - case TS_HARDWARE: - case TS_ONESTEP: - event = TRANS_EVENT; - break; - case TS_P2P1STEP: - event = TRANS_P2P1STEP; - break; - default: - return -1; - } - - if (p->delayMechanism == DM_E2E) { - pr_warning("port %hu: pdelay_req on E2E port", portnum(p)); - return 0; - } - if (p->delayMechanism == DM_AUTO) { - pr_info("port %hu: peer detected, switch to P2P", portnum(p)); - p->delayMechanism = DM_P2P; - port_set_delay_tmo(p); - } - if (p->peer_portid_valid) { - if (!pid_eq(&p->peer_portid, &m->header.sourcePortIdentity)) { - pr_err("port %hu: received pdelay_req msg with " - "unexpected peer port id %s", - portnum(p), - pid2str(&m->header.sourcePortIdentity)); - p->peer_portid_valid = 0; - port_capable(p); - } - } else { - p->peer_portid_valid = 1; - p->peer_portid = m->header.sourcePortIdentity; - pr_debug("port %hu: peer port id set to %s", portnum(p), - pid2str(&p->peer_portid)); - } - - rsp = msg_allocate(); - if (!rsp) { - return -1; - } - - fup = msg_allocate(); - if (!fup) { - msg_put(rsp); - return -1; - } - - rsp->hwts.type = p->timestamping; - - rsp->header.tsmt = PDELAY_RESP | p->transportSpecific; - rsp->header.ver = PTP_VERSION; - rsp->header.messageLength = sizeof(struct pdelay_resp_msg); - rsp->header.domainNumber = m->header.domainNumber; - rsp->header.sourcePortIdentity = p->portIdentity; - rsp->header.sequenceId = m->header.sequenceId; - rsp->header.control = CTL_OTHER; - rsp->header.logMessageInterval = 0x7f; - - /* - * NB - We do not have any fraction nanoseconds for the correction - * fields, neither in the response or the follow up. - */ - if (p->timestamping == TS_P2P1STEP) { - rsp->header.correction = m->header.correction; - rsp->header.correction += p->tx_timestamp_offset; - rsp->header.correction += p->rx_timestamp_offset; - } else { - rsp->header.flagField[0] |= TWO_STEP; - rsp->pdelay_resp.requestReceiptTimestamp = - tmv_to_Timestamp(m->hwts.ts); - } - rsp->pdelay_resp.requestingPortIdentity = m->header.sourcePortIdentity; - - if (msg_unicast(m)) { - rsp->address = m->address; - rsp->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, rsp, event); - if (err) { - pr_err("port %hu: send peer delay response failed", portnum(p)); - goto out; - } - if (p->timestamping == TS_P2P1STEP) { - goto out; - } else if (msg_sots_missing(rsp)) { - pr_err("missing timestamp on transmitted peer delay response"); - err = -1; - goto out; - } - - /* - * Send the follow up message right away. - */ - fup->hwts.type = p->timestamping; - - fup->header.tsmt = PDELAY_RESP_FOLLOW_UP | p->transportSpecific; - fup->header.ver = PTP_VERSION; - fup->header.messageLength = sizeof(struct pdelay_resp_fup_msg); - fup->header.domainNumber = m->header.domainNumber; - fup->header.correction = m->header.correction; - fup->header.sourcePortIdentity = p->portIdentity; - fup->header.sequenceId = m->header.sequenceId; - fup->header.control = CTL_OTHER; - fup->header.logMessageInterval = 0x7f; - - fup->pdelay_resp_fup.requestingPortIdentity = m->header.sourcePortIdentity; - - fup->pdelay_resp_fup.responseOriginTimestamp = - tmv_to_Timestamp(rsp->hwts.ts); - - if (msg_unicast(m)) { - fup->address = m->address; - fup->header.flagField[0] |= UNICAST; - } - - err = peer_prepare_and_send(p, fup, TRANS_GENERAL); - if (err) { - pr_err("port %hu: send pdelay_resp_fup failed", portnum(p)); - } -out: - msg_put(rsp); - msg_put(fup); - return err; -} - -static void port_peer_delay(struct port *p) -{ - tmv_t c1, c2, t1, t2, t3, t3c, t4; - struct ptp_message *req = p->peer_delay_req; - struct ptp_message *rsp = p->peer_delay_resp; - struct ptp_message *fup = p->peer_delay_fup; - - /* Check for response, validate port and sequence number. */ - - if (!rsp) - return; - - if (!pid_eq(&rsp->pdelay_resp.requestingPortIdentity, &p->portIdentity)) - return; - - if (rsp->header.sequenceId != ntohs(req->header.sequenceId)) - return; - - t1 = req->hwts.ts; - t4 = rsp->hwts.ts; - c1 = correction_to_tmv(rsp->header.correction + p->asymmetry); - - /* Process one-step response immediately. */ - if (one_step(rsp)) { - t2 = tmv_zero(); - t3 = tmv_zero(); - c2 = tmv_zero(); - goto calc; - } - - /* Check for follow up, validate port and sequence number. */ - - if (!fup) - return; - - if (!pid_eq(&fup->pdelay_resp_fup.requestingPortIdentity, &p->portIdentity)) - return; - - if (fup->header.sequenceId != rsp->header.sequenceId) - return; - - if (!source_pid_eq(fup, rsp)) - return; - - /* Process follow up response. */ - t2 = timestamp_to_tmv(rsp->ts.pdu); - t3 = timestamp_to_tmv(fup->ts.pdu); - c2 = correction_to_tmv(fup->header.correction); -calc: - t3c = tmv_add(t3, tmv_add(c1, c2)); - - if (p->follow_up_info) - port_nrate_calculate(p, t3c, t4); - - tsproc_set_clock_rate_ratio(p->tsproc, p->nrate.ratio * - clock_rate_ratio(p->clock)); - tsproc_up_ts(p->tsproc, t1, t2); - tsproc_down_ts(p->tsproc, t3c, t4); - if (tsproc_update_delay(p->tsproc, &p->peer_delay)) - return; - - p->peerMeanPathDelay = tmv_to_TimeInterval(p->peer_delay); - - if (p->state == PS_UNCALIBRATED || p->state == PS_SLAVE) { - clock_peer_delay(p->clock, p->peer_delay, t1, t2, - p->nrate.ratio); - } - - msg_put(p->peer_delay_req); - p->peer_delay_req = NULL; -} - -int process_pdelay_resp(struct port *p, struct ptp_message *m) -{ - if (p->peer_delay_resp) { - if (!source_pid_eq(p->peer_delay_resp, m)) { - pr_err("port %hu: multiple peer responses", portnum(p)); - if (!p->multiple_pdr_detected) { - p->multiple_pdr_detected = 1; - p->multiple_seq_pdr_count++; - } - if (p->multiple_seq_pdr_count >= 3) { - p->last_fault_type = FT_BAD_PEER_NETWORK; - return -1; - } - } - } - if (!p->peer_delay_req) { - pr_err("port %hu: rogue peer delay response", portnum(p)); - return -1; - } - if (p->peer_portid_valid) { - if (!pid_eq(&p->peer_portid, &m->header.sourcePortIdentity)) { - pr_err("port %hu: received pdelay_resp msg with " - "unexpected peer port id %s", - portnum(p), - pid2str(&m->header.sourcePortIdentity)); - p->peer_portid_valid = 0; - port_capable(p); - } - } else { - p->peer_portid_valid = 1; - p->peer_portid = m->header.sourcePortIdentity; - pr_debug("port %hu: peer port id set to %s", portnum(p), - pid2str(&p->peer_portid)); - } - - if (p->peer_delay_resp) { - msg_put(p->peer_delay_resp); - } - msg_get(m); - p->peer_delay_resp = m; - port_peer_delay(p); - return 0; -} - -void process_pdelay_resp_fup(struct port *p, struct ptp_message *m) -{ - if (!p->peer_delay_req) { - return; - } - - if (p->peer_delay_fup) { - msg_put(p->peer_delay_fup); - } - - msg_get(m); - p->peer_delay_fup = m; - port_peer_delay(p); -} - -void process_sync(struct port *p, struct ptp_message *m) -{ - enum syfu_event event; - struct PortIdentity master; - switch (p->state) { - case PS_INITIALIZING: - case PS_FAULTY: - case PS_DISABLED: - case PS_LISTENING: - case PS_PRE_MASTER: - case PS_MASTER: - case PS_GRAND_MASTER: - case PS_PASSIVE: - return; - case PS_UNCALIBRATED: - case PS_SLAVE: - break; - } - master = clock_parent_identity(p->clock); - if (!pid_eq(&master, &m->header.sourcePortIdentity)) { - return; - } - - if (!msg_unicast(m) && - m->header.logMessageInterval != p->log_sync_interval) { - p->log_sync_interval = m->header.logMessageInterval; - clock_sync_interval(p->clock, p->log_sync_interval); - } - - m->header.correction += p->asymmetry; - - if (one_step(m)) { - port_synchronize(p, m->hwts.ts, m->ts.pdu, - m->header.correction, 0); - flush_last_sync(p); - return; - } - - if (p->syfu == SF_HAVE_FUP && - fup_sync_ok(p->last_syncfup, m) && - p->last_syncfup->header.sequenceId == m->header.sequenceId) { - event = SYNC_MATCH; - } else { - event = SYNC_MISMATCH; - } - port_syfufsm(p, event, m); -} - -/* public methods */ - -void port_close(struct port *p) -{ - if (port_is_enabled(p)) { - port_disable(p); - } - - if (p->fda.fd[FD_RTNL] >= 0) { - rtnl_close(p->fda.fd[FD_RTNL]); - } - - unicast_service_cleanup(p); - transport_destroy(p->trp); - tsproc_destroy(p->tsproc); - if (p->fault_fd >= 0) { - close(p->fault_fd); - } - free(p); -} - -struct foreign_clock *port_compute_best(struct port *p) -{ - int (*dscmp)(struct dataset *a, struct dataset *b); - struct foreign_clock *fc; - struct ptp_message *tmp; - - dscmp = clock_dscmp(p->clock); - p->best = NULL; - - if (p->master_only) - return p->best; - - LIST_FOREACH(fc, &p->foreign_masters, list) { - tmp = TAILQ_FIRST(&fc->messages); - if (!tmp) - continue; - - announce_to_dataset(tmp, p, &fc->dataset); - - fc_prune(fc); - - if (fc->n_messages < FOREIGN_MASTER_THRESHOLD) - continue; - - if (!p->best) - p->best = fc; - else if (dscmp(&fc->dataset, &p->best->dataset) > 0) - p->best = fc; - else - fc_clear(fc); - } - - return p->best; -} - -static void port_e2e_transition(struct port *p, enum port_state next) -{ - port_clr_tmo(p->fda.fd[FD_ANNOUNCE_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_RX_TIMER]); - port_clr_tmo(p->fda.fd[FD_DELAY_TIMER]); - port_clr_tmo(p->fda.fd[FD_QUALIFICATION_TIMER]); - port_clr_tmo(p->fda.fd[FD_MANNO_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_TX_TIMER]); - /* Leave FD_UNICAST_REQ_TIMER running. */ - - switch (next) { - case PS_INITIALIZING: - break; - case PS_FAULTY: - case PS_DISABLED: - port_disable(p); - break; - case PS_LISTENING: - port_set_announce_tmo(p); - break; - case PS_PRE_MASTER: - port_set_qualification_tmo(p); - break; - case PS_MASTER: - case PS_GRAND_MASTER: - set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, -10); /*~1ms*/ - port_set_sync_tx_tmo(p); - break; - case PS_PASSIVE: - port_set_announce_tmo(p); - break; - case PS_UNCALIBRATED: - flush_last_sync(p); - flush_delay_req(p); - /* fall through */ - case PS_SLAVE: - port_set_announce_tmo(p); - port_set_delay_tmo(p); - break; - }; -} - -static void port_p2p_transition(struct port *p, enum port_state next) -{ - port_clr_tmo(p->fda.fd[FD_ANNOUNCE_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_RX_TIMER]); - /* Leave FD_DELAY_TIMER running. */ - port_clr_tmo(p->fda.fd[FD_QUALIFICATION_TIMER]); - port_clr_tmo(p->fda.fd[FD_MANNO_TIMER]); - port_clr_tmo(p->fda.fd[FD_SYNC_TX_TIMER]); - /* Leave FD_UNICAST_REQ_TIMER running. */ - - switch (next) { - case PS_INITIALIZING: - break; - case PS_FAULTY: - case PS_DISABLED: - port_disable(p); - break; - case PS_LISTENING: - port_set_announce_tmo(p); - port_set_delay_tmo(p); - break; - case PS_PRE_MASTER: - port_set_qualification_tmo(p); - break; - case PS_MASTER: - case PS_GRAND_MASTER: - set_tmo_log(p->fda.fd[FD_MANNO_TIMER], 1, -10); /*~1ms*/ - port_set_sync_tx_tmo(p); - break; - case PS_PASSIVE: - port_set_announce_tmo(p); - break; - case PS_UNCALIBRATED: - flush_last_sync(p); - flush_peer_delay(p); - /* fall through */ - case PS_SLAVE: - port_set_announce_tmo(p); - break; - }; -} - -void port_dispatch(struct port *p, enum fsm_event event, int mdiff) -{ - p->dispatch(p, event, mdiff); -} - -static void bc_dispatch(struct port *p, enum fsm_event event, int mdiff) -{ - if (clock_slave_only(p->clock)) { - if (event == EV_RS_MASTER || event == EV_RS_GRAND_MASTER) { - port_slave_priority_warning(p); - } - } - - if (!port_state_update(p, event, mdiff)) { - return; - } - - if (p->delayMechanism == DM_P2P) { - port_p2p_transition(p, p->state); - } else { - port_e2e_transition(p, p->state); - } - - if (p->jbod && p->state == PS_UNCALIBRATED) { - if (clock_switch_phc(p->clock, p->phc_index)) { - p->last_fault_type = FT_SWITCH_PHC; - port_dispatch(p, EV_FAULT_DETECTED, 0); - return; - } - clock_sync_interval(p->clock, p->log_sync_interval); - } -} - -void port_link_status(void *ctx, int linkup, int ts_index) -{ - struct port *p = ctx; - int link_state; - char ts_label[MAX_IFNAME_SIZE + 1] = {0}; - int required_modes; - - link_state = linkup ? LINK_UP : LINK_DOWN; - if (p->link_status & link_state) { - p->link_status = link_state; - } else { - p->link_status = link_state | LINK_STATE_CHANGED; - pr_notice("port %hu: link %s", portnum(p), linkup ? "up" : "down"); - } - - /* ts_label changed */ - if (if_indextoname(ts_index, ts_label) && strcmp(p->iface->ts_label, ts_label)) { - strncpy(p->iface->ts_label, ts_label, MAX_IFNAME_SIZE); - p->link_status |= TS_LABEL_CHANGED; - pr_notice("port %hu: ts label changed to %s", portnum(p), ts_label); - } - - /* Both link down/up and change ts_label may change phc index. */ - if (p->link_status & LINK_UP && - (p->link_status & LINK_STATE_CHANGED || p->link_status & TS_LABEL_CHANGED)) { - sk_get_ts_info(p->iface->ts_label, &p->iface->ts_info); - - /* Only switch phc with HW time stamping mode */ - if (p->iface->ts_info.valid && p->iface->ts_info.phc_index >= 0) { - required_modes = clock_required_modes(p->clock); - if ((p->iface->ts_info.so_timestamping & required_modes) != required_modes) { - pr_err("interface '%s' does not support requested " - "timestamping mode, set link status down by force.", - p->iface->ts_label); - p->link_status = LINK_DOWN | LINK_STATE_CHANGED; - } else if (p->phc_index != p->iface->ts_info.phc_index) { - p->phc_index = p->iface->ts_info.phc_index; - - if (clock_switch_phc(p->clock, p->phc_index)) { - p->last_fault_type = FT_SWITCH_PHC; - port_dispatch(p, EV_FAULT_DETECTED, 0); - return; - } - clock_sync_interval(p->clock, p->log_sync_interval); - } - } - } - - /* - * A port going down can affect the BMCA result. - * Force a state decision event. - */ - if (p->link_status & LINK_DOWN) - clock_set_sde(p->clock, 1); -} - -enum fsm_event port_event(struct port *p, int fd_index) -{ - return p->event(p, fd_index); -} - -static enum fsm_event bc_event(struct port *p, int fd_index) -{ - enum fsm_event event = EV_NONE; - struct ptp_message *msg; - int cnt, fd = p->fda.fd[fd_index], err; - - switch (fd_index) { - case FD_ANNOUNCE_TIMER: - case FD_SYNC_RX_TIMER: - pr_debug("port %hu: %s timeout", portnum(p), - fd_index == FD_SYNC_RX_TIMER ? "rx sync" : "announce"); - if (p->best) - fc_clear(p->best); - port_set_announce_tmo(p); - delay_req_prune(p); - if (clock_slave_only(p->clock) && p->delayMechanism != DM_P2P && - port_renew_transport(p)) { - return EV_FAULT_DETECTED; - } - return EV_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES; - - case FD_DELAY_TIMER: - pr_debug("port %hu: delay timeout", portnum(p)); - port_set_delay_tmo(p); - delay_req_prune(p); - return port_delay_request(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_QUALIFICATION_TIMER: - pr_debug("port %hu: qualification timeout", portnum(p)); - return EV_QUALIFICATION_TIMEOUT_EXPIRES; - - case FD_MANNO_TIMER: - pr_debug("port %hu: master tx announce timeout", portnum(p)); - port_set_manno_tmo(p); - return port_tx_announce(p, NULL) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_SYNC_TX_TIMER: - pr_debug("port %hu: master sync timeout", portnum(p)); - port_set_sync_tx_tmo(p); - return port_tx_sync(p, NULL) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_UNICAST_SRV_TIMER: - pr_debug("port %hu: unicast service timeout", portnum(p)); - return unicast_service_timer(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_UNICAST_REQ_TIMER: - pr_debug("port %hu: unicast request timeout", portnum(p)); - return unicast_client_timer(p) ? EV_FAULT_DETECTED : EV_NONE; - - case FD_RTNL: - pr_debug("port %hu: received link status notification", portnum(p)); - rtnl_link_status(fd, p->name, port_link_status, p); - if (p->link_status == (LINK_UP | LINK_STATE_CHANGED)) - return EV_FAULT_CLEARED; - else if ((p->link_status == (LINK_DOWN | LINK_STATE_CHANGED)) || - (p->link_status & TS_LABEL_CHANGED)) - return EV_FAULT_DETECTED; - else - return EV_NONE; - } - - msg = msg_allocate(); - if (!msg) - return EV_FAULT_DETECTED; - - msg->hwts.type = p->timestamping; - - cnt = transport_recv(p->trp, fd, msg); - if (cnt <= 0) { - pr_err("port %hu: recv message failed", portnum(p)); - msg_put(msg); - return EV_FAULT_DETECTED; - } - err = msg_post_recv(msg, cnt); - if (err) { - switch (err) { - case -EBADMSG: - pr_err("port %hu: bad message", portnum(p)); - break; - case -EPROTO: - pr_debug("port %hu: ignoring message", portnum(p)); - break; - } - msg_put(msg); - return EV_NONE; - } - if (port_ignore(p, msg)) { - msg_put(msg); - return EV_NONE; - } - if (msg_sots_missing(msg) && - !(p->timestamping == TS_P2P1STEP && msg_type(msg) == PDELAY_REQ)) { - pr_err("port %hu: received %s without timestamp", - portnum(p), msg_type_string(msg_type(msg))); - msg_put(msg); - return EV_NONE; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, -p->rx_timestamp_offset); - clock_check_ts(p->clock, tmv_to_nanoseconds(msg->hwts.ts)); - } - - switch (msg_type(msg)) { - case SYNC: - process_sync(p, msg); - break; - case DELAY_REQ: - if (process_delay_req(p, msg)) - event = EV_FAULT_DETECTED; - break; - case PDELAY_REQ: - if (process_pdelay_req(p, msg)) - event = EV_FAULT_DETECTED; - break; - case PDELAY_RESP: - if (process_pdelay_resp(p, msg)) - event = EV_FAULT_DETECTED; - break; - case FOLLOW_UP: - process_follow_up(p, msg); - break; - case DELAY_RESP: - process_delay_resp(p, msg); - break; - case PDELAY_RESP_FOLLOW_UP: - process_pdelay_resp_fup(p, msg); - break; - case ANNOUNCE: - if (process_announce(p, msg)) - event = EV_STATE_DECISION_EVENT; - break; - case SIGNALING: - if (process_signaling(p, msg)) { - event = EV_FAULT_DETECTED; - } - break; - case MANAGEMENT: - if (clock_manage(p->clock, p, msg)) - event = EV_STATE_DECISION_EVENT; - break; - } - - msg_put(msg); - return event; -} - -int port_forward(struct port *p, struct ptp_message *msg) -{ - int cnt; - cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, msg); - return cnt <= 0 ? -1 : 0; -} - -int port_forward_to(struct port *p, struct ptp_message *msg) -{ - int cnt; - cnt = transport_sendto(p->trp, &p->fda, TRANS_GENERAL, msg); - return cnt <= 0 ? -1 : 0; -} - -int port_prepare_and_send(struct port *p, struct ptp_message *msg, - enum transport_event event) -{ - int cnt; - - if (msg_pre_send(msg)) { - return -1; - } - if (msg_unicast(msg)) { - cnt = transport_sendto(p->trp, &p->fda, event, msg); - } else { - cnt = transport_send(p->trp, &p->fda, event, msg); - } - if (cnt <= 0) { - return -1; - } - if (msg_sots_valid(msg)) { - ts_add(&msg->hwts.ts, p->tx_timestamp_offset); - } - return 0; -} - -struct PortIdentity port_identity(struct port *p) -{ - return p->portIdentity; -} - -int port_number(struct port *p) -{ - return portnum(p); -} - -int port_link_status_get(struct port *p) -{ - return !!(p->link_status & LINK_UP); -} - -int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg) -{ - struct management_tlv *mgt; - UInteger16 target = msg->management.targetPortIdentity.portNumber; - - if (target != portnum(p) && target != 0xffff) { - return 0; - } - mgt = (struct management_tlv *) msg->management.suffix; - - switch (management_action(msg)) { - case GET: - if (port_management_get_response(p, ingress, mgt->id, msg)) - return 1; - break; - case SET: - if (port_management_set(p, ingress, mgt->id, msg)) - return 1; - break; - case COMMAND: - break; - default: - return -1; - } - - switch (mgt->id) { - case TLV_NULL_MANAGEMENT: - case TLV_CLOCK_DESCRIPTION: - case TLV_PORT_DATA_SET: - case TLV_LOG_ANNOUNCE_INTERVAL: - case TLV_ANNOUNCE_RECEIPT_TIMEOUT: - case TLV_LOG_SYNC_INTERVAL: - case TLV_VERSION_NUMBER: - case TLV_ENABLE_PORT: - case TLV_DISABLE_PORT: - case TLV_UNICAST_NEGOTIATION_ENABLE: - case TLV_UNICAST_MASTER_TABLE: - case TLV_UNICAST_MASTER_MAX_TABLE_SIZE: - case TLV_ACCEPTABLE_MASTER_TABLE_ENABLED: - case TLV_ALTERNATE_MASTER: - case TLV_TRANSPARENT_CLOCK_PORT_DATA_SET: - case TLV_DELAY_MECHANISM: - case TLV_LOG_MIN_PDELAY_REQ_INTERVAL: - port_management_send_error(p, ingress, msg, TLV_NOT_SUPPORTED); - break; - default: - port_management_send_error(p, ingress, msg, TLV_NO_SUCH_ID); - return -1; - } - return 1; -} - -int port_management_error(struct PortIdentity pid, struct port *ingress, - struct ptp_message *req, Enumeration16 error_id) -{ - struct management_error_status *mes; - struct management_tlv *mgt; - struct ptp_message *msg; - struct tlv_extra *extra; - int err = 0; - - mgt = (struct management_tlv *) req->management.suffix; - msg = port_management_reply(pid, ingress, req); - if (!msg) { - return -1; - } - - extra = msg_tlv_append(msg, sizeof(*mes)); - if (!extra) { - msg_put(msg); - return -ENOMEM; - } - mes = (struct management_error_status *) extra->tlv; - mes->type = TLV_MANAGEMENT_ERROR_STATUS; - mes->length = 8; - mes->error = error_id; - mes->id = mgt->id; - - err = port_prepare_and_send(ingress, msg, TRANS_GENERAL); - msg_put(msg); - return err; -} - -static struct ptp_message * -port_management_construct(struct PortIdentity pid, struct port *ingress, - UInteger16 sequenceId, - struct PortIdentity *targetPortIdentity, - UInteger8 boundaryHops, uint8_t action) -{ - struct ptp_message *msg; - - msg = msg_allocate(); - if (!msg) - return NULL; - - msg->hwts.type = ingress->timestamping; - - msg->header.tsmt = MANAGEMENT | ingress->transportSpecific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = sizeof(struct management_msg); - msg->header.domainNumber = clock_domain_number(ingress->clock); - msg->header.sourcePortIdentity = pid; - msg->header.sequenceId = sequenceId; - msg->header.control = CTL_MANAGEMENT; - msg->header.logMessageInterval = 0x7f; - - if (targetPortIdentity) - msg->management.targetPortIdentity = *targetPortIdentity; - msg->management.startingBoundaryHops = boundaryHops; - msg->management.boundaryHops = boundaryHops; - - switch (action) { - case GET: case SET: - msg->management.flags = RESPONSE; - break; - case COMMAND: - msg->management.flags = ACKNOWLEDGE; - break; - } - return msg; -} - -struct ptp_message *port_management_reply(struct PortIdentity pid, - struct port *ingress, - struct ptp_message *req) -{ - UInteger8 boundaryHops; - - boundaryHops = req->management.startingBoundaryHops - - req->management.boundaryHops; - return port_management_construct(pid, ingress, - req->header.sequenceId, - &req->header.sourcePortIdentity, - boundaryHops, - management_action(req)); -} - -struct ptp_message *port_management_notify(struct PortIdentity pid, - struct port *port) -{ - return port_management_construct(pid, port, 0, NULL, 1, GET); -} - -void port_notify_event(struct port *p, enum notification event) -{ - struct PortIdentity pid = port_identity(p); - struct ptp_message *msg; - int id; - - switch (event) { - case NOTIFY_PORT_STATE: - id = TLV_PORT_DATA_SET; - break; - default: - return; - } - /* targetPortIdentity and sequenceId will be filled by - * clock_send_notification */ - msg = port_management_notify(pid, p); - if (!msg) - return; - if (!port_management_fill_response(p, msg, id)) - goto err; - if (msg_pre_send(msg)) - goto err; - clock_send_notification(p->clock, msg, event); -err: - msg_put(msg); -} - -struct port *port_open(int phc_index, - enum timestamp_type timestamping, - int number, - struct interface *interface, - struct clock *clock) -{ - enum clock_type type = clock_type(clock); - struct config *cfg = clock_config(clock); - struct port *p = malloc(sizeof(*p)); - enum transport_type transport; - int i; - - if (!p) { - return NULL; - } - - memset(p, 0, sizeof(*p)); - TAILQ_INIT(&p->tc_transmitted); - - switch (type) { - case CLOCK_TYPE_ORDINARY: - case CLOCK_TYPE_BOUNDARY: - p->dispatch = bc_dispatch; - p->event = bc_event; - break; - case CLOCK_TYPE_P2P: - p->dispatch = p2p_dispatch; - p->event = p2p_event; - break; - case CLOCK_TYPE_E2E: - p->dispatch = e2e_dispatch; - p->event = e2e_event; - break; - case CLOCK_TYPE_MANAGEMENT: - goto err_port; - } - - p->state_machine = clock_slave_only(clock) ? ptp_slave_fsm : ptp_fsm; - p->phc_index = phc_index; - p->jbod = config_get_int(cfg, interface->name, "boundary_clock_jbod"); - transport = config_get_int(cfg, interface->name, "network_transport"); - - if (transport == TRANS_UDS) { - ; /* UDS cannot have a PHC. */ - } else if (!interface->ts_info.valid) { - pr_warning("port %d: get_ts_info not supported", number); - } else if (phc_index >= 0 && phc_index != interface->ts_info.phc_index) { - if (p->jbod) { - pr_warning("port %d: just a bunch of devices", number); - p->phc_index = interface->ts_info.phc_index; - } else { - pr_err("port %d: PHC device mismatch", number); - pr_err("port %d: /dev/ptp%d requested, ptp%d attached", - number, phc_index, interface->ts_info.phc_index); - goto err_port; - } - } - - p->name = interface->name; - p->iface = interface; - p->asymmetry = config_get_int(cfg, p->name, "delayAsymmetry"); - p->asymmetry <<= 16; - p->announce_span = transport == TRANS_UDS ? 0 : ANNOUNCE_SPAN; - p->follow_up_info = config_get_int(cfg, p->name, "follow_up_info"); - p->freq_est_interval = config_get_int(cfg, p->name, "freq_est_interval"); - p->net_sync_monitor = config_get_int(cfg, p->name, "net_sync_monitor"); - p->path_trace_enabled = config_get_int(cfg, p->name, "path_trace_enabled"); - p->tc_spanning_tree = config_get_int(cfg, p->name, "tc_spanning_tree"); - p->rx_timestamp_offset = config_get_int(cfg, p->name, "ingressLatency"); - p->rx_timestamp_offset <<= 16; - p->tx_timestamp_offset = config_get_int(cfg, p->name, "egressLatency"); - p->tx_timestamp_offset <<= 16; - p->link_status = LINK_UP; - p->clock = clock; - p->trp = transport_create(cfg, transport); - if (!p->trp) { - goto err_port; - } - p->timestamping = timestamping; - p->portIdentity.clockIdentity = clock_identity(clock); - p->portIdentity.portNumber = number; - p->state = PS_INITIALIZING; - p->delayMechanism = config_get_int(cfg, p->name, "delay_mechanism"); - p->versionNumber = PTP_VERSION; - - if (number && unicast_client_claim_table(p)) { - goto err_port; - } - if (unicast_client_enabled(p) && - config_set_section_int(cfg, p->name, "hybrid_e2e", 1)) { - goto err_port; - } - if (number && unicast_service_initialize(p)) { - goto err_port; - } - p->hybrid_e2e = config_get_int(cfg, p->name, "hybrid_e2e"); - - if (number && type == CLOCK_TYPE_P2P && p->delayMechanism != DM_P2P) { - pr_err("port %d: P2P TC needs P2P ports", number); - goto err_port; - } - if (number && type == CLOCK_TYPE_E2E && p->delayMechanism != DM_E2E) { - pr_err("port %d: E2E TC needs E2E ports", number); - goto err_port; - } - if (p->hybrid_e2e && p->delayMechanism != DM_E2E) { - pr_warning("port %d: hybrid_e2e only works with E2E", number); - } - if (p->net_sync_monitor && !p->hybrid_e2e) { - pr_warning("port %d: net_sync_monitor needs hybrid_e2e", number); - } - - /* Set fault timeouts to a default value */ - for (i = 0; i < FT_CNT; i++) { - p->flt_interval_pertype[i].type = FTMO_LOG2_SECONDS; - p->flt_interval_pertype[i].val = 4; - } - p->flt_interval_pertype[FT_BAD_PEER_NETWORK].type = FTMO_LINEAR_SECONDS; - p->flt_interval_pertype[FT_BAD_PEER_NETWORK].val = - config_get_int(cfg, p->name, "fault_badpeernet_interval"); - - p->flt_interval_pertype[FT_UNSPECIFIED].val = - config_get_int(cfg, p->name, "fault_reset_interval"); - - p->tsproc = tsproc_create(config_get_int(cfg, p->name, "tsproc_mode"), - config_get_int(cfg, p->name, "delay_filter"), - config_get_int(cfg, p->name, "delay_filter_length")); - if (!p->tsproc) { - pr_err("Failed to create time stamp processor"); - goto err_transport; - } - p->nrate.ratio = 1.0; - - port_clear_fda(p, N_POLLFD); - 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_tsproc; - } - } - return p; - -err_tsproc: - tsproc_destroy(p->tsproc); -err_transport: - transport_destroy(p->trp); -err_port: - free(p); - return NULL; -} - -enum port_state port_state(struct port *port) -{ - return port->state; -} - -int port_state_update(struct port *p, enum fsm_event event, int mdiff) -{ - enum port_state next = p->state_machine(p->state, event, mdiff); - - if (PS_FAULTY == next) { - struct fault_interval i; - fault_interval(p, last_fault_type(p), &i); - if (clear_fault_asap(&i)) { - pr_notice("port %hu: clearing fault immediately", portnum(p)); - next = p->state_machine(next, EV_FAULT_CLEARED, 0); - } - } - - if (PS_INITIALIZING == next) { - /* - * This is a special case. Since we initialize the - * port immediately, we can skip right to listening - * state if all goes well. - */ - if (port_is_enabled(p)) { - port_disable(p); - } - if (port_initialize(p)) { - event = EV_FAULT_DETECTED; - } else { - event = EV_INIT_COMPLETE; - } - next = p->state_machine(next, event, 0); - } - - if (next != p->state) { - port_show_transition(p, next, event); - p->state = next; - port_notify_event(p, NOTIFY_PORT_STATE); - unicast_client_state_changed(p); - return 1; - } - - return 0; -} diff --git a/ptp4l.8 b/ptp4l.8 index 39bf36e..10c5c2f 100644 --- a/ptp4l.8 +++ b/ptp4l.8 @@ -661,15 +661,6 @@ The time source is a single byte code that gives an idea of the kind of local clock in use. The value is purely informational, having no effect on the outcome of the Best Master Clock algorithm, and is advertised when the clock becomes grand master. -.TP -.B hwts_filter -Select the hardware time stamp filter setting mode. -Possible values are normal, check, full. -Normal mode set the filters as needed. -Check mode only check but do not set. -Full mode set the receive filter to mark all packets with hardware time stamp, - so all applications can get them. -The default is normal. .SH UNICAST DISCOVERY OPTIONS diff --git a/ptp4l.8.hwtsfilter b/ptp4l.8.hwtsfilter deleted file mode 100644 index 10c5c2f..0000000 --- a/ptp4l.8.hwtsfilter +++ /dev/null @@ -1,708 +0,0 @@ -.TH PTP4l 8 "April 2018" "linuxptp" -.SH NAME -ptp4l - PTP Boundary/Ordinary/Transparent Clock - -.SH SYNOPSIS -.B ptp4l -[ -.B \-AEP246HSLmqsv -] [ -.BI \-f " config" -] [ -.BI \-p " phc-device" -] [ -.BI \-l " print-level" -] -[ -.BI \-i " interface" -] [ -.I long-options -] -.I .\|.\|. - -.SH DESCRIPTION -.B ptp4l -is an implementation of the Precision Time Protocol (PTP) according to IEEE -standard 1588 for Linux. It implements Boundary Clock (BC), Ordinary Clock -(OC), and Transparent Clock (TC). - -.SH OPTIONS -.TP -.B \-A -Select the delay mechanism automatically. Start with E2E and switch to P2P when -a peer delay request is received. -.TP -.B \-E -Select the delay request-response (E2E) mechanism. This is the default -mechanism. All clocks on single PTP communication path must use the same -mechanism. A warning will be printed when a peer delay request is received on -port using the E2E mechanism. -.TP -.B \-P -Select the peer delay (P2P) mechanism. A warning will be printed when a delay -request is received on port using the P2P mechanism. -.TP -.B \-2 -Select the IEEE 802.3 network transport. -.TP -.B \-4 -Select the UDP IPv4 network transport. This is the default transport. -.TP -.B \-6 -Select the UDP IPv6 network transport. -.TP -.B \-H -Select the hardware time stamping. All ports specified by the -.B \-i -option and in the configuration file must be attached to the same PTP hardware -clock (PHC). This is the default time stamping. -.TP -.B \-S -Select the software time stamping. -.TP -.B \-L -Select the legacy hardware time stamping. -.TP -.BI \-f " config" -Read configuration from the specified file. No configuration file is read by -default. -.TP -.BI \-i " interface" -Specify a PTP port, it may be used multiple times. At least one port must be -specified by this option or in the configuration file. -.TP -.BI \-p " phc-device" -(This option is deprecated.) -Before Linux kernel v3.5 there was no way to discover the PHC device -associated with a network interface. This option specifies the PHC -device (e.g. /dev/ptp0) to be used when running on legacy kernels. -.TP -.B \-s -Enable the slaveOnly mode. -.TP -.BI \-l " print-level" -Set the maximum syslog level of messages which should be printed or sent to -the system logger. The default is 6 (LOG_INFO). -.TP -.B \-m -Print messages to the standard output. -.TP -.B \-q -Don't send messages to the system logger. -.TP -.B \-v -Prints the software version and exits. -.TP -.BI \-h -Display a help message. - -.SH LONG OPTIONS - -Each and every configuration file option (see below) may also appear -as a "long" style command line argument. For example, the slaveOnly -option may be set using either of these two forms. - -.RS -\f(CW\-\-slaveOnly 1 \-\-slaveOnly=1\fP -.RE - -Option values given on the command line override values in the global -section of the configuration file. - -.SH CONFIGURATION FILE - -The configuration file is divided into sections. Each section starts with a -line containing its name enclosed in brackets and it follows with settings. -Each setting is placed on a separate line, it contains the name of the -option and the value separated by whitespace characters. Empty lines and lines -starting with # are ignored. - -There are three different section types. - -.TP -.B 1. -The global section (indicated as -.BR [global] ) -sets the program options, clock options and default port options. Other -sections are port specific sections and they override the default port options. -.TP -.B 2. -Port sections give the name of the configured port (e.g. -.BR [eth0] ). -Ports specified in the configuration file don't need to be -specified by the -.B \-i -option. An empty port section can be used to replace the command line option. -.TP -.B 3. -Tables for configuring unicast discovery begin with -.B \%[unicast_master_table]. - -See UNICAST DISCOVERY OPTIONS, below. - -.SH PORT OPTIONS - -.TP -.B delayAsymmetry -The time difference in nanoseconds of the transmit and receive -paths. This value should be positive when the master-to-slave -propagation time is longer and negative when the slave-to-master time -is longer. The default is 0 nanoseconds. -.TP -.B logAnnounceInterval -The mean time interval between Announce messages. A shorter interval makes -ptp4l react faster to the changes in the master-slave hierarchy. The interval -should be the same in the whole domain. It's specified as a power of two in -seconds. -The default is 1 (2 seconds). -.TP -.B logSyncInterval -The mean time interval between Sync messages. A shorter interval may improve -accuracy of the local clock. It's specified as a power of two in seconds. -The default is 0 (1 second). -.TP -.B logMinDelayReqInterval -The minimum permitted mean time interval between Delay_Req messages. A shorter -interval makes ptp4l react faster to the changes in the path delay. It's -specified as a power of two in seconds. -The default is 0 (1 second). -.TP -.B logMinPdelayReqInterval -The minimum permitted mean time interval between Pdelay_Req messages. It's -specified as a power of two in seconds. -The default is 0 (1 second). -.TP -.B announceReceiptTimeout -The number of missed Announce messages before the last Announce messages -expires. -The default is 3. -.TP -.B syncReceiptTimeout -The number of sync/follow up messages that may go missing before -triggering a Best Master Clock election. This option is used for -running in gPTP mode according to the 802.1AS-2011 standard. Setting -this option to zero will disable the sync message timeout. -The default is 0 or disabled. -.TP -.B transportSpecific -The transport specific field. Must be in the range 0 to 255. -The default is 0. -.TP -.B ignore_transport_specific -By default, incoming messages are dropped if their transportSpecific -field does not match the configured value. However, many of -transports specified in the 1588 standard mandate ignoring this field. -Moreover, some equipment is known to set the reserved bits. -Configuring this option as 1 causes this field to be ignored -completely on receive. The default is 0. -.TP -.B path_trace_enabled -Enable the mechanism used to trace the route of the Announce messages. -The default is 0 (disabled). -.TP -.B follow_up_info -Include the 802.1AS data in the Follow_Up messages if enabled. -The default is 0 (disabled). -.TP -.B fault_reset_interval -The time in seconds between the detection of a port's fault and the fault -being reset. This value is expressed as a power of two. Setting this -value to \-128 or to the special key word "ASAP" will let the fault be -reset immediately. -The default is 4 (16 seconds). -.TP -.B fault_badpeernet_interval -The time in seconds between the detection of a peer network misconfiguration -and the fault being reset. The port is disabled for the duration of the -interval. The value is in seconds and the special key word ASAP will let -the fault be reset immediately. -The default is 16 seconds. -.TP -.B delay_mechanism -Select the delay mechanism. Possible values are E2E, P2P and Auto. -The default is E2E. -.TP -.B hybrid_e2e -Enables the "hybrid" delay mechanism from the draft Enterprise -Profile. When enabled, ports in the slave state send their delay -request messages to the unicast address taken from the master's -announce message. Ports in the master state will reply to unicast -delay requests using unicast delay responses. This option has no -effect if the delay_mechanism is set to P2P. -The default is 0 (disabled). -.TP -.B inhibit_multicast_service -Some unicast mode profiles insist that no multicast message are ever -transmitted. Setting this option inhibits multicast transmission. -The default is 0 (mutlicast enabled). -.TP -.B net_sync_monitor -Enables the NetSync Monitor (NSM) protocol. The NSM protocol allows a -station to measure how well another node is synchronized. The monitor -sends a unicast delay request to the node, which replies -unconditionally with unicast delay response, sync, and follow up -messages. If the monitor is synchronized to the GM, it can use the -time stamps in the message to estimate the node's offset. This option -requires that the 'hybrid_e2e' option be enabled as well. -The default is 0 (disabled). -.TP -.B unicast_listen -When enabled, this option allows the port to grant unicast message -contracts. Incoming requests for will be granted limited only by the -amount of memory available. -The default is 0 (disabled). -.TP -.B unicast_master_table -When set to a positive integer, this option specifies the table id to -be used for unicast discovery. Each table lives in its own section -and has a unique, positive numerical ID. Entries in the table are a -pair of transport type and protocol address. Tables may not be shared -between ports, but nothing prevents table entries from appearing in -more than table. -The default is 0 (unicast discovery disabled). -.TP -.B unicast_req_duration -The service time in seconds to be requested during unicast discovery. -Note that the remote node is free to grant a different duration. -The default is 3600 seconds or one hour. -.TP -.B ptp_dst_mac -The MAC address to which PTP messages should be sent. -Relevant only with L2 transport. The default is 01:1B:19:00:00:00. -.TP -.B p2p_dst_mac -The MAC address to which peer delay messages should be sent. -Relevant only with L2 transport. The default is 01:80:C2:00:00:0E. -.TP -.B network_transport -Select the network transport. Possible values are UDPv4, UDPv6 and L2. -The default is UDPv4. -.TP -.B neighborPropDelayThresh -Upper limit for peer delay in nanoseconds. If the estimated peer delay is -greater than this value the port is marked as not 802.1AS capable. -.TP -.B masterOnly -Setting this option to one (1) prevents the port from entering the -SLAVE state. In addition, the local clock will ignore Announce -messages received on this port. This option's intended use is to -support the Telecom Profiles according to ITU-T G.8265.1, G.8275.1, -and G.8275.2. The default value is zero or false. -.TP -.B G.8275.portDS.localPriority -The Telecom Profiles (ITU-T G.8275.1 and G.8275.2) specify an -alternate Best Master Clock Algorithm (BMCA) with a unique data set -comparison algorithm. The value of this option is associated with -Announce messages arriving on a particular port and is used as a tie -breaker whenever clockClass, clockAccuracy, offsetScaledLogVariance, -and priority2 are equal. This option is only used when -"dataset_comparison" is set to "telecom". -The default value is 128. - -Warning: the BMCA is guaranteed to produce a spanning tree (that is, a -timing network without loops) only when using the default values of -G.8275.defaultDS.localPriority and G.8275.portDS.localPriority. -Careful network engineering is needed when using non-default values. -.TP -.B min_neighbor_prop_delay -Lower limit for peer delay in nanoseconds. If the estimated peer delay is -smaller than this value the port is marked as not 802.1AS capable. -.TP -.B tsproc_mode -Select the time stamp processing mode used to calculate offset and delay. -Possible values are filter, raw, filter_weight, raw_weight. Raw modes perform -well when the rate of sync messages (logSyncInterval) is similar to the rate of -delay messages (logMinDelayReqInterval or logMinPdelayReqInterval). Weighting -is useful with larger network jitters (e.g. software time stamping). -The default is filter. -.TP -.B delay_filter -Select the algorithm used to filter the measured delay and peer delay. Possible -values are moving_average and moving_median. -The default is moving_median. -.TP -.B delay_filter_length -The length of the delay filter in samples. -The default is 10. -.TP -.B egressLatency -Specifies the difference in nanoseconds between the actual transmission -time at the reference plane and the reported transmit time stamp. This -value will be added to egress time stamps obtained from the hardware. -The default is 0. -.TP -.B ingressLatency -Specifies the difference in nanoseconds between the reported receive -time stamp and the actual reception time at reference plane. This value -will be subtracted from ingress time stamps obtained from the hardware. -The default is 0. -.TP -.B boundary_clock_jbod -When running as a boundary clock (that is, when more than one network -interface is configured), ptp4l performs a sanity check to make sure -that all of the ports share the same hardware clock device. This -option allows ptp4l to work as a boundary clock using "just a bunch of -devices" that are not synchronized to each other. For this mode, the -collection of clocks must be synchronized by an external program, for -example phc2sys(8) in "automatic" mode. -The default is 0 (disabled). -.TP -.B udp_ttl -Specifies the Time to live (TTL) value for IPv4 multicast messages and the hop -limit for IPv6 multicast messages. This option is only relevant with the IPv4 -and IPv6 UDP transports. The default is 1 to restrict the messages sent by -.B ptp4l -to the same subnet. - -.SH PROGRAM AND CLOCK OPTIONS - -.TP -.B twoStepFlag -Enable two-step mode for sync messages. One-step mode can be used only with -hardware time stamping. -The default is 1 (enabled). -.TP -.B slaveOnly -The local clock is a slave-only clock if enabled. -This option is only for use with 1588 clocks and should not be enabled -for 802.1AS clocks. -The default is 0 (disabled). -.TP -.B gmCapable -If this option is enabled, then the local clock is able to become grand master. -This is only for use with 802.1AS clocks and has no effect on 1588 clocks. -The default is 1 (enabled). -.TP -.B priority1 -The priority1 attribute of the local clock. It is used in the best master -selection algorithm, lower values take precedence. Must be in the range 0 to -255. -The default is 128. -.TP -.B priority2 -The priority2 attribute of the local clock. It is used in the best master -selection algorithm, lower values take precedence. Must be in the range 0 to -255. -The default is 128. -.TP -.B clockClass -The clockClass attribute of the local clock. It denotes the traceability of the -time distributed by the grandmaster clock. -The default is 248. -.TP -.B clockAccuracy -The clockAccuracy attribute of the local clock. It is used in the best master -selection algorithm. -The default is 0xFE. -.TP -.B offsetScaledLogVariance -The offsetScaledLogVariance attribute of the local clock. It characterizes the -stability of the clock. -The default is 0xFFFF. -.TP -.B G.8275.defaultDS.localPriority -The Telecom Profiles (ITU-T G.8275.1 and G.8275.2) specify an -alternate Best Master Clock Algorithm (BMCA) with a unique data set -comparison algorithm. The value of this option is associated with the -local clock and is used as a tie breaker whenever clockClass, -clockAccuracy, offsetScaledLogVariance, and priority2 are equal. This -option is only used when "dataset_comparison" is set to "telecom". -The default value is 128. - -Warning: the BMCA is guaranteed to produce a spanning tree (that is, a -timing network without loops) only when using the default values of -G.8275.defaultDS.localPriority and G.8275.portDS.localPriority. -Careful network engineering is needed when using non-default values. -.TP -.B domainNumber -The domain attribute of the local clock. -The default is 0. -.TP -.B utc_offset -The current offset between TAI and UTC. -The default is 37. -.TP -.B free_running -Don't adjust the local clock if enabled. -The default is 0 (disabled). -.TP -.B freq_est_interval -The time interval over which is estimated the ratio of the local and -peer clock frequencies. It is specified as a power of two in seconds. -The default is 1 (2 seconds). -.TP -.B assume_two_step -Treat one-step responses as two-step if enabled. It is used to work around -buggy 802.1AS switches. -The default is 0 (disabled). -.TP -.B tc_spanning_tree -When running as a Transparent Clock, increment the "stepsRemoved" -field of Announce messages that pass through the switch. Enabling -this option ensures that PTP message loops never form, provided the -switches all implement this option together with the BMCA. -.TP -.B tx_timestamp_timeout -The number of milliseconds to poll waiting for the tx time stamp from the kernel -when a message has recently been sent. -The default is 1. -.TP -.B check_fup_sync -Because of packet reordering that can occur in the network, in the -hardware, or in the networking stack, a follow up message can appear -to arrive in the application before the matching sync message. As this -is a normal occurrence, and the sequenceID message field ensures -proper matching, the ptp4l program accepts out of order packets. This -option adds an additional check using the software time stamps from -the networking stack to verify that the sync message did arrive -first. This option is only useful if you do not trust the sequence IDs -generated by the master. -The default is 0 (disabled). -.TP -.B clock_servo -The servo which is used to synchronize the local clock. Valid values -are "pi" for a PI controller, "linreg" for an adaptive controller -using linear regression, "ntpshm" for the NTP SHM reference clock to -allow another process to synchronize the local clock (the SHM segment -number is set to the domain number), and "nullf" for a servo that -always dials frequency offset zero (for use in SyncE nodes). -The default is "pi." -.TP -.B clock_type -Specifies the kind of PTP clock. Valid values are "OC" for ordinary -clock, "BC" for boundary clock, "P2P_TC" for peer to peer transparent -clock, and "E2E_TC" for end to end transparent clock. An multi-port -ordinary clock will automatically be configured as a boundary clock. -The default is "OC". -.TP -.B pi_proportional_const -The proportional constant of the PI controller. When set to 0.0, the -proportional constant will be set by the following formula from the current -sync interval. -The default is 0.0. - -kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync) -.TP -.B pi_integral_const -The integral constant of the PI controller. When set to 0.0, the -integral constant will be set by the following formula from the current -sync interval. -The default is 0.0. - -ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) -.TP -.B pi_proportional_scale -The kp_scale constant in the formula used to set the proportional constant of -the PI controller from the sync interval. When set to 0.0, the value will be -selected from 0.7 and 0.1 for the hardware and software time stamping -respectively. -The default is 0.0. -.TP -.B pi_proportional_exponent -The kp_exponent constant in the formula used to set the proportional constant of -the PI controller from the sync interval. -The default is \-0.3. -.TP -.B pi_proportional_norm_max -The kp_norm_max constant in the formula used to set the proportional constant of -the PI controller from the sync interval. -The default is 0.7 -.TP -.B pi_integral_scale -The ki_scale constant in the formula used to set the integral constant of -the PI controller from the sync interval. When set to 0.0, the value will be -selected from 0.3 and 0.001 for the hardware and software time stamping -respectively. -The default is 0.0. -.TP -.B pi_integral_exponent -The ki_exponent constant in the formula used to set the integral constant of -the PI controller from the sync interval. -The default is 0.4. -.TP -.B pi_integral_norm_max -The ki_norm_max constant in the formula used to set the integral constant of -the PI controller from the sync interval. -The default is 0.3. -.TP -.B step_threshold -The maximum offset the servo will correct by changing the clock -frequency instead of stepping the clock. When set to 0.0, the servo will -never step the clock except on start. It's specified in seconds. -The default is 0.0. -This option used to be called -.BR pi_offset_const . -.TP -.B first_step_threshold -The maximum offset the servo will correct by changing the clock -frequency instead of stepping the clock. This is only applied on the first -update. It's specified in seconds. When set to 0.0, the servo won't step -the clock on start. -The default is 0.00002 (20 microseconds). -This option used to be called -.BR pi_f_offset_const . -.TP -.B max_frequency -The maximum allowed frequency adjustment of the clock in parts per billion -(ppb). This is an additional limit to the maximum allowed by the hardware. When -set to 0, the hardware limit will be used. -The default is 900000000 (90%). -This option used to be called -.BR pi_max_frequency . -.TP -.B sanity_freq_limit -The maximum allowed frequency offset between uncorrected clock and the system -monotonic clock in parts per billion (ppb). This is used as a sanity check of -the synchronized clock. When a larger offset is measured, a warning message -will be printed and the servo will be reset. When set to 0, the sanity check is -disabled. The default is 200000000 (20%). -.TP -.B initial_delay -The initial path delay of the clock in nanoseconds used for synchronization of -the clock before the delay is measured using the E2E or P2P delay mechanism. If -set to 0, the clock will not be updated until the delay is measured. -The default is 0. -.TP -.B ntpshm_segment -The number of the SHM segment used by ntpshm servo. -The default is 0. -.TP -.B udp6_scope -Specifies the desired scope for the IPv6 multicast messages. This -will be used as the second byte of the primary address. This option -is only relevant with IPv6 transport. See RFC 4291. The default is -0x0E for the global scope. -.TP -.B uds_address -Specifies the address of the UNIX domain socket for receiving local -management messages. The default is /var/run/ptp4l. -.TP -.B dscp_event -Defines the Differentiated Services Codepoint (DSCP) to be used for PTP -event messages. Must be a value between 0 and 63. There are several media -streaming standards out there that require specific values for this option. -For example 46 (EF PHB) in AES67 or 48 (CS6 PHB) in RAVENNA. The default -is 0. -.TP -.B dscp_general -Defines the Differentiated Services Codepoint (DSCP) to be used for PTP -general messages. Must be a value between 0 and 63. There are several media -streaming standards out there that recommend specific values for this option. -For example 34 (AF41 PHB) in AES67 or 46 (EF PHB) in RAVENNA. The default -is 0. -.TP -.B dataset_comparison -Specifies the method to be used when comparing data sets during the -Best Master Clock Algorithm. The possible values are "ieee1588" and -"G.8275.x". The default is "ieee1588". -.TP -.B logging_level -The maximum logging level of messages which should be printed. -The default is 6 (LOG_INFO). -.TP -.B message_tag -The tag which is added to all messages printed to the standard output or system -log. -The default is an empty string (which cannot be set in the configuration file -as the option requires an argument). -.TP -.B verbose -Print messages to the standard output if enabled. -The default is 0 (disabled). -.TP -.B use_syslog -Print messages to the system log if enabled. -The default is 1 (enabled). -.TP -.B summary_interval -The time interval in which are printed summary statistics of the clock. It is -specified as a power of two in seconds. The statistics include offset root mean -square (RMS), maximum absolute offset, frequency offset mean and standard -deviation, and path delay mean and standard deviation. The units are -nanoseconds and parts per billion (ppb). If there is only one clock update in -the interval, the sample will be printed instead of the statistics. The -messages are printed at the LOG_INFO level. -The default is 0 (1 second). -.TP -.B time_stamping -The time stamping method. The allowed values are hardware, software and legacy. -The default is hardware. -.TP -.B productDescription -The product description string. Allowed values must be of the form -manufacturerName;modelNumber;instanceIdentifier and contain at most 64 -utf8 symbols. The default is ";;". -.TP -.B revisionData -The revision description string which contains the revisions for node -hardware (HW), firmware (FW), and software (SW). Allowed values are of -the form HW;FW;SW and contain at most 32 utf8 symbols. The default is -an ";;". -.TP -.B userDescription -The user description string. Allowed values are of the form -name;location and contain at most 128 utf8 symbols. The default is an -empty string. -.TP -.B manufacturerIdentity -The manufacturer id which should be an OUI owned by the manufacturer. -The default is 00:00:00. -.TP -.B kernel_leap -When a leap second is announced, let the kernel apply it by stepping the clock -instead of correcting the one-second offset with servo, which would correct the -one-second offset slowly by changing the clock frequency (unless the -.B step_threshold -option is set to correct such offset by stepping). -Relevant only with software time stamping. The default is 1 (enabled). -.TP -.B timeSource -The time source is a single byte code that gives an idea of the kind -of local clock in use. The value is purely informational, having no -effect on the outcome of the Best Master Clock algorithm, and is -advertised when the clock becomes grand master. - -.SH UNICAST DISCOVERY OPTIONS - -.TP -.B table_id -Each table must begin with a unique, positive table ID. The port that -claims a given table does so by including the ID as the value of -its 'unicast_master_table' option. -.TP -.B logQueryInterval -This option configures the time to wait between unicast negotiation -attempts. It is specified as a power of two in seconds. -The default is 0 (1 second). -.TP -.B peer_address -This option specifies the unicast address of the peer for use with the -peer to peer delay mechanism. If specified, the port owning the table -will negotiate unicast peer delay responses from the machine at the -given remote address, otherwise the port will send multicast peer -delay requests. -.TP -.B L2|UDPv4 -Each table entry specifies the transport type and network address of a -potential remote master. If multiple masters are specified, then -unicast negotiation will be performed with each if them. - -.SH TIME SCALE USAGE - -.B ptp4l -as domain master either uses PTP or UTC time scale depending on time stamping -mode. In software and legacy time stamping modes it announces Arbitrary time -scale mode, which is effectively UTC here, in hardware time stamping mode it -announces use of PTP time scale. - -When -.B ptp4l -is the domain master using hardware time stamping, it is up to -.B phc2sys -to maintain the correct offset between UTC and PTP times. See -.BR phc2sys (8) -manual page for more details. - -.SH SEE ALSO -.BR pmc (8), -.BR phc2sys (8) diff --git a/ptp4l.c b/ptp4l.c index 3a9f084..9ef8169 100644 --- a/ptp4l.c +++ b/ptp4l.c @@ -191,7 +191,6 @@ int main(int argc, char *argv[]) assume_two_step = config_get_int(cfg, NULL, "assume_two_step"); sk_check_fupsync = config_get_int(cfg, NULL, "check_fup_sync"); sk_tx_timeout = config_get_int(cfg, NULL, "tx_timestamp_timeout"); - sk_hwts_filter_mode = config_get_int(cfg, NULL, "hwts_filter"); if (config_get_int(cfg, NULL, "clock_servo") == CLOCK_SERVO_NTPSHM) { config_set_int(cfg, "kernel_leap", 0); diff --git a/ptp4l.c.hwtsfilter b/ptp4l.c.hwtsfilter deleted file mode 100644 index 9ef8169..0000000 --- a/ptp4l.c.hwtsfilter +++ /dev/null @@ -1,260 +0,0 @@ -/** - * @file ptp4l.c - * @brief PTP Boundary Clock or Transparent Clock main program - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include - -#include "clock.h" -#include "config.h" -#include "ntpshm.h" -#include "pi.h" -#include "print.h" -#include "raw.h" -#include "sk.h" -#include "transport.h" -#include "udp6.h" -#include "uds.h" -#include "util.h" -#include "version.h" - -static void usage(char *progname) -{ - fprintf(stderr, - "\nusage: %s [options]\n\n" - " Delay Mechanism\n\n" - " -A Auto, starting with E2E\n" - " -E E2E, delay request-response (default)\n" - " -P P2P, peer delay mechanism\n\n" - " Network Transport\n\n" - " -2 IEEE 802.3\n" - " -4 UDP IPV4 (default)\n" - " -6 UDP IPV6\n\n" - " Time Stamping\n\n" - " -H HARDWARE (default)\n" - " -S SOFTWARE\n" - " -L LEGACY HW\n\n" - " Other Options\n\n" - " -f [file] read configuration from 'file'\n" - " -i [dev] interface device to use, for example 'eth0'\n" - " (may be specified multiple times)\n" - " -p [dev] PTP hardware clock device to use, default auto\n" - " (ignored for SOFTWARE/LEGACY HW time stamping)\n" - " -s slave only mode (overrides configuration file)\n" - " -t transparent clock\n" - " -l [num] set the logging level to 'num'\n" - " -m print messages to stdout\n" - " -q do not print messages to the syslog\n" - " -v prints the software version and exits\n" - " -h prints this message and exits\n" - "\n", - progname); -} - -int main(int argc, char *argv[]) -{ - char *config = NULL, *req_phc = NULL, *progname; - enum clock_type type = CLOCK_TYPE_ORDINARY; - int c, err = -1, index, print_level; - struct clock *clock = NULL; - struct option *opts; - struct config *cfg; - - if (handle_term_signals()) - return -1; - - cfg = config_create(); - if (!cfg) { - return -1; - } - opts = config_long_options(cfg); - - /* Process the command line arguments. */ - progname = strrchr(argv[0], '/'); - progname = progname ? 1+progname : argv[0]; - while (EOF != (c = getopt_long(argc, argv, "AEP246HSLf:i:p:sl:mqvh", - opts, &index))) { - switch (c) { - case 0: - if (config_parse_option(cfg, opts[index].name, optarg)) - goto out; - break; - case 'A': - if (config_set_int(cfg, "delay_mechanism", DM_AUTO)) - goto out; - break; - case 'E': - if (config_set_int(cfg, "delay_mechanism", DM_E2E)) - goto out; - break; - case 'P': - if (config_set_int(cfg, "delay_mechanism", DM_P2P)) - goto out; - break; - case '2': - if (config_set_int(cfg, "network_transport", - TRANS_IEEE_802_3)) - goto out; - break; - case '4': - if (config_set_int(cfg, "network_transport", - TRANS_UDP_IPV4)) - goto out; - break; - case '6': - if (config_set_int(cfg, "network_transport", - TRANS_UDP_IPV6)) - goto out; - break; - case 'H': - if (config_set_int(cfg, "time_stamping", TS_HARDWARE)) - goto out; - break; - case 'S': - if (config_set_int(cfg, "time_stamping", TS_SOFTWARE)) - goto out; - break; - case 'L': - if (config_set_int(cfg, "time_stamping", TS_LEGACY_HW)) - goto out; - break; - case 'f': - config = optarg; - break; - case 'i': - if (!config_create_interface(optarg, cfg)) - goto out; - break; - case 'p': - req_phc = optarg; - break; - case 's': - if (config_set_int(cfg, "slaveOnly", 1)) { - goto out; - } - break; - case 'l': - if (get_arg_val_i(c, optarg, &print_level, - PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) - goto out; - config_set_int(cfg, "logging_level", print_level); - break; - case 'm': - config_set_int(cfg, "verbose", 1); - break; - case 'q': - config_set_int(cfg, "use_syslog", 0); - break; - case 'v': - version_show(stdout); - return 0; - case 'h': - usage(progname); - return 0; - case '?': - usage(progname); - goto out; - default: - usage(progname); - goto out; - } - } - - if (config && (c = config_read(config, cfg))) { - return c; - } - - print_set_progname(progname); - print_set_tag(config_get_string(cfg, NULL, "message_tag")); - print_set_verbose(config_get_int(cfg, NULL, "verbose")); - print_set_syslog(config_get_int(cfg, NULL, "use_syslog")); - print_set_level(config_get_int(cfg, NULL, "logging_level")); - - assume_two_step = config_get_int(cfg, NULL, "assume_two_step"); - sk_check_fupsync = config_get_int(cfg, NULL, "check_fup_sync"); - sk_tx_timeout = config_get_int(cfg, NULL, "tx_timestamp_timeout"); - - if (config_get_int(cfg, NULL, "clock_servo") == CLOCK_SERVO_NTPSHM) { - config_set_int(cfg, "kernel_leap", 0); - config_set_int(cfg, "sanity_freq_limit", 0); - } - - if (STAILQ_EMPTY(&cfg->interfaces)) { - fprintf(stderr, "no interface specified\n"); - usage(progname); - goto out; - } - - type = config_get_int(cfg, NULL, "clock_type"); - switch (type) { - case CLOCK_TYPE_ORDINARY: - if (cfg->n_interfaces > 1) { - type = CLOCK_TYPE_BOUNDARY; - } - break; - case CLOCK_TYPE_BOUNDARY: - if (cfg->n_interfaces < 2) { - fprintf(stderr, "BC needs at least two interfaces\n"); - goto out; - } - break; - case CLOCK_TYPE_P2P: - if (cfg->n_interfaces < 2) { - fprintf(stderr, "TC needs at least two interfaces\n"); - goto out; - } - if (DM_P2P != config_get_int(cfg, NULL, "delay_mechanism")) { - fprintf(stderr, "P2P_TC needs P2P delay mechanism\n"); - goto out; - } - break; - case CLOCK_TYPE_E2E: - if (cfg->n_interfaces < 2) { - fprintf(stderr, "TC needs at least two interfaces\n"); - goto out; - } - if (DM_E2E != config_get_int(cfg, NULL, "delay_mechanism")) { - fprintf(stderr, "E2E_TC needs E2E delay mechanism\n"); - goto out; - } - break; - case CLOCK_TYPE_MANAGEMENT: - goto out; - } - - clock = clock_create(type, cfg, req_phc); - if (!clock) { - fprintf(stderr, "failed to create a clock\n"); - goto out; - } - - err = 0; - - while (is_running()) { - if (clock_poll(clock)) - break; - } -out: - if (clock) - clock_destroy(clock); - config_destroy(cfg); - return err; -} diff --git a/rtnl.c b/rtnl.c index 59ed0ec..f9a572b 100644 --- a/rtnl.c +++ b/rtnl.c @@ -20,8 +20,6 @@ #include /* Must come before linux/netlink.h on some systems. */ #include #include -#include -#include #include #include #include @@ -32,39 +30,8 @@ #include "print.h" #include "rtnl.h" -#define BUF_SIZE 4096 -#define GENLMSG_DATA(glh) ((void *)(NLMSG_DATA(glh) + GENL_HDRLEN)) - static int rtnl_len; static char *rtnl_buf; -static int get_team_active_iface(int master_index); - -static int nl_close(int fd) -{ - return close(fd); -} - -static int nl_open(int family) -{ - int fd; - struct sockaddr_nl sa; - - memset(&sa, 0, sizeof(sa)); - sa.nl_family = AF_NETLINK; - sa.nl_groups = RTNLGRP_LINK; - - fd = socket(AF_NETLINK, SOCK_RAW, family); - if (fd < 0) { - pr_err("failed to open netlink socket: %m"); - return -1; - } - if (bind(fd, (struct sockaddr *) &sa, sizeof(sa))) { - pr_err("failed to bind netlink socket: %m"); - close(fd); - return -1; - } - return fd; -} int rtnl_close(int fd) { @@ -73,12 +40,7 @@ int rtnl_close(int fd) rtnl_buf = NULL; rtnl_len = 0; } - return nl_close(fd); -} - -int rtnl_open(void) -{ - return nl_open(NETLINK_ROUTE); + return close(fd); } static void rtnl_get_ts_device_callback(void *ctx, int linkup, int ts_index) @@ -154,24 +116,14 @@ int rtnl_link_query(int fd, char *device) return 0; } -static inline __u8 rta_getattr_u8(struct rtattr *rta) -{ - return *(__u8 *)RTA_DATA(rta); -} - -static inline __u16 rta_getattr_u16(struct rtattr *rta) -{ - return *(__u16 *)RTA_DATA(rta); -} - -static inline __u32 rta_getattr_u32(struct rtattr *rta) +static inline __u32 rta_getattr_u32(const struct rtattr *rta) { return *(__u32 *)RTA_DATA(rta); } -static inline char *rta_getattr_str(struct rtattr *rta) +static inline const char *rta_getattr_str(const struct rtattr *rta) { - return (char *)RTA_DATA(rta); + return (const char *)RTA_DATA(rta); } static int rtnl_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta, int len) @@ -198,12 +150,12 @@ static inline int rtnl_nested_rtattr_parse(struct rtattr *tb[], int max, struct return rtnl_rtattr_parse(tb, max, RTA_DATA(rta), RTA_PAYLOAD(rta)); } -static int rtnl_linkinfo_parse(int master_index, struct rtattr *rta) +static int rtnl_linkinfo_parse(struct rtattr *rta) { + int index = -1; + const char *kind; struct rtattr *linkinfo[IFLA_INFO_MAX]; struct rtattr *bond[IFLA_BOND_MAX]; - int index = -1; - char *kind; if (rtnl_nested_rtattr_parse(linkinfo, IFLA_INFO_MAX, rta) < 0) return -1; @@ -220,8 +172,6 @@ static int rtnl_linkinfo_parse(int master_index, struct rtattr *rta) if (bond[IFLA_BOND_ACTIVE_SLAVE]) { index = rta_getattr_u32(bond[IFLA_BOND_ACTIVE_SLAVE]); } - } else if (kind && !strncmp(kind, "team", 4)) { - index = get_team_active_iface(master_index); } } return index; @@ -229,18 +179,18 @@ static int rtnl_linkinfo_parse(int master_index, struct rtattr *rta) int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx) { - struct rtattr *tb[IFLA_MAX+1]; - struct ifinfomsg *info = NULL; int index, len, link_up; - struct sockaddr_nl sa; int slave_index = -1; - struct nlmsghdr *nh; - struct msghdr msg; struct iovec iov; + struct sockaddr_nl sa; + struct msghdr msg; + struct nlmsghdr *nh; + struct ifinfomsg *info = NULL; + struct rtattr *tb[IFLA_MAX+1]; index = if_nametoindex(device); if (!rtnl_buf) { - rtnl_len = BUF_SIZE; + rtnl_len = 4096; rtnl_buf = malloc(rtnl_len); if (!rtnl_buf) { pr_err("rtnl: low memory"); @@ -296,7 +246,7 @@ int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx) IFLA_PAYLOAD(nh)); if (tb[IFLA_LINKINFO]) - slave_index = rtnl_linkinfo_parse(index, tb[IFLA_LINKINFO]); + slave_index = rtnl_linkinfo_parse(tb[IFLA_LINKINFO]); if (cb) cb(ctx, link_up, slave_index); @@ -305,163 +255,24 @@ int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx) return 0; } -static int genl_send_msg(int fd, int family_id, int genl_cmd, int genl_version, - int rta_type, void *rta_data, int rta_len) +int rtnl_open(void) { - struct sockaddr_nl daddr; - struct genlmsghdr *gnlh; - struct nlmsghdr *nlh; - struct rtattr *attr; - char msg[BUF_SIZE]; - - memset(&daddr, 0, sizeof(daddr)); - daddr.nl_family = AF_NETLINK; - - memset(&msg, 0, sizeof(msg)); - nlh = (struct nlmsghdr *) msg; - nlh->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN); - nlh->nlmsg_type = family_id; - nlh->nlmsg_flags = NLM_F_REQUEST; - - gnlh = (struct genlmsghdr *) NLMSG_DATA(nlh); - gnlh->cmd = genl_cmd; - gnlh->version = genl_version; - - if (rta_data && rta_len > 0) { - attr = (struct rtattr *) GENLMSG_DATA(msg); - attr->rta_type = rta_type; - attr->rta_len = RTA_LENGTH(rta_len); - nlh->nlmsg_len += NLMSG_ALIGN(attr->rta_len); - if (nlh->nlmsg_len < sizeof(msg)) - memcpy(RTA_DATA(attr), rta_data, rta_len); - else - return -1; - } + int fd; + struct sockaddr_nl sa; - return sendto(fd, &msg, nlh->nlmsg_len, 0, - (struct sockaddr *)&daddr, sizeof(daddr)); -} + memset(&sa, 0, sizeof(sa)); + sa.nl_family = AF_NETLINK; + sa.nl_groups = RTNLGRP_LINK; -static int genl_get_family_id(int fd, void *family_name) -{ - struct rtattr *tb[CTRL_ATTR_MAX+1]; - struct nlmsghdr *nlh; - struct rtattr *attr; - char msg[BUF_SIZE]; - int len, gf_id; - - len = genl_send_msg(fd, GENL_ID_CTRL, CTRL_CMD_GETFAMILY, 1, - CTRL_ATTR_FAMILY_NAME, family_name, - strlen(family_name) + 1); - if (len < 0) - return len; - - len = recv(fd, &msg, sizeof(msg), 0); - if (len < 0) - return len; - - nlh = (struct nlmsghdr *) msg; - if (nlh->nlmsg_type == NLMSG_ERROR || !NLMSG_OK(nlh, len)) + fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + if (fd < 0) { + pr_err("failed to open netlink socket: %m"); return -1; - - attr = (struct rtattr *) GENLMSG_DATA(msg); - rtnl_rtattr_parse(tb, CTRL_ATTR_MAX, attr, NLMSG_PAYLOAD(nlh, GENL_HDRLEN)); - - if (tb[CTRL_ATTR_FAMILY_ID]) - gf_id = rta_getattr_u16(tb[CTRL_ATTR_FAMILY_ID]); - else - gf_id = -1; - - return gf_id; -} - -static int parase_team_list_option(struct rtattr *attr) -{ - struct rtattr *tb[TEAM_ATTR_OPTION_MAX+1]; - int len = RTA_PAYLOAD(attr); - const char *optname = ""; - const char *mode = ""; - int active_index = -1; - - for (attr = RTA_DATA(attr); RTA_OK(attr, len); attr = RTA_NEXT(attr, len)) { - rtnl_nested_rtattr_parse(tb, TEAM_ATTR_OPTION_MAX, attr); - - if (tb[TEAM_ATTR_OPTION_NAME]) - optname = rta_getattr_str(tb[TEAM_ATTR_OPTION_NAME]); - - if (!strcmp(optname, "mode") && tb[TEAM_ATTR_OPTION_TYPE] && - rta_getattr_u8(tb[TEAM_ATTR_OPTION_TYPE]) == NLA_STRING) - mode = rta_getattr_str(tb[TEAM_ATTR_OPTION_DATA]); - - if (!strcmp(optname, "activeport") && tb[TEAM_ATTR_OPTION_TYPE] && - rta_getattr_u8(tb[TEAM_ATTR_OPTION_TYPE]) == NLA_U32) - active_index = rta_getattr_u32(tb[TEAM_ATTR_OPTION_DATA]); } - - if (strcmp(mode, "activebackup")) { - pr_err("team supported only in activebackup mode"); + if (bind(fd, (struct sockaddr *) &sa, sizeof(sa))) { + pr_err("failed to bind netlink socket: %m"); + close(fd); return -1; - } else { - return active_index; - } -} - -static int get_team_active_iface(int master_index) -{ - struct rtattr *tb[TEAM_ATTR_MAX+1]; - struct genlmsghdr *gnlh; - struct nlmsghdr *nlh; - char msg[BUF_SIZE]; - int fd, gf_id, len; - int index = -1; - - fd = nl_open(NETLINK_GENERIC); - if (fd < 0) - return fd; - - gf_id = genl_get_family_id(fd, TEAM_GENL_NAME); - if (gf_id < 0) { - pr_err("get genl family failed"); - goto no_info; } - - len = genl_send_msg(fd, gf_id, TEAM_CMD_OPTIONS_GET, - TEAM_GENL_VERSION, TEAM_ATTR_TEAM_IFINDEX, - &master_index, sizeof(master_index)); - if (len < 0) { - pr_err("send team info request failed: %m"); - goto no_info; - } - - len = recv(fd, msg, sizeof(msg), 0); - if (len < 0) { - pr_err("recv team info failed: %m"); - goto no_info; - } - - nlh = (struct nlmsghdr *) msg; - for ( ; NLMSG_OK(nlh, len); nlh = NLMSG_NEXT(nlh, len)) { - if (nlh->nlmsg_type != gf_id) - continue; - - gnlh = (struct genlmsghdr *) NLMSG_DATA(nlh); - if (gnlh->cmd != TEAM_CMD_OPTIONS_GET) - continue; - - rtnl_rtattr_parse(tb, TEAM_ATTR_MAX, (struct rtattr *)GENLMSG_DATA(msg), - NLMSG_PAYLOAD(nlh, GENL_HDRLEN)); - - if (tb[TEAM_ATTR_TEAM_IFINDEX] && - master_index != rta_getattr_u32(tb[TEAM_ATTR_TEAM_IFINDEX])) - continue; - - if (tb[TEAM_ATTR_LIST_OPTION]) { - index = parase_team_list_option(tb[TEAM_ATTR_LIST_OPTION]); - break; - } - } - -no_info: - nl_close(fd); - return index; + return fd; } diff --git a/rtnl.c.team b/rtnl.c.team deleted file mode 100644 index f9a572b..0000000 --- a/rtnl.c.team +++ /dev/null @@ -1,278 +0,0 @@ -/** - * @file rtnl.c - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include /* Must come before linux/netlink.h on some systems. */ -#include -#include -#include -#include -#include -#include -#include - -#include "missing.h" -#include "print.h" -#include "rtnl.h" - -static int rtnl_len; -static char *rtnl_buf; - -int rtnl_close(int fd) -{ - if (rtnl_buf) { - free(rtnl_buf); - rtnl_buf = NULL; - rtnl_len = 0; - } - return close(fd); -} - -static void rtnl_get_ts_device_callback(void *ctx, int linkup, int ts_index) -{ - int *dst = ctx; - *dst = ts_index; -} - -int rtnl_get_ts_device(char *device, char *ts_device) -{ - int err, fd; - int ts_index = -1; - - fd = rtnl_open(); - if (fd < 0) - return fd; - - err = rtnl_link_query(fd, device); - if (err) { - goto no_info; - } - - rtnl_link_status(fd, device, rtnl_get_ts_device_callback, &ts_index); - if (ts_index > 0 && if_indextoname(ts_index, ts_device)) - err = 0; - else - err = -1; - -no_info: - rtnl_close(fd); - return err; -} - -int rtnl_link_query(int fd, char *device) -{ - struct sockaddr_nl sa; - struct msghdr msg; - struct iovec iov; - int cnt; - - struct { - struct nlmsghdr hdr; - struct ifinfomsg ifm; - } __attribute__((packed)) request; - - memset(&sa, 0, sizeof(sa)); - sa.nl_family = AF_NETLINK; - - memset(&request, 0, sizeof(request)); - request.hdr.nlmsg_len = NLMSG_LENGTH(sizeof(request.ifm)); - request.hdr.nlmsg_type = RTM_GETLINK; - request.hdr.nlmsg_flags = NLM_F_REQUEST; - request.hdr.nlmsg_seq = 1; - request.hdr.nlmsg_pid = 0; - request.ifm.ifi_family = AF_UNSPEC; - request.ifm.ifi_index = if_nametoindex(device ? device : ""); - request.ifm.ifi_change = 0xffffffff; - - iov.iov_base = &request; - iov.iov_len = sizeof(request); - - memset(&msg, 0, sizeof(msg)); - msg.msg_name = &sa; - msg.msg_namelen = sizeof(sa); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - - cnt = sendmsg(fd, &msg, 0); - if (cnt < 0) { - pr_err("rtnl: sendmsg: %m"); - return -1; - } - return 0; -} - -static inline __u32 rta_getattr_u32(const struct rtattr *rta) -{ - return *(__u32 *)RTA_DATA(rta); -} - -static inline const char *rta_getattr_str(const struct rtattr *rta) -{ - return (const char *)RTA_DATA(rta); -} - -static int rtnl_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta, int len) -{ - unsigned short type; - - memset(tb, 0, sizeof(struct rtattr *) * max); - while (RTA_OK(rta, len)) { - type = rta->rta_type; - if ((type < max) && (!tb[type])) - tb[type] = rta; - rta = RTA_NEXT(rta, len); - } - if (len) { - pr_err("Length mismatch: len %d, rta_len=%d\n", len, rta->rta_len); - return -1; - } - - return 0; -} - -static inline int rtnl_nested_rtattr_parse(struct rtattr *tb[], int max, struct rtattr *rta) -{ - return rtnl_rtattr_parse(tb, max, RTA_DATA(rta), RTA_PAYLOAD(rta)); -} - -static int rtnl_linkinfo_parse(struct rtattr *rta) -{ - int index = -1; - const char *kind; - struct rtattr *linkinfo[IFLA_INFO_MAX]; - struct rtattr *bond[IFLA_BOND_MAX]; - - if (rtnl_nested_rtattr_parse(linkinfo, IFLA_INFO_MAX, rta) < 0) - return -1; - - if (linkinfo[IFLA_INFO_KIND]) { - kind = rta_getattr_str(linkinfo[IFLA_INFO_KIND]); - - if (kind && !strncmp(kind, "bond", 4) && - linkinfo[IFLA_INFO_DATA]) { - if (rtnl_nested_rtattr_parse(bond, IFLA_BOND_MAX, - linkinfo[IFLA_INFO_DATA]) < 0) - return -1; - - if (bond[IFLA_BOND_ACTIVE_SLAVE]) { - index = rta_getattr_u32(bond[IFLA_BOND_ACTIVE_SLAVE]); - } - } - } - return index; -} - -int rtnl_link_status(int fd, char *device, rtnl_callback cb, void *ctx) -{ - int index, len, link_up; - int slave_index = -1; - struct iovec iov; - struct sockaddr_nl sa; - struct msghdr msg; - struct nlmsghdr *nh; - struct ifinfomsg *info = NULL; - struct rtattr *tb[IFLA_MAX+1]; - - index = if_nametoindex(device); - if (!rtnl_buf) { - rtnl_len = 4096; - rtnl_buf = malloc(rtnl_len); - if (!rtnl_buf) { - pr_err("rtnl: low memory"); - return -1; - } - } - - iov.iov_base = rtnl_buf; - iov.iov_len = rtnl_len; - memset(&msg, 0, sizeof(msg)); - msg.msg_name = &sa; - msg.msg_namelen = sizeof(sa); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - - len = recvmsg(fd, &msg, MSG_PEEK | MSG_TRUNC); - if (len < 1) { - pr_err("rtnl: recvmsg: %m"); - return -1; - } - if (len > rtnl_len) { - free(rtnl_buf); - rtnl_len = len; - rtnl_buf = malloc(len); - if (!rtnl_buf) { - pr_err("rtnl: failed to resize to %d bytes", len); - return -1; - } - iov.iov_base = rtnl_buf; - iov.iov_len = rtnl_len; - } - - len = recvmsg(fd, &msg, 0); - if (len < 1) { - pr_err("rtnl: recvmsg: %m"); - return -1; - } - nh = (struct nlmsghdr *) rtnl_buf; - - for ( ; NLMSG_OK(nh, len); nh = NLMSG_NEXT(nh, len)) { - if (nh->nlmsg_type != RTM_NEWLINK) - continue; - - info = NLMSG_DATA(nh); - if (index != info->ifi_index) - continue; - - link_up = info->ifi_flags & IFF_RUNNING ? 1 : 0; - pr_debug("interface index %d is %s", index, - link_up ? "up" : "down"); - - rtnl_rtattr_parse(tb, IFLA_MAX, IFLA_RTA(info), - IFLA_PAYLOAD(nh)); - - if (tb[IFLA_LINKINFO]) - slave_index = rtnl_linkinfo_parse(tb[IFLA_LINKINFO]); - - if (cb) - cb(ctx, link_up, slave_index); - } - - return 0; -} - -int rtnl_open(void) -{ - int fd; - struct sockaddr_nl sa; - - memset(&sa, 0, sizeof(sa)); - sa.nl_family = AF_NETLINK; - sa.nl_groups = RTNLGRP_LINK; - - fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); - if (fd < 0) { - pr_err("failed to open netlink socket: %m"); - return -1; - } - if (bind(fd, (struct sockaddr *) &sa, sizeof(sa))) { - pr_err("failed to bind netlink socket: %m"); - close(fd); - return -1; - } - return fd; -} diff --git a/sk.c b/sk.c index 0aeb33a..f18b2bf 100644 --- a/sk.c +++ b/sk.c @@ -18,7 +18,6 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include -#include #include #include #include @@ -41,76 +40,39 @@ int sk_tx_timeout = 1; int sk_check_fupsync; -enum hwts_filter_mode sk_hwts_filter_mode = HWTS_FILTER_NORMAL; /* private methods */ -static void init_ifreq(struct ifreq *ifreq, struct hwtstamp_config *cfg, - const char *device) -{ - memset(ifreq, 0, sizeof(*ifreq)); - memset(cfg, 0, sizeof(*cfg)); - - strncpy(ifreq->ifr_name, device, sizeof(ifreq->ifr_name) - 1); - - ifreq->ifr_data = (void *) cfg; -} - -static int hwts_init(int fd, const char *device, int rx_filter, - int rx_filter2, int tx_type) +static int hwts_init(int fd, const char *device, int rx_filter, int tx_type) { struct ifreq ifreq; - struct hwtstamp_config cfg; + struct hwtstamp_config cfg, req; int err; - init_ifreq(&ifreq, &cfg, device); + memset(&ifreq, 0, sizeof(ifreq)); + memset(&cfg, 0, sizeof(cfg)); - switch (sk_hwts_filter_mode) { - case HWTS_FILTER_CHECK: - err = ioctl(fd, SIOCGHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGHWTSTAMP failed: %m"); - return err; - } - break; - case HWTS_FILTER_FULL: - cfg.tx_type = tx_type; - cfg.rx_filter = HWTSTAMP_FILTER_ALL; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - break; - case HWTS_FILTER_NORMAL: - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_info("driver rejected most general HWTSTAMP filter"); + strncpy(ifreq.ifr_name, device, sizeof(ifreq.ifr_name) - 1); - init_ifreq(&ifreq, &cfg, device); - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter2; + ifreq.ifr_data = (void *) &cfg; + cfg.tx_type = tx_type; + cfg.rx_filter = rx_filter; + req = cfg; + err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); + if (err < 0) + return err; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - } - break; - } + if (memcmp(&cfg, &req, sizeof(cfg))) { - if (cfg.tx_type != tx_type || - (cfg.rx_filter != rx_filter && - cfg.rx_filter != rx_filter2 && - cfg.rx_filter != HWTSTAMP_FILTER_ALL)) { - pr_debug("tx_type %d not %d", cfg.tx_type, tx_type); - pr_debug("rx_filter %d not %d or %d", cfg.rx_filter, rx_filter, - rx_filter2); - pr_err("The current filter does not match the required"); - return -1; + pr_debug("driver changed our HWTSTAMP options"); + pr_debug("tx_type %d not %d", cfg.tx_type, req.tx_type); + pr_debug("rx_filter %d not %d", cfg.rx_filter, req.rx_filter); + + if (cfg.tx_type != req.tx_type || + (cfg.rx_filter != HWTSTAMP_FILTER_ALL && + cfg.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)) { + return -1; + } } return 0; @@ -359,7 +321,7 @@ int sk_receive(int fd, void *buf, int buflen, } cnt = recvmsg(fd, &msg, flags); - if (cnt < 0) + if (cnt < 1) pr_err("recvmsg%sfailed: %m", flags == MSG_ERRQUEUE ? " tx timestamp " : " "); @@ -488,9 +450,15 @@ int sk_timestamping_init(int fd, const char *device, enum timestamp_type type, case TRANS_UDS: return -1; } - err = hwts_init(fd, device, filter1, filter2, tx_type); - if (err) - return err; + err = hwts_init(fd, device, filter1, tx_type); + if (err) { + pr_info("driver rejected most general HWTSTAMP filter"); + err = hwts_init(fd, device, filter2, tx_type); + if (err) { + pr_err("ioctl SIOCSHWTSTAMP failed: %m"); + return err; + } + } } if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, diff --git a/sk.c.headers b/sk.c.headers deleted file mode 100644 index f18b2bf..0000000 --- a/sk.c.headers +++ /dev/null @@ -1,484 +0,0 @@ -/** - * @file sk.c - * @brief Implements protocol independent socket methods. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "address.h" -#include "ether.h" -#include "missing.h" -#include "print.h" -#include "sk.h" - -/* globals */ - -int sk_tx_timeout = 1; -int sk_check_fupsync; - -/* private methods */ - -static int hwts_init(int fd, const char *device, int rx_filter, int tx_type) -{ - struct ifreq ifreq; - struct hwtstamp_config cfg, req; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - memset(&cfg, 0, sizeof(cfg)); - - strncpy(ifreq.ifr_name, device, sizeof(ifreq.ifr_name) - 1); - - ifreq.ifr_data = (void *) &cfg; - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter; - req = cfg; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) - return err; - - if (memcmp(&cfg, &req, sizeof(cfg))) { - - pr_debug("driver changed our HWTSTAMP options"); - pr_debug("tx_type %d not %d", cfg.tx_type, req.tx_type); - pr_debug("rx_filter %d not %d", cfg.rx_filter, req.rx_filter); - - if (cfg.tx_type != req.tx_type || - (cfg.rx_filter != HWTSTAMP_FILTER_ALL && - cfg.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)) { - return -1; - } - } - - return 0; -} - -/* public methods */ - -int sk_interface_fd(void) -{ - int fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - return fd; -} - -int sk_interface_index(int fd, const char *name) -{ - struct ifreq ifreq; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - err = ioctl(fd, SIOCGIFINDEX, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFINDEX failed: %m"); - return err; - } - return ifreq.ifr_ifindex; -} - -int sk_general_init(int fd) -{ - int on = sk_check_fupsync ? 1 : 0; - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPNS, &on, sizeof(on)) < 0) { - pr_err("ioctl SO_TIMESTAMPNS failed: %m"); - return -1; - } - return 0; -} - -int sk_get_ts_info(const char *name, struct sk_ts_info *sk_info) -{ -#ifdef ETHTOOL_GET_TS_INFO - struct ethtool_ts_info info; - struct ifreq ifr; - int fd, err; - - memset(&ifr, 0, sizeof(ifr)); - memset(&info, 0, sizeof(info)); - info.cmd = ETHTOOL_GET_TS_INFO; - strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - ifr.ifr_data = (char *) &info; - fd = socket(AF_INET, SOCK_DGRAM, 0); - - if (fd < 0) { - pr_err("socket failed: %m"); - goto failed; - } - - err = ioctl(fd, SIOCETHTOOL, &ifr); - if (err < 0) { - pr_err("ioctl SIOCETHTOOL failed: %m"); - close(fd); - goto failed; - } - - close(fd); - - /* copy the necessary data to sk_info */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - sk_info->valid = 1; - sk_info->phc_index = info.phc_index; - sk_info->so_timestamping = info.so_timestamping; - sk_info->tx_types = info.tx_types; - sk_info->rx_filters = info.rx_filters; - - return 0; -failed: -#endif - /* clear data and ensure it is not marked valid */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - return -1; -} - -static int sk_interface_guidaddr(const char *name, unsigned char *guid) -{ - char file_name[64], buf[64], addr[8]; - FILE *f; - char *err; - int res; - - snprintf(file_name, sizeof buf, "/sys/class/net/%s/address", name); - f = fopen(file_name, "r"); - if (!f) { - pr_err("failed to open %s: %m", buf); - return -1; - } - - /* Set the file position to the beginning of the GUID */ - res = fseek(f, GUID_OFFSET, SEEK_SET); - if (res) { - pr_err("fseek failed: %m"); - goto error; - } - - err = fgets(buf, sizeof buf, f); - if (err == NULL) { - pr_err("fseek failed: %m"); - goto error; - } - - res = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", - &addr[0], &addr[1], &addr[2], &addr[3], - &addr[4], &addr[5], &addr[6], &addr[7]); - if (res != GUID_LEN) { - pr_err("sscanf failed: %m"); - goto error; - } - - memcpy(guid, addr, GUID_LEN); - fclose(f); - - return 0; - -error: - fclose(f); - return -1; -} - -int sk_interface_macaddr(const char *name, struct address *mac) -{ - struct ifreq ifreq; - int err, fd, type; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - - fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - - err = ioctl(fd, SIOCGIFHWADDR, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFHWADDR failed: %m"); - close(fd); - return -1; - } - - close(fd); - - /* Get interface type */ - type = ifreq.ifr_hwaddr.sa_family; - switch (type) { - case ARPHRD_INFINIBAND: - err = sk_interface_guidaddr(name, mac->sll.sll_addr); - if (err) { - pr_err("fail to get address using sysfs: %m"); - return -1; - } - mac->sll.sll_halen = EUI64; - break; - default: - memcpy(mac->sll.sll_addr, &ifreq.ifr_hwaddr.sa_data, MAC_LEN); - mac->sll.sll_halen = EUI48; - } - - mac->sll.sll_family = AF_PACKET; - mac->len = sizeof(mac->sll); - return 0; -} - -int sk_interface_addr(const char *name, int family, struct address *addr) -{ - struct ifaddrs *ifaddr, *i; - int result = -1; - - if (getifaddrs(&ifaddr) == -1) { - pr_err("getifaddrs failed: %m"); - return -1; - } - for (i = ifaddr; i; i = i->ifa_next) { - if (i->ifa_addr && family == i->ifa_addr->sa_family && - strcmp(name, i->ifa_name) == 0) - { - switch (family) { - case AF_INET: - addr->len = sizeof(addr->sin); - memcpy(&addr->sin, i->ifa_addr, addr->len); - break; - case AF_INET6: - addr->len = sizeof(addr->sin6); - memcpy(&addr->sin6, i->ifa_addr, addr->len); - break; - default: - continue; - } - result = 0; - break; - } - } - freeifaddrs(ifaddr); - return result; -} - -static short sk_events = POLLPRI; -static short sk_revents = POLLPRI; - -int sk_receive(int fd, void *buf, int buflen, - struct address *addr, struct hw_timestamp *hwts, int flags) -{ - char control[256]; - int cnt = 0, res = 0, level, type; - struct cmsghdr *cm; - struct iovec iov = { buf, buflen }; - struct msghdr msg; - struct timespec *sw, *ts = NULL; - - memset(control, 0, sizeof(control)); - memset(&msg, 0, sizeof(msg)); - if (addr) { - msg.msg_name = &addr->ss; - msg.msg_namelen = sizeof(addr->ss); - } - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - if (flags == MSG_ERRQUEUE) { - struct pollfd pfd = { fd, sk_events, 0 }; - res = poll(&pfd, 1, sk_tx_timeout); - if (res < 1) { - pr_err(res ? "poll for tx timestamp failed: %m" : - "timed out while polling for tx timestamp"); - pr_err("increasing tx_timestamp_timeout may correct " - "this issue, but it is likely caused by a driver bug"); - return res; - } else if (!(pfd.revents & sk_revents)) { - pr_err("poll for tx timestamp woke up on non ERR event"); - return -1; - } - } - - cnt = recvmsg(fd, &msg, flags); - if (cnt < 1) - pr_err("recvmsg%sfailed: %m", - flags == MSG_ERRQUEUE ? " tx timestamp " : " "); - - for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { - level = cm->cmsg_level; - type = cm->cmsg_type; - if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { - if (cm->cmsg_len < sizeof(*ts) * 3) { - pr_warning("short SO_TIMESTAMPING message"); - return -1; - } - ts = (struct timespec *) CMSG_DATA(cm); - } - if (SOL_SOCKET == level && SO_TIMESTAMPNS == type) { - if (cm->cmsg_len < sizeof(*sw)) { - pr_warning("short SO_TIMESTAMPNS message"); - return -1; - } - sw = (struct timespec *) CMSG_DATA(cm); - hwts->sw = timespec_to_tmv(*sw); - } - } - - if (addr) - addr->len = msg.msg_namelen; - - if (!ts) { - memset(&hwts->ts, 0, sizeof(hwts->ts)); - return cnt; - } - - switch (hwts->type) { - case TS_SOFTWARE: - hwts->ts = timespec_to_tmv(ts[0]); - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - hwts->ts = timespec_to_tmv(ts[2]); - break; - case TS_LEGACY_HW: - hwts->ts = timespec_to_tmv(ts[1]); - break; - } - return cnt; -} - -int sk_set_priority(int fd, uint8_t dscp) -{ - int tos; - socklen_t tos_len; - - tos_len = sizeof(tos); - if (getsockopt(fd, SOL_IP, IP_TOS, &tos, &tos_len) < 0) { - tos = 0; - } - - /* clear old DSCP value */ - tos &= ~0xFC; - - /* set new DSCP value */ - tos |= dscp<<2; - tos_len = sizeof(tos); - if (setsockopt(fd, SOL_IP, IP_TOS, &tos, tos_len) < 0) { - return -1; - } - - return 0; -} - -int sk_timestamping_init(int fd, const char *device, enum timestamp_type type, - enum transport_type transport) -{ - int err, filter1, filter2 = 0, flags, tx_type = HWTSTAMP_TX_ON; - - switch (type) { - case TS_SOFTWARE: - flags = SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - break; - case TS_LEGACY_HW: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_SYS_HARDWARE; - break; - default: - return -1; - } - - if (type != TS_SOFTWARE) { - filter1 = HWTSTAMP_FILTER_PTP_V2_EVENT; - switch (type) { - case TS_SOFTWARE: - tx_type = HWTSTAMP_TX_OFF; - break; - case TS_HARDWARE: - case TS_LEGACY_HW: - tx_type = HWTSTAMP_TX_ON; - break; - case TS_ONESTEP: - tx_type = HWTSTAMP_TX_ONESTEP_SYNC; - break; - case TS_P2P1STEP: - tx_type = HWTSTAMP_TX_ONESTEP_P2P; - break; - } - switch (transport) { - case TRANS_UDP_IPV4: - case TRANS_UDP_IPV6: - filter2 = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; - break; - case TRANS_IEEE_802_3: - filter2 = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; - break; - case TRANS_DEVICENET: - case TRANS_CONTROLNET: - case TRANS_PROFINET: - case TRANS_UDS: - return -1; - } - err = hwts_init(fd, device, filter1, tx_type); - if (err) { - pr_info("driver rejected most general HWTSTAMP filter"); - err = hwts_init(fd, device, filter2, tx_type); - if (err) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - } - } - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, - &flags, sizeof(flags)) < 0) { - pr_err("ioctl SO_TIMESTAMPING failed: %m"); - return -1; - } - - flags = 1; - if (setsockopt(fd, SOL_SOCKET, SO_SELECT_ERR_QUEUE, - &flags, sizeof(flags)) < 0) { - pr_warning("%s: SO_SELECT_ERR_QUEUE: %m", device); - sk_events = 0; - sk_revents = POLLERR; - } - - /* Enable the sk_check_fupsync option, perhaps. */ - if (sk_general_init(fd)) { - return -1; - } - - return 0; -} diff --git a/sk.c.hwtsfilter b/sk.c.hwtsfilter deleted file mode 100644 index 386b4c8..0000000 --- a/sk.c.hwtsfilter +++ /dev/null @@ -1,485 +0,0 @@ -/** - * @file sk.c - * @brief Implements protocol independent socket methods. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "address.h" -#include "ether.h" -#include "missing.h" -#include "print.h" -#include "sk.h" - -/* globals */ - -int sk_tx_timeout = 1; -int sk_check_fupsync; - -/* private methods */ - -static int hwts_init(int fd, const char *device, int rx_filter, int tx_type) -{ - struct ifreq ifreq; - struct hwtstamp_config cfg, req; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - memset(&cfg, 0, sizeof(cfg)); - - strncpy(ifreq.ifr_name, device, sizeof(ifreq.ifr_name) - 1); - - ifreq.ifr_data = (void *) &cfg; - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter; - req = cfg; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) - return err; - - if (memcmp(&cfg, &req, sizeof(cfg))) { - - pr_debug("driver changed our HWTSTAMP options"); - pr_debug("tx_type %d not %d", cfg.tx_type, req.tx_type); - pr_debug("rx_filter %d not %d", cfg.rx_filter, req.rx_filter); - - if (cfg.tx_type != req.tx_type || - (cfg.rx_filter != HWTSTAMP_FILTER_ALL && - cfg.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)) { - return -1; - } - } - - return 0; -} - -/* public methods */ - -int sk_interface_fd(void) -{ - int fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - return fd; -} - -int sk_interface_index(int fd, const char *name) -{ - struct ifreq ifreq; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - err = ioctl(fd, SIOCGIFINDEX, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFINDEX failed: %m"); - return err; - } - return ifreq.ifr_ifindex; -} - -int sk_general_init(int fd) -{ - int on = sk_check_fupsync ? 1 : 0; - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPNS, &on, sizeof(on)) < 0) { - pr_err("ioctl SO_TIMESTAMPNS failed: %m"); - return -1; - } - return 0; -} - -int sk_get_ts_info(const char *name, struct sk_ts_info *sk_info) -{ -#ifdef ETHTOOL_GET_TS_INFO - struct ethtool_ts_info info; - struct ifreq ifr; - int fd, err; - - memset(&ifr, 0, sizeof(ifr)); - memset(&info, 0, sizeof(info)); - info.cmd = ETHTOOL_GET_TS_INFO; - strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - ifr.ifr_data = (char *) &info; - fd = socket(AF_INET, SOCK_DGRAM, 0); - - if (fd < 0) { - pr_err("socket failed: %m"); - goto failed; - } - - err = ioctl(fd, SIOCETHTOOL, &ifr); - if (err < 0) { - pr_err("ioctl SIOCETHTOOL failed: %m"); - close(fd); - goto failed; - } - - close(fd); - - /* copy the necessary data to sk_info */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - sk_info->valid = 1; - sk_info->phc_index = info.phc_index; - sk_info->so_timestamping = info.so_timestamping; - sk_info->tx_types = info.tx_types; - sk_info->rx_filters = info.rx_filters; - - return 0; -failed: -#endif - /* clear data and ensure it is not marked valid */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - return -1; -} - -static int sk_interface_guidaddr(const char *name, unsigned char *guid) -{ - char file_name[64], buf[64], addr[8]; - FILE *f; - char *err; - int res; - - snprintf(file_name, sizeof buf, "/sys/class/net/%s/address", name); - f = fopen(file_name, "r"); - if (!f) { - pr_err("failed to open %s: %m", buf); - return -1; - } - - /* Set the file position to the beginning of the GUID */ - res = fseek(f, GUID_OFFSET, SEEK_SET); - if (res) { - pr_err("fseek failed: %m"); - goto error; - } - - err = fgets(buf, sizeof buf, f); - if (err == NULL) { - pr_err("fseek failed: %m"); - goto error; - } - - res = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", - &addr[0], &addr[1], &addr[2], &addr[3], - &addr[4], &addr[5], &addr[6], &addr[7]); - if (res != GUID_LEN) { - pr_err("sscanf failed: %m"); - goto error; - } - - memcpy(guid, addr, GUID_LEN); - fclose(f); - - return 0; - -error: - fclose(f); - return -1; -} - -int sk_interface_macaddr(const char *name, struct address *mac) -{ - struct ifreq ifreq; - int err, fd, type; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - - fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - - err = ioctl(fd, SIOCGIFHWADDR, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFHWADDR failed: %m"); - close(fd); - return -1; - } - - close(fd); - - /* Get interface type */ - type = ifreq.ifr_hwaddr.sa_family; - switch (type) { - case ARPHRD_INFINIBAND: - err = sk_interface_guidaddr(name, mac->sll.sll_addr); - if (err) { - pr_err("fail to get address using sysfs: %m"); - return -1; - } - mac->sll.sll_halen = EUI64; - break; - default: - memcpy(mac->sll.sll_addr, &ifreq.ifr_hwaddr.sa_data, MAC_LEN); - mac->sll.sll_halen = EUI48; - } - - mac->sll.sll_family = AF_PACKET; - mac->len = sizeof(mac->sll); - return 0; -} - -int sk_interface_addr(const char *name, int family, struct address *addr) -{ - struct ifaddrs *ifaddr, *i; - int result = -1; - - if (getifaddrs(&ifaddr) == -1) { - pr_err("getifaddrs failed: %m"); - return -1; - } - for (i = ifaddr; i; i = i->ifa_next) { - if (i->ifa_addr && family == i->ifa_addr->sa_family && - strcmp(name, i->ifa_name) == 0) - { - switch (family) { - case AF_INET: - addr->len = sizeof(addr->sin); - memcpy(&addr->sin, i->ifa_addr, addr->len); - break; - case AF_INET6: - addr->len = sizeof(addr->sin6); - memcpy(&addr->sin6, i->ifa_addr, addr->len); - break; - default: - continue; - } - result = 0; - break; - } - } - freeifaddrs(ifaddr); - return result; -} - -static short sk_events = POLLPRI; -static short sk_revents = POLLPRI; - -int sk_receive(int fd, void *buf, int buflen, - struct address *addr, struct hw_timestamp *hwts, int flags) -{ - char control[256]; - int cnt = 0, res = 0, level, type; - struct cmsghdr *cm; - struct iovec iov = { buf, buflen }; - struct msghdr msg; - struct timespec *sw, *ts = NULL; - - memset(control, 0, sizeof(control)); - memset(&msg, 0, sizeof(msg)); - if (addr) { - msg.msg_name = &addr->ss; - msg.msg_namelen = sizeof(addr->ss); - } - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - if (flags == MSG_ERRQUEUE) { - struct pollfd pfd = { fd, sk_events, 0 }; - res = poll(&pfd, 1, sk_tx_timeout); - if (res < 1) { - pr_err(res ? "poll for tx timestamp failed: %m" : - "timed out while polling for tx timestamp"); - pr_err("increasing tx_timestamp_timeout may correct " - "this issue, but it is likely caused by a driver bug"); - return res; - } else if (!(pfd.revents & sk_revents)) { - pr_err("poll for tx timestamp woke up on non ERR event"); - return -1; - } - } - - cnt = recvmsg(fd, &msg, flags); - if (cnt < 1) - pr_err("recvmsg%sfailed: %m", - flags == MSG_ERRQUEUE ? " tx timestamp " : " "); - - for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { - level = cm->cmsg_level; - type = cm->cmsg_type; - if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { - if (cm->cmsg_len < sizeof(*ts) * 3) { - pr_warning("short SO_TIMESTAMPING message"); - return -1; - } - ts = (struct timespec *) CMSG_DATA(cm); - } - if (SOL_SOCKET == level && SO_TIMESTAMPNS == type) { - if (cm->cmsg_len < sizeof(*sw)) { - pr_warning("short SO_TIMESTAMPNS message"); - return -1; - } - sw = (struct timespec *) CMSG_DATA(cm); - hwts->sw = timespec_to_tmv(*sw); - } - } - - if (addr) - addr->len = msg.msg_namelen; - - if (!ts) { - memset(&hwts->ts, 0, sizeof(hwts->ts)); - return cnt; - } - - switch (hwts->type) { - case TS_SOFTWARE: - hwts->ts = timespec_to_tmv(ts[0]); - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - hwts->ts = timespec_to_tmv(ts[2]); - break; - case TS_LEGACY_HW: - hwts->ts = timespec_to_tmv(ts[1]); - break; - } - return cnt; -} - -int sk_set_priority(int fd, uint8_t dscp) -{ - int tos; - socklen_t tos_len; - - tos_len = sizeof(tos); - if (getsockopt(fd, SOL_IP, IP_TOS, &tos, &tos_len) < 0) { - tos = 0; - } - - /* clear old DSCP value */ - tos &= ~0xFC; - - /* set new DSCP value */ - tos |= dscp<<2; - tos_len = sizeof(tos); - if (setsockopt(fd, SOL_IP, IP_TOS, &tos, tos_len) < 0) { - return -1; - } - - return 0; -} - -int sk_timestamping_init(int fd, const char *device, enum timestamp_type type, - enum transport_type transport) -{ - int err, filter1, filter2 = 0, flags, tx_type = HWTSTAMP_TX_ON; - - switch (type) { - case TS_SOFTWARE: - flags = SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - break; - case TS_LEGACY_HW: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_SYS_HARDWARE; - break; - default: - return -1; - } - - if (type != TS_SOFTWARE) { - filter1 = HWTSTAMP_FILTER_PTP_V2_EVENT; - switch (type) { - case TS_SOFTWARE: - tx_type = HWTSTAMP_TX_OFF; - break; - case TS_HARDWARE: - case TS_LEGACY_HW: - tx_type = HWTSTAMP_TX_ON; - break; - case TS_ONESTEP: - tx_type = HWTSTAMP_TX_ONESTEP_SYNC; - break; - case TS_P2P1STEP: - tx_type = HWTSTAMP_TX_ONESTEP_P2P; - break; - } - switch (transport) { - case TRANS_UDP_IPV4: - case TRANS_UDP_IPV6: - filter2 = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; - break; - case TRANS_IEEE_802_3: - filter2 = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; - break; - case TRANS_DEVICENET: - case TRANS_CONTROLNET: - case TRANS_PROFINET: - case TRANS_UDS: - return -1; - } - err = hwts_init(fd, device, filter1, tx_type); - if (err) { - pr_info("driver rejected most general HWTSTAMP filter"); - err = hwts_init(fd, device, filter2, tx_type); - if (err) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - } - } - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, - &flags, sizeof(flags)) < 0) { - pr_err("ioctl SO_TIMESTAMPING failed: %m"); - return -1; - } - - flags = 1; - if (setsockopt(fd, SOL_SOCKET, SO_SELECT_ERR_QUEUE, - &flags, sizeof(flags)) < 0) { - pr_warning("%s: SO_SELECT_ERR_QUEUE: %m", device); - sk_events = 0; - sk_revents = POLLERR; - } - - /* Enable the sk_check_fupsync option, perhaps. */ - if (sk_general_init(fd)) { - return -1; - } - - return 0; -} diff --git a/sk.c.zerolength b/sk.c.zerolength deleted file mode 100644 index 3852cbb..0000000 --- a/sk.c.zerolength +++ /dev/null @@ -1,516 +0,0 @@ -/** - * @file sk.c - * @brief Implements protocol independent socket methods. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "address.h" -#include "ether.h" -#include "missing.h" -#include "print.h" -#include "sk.h" - -/* globals */ - -int sk_tx_timeout = 1; -int sk_check_fupsync; -enum hwts_filter_mode sk_hwts_filter_mode = HWTS_FILTER_NORMAL; - -/* private methods */ - -static void init_ifreq(struct ifreq *ifreq, struct hwtstamp_config *cfg, - const char *device) -{ - memset(ifreq, 0, sizeof(*ifreq)); - memset(cfg, 0, sizeof(*cfg)); - - strncpy(ifreq->ifr_name, device, sizeof(ifreq->ifr_name) - 1); - - ifreq->ifr_data = (void *) cfg; -} - -static int hwts_init(int fd, const char *device, int rx_filter, - int rx_filter2, int tx_type) -{ - struct ifreq ifreq; - struct hwtstamp_config cfg; - int err; - - init_ifreq(&ifreq, &cfg, device); - - switch (sk_hwts_filter_mode) { - case HWTS_FILTER_CHECK: - err = ioctl(fd, SIOCGHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGHWTSTAMP failed: %m"); - return err; - } - break; - case HWTS_FILTER_FULL: - cfg.tx_type = tx_type; - cfg.rx_filter = HWTSTAMP_FILTER_ALL; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - break; - case HWTS_FILTER_NORMAL: - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_info("driver rejected most general HWTSTAMP filter"); - - init_ifreq(&ifreq, &cfg, device); - cfg.tx_type = tx_type; - cfg.rx_filter = rx_filter2; - - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - return err; - } - } - break; - } - - if (cfg.tx_type != tx_type || - (cfg.rx_filter != rx_filter && - cfg.rx_filter != rx_filter2 && - cfg.rx_filter != HWTSTAMP_FILTER_ALL)) { - pr_debug("tx_type %d not %d", cfg.tx_type, tx_type); - pr_debug("rx_filter %d not %d or %d", cfg.rx_filter, rx_filter, - rx_filter2); - pr_err("The current filter does not match the required"); - return -1; - } - - return 0; -} - -/* public methods */ - -int sk_interface_fd(void) -{ - int fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - return fd; -} - -int sk_interface_index(int fd, const char *name) -{ - struct ifreq ifreq; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - err = ioctl(fd, SIOCGIFINDEX, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFINDEX failed: %m"); - return err; - } - return ifreq.ifr_ifindex; -} - -int sk_general_init(int fd) -{ - int on = sk_check_fupsync ? 1 : 0; - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPNS, &on, sizeof(on)) < 0) { - pr_err("ioctl SO_TIMESTAMPNS failed: %m"); - return -1; - } - return 0; -} - -int sk_get_ts_info(const char *name, struct sk_ts_info *sk_info) -{ -#ifdef ETHTOOL_GET_TS_INFO - struct ethtool_ts_info info; - struct ifreq ifr; - int fd, err; - - memset(&ifr, 0, sizeof(ifr)); - memset(&info, 0, sizeof(info)); - info.cmd = ETHTOOL_GET_TS_INFO; - strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - ifr.ifr_data = (char *) &info; - fd = socket(AF_INET, SOCK_DGRAM, 0); - - if (fd < 0) { - pr_err("socket failed: %m"); - goto failed; - } - - err = ioctl(fd, SIOCETHTOOL, &ifr); - if (err < 0) { - pr_err("ioctl SIOCETHTOOL failed: %m"); - close(fd); - goto failed; - } - - close(fd); - - /* copy the necessary data to sk_info */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - sk_info->valid = 1; - sk_info->phc_index = info.phc_index; - sk_info->so_timestamping = info.so_timestamping; - sk_info->tx_types = info.tx_types; - sk_info->rx_filters = info.rx_filters; - - return 0; -failed: -#endif - /* clear data and ensure it is not marked valid */ - memset(sk_info, 0, sizeof(struct sk_ts_info)); - return -1; -} - -static int sk_interface_guidaddr(const char *name, unsigned char *guid) -{ - char file_name[64], buf[64], addr[8]; - FILE *f; - char *err; - int res; - - snprintf(file_name, sizeof buf, "/sys/class/net/%s/address", name); - f = fopen(file_name, "r"); - if (!f) { - pr_err("failed to open %s: %m", buf); - return -1; - } - - /* Set the file position to the beginning of the GUID */ - res = fseek(f, GUID_OFFSET, SEEK_SET); - if (res) { - pr_err("fseek failed: %m"); - goto error; - } - - err = fgets(buf, sizeof buf, f); - if (err == NULL) { - pr_err("fseek failed: %m"); - goto error; - } - - res = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", - &addr[0], &addr[1], &addr[2], &addr[3], - &addr[4], &addr[5], &addr[6], &addr[7]); - if (res != GUID_LEN) { - pr_err("sscanf failed: %m"); - goto error; - } - - memcpy(guid, addr, GUID_LEN); - fclose(f); - - return 0; - -error: - fclose(f); - return -1; -} - -int sk_interface_macaddr(const char *name, struct address *mac) -{ - struct ifreq ifreq; - int err, fd, type; - - memset(&ifreq, 0, sizeof(ifreq)); - strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); - - fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - - err = ioctl(fd, SIOCGIFHWADDR, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFHWADDR failed: %m"); - close(fd); - return -1; - } - - close(fd); - - /* Get interface type */ - type = ifreq.ifr_hwaddr.sa_family; - switch (type) { - case ARPHRD_INFINIBAND: - err = sk_interface_guidaddr(name, mac->sll.sll_addr); - if (err) { - pr_err("fail to get address using sysfs: %m"); - return -1; - } - mac->sll.sll_halen = EUI64; - break; - default: - memcpy(mac->sll.sll_addr, &ifreq.ifr_hwaddr.sa_data, MAC_LEN); - mac->sll.sll_halen = EUI48; - } - - mac->sll.sll_family = AF_PACKET; - mac->len = sizeof(mac->sll); - return 0; -} - -int sk_interface_addr(const char *name, int family, struct address *addr) -{ - struct ifaddrs *ifaddr, *i; - int result = -1; - - if (getifaddrs(&ifaddr) == -1) { - pr_err("getifaddrs failed: %m"); - return -1; - } - for (i = ifaddr; i; i = i->ifa_next) { - if (i->ifa_addr && family == i->ifa_addr->sa_family && - strcmp(name, i->ifa_name) == 0) - { - switch (family) { - case AF_INET: - addr->len = sizeof(addr->sin); - memcpy(&addr->sin, i->ifa_addr, addr->len); - break; - case AF_INET6: - addr->len = sizeof(addr->sin6); - memcpy(&addr->sin6, i->ifa_addr, addr->len); - break; - default: - continue; - } - result = 0; - break; - } - } - freeifaddrs(ifaddr); - return result; -} - -static short sk_events = POLLPRI; -static short sk_revents = POLLPRI; - -int sk_receive(int fd, void *buf, int buflen, - struct address *addr, struct hw_timestamp *hwts, int flags) -{ - char control[256]; - int cnt = 0, res = 0, level, type; - struct cmsghdr *cm; - struct iovec iov = { buf, buflen }; - struct msghdr msg; - struct timespec *sw, *ts = NULL; - - memset(control, 0, sizeof(control)); - memset(&msg, 0, sizeof(msg)); - if (addr) { - msg.msg_name = &addr->ss; - msg.msg_namelen = sizeof(addr->ss); - } - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - if (flags == MSG_ERRQUEUE) { - struct pollfd pfd = { fd, sk_events, 0 }; - res = poll(&pfd, 1, sk_tx_timeout); - if (res < 1) { - pr_err(res ? "poll for tx timestamp failed: %m" : - "timed out while polling for tx timestamp"); - pr_err("increasing tx_timestamp_timeout may correct " - "this issue, but it is likely caused by a driver bug"); - return res; - } else if (!(pfd.revents & sk_revents)) { - pr_err("poll for tx timestamp woke up on non ERR event"); - return -1; - } - } - - cnt = recvmsg(fd, &msg, flags); - if (cnt < 1) - pr_err("recvmsg%sfailed: %m", - flags == MSG_ERRQUEUE ? " tx timestamp " : " "); - - for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { - level = cm->cmsg_level; - type = cm->cmsg_type; - if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { - if (cm->cmsg_len < sizeof(*ts) * 3) { - pr_warning("short SO_TIMESTAMPING message"); - return -1; - } - ts = (struct timespec *) CMSG_DATA(cm); - } - if (SOL_SOCKET == level && SO_TIMESTAMPNS == type) { - if (cm->cmsg_len < sizeof(*sw)) { - pr_warning("short SO_TIMESTAMPNS message"); - return -1; - } - sw = (struct timespec *) CMSG_DATA(cm); - hwts->sw = timespec_to_tmv(*sw); - } - } - - if (addr) - addr->len = msg.msg_namelen; - - if (!ts) { - memset(&hwts->ts, 0, sizeof(hwts->ts)); - return cnt; - } - - switch (hwts->type) { - case TS_SOFTWARE: - hwts->ts = timespec_to_tmv(ts[0]); - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - hwts->ts = timespec_to_tmv(ts[2]); - break; - case TS_LEGACY_HW: - hwts->ts = timespec_to_tmv(ts[1]); - break; - } - return cnt; -} - -int sk_set_priority(int fd, uint8_t dscp) -{ - int tos; - socklen_t tos_len; - - tos_len = sizeof(tos); - if (getsockopt(fd, SOL_IP, IP_TOS, &tos, &tos_len) < 0) { - tos = 0; - } - - /* clear old DSCP value */ - tos &= ~0xFC; - - /* set new DSCP value */ - tos |= dscp<<2; - tos_len = sizeof(tos); - if (setsockopt(fd, SOL_IP, IP_TOS, &tos, tos_len) < 0) { - return -1; - } - - return 0; -} - -int sk_timestamping_init(int fd, const char *device, enum timestamp_type type, - enum transport_type transport) -{ - int err, filter1, filter2 = 0, flags, tx_type = HWTSTAMP_TX_ON; - - switch (type) { - case TS_SOFTWARE: - flags = SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - break; - case TS_HARDWARE: - case TS_ONESTEP: - case TS_P2P1STEP: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - break; - case TS_LEGACY_HW: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_SYS_HARDWARE; - break; - default: - return -1; - } - - if (type != TS_SOFTWARE) { - filter1 = HWTSTAMP_FILTER_PTP_V2_EVENT; - switch (type) { - case TS_SOFTWARE: - tx_type = HWTSTAMP_TX_OFF; - break; - case TS_HARDWARE: - case TS_LEGACY_HW: - tx_type = HWTSTAMP_TX_ON; - break; - case TS_ONESTEP: - tx_type = HWTSTAMP_TX_ONESTEP_SYNC; - break; - case TS_P2P1STEP: - tx_type = HWTSTAMP_TX_ONESTEP_P2P; - break; - } - switch (transport) { - case TRANS_UDP_IPV4: - case TRANS_UDP_IPV6: - filter2 = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; - break; - case TRANS_IEEE_802_3: - filter2 = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; - break; - case TRANS_DEVICENET: - case TRANS_CONTROLNET: - case TRANS_PROFINET: - case TRANS_UDS: - return -1; - } - err = hwts_init(fd, device, filter1, filter2, tx_type); - if (err) - return err; - } - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, - &flags, sizeof(flags)) < 0) { - pr_err("ioctl SO_TIMESTAMPING failed: %m"); - return -1; - } - - flags = 1; - if (setsockopt(fd, SOL_SOCKET, SO_SELECT_ERR_QUEUE, - &flags, sizeof(flags)) < 0) { - pr_warning("%s: SO_SELECT_ERR_QUEUE: %m", device); - sk_events = 0; - sk_revents = POLLERR; - } - - /* Enable the sk_check_fupsync option, perhaps. */ - if (sk_general_init(fd)) { - return -1; - } - - return 0; -} diff --git a/sk.h b/sk.h index fd4d820..d91d5d8 100644 --- a/sk.h +++ b/sk.h @@ -24,16 +24,6 @@ #include "transport.h" /** - * Defines the available Hardware time-stamp setting modes. - */ - -enum hwts_filter_mode { - HWTS_FILTER_NORMAL, /* set hardware filters in normal way */ - HWTS_FILTER_CHECK, /* check filters but do not change them */ - HWTS_FILTER_FULL, /* Use time-stamp on all received packets */ -}; - -/** * Contains timestamping information returned by the GET_TS_INFO ioctl. * @valid: set to non-zero when the info struct contains valid data. * @phc_index: index of the PHC device. @@ -141,9 +131,4 @@ extern int sk_tx_timeout; */ extern int sk_check_fupsync; -/** - * Hardware time-stamp setting mode - */ -extern enum hwts_filter_mode sk_hwts_filter_mode; - #endif diff --git a/sk.h.hwtsfilter b/sk.h.hwtsfilter deleted file mode 100644 index d91d5d8..0000000 --- a/sk.h.hwtsfilter +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file sk.h - * @brief Implements protocol independent socket methods. - * @note Copyright (C) 2012 Richard Cochran - * - * 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_SK_H -#define HAVE_SK_H - -#include "address.h" -#include "transport.h" - -/** - * Contains timestamping information returned by the GET_TS_INFO ioctl. - * @valid: set to non-zero when the info struct contains valid data. - * @phc_index: index of the PHC device. - * @so_timestamping: supported time stamping modes. - * @tx_types: driver level transmit options for the HWTSTAMP ioctl. - * @rx_filters: driver level receive options for the HWTSTAMP ioctl. - */ -struct sk_ts_info { - int valid; - int phc_index; - unsigned int so_timestamping; - unsigned int tx_types; - unsigned int rx_filters; -}; - -/** - * Obtains a socket suitable for use with sk_interface_index(). - * @return An open socket on success, -1 otherwise. - */ -int sk_interface_fd(void); - -/** - * Obtain the numerical index from a network interface by name. - * @param fd An open socket. - * @param device The name of the network interface of interest. - * @return The result from the SIOCGIFINDEX ioctl. - */ -int sk_interface_index(int fd, const char *device); - -/** - * Prepare a given socket for PTP "general" messages. - * @param fd An open socket. - * @return Zero on success, non-zero otherwise. - */ -int sk_general_init(int fd); - -/** - * Obtain supported timestamping information - * @param name The name of the interface - * @param info Struct containing obtained timestamping information. - * @return zero on success, negative on failure. - */ -int sk_get_ts_info(const char *name, struct sk_ts_info *sk_info); - -/** - * Obtain the MAC address of a network interface. - * @param name The name of the interface - * @param mac Buffer to hold the result - * @return Zero on success, non-zero otherwise. - */ -int sk_interface_macaddr(const char *name, struct address *mac); - -/** - * Obtains the first IP address assigned to a network interface. - * @param name The name of the interface - * @param family The family of the address to get: AF_INET or AF_INET6 - * @param addr Buffer to hold the result - * @return The number of bytes written to addr on success, -1 otherwise. - */ -int sk_interface_addr(const char *name, int family, struct address *addr); - -/** - * Read a message from a socket. - * @param fd An open socket. - * @param buf Buffer to receive the message. - * @param buflen Size of 'buf' in bytes. - * @param addr Pointer to a buffer to receive the message's source - * address. May be NULL. - * @param hwts Pointer to a buffer to receive the message's time stamp. - * @param flags Flags to pass to RECV(2). - * @return - */ -int sk_receive(int fd, void *buf, int buflen, - struct address *addr, struct hw_timestamp *hwts, int flags); - -/** - * Set DSCP value for socket. - * @param fd An open socket. - * @param dscp The desired DSCP code. - * @return Zero on success, negative on failure - */ -int sk_set_priority(int fd, uint8_t dscp); - -/** - * Enable time stamping on a given network interface. - * @param fd An open socket. - * @param device The name of the network interface to configure. - * @param type The requested flavor of time stamping. - * @param transport The type of transport used. - * @return Zero on success, non-zero otherwise. - */ -int sk_timestamping_init(int fd, const char *device, enum timestamp_type type, - enum transport_type transport); - -/** - * Limits the time that RECVMSG(2) will poll while waiting for the tx timestamp - * if MSG_ERRQUEUE is set. Specified in milliseconds. - */ -extern int sk_tx_timeout; - -/** - * Enables the SO_TIMESTAMPNS socket option on the both the event and - * general sockets in order to test the order of paired sync and - * follow up messages using their network stack receipt time stamps. - */ -extern int sk_check_fupsync; - -#endif diff --git a/sysoff.c b/sysoff.c index b993ee9..f7b6240 100644 --- a/sysoff.c +++ b/sysoff.c @@ -18,11 +18,9 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include -#include #include #include -#include "print.h" #include "sysoff.h" #define NS_PER_SEC 1000000000LL @@ -40,23 +38,6 @@ static struct { uint64_t timestamp; } samples[PTP_MAX_SAMPLES]; -static int sysoff_precise(int fd, int64_t *result, uint64_t *ts) -{ -#ifdef PTP_SYS_OFFSET_PRECISE - struct ptp_sys_offset_precise pso; - memset(&pso, 0, sizeof(pso)); - if (ioctl(fd, PTP_SYS_OFFSET_PRECISE, &pso)) { - pr_debug("ioctl PTP_SYS_OFFSET_PRECISE: %m"); - return SYSOFF_RUN_TIME_MISSING; - } - *result = pctns(&pso.sys_realtime) - pctns(&pso.device); - *ts = pctns(&pso.sys_realtime); - return SYSOFF_PRECISE; -#else - return SYSOFF_COMPILE_TIME_MISSING; -#endif -} - static void insertion_sort(int length, int64_t interval, int64_t offset, uint64_t ts) { int i = length - 1; @@ -71,23 +52,17 @@ static void insertion_sort(int length, int64_t interval, int64_t offset, uint64_ samples[i+1].timestamp = ts; } -static int64_t sysoff_estimate(struct ptp_clock_time *pct, int extended, - int n_samples, uint64_t *ts, int64_t *delay) +static int64_t sysoff_estimate(struct ptp_clock_time *pct, int n_samples, + uint64_t *ts, int64_t *delay) { int64_t t1, t2, tp; int64_t interval, offset; int i; for (i = 0; i < n_samples; i++) { - if (extended) { - t1 = pctns(&pct[3*i]); - tp = pctns(&pct[3*i+1]); - t2 = pctns(&pct[3*i+2]); - } else { - t1 = pctns(&pct[2*i]); - tp = pctns(&pct[2*i+1]); - t2 = pctns(&pct[2*i+2]); - } + t1 = pctns(&pct[2*i]); + tp = pctns(&pct[2*i+1]); + t2 = pctns(&pct[2*i+2]); interval = t2 - t1; offset = (t2 + t1) / 2 - tp; insertion_sort(i, interval, offset, (t2 + t1) / 2); @@ -97,58 +72,23 @@ static int64_t sysoff_estimate(struct ptp_clock_time *pct, int extended, return samples[0].offset; } -static int sysoff_extended(int fd, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay) -{ -#ifdef PTP_SYS_OFFSET_EXTENDED - struct ptp_sys_offset_extended pso; - memset(&pso, 0, sizeof(pso)); - pso.n_samples = n_samples; - if (ioctl(fd, PTP_SYS_OFFSET_EXTENDED, &pso)) { - pr_debug("ioctl PTP_SYS_OFFSET_EXTENDED: %m"); - return SYSOFF_RUN_TIME_MISSING; - } - *result = sysoff_estimate(&pso.ts[0][0], 1, n_samples, ts, delay); - return SYSOFF_EXTENDED; -#else - return SYSOFF_COMPILE_TIME_MISSING; -#endif -} - -static int sysoff_basic(int fd, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay) +int sysoff_measure(int fd, int n_samples, + int64_t *result, uint64_t *ts, int64_t *delay) { struct ptp_sys_offset pso; - memset(&pso, 0, sizeof(pso)); pso.n_samples = n_samples; if (ioctl(fd, PTP_SYS_OFFSET, &pso)) { perror("ioctl PTP_SYS_OFFSET"); return SYSOFF_RUN_TIME_MISSING; } - *result = sysoff_estimate(pso.ts, 0, n_samples, ts, delay); - return SYSOFF_BASIC; -} - -int sysoff_measure(int fd, int method, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay) -{ - switch (method) { - case SYSOFF_PRECISE: - *delay = 0; - return sysoff_precise(fd, result, ts); - case SYSOFF_EXTENDED: - return sysoff_extended(fd, n_samples, result, ts, delay); - case SYSOFF_BASIC: - return sysoff_basic(fd, n_samples, result, ts, delay); - } - return SYSOFF_COMPILE_TIME_MISSING; + *result = sysoff_estimate(pso.ts, n_samples, ts, delay); + return SYSOFF_SUPPORTED; } int sysoff_probe(int fd, int n_samples) { int64_t junk, delay; uint64_t ts; - int i; if (n_samples > PTP_MAX_SAMPLES) { fprintf(stderr, "warning: %d exceeds kernel max readings %d\n", @@ -157,13 +97,7 @@ int sysoff_probe(int fd, int n_samples) return SYSOFF_RUN_TIME_MISSING; } - for (i = 0; i < SYSOFF_LAST; i++) { - if (sysoff_measure(fd, i, n_samples, &junk, &ts, &delay) < 0) - continue; - return i; - } - - return SYSOFF_RUN_TIME_MISSING; + return sysoff_measure(fd, n_samples, &junk, &ts, &delay); } #else /* !PTP_SYS_OFFSET */ diff --git a/sysoff.c.sysoff b/sysoff.c.sysoff deleted file mode 100644 index f7b6240..0000000 --- a/sysoff.c.sysoff +++ /dev/null @@ -1,116 +0,0 @@ -/** - * @file sysoff.c - * @brief Implements the system offset estimation method. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ -#include -#include -#include - -#include "sysoff.h" - -#define NS_PER_SEC 1000000000LL - -#ifdef PTP_SYS_OFFSET - -static int64_t pctns(struct ptp_clock_time *t) -{ - return t->sec * NS_PER_SEC + t->nsec; -} - -static struct { - int64_t interval; - int64_t offset; - uint64_t timestamp; -} samples[PTP_MAX_SAMPLES]; - -static void insertion_sort(int length, int64_t interval, int64_t offset, uint64_t ts) -{ - int i = length - 1; - while (i >= 0) { - if (samples[i].interval < interval) - break; - samples[i+1] = samples[i]; - i--; - } - samples[i+1].interval = interval; - samples[i+1].offset = offset; - samples[i+1].timestamp = ts; -} - -static int64_t sysoff_estimate(struct ptp_clock_time *pct, int n_samples, - uint64_t *ts, int64_t *delay) -{ - int64_t t1, t2, tp; - int64_t interval, offset; - int i; - - for (i = 0; i < n_samples; i++) { - t1 = pctns(&pct[2*i]); - tp = pctns(&pct[2*i+1]); - t2 = pctns(&pct[2*i+2]); - interval = t2 - t1; - offset = (t2 + t1) / 2 - tp; - insertion_sort(i, interval, offset, (t2 + t1) / 2); - } - *ts = samples[0].timestamp; - *delay = samples[0].interval; - return samples[0].offset; -} - -int sysoff_measure(int fd, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay) -{ - struct ptp_sys_offset pso; - pso.n_samples = n_samples; - if (ioctl(fd, PTP_SYS_OFFSET, &pso)) { - perror("ioctl PTP_SYS_OFFSET"); - return SYSOFF_RUN_TIME_MISSING; - } - *result = sysoff_estimate(pso.ts, n_samples, ts, delay); - return SYSOFF_SUPPORTED; -} - -int sysoff_probe(int fd, int n_samples) -{ - int64_t junk, delay; - uint64_t ts; - - if (n_samples > PTP_MAX_SAMPLES) { - fprintf(stderr, "warning: %d exceeds kernel max readings %d\n", - n_samples, PTP_MAX_SAMPLES); - fprintf(stderr, "falling back to clock_gettime method\n"); - return SYSOFF_RUN_TIME_MISSING; - } - - return sysoff_measure(fd, n_samples, &junk, &ts, &delay); -} - -#else /* !PTP_SYS_OFFSET */ - -int sysoff_measure(int fd, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay) -{ - return SYSOFF_COMPILE_TIME_MISSING; -} - -int sysoff_probe(int fd, int n_samples) -{ - return SYSOFF_COMPILE_TIME_MISSING; -} - -#endif /* PTP_SYS_OFFSET */ diff --git a/sysoff.h b/sysoff.h index 79d2290..cb70265 100644 --- a/sysoff.h +++ b/sysoff.h @@ -21,16 +21,13 @@ #include enum { - SYSOFF_COMPILE_TIME_MISSING = -2, - SYSOFF_RUN_TIME_MISSING = -1, - SYSOFF_PRECISE, - SYSOFF_EXTENDED, - SYSOFF_BASIC, - SYSOFF_LAST, + SYSOFF_SUPPORTED, + SYSOFF_COMPILE_TIME_MISSING, + SYSOFF_RUN_TIME_MISSING, }; /** - * Check to see if a PTP_SYS_OFFSET ioctl is supported. + * Check to see if the PTP_SYS_OFFSET ioctl is supported. * @param fd An open file descriptor to a PHC device. * @return One of the SYSOFF_ enumeration values. */ @@ -39,12 +36,11 @@ int sysoff_probe(int fd, int n_samples); /** * Measure the offset between a PHC and the system time. * @param fd An open file descriptor to a PHC device. - * @param method A non-negative SYSOFF_ value returned by sysoff_probe(). * @param n_samples The number of consecutive readings to make. * @param result The estimated offset in nanoseconds. * @param ts The system time corresponding to the 'result'. * @param delay The delay in reading of the clock in nanoseconds. * @return One of the SYSOFF_ enumeration values. */ -int sysoff_measure(int fd, int method, int n_samples, +int sysoff_measure(int fd, int n_samples, int64_t *result, uint64_t *ts, int64_t *delay); diff --git a/sysoff.h.sysoff b/sysoff.h.sysoff deleted file mode 100644 index cb70265..0000000 --- a/sysoff.h.sysoff +++ /dev/null @@ -1,46 +0,0 @@ -/** - * @file sysoff.h - * @brief Implements the system offset estimation method. - * @note Copyright (C) 2012 Richard Cochran - * - * 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. - */ - -#include - -enum { - SYSOFF_SUPPORTED, - SYSOFF_COMPILE_TIME_MISSING, - SYSOFF_RUN_TIME_MISSING, -}; - -/** - * Check to see if the PTP_SYS_OFFSET ioctl is supported. - * @param fd An open file descriptor to a PHC device. - * @return One of the SYSOFF_ enumeration values. - */ -int sysoff_probe(int fd, int n_samples); - -/** - * Measure the offset between a PHC and the system time. - * @param fd An open file descriptor to a PHC device. - * @param n_samples The number of consecutive readings to make. - * @param result The estimated offset in nanoseconds. - * @param ts The system time corresponding to the 'result'. - * @param delay The delay in reading of the clock in nanoseconds. - * @return One of the SYSOFF_ enumeration values. - */ -int sysoff_measure(int fd, int n_samples, - int64_t *result, uint64_t *ts, int64_t *delay); diff --git a/timemaster.c b/timemaster.c index 28c215a..4ba921e 100644 --- a/timemaster.c +++ b/timemaster.c @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include diff --git a/timemaster.c.headers b/timemaster.c.headers deleted file mode 100644 index 4ba921e..0000000 --- a/timemaster.c.headers +++ /dev/null @@ -1,1367 +0,0 @@ -/** - * @file timemaster.c - * @brief Program to run NTP with PTP as reference clocks. - * @note Copyright (C) 2014 Miroslav Lichvar - * - * 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. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "print.h" -#include "rtnl.h" -#include "sk.h" -#include "util.h" -#include "version.h" - -#define DEFAULT_RUNDIR "/var/run/timemaster" - -#define DEFAULT_FIRST_SHM_SEGMENT 0 -#define DEFAULT_RESTART_PROCESSES 1 - -#define DEFAULT_NTP_PROGRAM CHRONYD -#define DEFAULT_NTP_MINPOLL 6 -#define DEFAULT_NTP_MAXPOLL 10 -#define DEFAULT_PTP_DELAY 1e-4 -#define DEFAULT_PTP_NTP_POLL 2 -#define DEFAULT_PTP_PHC2SYS_POLL 0 - -#define DEFAULT_CHRONYD_SETTINGS \ - "makestep 1 3" -#define DEFAULT_NTPD_SETTINGS \ - "restrict default nomodify notrap nopeer noquery", \ - "restrict 127.0.0.1", \ - "restrict ::1" -#define DEFAULT_PTP4L_OPTIONS "-l", "5" -#define DEFAULT_PHC2SYS_OPTIONS "-l", "5" - -enum source_type { - NTP_SERVER, - PTP_DOMAIN, -}; - -enum ntp_program { - CHRONYD, - NTPD, -}; - -struct ntp_server { - char *address; - int minpoll; - int maxpoll; - int iburst; - char *ntp_options; -}; - -struct ptp_domain { - int domain; - int ntp_poll; - int phc2sys_poll; - double delay; - char **interfaces; - char **ptp4l_settings; - char *ntp_options; -}; - -struct source { - enum source_type type; - union { - struct ntp_server ntp; - struct ptp_domain ptp; - }; -}; - -struct program_config { - char *path; - char **options; - char **settings; -}; - -struct timemaster_config { - struct source **sources; - enum ntp_program ntp_program; - char *rundir; - int first_shm_segment; - int restart_processes; - struct program_config chronyd; - struct program_config ntpd; - struct program_config phc2sys; - struct program_config ptp4l; -}; - -struct config_file { - char *path; - char *content; -}; - -struct script { - struct config_file **configs; - char ***commands; - int **command_groups; - int restart_groups; - int no_restart_group; -}; - -static void free_parray(void **a) -{ - void **p; - - for (p = a; *p; p++) - free(*p); - free(a); -} - -static void extend_string_array(char ***a, char **strings) -{ - char **s; - - for (s = strings; *s; s++) - parray_append((void ***)a, xstrdup(*s)); -} - -static void extend_config_string(char **s, char **lines) -{ - for (; *lines; lines++) - string_appendf(s, "%s\n", *lines); -} - -static int parse_bool(char *s, int *b) -{ - if (get_ranged_int(s, b, 0, 1) != PARSED_OK) - return 1; - - return 0; -} - -static int parse_int(char *s, int *i) -{ - if (get_ranged_int(s, i, INT_MIN, INT_MAX) != PARSED_OK) - return 1; - - return 0; -} - -static int parse_double(char *s, double *d) -{ - if (get_ranged_double(s, d, INT_MIN, INT_MAX) != PARSED_OK) - return 1; - - return 0; -} - -static char *parse_word(char *s) -{ - while (*s && !isspace(*s)) - s++; - while (*s && isspace(*s)) - *(s++) = '\0'; - return s; -} - -static void parse_words(char *s, char ***a) -{ - char *w; - - if (**a) { - free_parray((void **)(*a)); - *a = (char **)parray_new(); - } - while (*s) { - w = s; - s = parse_word(s); - parray_append((void ***)a, xstrdup(w)); - } -} - -static void replace_string(char *s, char **str) -{ - if (*str) - free(*str); - *str = xstrdup(s); -} - -static char *parse_section_name(char *s) -{ - char *s1, *s2; - - s1 = s + 1; - for (s2 = s1; *s2 && *s2 != ']'; s2++) - ; - *s2 = '\0'; - - return xstrdup(s1); -} - -static void parse_setting(char *s, char **name, char **value) -{ - *name = s; - for (*value = s; **value && !isspace(**value); (*value)++) - ; - for (; **value && !isspace(**value); (*value)++) - ; - for (; **value && isspace(**value); (*value)++) - **value = '\0'; -} - -static void source_destroy(struct source *source) -{ - switch (source->type) { - case NTP_SERVER: - free(source->ntp.address); - free(source->ntp.ntp_options); - break; - case PTP_DOMAIN: - free_parray((void **)source->ptp.interfaces); - free_parray((void **)source->ptp.ptp4l_settings); - free(source->ptp.ntp_options); - break; - } - free(source); -} - -static struct source *source_ntp_parse(char *parameter, char **settings) -{ - char *name, *value; - struct source *source; - int r = 0; - - if (!*parameter) { - pr_err("missing address for ntp_server"); - return NULL; - } - - source = xmalloc(sizeof(*source)); - source->type = NTP_SERVER; - source->ntp.address = xstrdup(parameter); - source->ntp.minpoll = DEFAULT_NTP_MINPOLL; - source->ntp.maxpoll = DEFAULT_NTP_MAXPOLL; - source->ntp.iburst = 0; - source->ntp.ntp_options = xstrdup(""); - - for (; *settings; settings++) { - parse_setting(*settings, &name, &value); - if (!strcasecmp(name, "minpoll")) { - r = parse_int(value, &source->ntp.minpoll); - } else if (!strcasecmp(name, "maxpoll")) { - r = parse_int(value, &source->ntp.maxpoll); - } else if (!strcasecmp(name, "iburst")) { - r = parse_bool(value, &source->ntp.iburst); - } else if (!strcasecmp(name, "ntp_options")) { - replace_string(value, &source->ntp.ntp_options); - } else { - pr_err("unknown ntp_server setting %s", name); - goto failed; - } - if (r) { - pr_err("invalid value %s for %s", value, name); - goto failed; - } - } - - return source; -failed: - source_destroy(source); - return NULL; -} - -static struct source *source_ptp_parse(char *parameter, char **settings) -{ - char *name, *value; - struct source *source; - int r = 0; - - source = xmalloc(sizeof(*source)); - source->type = PTP_DOMAIN; - source->ptp.delay = DEFAULT_PTP_DELAY; - source->ptp.ntp_poll = DEFAULT_PTP_NTP_POLL; - source->ptp.phc2sys_poll = DEFAULT_PTP_PHC2SYS_POLL; - source->ptp.interfaces = (char **)parray_new(); - source->ptp.ptp4l_settings = (char **)parray_new(); - source->ptp.ntp_options = xstrdup(""); - - if (parse_int(parameter, &source->ptp.domain)) { - pr_err("invalid ptp_domain number %s", parameter); - goto failed; - } - - for (; *settings; settings++) { - parse_setting(*settings, &name, &value); - if (!strcasecmp(name, "delay")) { - r = parse_double(value, &source->ptp.delay); - } else if (!strcasecmp(name, "ntp_poll")) { - r = parse_int(value, &source->ptp.ntp_poll); - } else if (!strcasecmp(name, "phc2sys_poll")) { - r = parse_int(value, &source->ptp.phc2sys_poll); - } else if (!strcasecmp(name, "ptp4l_option")) { - parray_append((void ***)&source->ptp.ptp4l_settings, - xstrdup(value)); - } else if (!strcasecmp(name, "ntp_options")) { - replace_string(value, &source->ptp.ntp_options); - } else if (!strcasecmp(name, "interfaces")) { - parse_words(value, &source->ptp.interfaces); - } else { - pr_err("unknown ptp_domain setting %s", name); - goto failed; - } - - if (r) { - pr_err("invalid value %s for %s", value, name); - goto failed; - } - } - - if (!*source->ptp.interfaces) { - pr_err("no interfaces specified for ptp_domain %d", - source->ptp.domain); - goto failed; - } - - return source; -failed: - source_destroy(source); - return NULL; -} - -static int parse_program_settings(char **settings, - struct program_config *config) -{ - char *name, *value; - - for (; *settings; settings++) { - parse_setting(*settings, &name, &value); - if (!strcasecmp(name, "path")) { - replace_string(value, &config->path); - } else if (!strcasecmp(name, "options")) { - parse_words(value, &config->options); - } else { - pr_err("unknown program setting %s", name); - return 1; - } - } - - return 0; -} - -static int parse_timemaster_settings(char **settings, - struct timemaster_config *config) -{ - char *name, *value; - int r = 0; - - for (; *settings; settings++) { - parse_setting(*settings, &name, &value); - if (!strcasecmp(name, "ntp_program")) { - if (!strcasecmp(value, "chronyd")) { - config->ntp_program = CHRONYD; - } else if (!strcasecmp(value, "ntpd")) { - config->ntp_program = NTPD; - } else { - pr_err("unknown ntp program %s", value); - return 1; - } - } else if (!strcasecmp(name, "rundir")) { - replace_string(value, &config->rundir); - } else if (!strcasecmp(name, "first_shm_segment")) { - r = parse_int(value, &config->first_shm_segment); - } else if (!strcasecmp(name, "restart_processes")) { - r = parse_int(value, &config->restart_processes); - } else { - pr_err("unknown timemaster setting %s", name); - return 1; - } - if (r) { - pr_err("invalid value %s for %s", value, name); - return 1; - } - } - - return 0; -} - -static int parse_section(char **settings, char *name, - struct timemaster_config *config) -{ - struct source *source = NULL; - char ***settings_dst = NULL; - char *parameter = parse_word(name); - - if (!strcasecmp(name, "ntp_server")) { - source = source_ntp_parse(parameter, settings); - if (!source) - return 1; - } else if (!strcasecmp(name, "ptp_domain")) { - source = source_ptp_parse(parameter, settings); - if (!source) - return 1; - } else if (!strcasecmp(name, "chrony.conf")) { - settings_dst = &config->chronyd.settings; - } else if (!strcasecmp(name, "ntp.conf")) { - settings_dst = &config->ntpd.settings; - } else if (!strcasecmp(name, "ptp4l.conf")) { - settings_dst = &config->ptp4l.settings; - } else if (!strcasecmp(name, "chronyd")) { - if (parse_program_settings(settings, &config->chronyd)) - return 1; - } else if (!strcasecmp(name, "ntpd")) { - if (parse_program_settings(settings, &config->ntpd)) - return 1; - } else if (!strcasecmp(name, "phc2sys")) { - if (parse_program_settings(settings, &config->phc2sys)) - return 1; - } else if (!strcasecmp(name, "ptp4l")) { - if (parse_program_settings(settings, &config->ptp4l)) - return 1; - } else if (!strcasecmp(name, "timemaster")) { - if (parse_timemaster_settings(settings, config)) - return 1; - } else { - pr_err("unknown section %s", name); - return 1; - } - - if (source) - parray_append((void ***)&config->sources, source); - - if (settings_dst) { - free_parray((void **)*settings_dst); - *settings_dst = (char **)parray_new(); - extend_string_array(settings_dst, settings); - } - - return 0; -} - -static void init_program_config(struct program_config *config, - const char *name, ...) -{ - const char *s; - va_list ap; - - config->path = xstrdup(name); - config->settings = (char **)parray_new(); - config->options = (char **)parray_new(); - - va_start(ap, name); - - /* add default options and settings */ - while ((s = va_arg(ap, const char *))) - parray_append((void ***)&config->options, xstrdup(s)); - while ((s = va_arg(ap, const char *))) - parray_append((void ***)&config->settings, xstrdup(s)); - - va_end(ap); -} - -static void free_program_config(struct program_config *config) -{ - free(config->path); - free_parray((void **)config->settings); - free_parray((void **)config->options); -} - -static void config_destroy(struct timemaster_config *config) -{ - struct source **sources; - - for (sources = config->sources; *sources; sources++) - source_destroy(*sources); - free(config->sources); - - free_program_config(&config->chronyd); - free_program_config(&config->ntpd); - free_program_config(&config->phc2sys); - free_program_config(&config->ptp4l); - - free(config->rundir); - free(config); -} - -static struct timemaster_config *config_parse(char *path) -{ - struct timemaster_config *config = xcalloc(1, sizeof(*config)); - FILE *f; - char buf[4096], *line, *section_name = NULL; - char **section_lines = NULL; - int ret = 0; - - config->sources = (struct source **)parray_new(); - config->ntp_program = DEFAULT_NTP_PROGRAM; - config->rundir = xstrdup(DEFAULT_RUNDIR); - config->first_shm_segment = DEFAULT_FIRST_SHM_SEGMENT; - config->restart_processes = DEFAULT_RESTART_PROCESSES; - - init_program_config(&config->chronyd, "chronyd", - NULL, DEFAULT_CHRONYD_SETTINGS, NULL); - init_program_config(&config->ntpd, "ntpd", - NULL, DEFAULT_NTPD_SETTINGS, NULL); - init_program_config(&config->phc2sys, "phc2sys", - DEFAULT_PHC2SYS_OPTIONS, NULL, NULL); - init_program_config(&config->ptp4l, "ptp4l", - DEFAULT_PTP4L_OPTIONS, NULL, NULL); - - f = fopen(path, "r"); - if (!f) { - pr_err("failed to open %s: %m", path); - free(config); - return NULL; - } - - while (fgets(buf, sizeof(buf), f)) { - /* remove trailing and leading whitespace */ - for (line = buf + strlen(buf) - 1; - line >= buf && isspace(*line); line--) - *line = '\0'; - for (line = buf; *line && isspace(*line); line++) - ; - /* skip comments and empty lines */ - if (!*line || *line == '#') - continue; - - if (*line == '[') { - /* parse previous section before starting another */ - if (section_name) { - if (parse_section(section_lines, section_name, - config)) { - ret = 1; - break; - } - free_parray((void **)section_lines); - free(section_name); - } - section_name = parse_section_name(line); - section_lines = (char **)parray_new(); - continue; - } - - if (!section_lines) { - pr_err("settings outside section"); - ret = 1; - break; - } - - parray_append((void ***)§ion_lines, xstrdup(line)); - } - - if (!ret && section_name && - parse_section(section_lines, section_name, config)) { - ret = 1; - } - - fclose(f); - - if (section_name) - free(section_name); - if (section_lines) - free_parray((void **)section_lines); - - if (ret) { - config_destroy(config); - return NULL; - } - - return config; -} - -static char **get_ptp4l_command(struct program_config *config, - struct config_file *file, char **interfaces, - int hw_ts) -{ - char **command = (char **)parray_new(); - - parray_append((void ***)&command, xstrdup(config->path)); - extend_string_array(&command, config->options); - parray_extend((void ***)&command, - xstrdup("-f"), xstrdup(file->path), - xstrdup(hw_ts ? "-H" : "-S"), NULL); - - for (; *interfaces; interfaces++) - parray_extend((void ***)&command, - xstrdup("-i"), xstrdup(*interfaces), NULL); - - return command; -} - -static char **get_phc2sys_command(struct program_config *config, int domain, - int poll, int shm_segment, char *uds_path, - char *message_tag) -{ - char **command = (char **)parray_new(); - - parray_append((void ***)&command, xstrdup(config->path)); - extend_string_array(&command, config->options); - parray_extend((void ***)&command, - xstrdup("-a"), xstrdup("-r"), - xstrdup("-R"), string_newf("%.2f", poll > 0 ? - 1.0 / (1 << poll) : 1 << -poll), - xstrdup("-z"), xstrdup(uds_path), - xstrdup("-t"), xstrdup(message_tag), - xstrdup("-n"), string_newf("%d", domain), - xstrdup("-E"), xstrdup("ntpshm"), - xstrdup("-M"), string_newf("%d", shm_segment), NULL); - - return command; -} - -static char *get_refid(char *prefix, unsigned int number) -{ - if (number < 10) - return string_newf("%.3s%u", prefix, number); - else if (number < 100) - return string_newf("%.2s%u", prefix, number); - else if (number < 1000) - return string_newf("%.1s%u", prefix, number); - return NULL; -}; - -static void add_command(char **command, int command_group, - struct script *script) -{ - int *group; - - parray_append((void ***)&script->commands, command); - - group = xmalloc(sizeof(int)); - *group = command_group; - parray_append((void ***)&script->command_groups, group); -} - -static void add_shm_source(int shm_segment, int poll, int dpoll, double delay, - char *ntp_options, char *prefix, - struct timemaster_config *config, char **ntp_config) -{ - char *refid = get_refid(prefix, shm_segment); - - switch (config->ntp_program) { - case CHRONYD: - string_appendf(ntp_config, - "refclock SHM %d poll %d dpoll %d " - "refid %s precision 1.0e-9 delay %.1e %s\n", - shm_segment, poll, dpoll, refid, delay, - ntp_options); - break; - case NTPD: - string_appendf(ntp_config, - "server 127.127.28.%d minpoll %d maxpoll %d " - "mode 1 %s\n" - "fudge 127.127.28.%d refid %s\n", - shm_segment, poll, poll, ntp_options, - shm_segment, refid); - break; - } - - free(refid); -} - -static int add_ntp_source(struct ntp_server *source, char **ntp_config) -{ - pr_debug("adding NTP server %s", source->address); - - string_appendf(ntp_config, "server %s minpoll %d maxpoll %d %s %s\n", - source->address, source->minpoll, source->maxpoll, - source->iburst ? "iburst" : "", source->ntp_options); - return 0; -} - -static int add_ptp_source(struct ptp_domain *source, - struct timemaster_config *config, int *shm_segment, - int *command_group, int ***allocated_phcs, - char **ntp_config, struct script *script) -{ - struct config_file *config_file; - char **command, *uds_path, **interfaces, *message_tag; - char ts_interface[IF_NAMESIZE]; - int i, j, num_interfaces, *phc, *phcs, hw_ts, sw_ts; - struct sk_ts_info ts_info; - - pr_debug("adding PTP domain %d", source->domain); - - hw_ts = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - sw_ts = SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - - for (num_interfaces = 0; - source->interfaces[num_interfaces]; num_interfaces++) - ; - - if (!num_interfaces) - return 0; - - /* get PHCs used by specified interfaces */ - phcs = xmalloc(num_interfaces * sizeof(int)); - for (i = 0; i < num_interfaces; i++) { - phcs[i] = -1; - - /* - * if it is a bonded interface, use the name of the active - * slave interface (which will be timestamping packets) - */ - if (!rtnl_get_ts_device(source->interfaces[i], ts_interface)) { - pr_debug("slave interface of %s: %s", - source->interfaces[i], ts_interface); - } else { - snprintf(ts_interface, sizeof(ts_interface), "%s", - source->interfaces[i]); - } - - /* check if the interface has a usable PHC */ - if (sk_get_ts_info(ts_interface, &ts_info)) { - pr_err("failed to get time stamping info for %s", - ts_interface); - free(phcs); - return 1; - } - - if (((ts_info.so_timestamping & hw_ts) != hw_ts)) { - pr_debug("interface %s: no PHC", ts_interface); - if ((ts_info.so_timestamping & sw_ts) != sw_ts) { - pr_err("time stamping not supported on %s", - ts_interface); - free(phcs); - return 1; - } - continue; - } - - pr_debug("interface %s: PHC %d", ts_interface, - ts_info.phc_index); - - /* and the PHC isn't already used in another source */ - for (j = 0; (*allocated_phcs)[j]; j++) { - if (*(*allocated_phcs)[j] == ts_info.phc_index) { - pr_debug("PHC %d already allocated", - ts_info.phc_index); - break; - } - } - if (!(*allocated_phcs)[j]) - phcs[i] = ts_info.phc_index; - } - - for (i = 0; i < num_interfaces; i++) { - /* skip if already used by ptp4l in this domain */ - if (phcs[i] == -2) - continue; - - interfaces = (char **)parray_new(); - parray_append((void ***)&interfaces, source->interfaces[i]); - - /* merge all interfaces sharing PHC to one ptp4l command */ - if (phcs[i] >= 0) { - for (j = i + 1; j < num_interfaces; j++) { - if (phcs[i] == phcs[j]) { - parray_append((void ***)&interfaces, - source->interfaces[j]); - /* mark the interface as used */ - phcs[j] = -2; - } - } - - /* don't use this PHC in other sources */ - phc = xmalloc(sizeof(int)); - *phc = phcs[i]; - parray_append((void ***)allocated_phcs, phc); - } - - uds_path = string_newf("%s/ptp4l.%d.socket", - config->rundir, *shm_segment); - - message_tag = string_newf("[%d", source->domain); - for (j = 0; interfaces[j]; j++) - string_appendf(&message_tag, "%s%s", j ? "+" : ":", - interfaces[j]); - string_appendf(&message_tag, "]"); - - config_file = xmalloc(sizeof(*config_file)); - config_file->path = string_newf("%s/ptp4l.%d.conf", - config->rundir, *shm_segment); - config_file->content = xstrdup("[global]\n"); - extend_config_string(&config_file->content, - config->ptp4l.settings); - extend_config_string(&config_file->content, - source->ptp4l_settings); - string_appendf(&config_file->content, - "slaveOnly 1\n" - "domainNumber %d\n" - "uds_address %s\n" - "message_tag %s\n", - source->domain, uds_path, message_tag); - - if (phcs[i] >= 0) { - /* HW time stamping */ - command = get_ptp4l_command(&config->ptp4l, config_file, - interfaces, 1); - add_command(command, *command_group, script); - - command = get_phc2sys_command(&config->phc2sys, - source->domain, - source->phc2sys_poll, - *shm_segment, uds_path, - message_tag); - add_command(command, (*command_group)++, script); - } else { - /* SW time stamping */ - command = get_ptp4l_command(&config->ptp4l, config_file, - interfaces, 0); - add_command(command, (*command_group)++, script); - - string_appendf(&config_file->content, - "clock_servo ntpshm\n" - "ntpshm_segment %d\n", *shm_segment); - } - - parray_append((void ***)&script->configs, config_file); - - add_shm_source(*shm_segment, source->ntp_poll, - source->phc2sys_poll, source->delay, - source->ntp_options, "PTP", config, ntp_config); - - (*shm_segment)++; - - free(message_tag); - free(uds_path); - free(interfaces); - } - - free(phcs); - - return 0; -} - -static char **get_chronyd_command(struct program_config *config, - struct config_file *file) -{ - char **command = (char **)parray_new(); - - parray_append((void ***)&command, xstrdup(config->path)); - extend_string_array(&command, config->options); - parray_extend((void ***)&command, xstrdup("-n"), - xstrdup("-f"), xstrdup(file->path), NULL); - - return command; -} - -static char **get_ntpd_command(struct program_config *config, - struct config_file *file) -{ - char **command = (char **)parray_new(); - - parray_append((void ***)&command, xstrdup(config->path)); - extend_string_array(&command, config->options); - parray_extend((void ***)&command, xstrdup("-n"), - xstrdup("-c"), xstrdup(file->path), NULL); - - return command; -} - -static struct config_file *add_ntp_program(struct timemaster_config *config, - struct script *script, - int command_group) -{ - struct config_file *ntp_config = xmalloc(sizeof(*ntp_config)); - char **command = NULL; - - ntp_config->content = xstrdup(""); - - switch (config->ntp_program) { - case CHRONYD: - extend_config_string(&ntp_config->content, - config->chronyd.settings); - ntp_config->path = string_newf("%s/chrony.conf", - config->rundir); - command = get_chronyd_command(&config->chronyd, ntp_config); - break; - case NTPD: - extend_config_string(&ntp_config->content, - config->ntpd.settings); - ntp_config->path = string_newf("%s/ntp.conf", config->rundir); - command = get_ntpd_command(&config->ntpd, ntp_config); - break; - } - - parray_append((void ***)&script->configs, ntp_config); - add_command(command, command_group, script); - - return ntp_config; -} - -static void script_destroy(struct script *script) -{ - char ***commands, **command; - int **groups; - struct config_file *config, **configs; - - for (configs = script->configs; *configs; configs++) { - config = *configs; - free(config->path); - free(config->content); - free(config); - } - free(script->configs); - - for (commands = script->commands; *commands; commands++) { - for (command = *commands; *command; command++) - free(*command); - free(*commands); - } - free(script->commands); - - for (groups = script->command_groups; *groups; groups++) - free(*groups); - free(script->command_groups); - - free(script); -} - -static struct script *script_create(struct timemaster_config *config) -{ - struct script *script = xmalloc(sizeof(*script)); - struct source *source, **sources; - struct config_file *ntp_config = NULL; - int **allocated_phcs = (int **)parray_new(); - int ret = 0, shm_segment, command_group = 0; - - script->configs = (struct config_file **)parray_new(); - script->commands = (char ***)parray_new(); - script->command_groups = (int **)parray_new(); - script->no_restart_group = command_group; - script->restart_groups = config->restart_processes; - - ntp_config = add_ntp_program(config, script, command_group++); - shm_segment = config->first_shm_segment; - - for (sources = config->sources; (source = *sources); sources++) { - switch (source->type) { - case NTP_SERVER: - if (add_ntp_source(&source->ntp, &ntp_config->content)) - ret = 1; - break; - case PTP_DOMAIN: - if (add_ptp_source(&source->ptp, config, &shm_segment, - &command_group, &allocated_phcs, - &ntp_config->content, script)) - ret = 1; - break; - } - } - - free_parray((void **)allocated_phcs); - - if (ret) { - script_destroy(script); - return NULL; - } - - return script; -} - -static pid_t start_program(char **command, sigset_t *mask) -{ - char **arg, *s; - pid_t pid; - -#ifdef HAVE_POSIX_SPAWN - posix_spawnattr_t attr; - - if (posix_spawnattr_init(&attr)) { - pr_err("failed to init spawn attributes: %m"); - return 0; - } - - if (posix_spawnattr_setsigmask(&attr, mask) || - posix_spawnattr_setflags(&attr, POSIX_SPAWN_SETSIGMASK) || - posix_spawnp(&pid, command[0], NULL, &attr, command, environ)) { - pr_err("failed to spawn %s: %m", command[0]); - posix_spawnattr_destroy(&attr); - return 0; - } - - posix_spawnattr_destroy(&attr); -#else - pid = fork(); - - if (pid < 0) { - pr_err("fork() failed: %m"); - return 0; - } - - if (!pid) { - /* restore the signal mask */ - if (sigprocmask(SIG_SETMASK, mask, NULL) < 0) { - pr_err("sigprocmask() failed: %m"); - exit(100); - } - - execvp(command[0], (char **)command); - - pr_err("failed to execute %s: %m", command[0]); - - exit(101); - } -#endif - - for (s = xstrdup(""), arg = command; *arg; arg++) - string_appendf(&s, "%s ", *arg); - - pr_info("process %d started: %s", pid, s); - - free(s); - - return pid; -} - -static int create_config_files(struct config_file **configs) -{ - struct config_file *config; - FILE *file; - char *tmp, *dir; - struct stat st; - - for (; (config = *configs); configs++) { - tmp = xstrdup(config->path); - dir = dirname(tmp); - if (stat(dir, &st) < 0 && errno == ENOENT && - mkdir(dir, 0755) < 0) { - pr_err("failed to create %s: %m", dir); - free(tmp); - return 1; - } - free(tmp); - - pr_debug("creating %s", config->path); - - file = fopen(config->path, "w"); - if (!file) { - pr_err("failed to open %s: %m", config->path); - return 1; - } - - if (fwrite(config->content, - strlen(config->content), 1, file) != 1) { - pr_err("failed to write to %s", config->path); - fclose(file); - return 1; - } - - fclose(file); - } - - return 0; -} - -static int remove_config_files(struct config_file **configs) -{ - struct config_file *config; - - for (; (config = *configs); configs++) { - pr_debug("removing %s", config->path); - - if (unlink(config->path)) - pr_err("failed to remove %s: %m", config->path); - } - - return 0; -} - -static int script_run(struct script *script) -{ - struct timespec ts_start, ts_now; - sigset_t mask, old_mask; - siginfo_t info; - pid_t pid, *pids; - int i, group, num_commands, status, quit = 0, ret = 0; - - for (num_commands = 0; script->commands[num_commands]; num_commands++) - ; - - if (!num_commands) { - /* nothing to do */ - return 0; - } - - if (create_config_files(script->configs)) - return 1; - - sigemptyset(&mask); - sigaddset(&mask, SIGCHLD); - sigaddset(&mask, SIGTERM); - sigaddset(&mask, SIGQUIT); - sigaddset(&mask, SIGINT); - - /* block the signals */ - if (sigprocmask(SIG_BLOCK, &mask, &old_mask) < 0) { - pr_err("sigprocmask() failed: %m"); - return 1; - } - - pids = xcalloc(num_commands, sizeof(*pids)); - - for (i = 0; i < num_commands; i++) { - pids[i] = start_program(script->commands[i], &old_mask); - if (!pids[i]) { - kill(getpid(), SIGTERM); - break; - } - } - - clock_gettime(CLOCK_MONOTONIC, &ts_start); - - /* process the blocked signals */ - while (1) { - if (sigwaitinfo(&mask, &info) < 0) { - if (errno == EINTR) - continue; - pr_err("sigwaitinfo() failed: %m"); - break; - } - - clock_gettime(CLOCK_MONOTONIC, &ts_now); - - if (info.si_signo != SIGCHLD) { - if (quit) - continue; - - quit = 1; - pr_debug("exiting on signal %d", info.si_signo); - - /* terminate remaining processes */ - for (i = 0; i < num_commands; i++) { - if (pids[i] > 0) { - pr_debug("killing process %d", pids[i]); - kill(pids[i], SIGTERM); - } - } - - continue; - } - - /* wait for all terminated processes */ - while (1) { - pid = waitpid(-1, &status, WNOHANG); - if (pid <= 0) - break; - - if (!WIFEXITED(status)) { - pr_info("process %d terminated abnormally", - pid); - } else { - pr_info("process %d terminated with status %d", - pid, WEXITSTATUS(status)); - } - - for (i = 0; i < num_commands; i++) { - if (pids[i] == pid) - pids[i] = 0; - } - } - - /* wait for all processes to terminate when exiting */ - if (quit) { - for (i = 0; i < num_commands; i++) { - if (pids[i]) - break; - } - if (i == num_commands) - break; - - pr_debug("waiting for other processes to terminate"); - continue; - } - - /* - * terminate (and then restart if allowed) all processes in - * groups that have a terminated process - */ - for (group = 0; group < num_commands; group++) { - int terminated = 0, running = 0; - - for (i = 0; i < num_commands; i++) { - if (*(script->command_groups[i]) != group) - continue; - if (pids[i]) - running++; - else - terminated++; - } - - if (!terminated) - continue; - - /* - * exit with a non-zero status if the group should not - * be restarted (i.e. chronyd/ntpd), timemaster is - * running only for a short time (and it is likely a - * configuration error), or restarting is disabled - * completely - */ - if (group == script->no_restart_group || - ts_now.tv_sec - ts_start.tv_sec <= 1 || - !script->restart_groups) { - kill(getpid(), SIGTERM); - ret = 1; - break; - } - - for (i = 0; i < num_commands; i++) { - if (*(script->command_groups[i]) != group) - continue; - - /* terminate all processes in the group first */ - if (running && pids[i]) { - pr_debug("killing process %d", pids[i]); - kill(pids[i], SIGTERM); - } else if (!running && !pids[i]) { - pids[i] = start_program(script->commands[i], - &old_mask); - if (!pids[i]) - kill(getpid(), SIGTERM); - - /* limit restarting rate */ - sleep(1); - } - } - } - } - - free(pids); - - if (remove_config_files(script->configs)) - return 1; - - return ret; -} - -static void script_print(struct script *script) -{ - char ***commands, **command; - int **groups; - struct config_file *config, **configs; - - for (configs = script->configs; *configs; configs++) { - config = *configs; - fprintf(stderr, "%s:\n\n%s\n", config->path, config->content); - } - - fprintf(stderr, "commands:\n\n"); - for (commands = script->commands, groups = script->command_groups; - *commands; commands++, groups++) { - fprintf(stderr, "[%d] ", **groups); - for (command = *commands; *command; command++) - fprintf(stderr, "%s ", *command); - fprintf(stderr, "\n"); - } -} - -static void usage(char *progname) -{ - fprintf(stderr, - "\nusage: %s [options] -f file\n\n" - " -f file specify path to configuration file\n" - " -n only print generated files and commands\n" - " -l level set logging level (6)\n" - " -m print messages to stdout\n" - " -q do not print messages to syslog\n" - " -v print version and exit\n" - " -h print this message and exit\n", - progname); -} - -int main(int argc, char **argv) -{ - struct timemaster_config *config; - struct script *script; - char *progname, *config_path = NULL; - int c, ret = 0, log_stdout = 0, log_syslog = 1, dry_run = 0; - - progname = strrchr(argv[0], '/'); - progname = progname ? progname + 1 : argv[0]; - - print_set_progname(progname); - print_set_verbose(1); - print_set_syslog(0); - - while (EOF != (c = getopt(argc, argv, "f:nl:mqvh"))) { - switch (c) { - case 'f': - config_path = optarg; - break; - case 'n': - dry_run = 1; - break; - case 'l': - print_set_level(atoi(optarg)); - break; - case 'm': - log_stdout = 1; - break; - case 'q': - log_syslog = 0; - break; - case 'v': - version_show(stdout); - return 0; - case 'h': - usage(progname); - return 0; - default: - usage(progname); - return 1; - } - } - - if (!config_path) { - pr_err("no configuration file specified"); - return 1; - } - - config = config_parse(config_path); - if (!config) - return 1; - - script = script_create(config); - config_destroy(config); - if (!script) - return 1; - - print_set_verbose(log_stdout); - print_set_syslog(log_syslog); - - if (dry_run) - script_print(script); - else - ret = script_run(script); - - script_destroy(script); - - if (!dry_run) - pr_info("exiting"); - - return ret; -} diff --git a/unicast_service.c b/unicast_service.c index c6c17c6..ad0e06a 100644 --- a/unicast_service.c +++ b/unicast_service.c @@ -31,9 +31,6 @@ #include "unicast_service.h" #include "util.h" -#define MIN_LOG_INTER_MESSAGE_PERIOD -7 -#define MAX_LOG_INTER_MESSAGE_PERIOD 16 -#define MAX_DURATION 3600 #define QUEUE_LEN 16 struct unicast_client_address { @@ -292,15 +289,6 @@ int unicast_service_add(struct port *p, struct ptp_message *m, return SERVICE_DENIED; } - if (req->logInterMessagePeriod < MIN_LOG_INTER_MESSAGE_PERIOD || - req->logInterMessagePeriod > MAX_LOG_INTER_MESSAGE_PERIOD) { - return SERVICE_DENIED; - } - - if (req->durationField > MAX_DURATION) { - req->durationField = MAX_DURATION; - } - LIST_FOREACH(itmp, &p->unicast_service->intervals, list) { /* * Remember the interval of interest. @@ -514,7 +502,7 @@ int unicast_service_timer(struct port *p) 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) { + if (timespec_compare(&now, &interval->tmo) >= 0) { break; } interval = pqueue_extract(p->unicast_service->queue); diff --git a/unicast_service.c.timeout b/unicast_service.c.timeout deleted file mode 100644 index ad0e06a..0000000 --- a/unicast_service.c.timeout +++ /dev/null @@ -1,531 +0,0 @@ -/** - * @file unicast_service.c - * @brief Unicast service - * @note Copyright (C) 2018 Richard Cochran - * - * 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 -#include -#include - -#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; - } - if (config_set_section_int(cfg, p->name, "hybrid_e2e", 1)) { - return -1; - } - 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; - } - p->inhibit_multicast_service = - config_get_int(cfg, p->name, "inhibit_multicast_service"); - - 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; -} diff --git a/unicast_service.c.ucastrate b/unicast_service.c.ucastrate deleted file mode 100644 index 9c9b95b..0000000 --- a/unicast_service.c.ucastrate +++ /dev/null @@ -1,531 +0,0 @@ -/** - * @file unicast_service.c - * @brief Unicast service - * @note Copyright (C) 2018 Richard Cochran - * - * 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 -#include -#include - -#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; - } - if (config_set_section_int(cfg, p->name, "hybrid_e2e", 1)) { - return -1; - } - 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; - } - p->inhibit_multicast_service = - config_get_int(cfg, p->name, "inhibit_multicast_service"); - - 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; -} diff --git a/util.c b/util.c index a59c823..73fb37e 100644 --- a/util.c +++ b/util.c @@ -78,7 +78,7 @@ int addreq(enum transport_type type, struct address *a, struct address *b) case TRANS_UDP_IPV4: bufa = &a->sin.sin_addr; bufb = &b->sin.sin_addr; - len = sizeof(a->sin.sin_addr); + len = sizeof(a->sin); break; case TRANS_IEEE_802_3: bufa = &a->sll.sll_addr; diff --git a/util.c.addreq b/util.c.addreq deleted file mode 100644 index 73fb37e..0000000 --- a/util.c.addreq +++ /dev/null @@ -1,653 +0,0 @@ -/** - * @file util.c - * @note Copyright (C) 2011 Richard Cochran - * - * 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. - */ -#include -#include -#include -#include -#include -#include -#include - -#include "address.h" -#include "print.h" -#include "sk.h" -#include "util.h" - -#define NS_PER_SEC 1000000000LL -#define NS_PER_HOUR (3600 * NS_PER_SEC) -#define NS_PER_DAY (24 * NS_PER_HOUR) - -static int running = 1; - -const char *ps_str[] = { - "NONE", - "INITIALIZING", - "FAULTY", - "DISABLED", - "LISTENING", - "PRE_MASTER", - "MASTER", - "PASSIVE", - "UNCALIBRATED", - "SLAVE", - "GRAND_MASTER", -}; - -const char *ev_str[] = { - "NONE", - "POWERUP", - "INITIALIZE", - "DESIGNATED_ENABLED", - "DESIGNATED_DISABLED", - "FAULT_CLEARED", - "FAULT_DETECTED", - "STATE_DECISION_EVENT", - "QUALIFICATION_TIMEOUT_EXPIRES", - "ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES", - "SYNCHRONIZATION_FAULT", - "MASTER_CLOCK_SELECTED", - "INIT_COMPLETE", - "RS_MASTER", - "RS_GRAND_MASTER", - "RS_SLAVE", - "RS_PASSIVE", -}; - -int addreq(enum transport_type type, struct address *a, struct address *b) -{ - void *bufa, *bufb; - int len; - - switch (type) { - case TRANS_UDP_IPV4: - bufa = &a->sin.sin_addr; - bufb = &b->sin.sin_addr; - len = sizeof(a->sin); - break; - case TRANS_IEEE_802_3: - bufa = &a->sll.sll_addr; - bufb = &b->sll.sll_addr; - len = MAC_LEN; - break; - case TRANS_UDS: - case TRANS_UDP_IPV6: - case TRANS_DEVICENET: - case TRANS_CONTROLNET: - case TRANS_PROFINET: - default: - pr_err("sorry, cannot compare addresses for this transport"); - return 0; - } - return memcmp(bufa, bufb, len) == 0 ? 1 : 0; -} - -char *bin2str_impl(Octet *data, int len, char *buf, int buf_len) -{ - int i, offset = 0; - if (len > MAX_PRINT_BYTES) - len = MAX_PRINT_BYTES; - buf[0] = '\0'; - if (!data) - return buf; - if (len) - offset += snprintf(buf, buf_len, "%02hhx", data[0]); - for (i = 1; i < len; i++) { - if (offset >= buf_len) - /* truncated output */ - break; - offset += snprintf(buf + offset, buf_len - offset, ":%02hhx", data[i]); - } - return buf; -} - -char *cid2str(struct ClockIdentity *id) -{ - static char buf[64]; - unsigned char *ptr = id->id; - snprintf(buf, sizeof(buf), "%02x%02x%02x.%02x%02x.%02x%02x%02x", - ptr[0], ptr[1], ptr[2], ptr[3], - ptr[4], ptr[5], ptr[6], ptr[7]); - return buf; -} - -int count_char(const char *str, char c) -{ - int num = 0; - char s; - while ((s = *(str++))) { - if (s == c) - num++; - } - return num; -} - -char *pid2str(struct PortIdentity *id) -{ - static char buf[64]; - unsigned char *ptr = id->clockIdentity.id; - snprintf(buf, sizeof(buf), "%02x%02x%02x.%02x%02x.%02x%02x%02x-%hu", - ptr[0], ptr[1], ptr[2], ptr[3], - ptr[4], ptr[5], ptr[6], ptr[7], - id->portNumber); - return buf; -} - -char *portaddr2str(struct PortAddress *addr) -{ - static char buf[BIN_BUF_SIZE]; - switch (align16(&addr->networkProtocol)) { - case TRANS_UDP_IPV4: - if (align16(&addr->addressLength) == 4 - && inet_ntop(AF_INET, addr->address, buf, sizeof(buf))) - return buf; - break; - case TRANS_UDP_IPV6: - if (align16(&addr->addressLength) == 16 - && inet_ntop(AF_INET6, addr->address, buf, sizeof(buf))) - return buf; - break; - } - bin2str_impl(addr->address, align16(&addr->addressLength), buf, sizeof(buf)); - return buf; -} - -int str2addr(enum transport_type type, const char *s, struct address *addr) -{ - unsigned char mac[MAC_LEN]; - struct in_addr ipv4_addr; - - memset(addr, 0, sizeof(*addr)); - - switch (type) { - case TRANS_UDS: - case TRANS_UDP_IPV6: - case TRANS_DEVICENET: - case TRANS_CONTROLNET: - case TRANS_PROFINET: - pr_err("sorry, cannot convert addresses for this transport"); - return -1; - case TRANS_UDP_IPV4: - if (!inet_aton(s, &ipv4_addr)) { - pr_err("bad IPv4 address"); - return -1; - } - addr->sin.sin_family = AF_INET; - addr->sin.sin_addr = ipv4_addr; - addr->len = sizeof(addr->sin); - break; - case TRANS_IEEE_802_3: - if (str2mac(s, mac)) { - pr_err("bad Layer-2 address"); - return -1; - } - addr->sll.sll_family = AF_PACKET; - addr->sll.sll_halen = MAC_LEN; - memcpy(&addr->sll.sll_addr, mac, MAC_LEN); - addr->len = sizeof(addr->sll); - break; - } - return 0; -} - -int str2mac(const char *s, unsigned char mac[MAC_LEN]) -{ - unsigned char buf[MAC_LEN]; - int c; - c = sscanf(s, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", - &buf[0], &buf[1], &buf[2], &buf[3], &buf[4], &buf[5]); - if (c != MAC_LEN) { - return -1; - } - memcpy(mac, buf, MAC_LEN); - return 0; -} - -int str2pid(const char *s, struct PortIdentity *result) -{ - struct PortIdentity pid; - unsigned char *ptr = pid.clockIdentity.id; - int c; - c = sscanf(s, " %02hhx%02hhx%02hhx.%02hhx%02hhx.%02hhx%02hhx%02hhx-%hu", - &ptr[0], &ptr[1], &ptr[2], &ptr[3], - &ptr[4], &ptr[5], &ptr[6], &ptr[7], - &pid.portNumber); - if (c == 9) { - *result = pid; - return 0; - } - return -1; -} - -int generate_clock_identity(struct ClockIdentity *ci, const char *name) -{ - struct address addr; - - if (sk_interface_macaddr(name, &addr)) - return -1; - - switch (addr.sll.sll_halen) { - case EUI48: - ci->id[0] = addr.sll.sll_addr[0]; - ci->id[1] = addr.sll.sll_addr[1]; - ci->id[2] = addr.sll.sll_addr[2]; - ci->id[3] = 0xFF; - ci->id[4] = 0xFE; - ci->id[5] = addr.sll.sll_addr[3]; - ci->id[6] = addr.sll.sll_addr[4]; - ci->id[7] = addr.sll.sll_addr[5]; - break; - case EUI64: - ci->id[0] = addr.sll.sll_addr[0]; - ci->id[1] = addr.sll.sll_addr[1]; - ci->id[2] = addr.sll.sll_addr[2]; - ci->id[3] = addr.sll.sll_addr[3]; - ci->id[4] = addr.sll.sll_addr[4]; - ci->id[5] = addr.sll.sll_addr[5]; - ci->id[6] = addr.sll.sll_addr[6]; - ci->id[7] = addr.sll.sll_addr[7]; - break; - default: - return -1; - } - - return 0; -} - -/* Naive count of utf8 symbols. Doesn't detect invalid UTF-8 and - * probably doesn't count combining characters correctly. */ -static size_t strlen_utf8(const Octet *s) -{ - size_t len = 0; - char c; - while ((c = *(s++))) { - if ((c & 0xC0) != 0x80) - len++; - } - return len; -} - -int static_ptp_text_copy(struct static_ptp_text *dst, const struct PTPText *src) -{ - int len = src->length; - if (dst->max_symbols > 0 && strlen_utf8(src->text) > dst->max_symbols) - return -1; - dst->length = len; - memcpy(dst->text, src->text, len); - dst->text[len] = '\0'; - return 0; -} - -void ptp_text_copy(struct PTPText *dst, const struct static_ptp_text *src) -{ - dst->length = src->length; - memcpy(dst->text, src->text, src->length); -} - -int ptp_text_set(struct PTPText *dst, const char *src) -{ - size_t len; - if (src) { - len = strlen(src); - if (len > MAX_PTP_OCTETS) - return -1; - dst->length = len; - memcpy(dst->text, src, len); - } else { - dst->length = 0; - } - return 0; -} - -int static_ptp_text_set(struct static_ptp_text *dst, const char *src) -{ - int len = strlen(src); - if (len > MAX_PTP_OCTETS) - return -1; - if (dst->max_symbols > 0 && strlen_utf8((Octet *) src) > dst->max_symbols) - return -1; - dst->length = len; - memcpy(dst->text, src, len); - dst->text[len] = '\0'; - return 0; -} - -int is_utc_ambiguous(uint64_t ts) -{ - /* The Linux kernel inserts leap second by stepping the clock backwards - at 0:00 UTC, the last second before midnight is played twice. */ - if (NS_PER_DAY - ts % NS_PER_DAY <= NS_PER_SEC) - return 1; - return 0; -} - -int leap_second_status(uint64_t ts, int leap_set, int *leap, int *utc_offset) -{ - int leap_status = leap_set; - - /* The leap bits obtained by PTP should be set at most 12 hours before - midnight and unset at most 2 announce intervals after midnight. - Split updates which are too early and which are too late at 6 hours - after midnight. */ - if (ts % NS_PER_DAY > 6 * NS_PER_HOUR) { - if (!leap_status) - leap_status = *leap; - } else { - if (leap_status) - leap_status = 0; - } - - /* Fix early or late update of leap and utc_offset. */ - if (!*leap && leap_status) { - *utc_offset -= leap_status; - *leap = leap_status; - } else if (*leap && !leap_status) { - *utc_offset += *leap; - *leap = leap_status; - } - - return leap_status; -} - -enum parser_result get_ranged_int(const char *str_val, int *result, - int min, int max) -{ - long parsed_val; - char *endptr = NULL; - errno = 0; - parsed_val = strtol(str_val, &endptr, 0); - if (*endptr != '\0' || endptr == str_val) - return MALFORMED; - if (errno == ERANGE || parsed_val < min || parsed_val > max) - return OUT_OF_RANGE; - *result = parsed_val; - return PARSED_OK; -} - -enum parser_result get_ranged_uint(const char *str_val, unsigned int *result, - unsigned int min, unsigned int max) -{ - unsigned long parsed_val; - char *endptr = NULL; - errno = 0; - parsed_val = strtoul(str_val, &endptr, 0); - if (*endptr != '\0' || endptr == str_val) - return MALFORMED; - if (errno == ERANGE || parsed_val < min || parsed_val > max) - return OUT_OF_RANGE; - *result = parsed_val; - return PARSED_OK; -} - -enum parser_result get_ranged_double(const char *str_val, double *result, - double min, double max) -{ - double parsed_val; - char *endptr = NULL; - errno = 0; - parsed_val = strtod(str_val, &endptr); - if (*endptr != '\0' || endptr == str_val) - return MALFORMED; - if (errno == ERANGE || parsed_val < min || parsed_val > max) - return OUT_OF_RANGE; - *result = parsed_val; - return PARSED_OK; -} - -int get_arg_val_i(int op, const char *optarg, int *val, int min, int max) -{ - enum parser_result r; - r = get_ranged_int(optarg, val, min, max); - if (r == MALFORMED) { - fprintf(stderr, - "-%c: %s is a malformed value\n", op, optarg); - return -1; - } - if (r == OUT_OF_RANGE) { - fprintf(stderr, - "-%c: %s is out of range. Must be in the range %d to %d\n", - op, optarg, min, max); - return -1; - } - return 0; -} - -int get_arg_val_ui(int op, const char *optarg, unsigned int *val, - unsigned int min, unsigned int max) -{ - enum parser_result r; - r = get_ranged_uint(optarg, val, min, max); - if (r == MALFORMED) { - fprintf(stderr, - "-%c: %s is a malformed value\n", op, optarg); - return -1; - } - if (r == OUT_OF_RANGE) { - fprintf(stderr, - "-%c: %s is out of range. Must be in the range %u to %u\n", - op, optarg, min, max); - return -1; - } - return 0; -} - -int get_arg_val_d(int op, const char *optarg, double *val, - double min, double max) -{ - enum parser_result r; - r = get_ranged_double(optarg, val, min, max); - if (r == MALFORMED) { - fprintf(stderr, - "-%c: %s is a malformed value\n", op, optarg); - return -1; - } - if (r == OUT_OF_RANGE) { - fprintf(stderr, - "-%c: %s is out of range. Must be in the range %e to %e\n", - op, optarg, min, max); - return -1; - } - return 0; -} - -static void handle_int_quit_term(int s) -{ - running = 0; -} - -int handle_term_signals(void) -{ - if (SIG_ERR == signal(SIGINT, handle_int_quit_term)) { - fprintf(stderr, "cannot handle SIGINT\n"); - return -1; - } - if (SIG_ERR == signal(SIGQUIT, handle_int_quit_term)) { - fprintf(stderr, "cannot handle SIGQUIT\n"); - return -1; - } - if (SIG_ERR == signal(SIGTERM, handle_int_quit_term)) { - fprintf(stderr, "cannot handle SIGTERM\n"); - return -1; - } - return 0; -} - -int is_running(void) -{ - return running; -} - -void *xmalloc(size_t size) -{ - void *r; - - r = malloc(size); - if (!r) { - pr_err("failed to allocate memory"); - exit(1); - } - - return r; -} - -void *xcalloc(size_t nmemb, size_t size) -{ - void *r; - - r = calloc(nmemb, size); - if (!r) { - pr_err("failed to allocate memory"); - exit(1); - } - - return r; -} - -void *xrealloc(void *ptr, size_t size) -{ - void *r; - - r = realloc(ptr, size); - if (!r) { - pr_err("failed to allocate memory"); - exit(1); - } - - return r; -} - -char *xstrdup(const char *s) -{ - void *r; - - r = strdup(s); - if (!r) { - pr_err("failed to allocate memory"); - exit(1); - } - - return r; -} - -char *string_newf(const char *format, ...) -{ - va_list ap; - char *s; - - va_start(ap, format); - if (vasprintf(&s, format, ap) < 0) { - pr_err("failed to allocate memory"); - exit(1); - } - va_end(ap); - - return s; -} - -void string_append(char **s, const char *str) -{ - size_t len1, len2; - - len1 = strlen(*s); - len2 = strlen(str); - *s = xrealloc(*s, len1 + len2 + 1); - memcpy((*s) + len1, str, len2 + 1); -} - -void string_appendf(char **s, const char *format, ...) -{ - va_list ap; - size_t len1; - int len2; - char *s2; - - len1 = strlen(*s); - - va_start(ap, format); - len2 = vasprintf(&s2, format, ap); - va_end(ap); - - if (len2 < 0) { - *s = NULL; - return; - } - - *s = xrealloc(*s, len1 + len2 + 1); - memcpy((*s) + len1, s2, len2 + 1); - free(s2); -} - -void **parray_new(void) -{ - void **a; - - a = xmalloc(sizeof(*a)); - *a = NULL; - - return a; -} - -void parray_append(void ***a, void *p) -{ - parray_extend(a, p, NULL); -} - -void parray_extend(void ***a, ...) -{ - va_list ap; - int ilen, len, alloced; - void *p; - - for (len = 0; (*a)[len]; len++) - ; - len++; - - va_start(ap, a); - for (ilen = 0; va_arg(ap, void *); ilen++) - ; - va_end(ap); - - /* Reallocate in exponentially increasing sizes. */ - for (alloced = 1; alloced < len; alloced <<= 1) - ; - if (alloced < len + ilen) { - while (alloced < len + ilen) - alloced *= 2; - *a = xrealloc(*a, alloced * sizeof **a); - } - - va_start(ap, a); - while ((p = va_arg(ap, void *))) - (*a)[len++ - 1] = p; - va_end(ap); - (*a)[len - 1] = NULL; -} - -int rate_limited(int interval, time_t *last) -{ - struct timespec ts; - - if (clock_gettime(CLOCK_MONOTONIC, &ts)) - return 1; - if (*last + interval > ts.tv_sec) - return 1; - - *last = ts.tv_sec; - - return 0; -}