From 49cd10e678a60855fc8e93b8af343b5a5247490a Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Sat, 12 Nov 2011 18:44:55 +0100 Subject: [PATCH] Implement the PTP clock. Signed-off-by: Richard Cochran --- clock.c | 428 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ clock.h | 1 + foreign.h | 62 ++++++++ makefile | 2 +- port.h | 72 ++++++++- 5 files changed, 562 insertions(+), 3 deletions(-) create mode 100644 clock.c create mode 100644 foreign.h diff --git a/clock.c b/clock.c new file mode 100644 index 0000000..bc2c061 --- /dev/null +++ b/clock.c @@ -0,0 +1,428 @@ +/** + * @file clock.c + * @note Copyright (C) 2011 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include +#include +#include +#include + +#include "bmc.h" +#include "clock.h" +#include "foreign.h" +#include "missing.h" +#include "msg.h" +#include "phc.h" +#include "port.h" +#include "servo.h" +#include "print.h" +#include "tmv.h" +#include "util.h" + +#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) + +struct clock { + clockid_t clkid; + struct servo *servo; + struct defaultDS dds; + struct dataset default_dataset; + struct currentDS cur; + struct parentDS dad; + struct timePropertiesDS tds; + struct foreign_clock *best; + struct port *port[MAX_PORTS]; + struct pollfd pollfd[MAX_PORTS*N_POLLFD]; + int nports; + tmv_t master_offset; + tmv_t path_delay; + tmv_t c1; + tmv_t c2; + tmv_t t1; + tmv_t t2; +}; + +struct clock the_clock; + +static void handle_state_decision_event(struct clock *c); + +static void clock_destroy(struct clock *c) +{ + int i; + for (i = 0; i < c->nports; i++) { + port_close(c->port[i]); + } + if (c->clkid != CLOCK_REALTIME) { + phc_close(c->clkid); + } + memset(c, 0, sizeof(*c)); +} + +static void clock_ppb(clockid_t clkid, double ppb) +{ + struct timex tx; + memset(&tx, 0, sizeof(tx)); + tx.modes = ADJ_FREQUENCY; + tx.freq = (long) (ppb * 65.536); + if (clock_adjtime(clkid, &tx) < 0) + pr_err("failed to adjust the clock: %m"); +} + +static void clock_step(clockid_t clkid, int64_t ns) +{ + struct timex tx; + int sign = 1; + if (ns < 0) { + sign = -1; + ns *= -1; + } + memset(&tx, 0, sizeof(tx)); + tx.modes = ADJ_SETOFFSET | ADJ_NANO; + tx.time.tv_sec = sign * (ns / NS_PER_SEC); + tx.time.tv_usec = sign * (ns % NS_PER_SEC); + /* + * The value of a timeval is the sum of its fields, but the + * field tv_usec must always be non-negative. + */ + if (tx.time.tv_usec < 0) { + tx.time.tv_sec -= 1; + tx.time.tv_usec += 1000000000; + } + if (clock_adjtime(clkid, &tx) < 0) + pr_err("failed to step clock: %m"); +} + +static void clock_update_grandmaster(struct clock *c) +{ + memset(&c->cur, 0, sizeof(c->cur)); + c->dad.parentPortIdentity.clockIdentity = c->dds.clockIdentity; + c->dad.parentPortIdentity.portNumber = 0; + c->dad.grandmasterIdentity = c->dds.clockIdentity; + c->dad.grandmasterClockQuality = c->dds.clockQuality; + c->dad.grandmasterPriority1 = c->dds.priority1; + c->dad.grandmasterPriority2 = c->dds.priority2; + c->tds.currentUtcOffset = CURRENT_UTC_OFFSET; + c->tds.currentUtcOffsetValid = FALSE; + c->tds.leap61 = FALSE; + c->tds.leap59 = FALSE; + c->tds.timeTraceable = FALSE; + c->tds.frequencyTraceable = FALSE; + c->tds.ptpTimescale = TRUE; + c->tds.timeSource = INTERNAL_OSCILLATOR; +} + +static void clock_update_slave(struct clock *c) +{ + struct ptp_message *msg = TAILQ_FIRST(&c->best->messages); + c->cur.stepsRemoved = 1 + c->best->dataset.stepsRemoved; + c->dad.parentPortIdentity = c->best->dataset.sender; + c->dad.grandmasterIdentity = msg->announce.grandmasterIdentity; + c->dad.grandmasterClockQuality = msg->announce.grandmasterClockQuality; + c->dad.grandmasterPriority1 = msg->announce.grandmasterPriority1; + c->dad.grandmasterPriority2 = msg->announce.grandmasterPriority2; + c->tds.currentUtcOffset = msg->announce.currentUtcOffset; + c->tds.currentUtcOffsetValid = field_is_set(msg, 1, UTC_OFF_VALID); + c->tds.leap61 = field_is_set(msg, 1, LEAP_61); + c->tds.leap59 = field_is_set(msg, 1, LEAP_59); + c->tds.timeTraceable = field_is_set(msg, 1, TIME_TRACEABLE); + c->tds.frequencyTraceable = field_is_set(msg, 1, FREQ_TRACEABLE); + c->tds.ptpTimescale = field_is_set(msg, 1, PTP_TIMESCALE); + c->tds.timeSource = msg->announce.timeSource; +} + +/* public methods */ + +UInteger8 clock_class(struct clock *c) +{ + return c->dds.clockQuality.clockClass; +} + +struct clock *clock_create(char *phc, struct interface *iface, int count, + struct defaultDS *ds) +{ + int i, max_adj; + struct clock *c = &the_clock; + + if (c->nports) + clock_destroy(c); + + if (phc) { + c->clkid = phc_open(phc); + if (c->clkid == CLOCK_INVALID) { + pr_err("Failed to open %s: %m", phc); + return NULL; + } + max_adj = phc_max_adj(c->clkid); + if (!max_adj) { + pr_err("clock is not adjustable"); + return NULL; + } + } else { + c->clkid = CLOCK_REALTIME; + max_adj = 512000; + } + + c->servo = servo_create("pi", max_adj); + if (!c->servo) { + pr_err("Failed to create clock servo"); + return NULL; + } + + c->dds = *ds; + + /* Initialize the parentDS. */ + c->dad.parentPortIdentity.clockIdentity = c->dds.clockIdentity; + c->dad.parentPortIdentity.portNumber = 0; + c->dad.parentStats = 0; + c->dad.observedParentOffsetScaledLogVariance = 0xffff; + c->dad.observedParentClockPhaseChangeRate = 0x7fffffff; + c->dad.grandmasterPriority1 = c->dds.priority1; + c->dad.grandmasterClockQuality = c->dds.clockQuality; + c->dad.grandmasterPriority2 = c->dds.priority1; + c->dad.grandmasterIdentity = c->dds.clockIdentity; + + for (i = 0; i < ARRAY_SIZE(c->pollfd); i++) { + c->pollfd[i].fd = -1; + c->pollfd[i].events = 0; + } + + for (i = 0; i < count; i++) { + c->port[i] = port_open(iface[i].name, iface[i].transport, + iface[i].timestamping, 1+i, DM_E2E, c); + if (!c->port[i]) { + pr_err("failed to open port %s", iface[i].name); + return NULL; + } + } + + c->dds.numberPorts = c->nports = count; + + for (i = 0; i < c->nports; i++) + port_dispatch(c->port[i], EV_INITIALIZE); + + return c; +} + +struct dataset *clock_best_foreign(struct clock *c) +{ + return c->best ? &c->best->dataset : NULL; +} + +struct port *clock_best_port(struct clock *c) +{ + return c->best ? c->best->port : NULL; +} + +struct dataset *clock_default_ds(struct clock *c) +{ + struct dataset *out = &c->default_dataset; + struct defaultDS *in = &c->dds; + + out->priority1 = in->priority1; + out->identity = in->clockIdentity; + out->quality = in->clockQuality; + out->priority2 = in->priority2; + out->stepsRemoved = 0; + out->sender.clockIdentity = in->clockIdentity; + out->sender.portNumber = 0; + out->receiver.clockIdentity = in->clockIdentity; + out->receiver.portNumber = 0; + + return out; +} + +UInteger8 clock_domain_number(struct clock *c) +{ + return c->dds.domainNumber; +} + +struct ClockIdentity clock_identity(struct clock *c) +{ + return c->dds.clockIdentity; +} + +void clock_install_fda(struct clock *c, struct port *p, struct fdarray fda) +{ + int i, j, k; + for (i = 0; i < c->nports; i++) { + if (p == c->port[i]) + break; + } + for (j = 0; j < fda.cnt; j++) { + k = N_POLLFD * i + j; + c->pollfd[k].fd = fda.fd[j]; + c->pollfd[k].events = POLLIN|POLLPRI; + } +} + +struct PortIdentity clock_parent_identity(struct clock *c) +{ + return c->dad.parentPortIdentity; +} + +int clock_poll(struct clock *c) +{ + int cnt, i, j, k, sde = 0; + enum fsm_event event; + + cnt = poll(c->pollfd, ARRAY_SIZE(c->pollfd), -1); + if (cnt < 0) { + if (EINTR == errno) { + return 0; + } else { + pr_emerg("poll failed"); + return -1; + } + } else if (!cnt) { + return 0; + } + + for (i = 0; i < c->nports; i++) { + for (j = 0; j < N_POLLFD; j++) { + k = N_POLLFD * i + j; + if (c->pollfd[k].revents & (POLLIN|POLLPRI)) { + event = port_event(c->port[i], j); + if (EV_STATE_DECISION_EVENT == event) + sde = 1; + else + port_dispatch(c->port[i], event); + } + } + } + + if (sde) + handle_state_decision_event(c); + + return 0; +} + +void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, + Integer64 correction) +{ + tmv_t c1, c2, c3, t1, t2, t3, t4; + + c1 = c->c1; + c2 = c->c2; + c3 = correction_to_tmv(correction); + t1 = c->t1; + t2 = c->t2; + t3 = timespec_to_tmv(req); + t4 = timestamp_to_tmv(rx); + + /* + * c->path_delay = (t2 - t3) + (t4 - t1); + * c->path_delay -= c_sync + c_fup + c_delay_resp; + * c->path_delay /= 2.0; + */ + c->path_delay = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1)); + c->path_delay = tmv_sub(c->path_delay, tmv_add(c1, tmv_add(c2, c3))); + c->path_delay = tmv_div(c->path_delay, 2); + + pr_debug("path delay %10lld", c->path_delay); +} + +void clock_synchronize(struct clock *c, + struct timespec ingress_ts, struct timestamp origin_ts, + Integer64 correction1, Integer64 correction2) +{ + double adj; + tmv_t ingress, origin; + enum servo_state state; + + ingress = timespec_to_tmv(ingress_ts); + origin = timestamp_to_tmv(origin_ts); + + c->t1 = origin; + c->t2 = ingress; + + c->c1 = correction_to_tmv(correction1); + c->c2 = correction_to_tmv(correction2); + + /* + * c->master_offset = ingress - origin - c->path_delay - c->c1 - c->c2; + */ + c->master_offset = tmv_sub(ingress, + tmv_add(origin, tmv_add(c->path_delay, tmv_add(c->c1, c->c2)))); + + if (!c->path_delay) + return; + + adj = servo_sample(c->servo, c->master_offset, ingress, &state); + + pr_debug("master offset %10lld s%d adj %+7.0f", + c->master_offset, state, adj); + + switch (state) { + case SERVO_UNLOCKED: + break; + case SERVO_JUMP: + clock_step(c->clkid, -c->master_offset); + break; + case SERVO_LOCKED: + clock_ppb(c->clkid, -adj); + break; + } +} + +static void handle_state_decision_event(struct clock *c) +{ + struct foreign_clock *best = NULL, *fc; + int i; + + for (i = 0; i < c->nports; i++) { + fc = port_compute_best(c->port[i]); + if (!fc) + continue; + if (!best || dscmp(&fc->dataset, &best->dataset) > 0) + best = fc; + } + + if (!best) + return; + + pr_info("selected best master clock %s", + cid2str(&best->dataset.identity)); + + c->best = best; + + for (i = 0; i < c->nports; i++) { + enum port_state ps; + enum fsm_event event; + ps = bmc_state_decision(c, c->port[i]); + switch (ps) { + case PS_LISTENING: + event = EV_NONE; + break; + case PS_GRAND_MASTER: + clock_update_grandmaster(c); + /*fall through*/ + case PS_MASTER: + event = EV_RS_MASTER; + break; + case PS_PASSIVE: + event = EV_RS_PASSIVE; + break; + case PS_SLAVE: + clock_update_slave(c); + event = EV_RS_SLAVE; + break; + default: + event = EV_INITIALIZE; + break; + } + port_dispatch(c->port[i], event); + } +} diff --git a/clock.h b/clock.h index e02dcfd..b9a37b3 100644 --- a/clock.h +++ b/clock.h @@ -1,5 +1,6 @@ /** * @file clock.h + * @brief Implements a PTP clock. * @note Copyright (C) 2011 Richard Cochran * * This program is free software; you can redistribute it and/or modify diff --git a/foreign.h b/foreign.h new file mode 100644 index 0000000..db24b84 --- /dev/null +++ b/foreign.h @@ -0,0 +1,62 @@ +/** + * @file foreign.h + * @brief Defines a foreign clock record. + * @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. + */ +#ifndef HAVE_FOREIGN_H +#define HAVE_FOREIGN_H + +#include + +#include "ds.h" +#include "port.h" + +#define FOREIGN_MASTER_THRESHOLD 2 + +struct foreign_clock { + /** + * Pointer to next foreign_clock in list. + */ + LIST_ENTRY(foreign_clock) list; + + /** + * A list of received announce messages. + * + * The data set field, foreignMasterPortIdentity, is the + * sourcePortIdentity of the first message. + */ + TAILQ_HEAD(messages, ptp_message) messages; + + /** + * Number of elements in the message list, + * aka foreignMasterAnnounceMessages. + */ + unsigned int n_messages; + + /** + * Pointer to the associated port. + */ + struct port *port; + + /** + * Contains the information from the latest announce message + * in a form suitable for comparision in the BMCA. + */ + struct dataset dataset; +}; + +#endif diff --git a/makefile b/makefile index db0f514..5dd0c21 100644 --- a/makefile +++ b/makefile @@ -24,7 +24,7 @@ CFLAGS = -Wall $(INC) $(DEBUG) LDFLAGS = LDLIBS = -lm -lrt PRG = linuxptp -OBJ = bmc.o fsm.o linuxptp.o msg.o phc.o pi.o print.o \ +OBJ = bmc.o clock.o fsm.o linuxptp.o msg.o phc.o pi.o print.o \ servo.o transport.o udp.o util.o SRC = $(OBJ:.o=.c) diff --git a/port.h b/port.h index 5a542cf..eb4a60e 100644 --- a/port.h +++ b/port.h @@ -19,18 +19,86 @@ #ifndef HAVE_PORT_H #define HAVE_PORT_H +#include "fd.h" +#include "foreign.h" +#include "fsm.h" +#include "transport.h" + +/** Defines the possible delay mechanisms. */ +enum delay_mechanism { + DM_AUTO, /**< Just go with the flow */ + DM_E2E, /**< End to End mechanism */ + DM_P2P, /**< Peer to Peer mechanism */ +}; + /** Opaque type. */ struct port; /** * Returns the dataset from a port's best foreign clock record, if any - * has yet been discovered. + * has yet been discovered. This function does not bring the returned + * dataset up to date, so the caller should invoke port_compute_best() + * beforehand. * - * @param port A port instance. + * @param port A pointer previously obtained via port_open(). * @return A pointer to a dataset, or NULL. */ struct dataset *port_best_foreign(struct port *port); +/** + * Close a port and free its associated resources. After this call + * returns, @a port is no longer a valid port instance. + * + * @param port A pointer previously obtained via port_open(). + */ +void port_close(struct port *port); + +/** + * Computes the 'best' foreign master discovered on a port. This has + * the side effect of updating the 'dataset' field of the returned + * foreign master. + * + * @param port A pointer previously obtained via port_open(). + * @return A pointer to the port's best foreign master, or NULL. + */ +struct foreign_clock *port_compute_best(struct port *port); + +/** + * Dispatch a port event. This may cause a state transition on the + * port, with the associated side effect. + * + * @param port A pointer previously obtained via port_open(). + * @param event One of the @a fsm_event codes. + */ +void port_dispatch(struct port *port, enum fsm_event event); + +/** + * Generates state machine events based on activity on a port's file + * descriptors. + * + * @param port A pointer previously obtained via port_open(). + * @param fd_index The index of the active file descriptor. + * @return One of the @a fsm_event codes. + */ +enum fsm_event port_event(struct port *port, int fd_index); + +/** + * Open a network port. + * @param name The name of the network interface. + * @param transport The network transport type to use on this port. + * @param timestamping The flavor of time stamping to use on this port. + * @param number An arbitrary port number for this port. + * @param dm Which delay mechanism to use on this port. + * @param clock A pointer to the system PTP clock. + * @return A pointer to an open port on success, or NULL otherwise. + */ +struct port *port_open(char *name, + enum transport_type transport, + enum timestamp_type timestamping, + int number, + enum delay_mechanism dm, + struct clock *clock); + /** * Returns a port's current state. * @param port A port instance.