diff --git a/port.c b/port.c index c945c13..a019974 100644 --- a/port.c +++ b/port.c @@ -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.zerolength b/port.c.zerolength new file mode 100644 index 0000000..c945c13 --- /dev/null +++ b/port.c.zerolength @@ -0,0 +1,3011 @@ +/** + * @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/sk.c b/sk.c index 3852cbb..0aeb33a 100644 --- a/sk.c +++ b/sk.c @@ -359,7 +359,7 @@ int sk_receive(int fd, void *buf, int buflen, } cnt = recvmsg(fd, &msg, flags); - if (cnt < 1) + if (cnt < 0) pr_err("recvmsg%sfailed: %m", flags == MSG_ERRQUEUE ? " tx timestamp " : " "); diff --git a/sk.c.zerolength b/sk.c.zerolength new file mode 100644 index 0000000..3852cbb --- /dev/null +++ b/sk.c.zerolength @@ -0,0 +1,516 @@ +/** + * @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; +}