linuxptp-3.1.1.tgz

/** * @file port.c * @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com> * * 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 <arpa/inet.h> #include <errno.h> #include <malloc.h> #include <stdlib.h> #include <string.h> #include <unistd.h> #include <sys/queue.h> #include <net/if.h> #include "bmc.h" #include "clock.h" #include "designated_fsm.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_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 int check_source_identity(struct port *p, struct ptp_message *m) { struct PortIdentity master; if (p->ignore_source_id) { return 0; } master = clock_parent_identity(p->clock); return pid_eq(&master, &m->header.sourcePortIdentity) ? 0 : -1; } 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 <= -31) { 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; if (!fc) { return 0; } 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