Implement the PTP clock.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>

clock.c

@@ -0,0 +1,428 @@
+/**
+ * @file clock.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 <errno.h>
+#include <poll.h>
+#include <string.h>
+#include <time.h>
+
+#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);
+	}
+}

clock.h

@@ -1,5 +1,6 @@
 /**
  * @file clock.h
+ * @brief Implements a PTP clock.
  * @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
  *
  * This program is free software; you can redistribute it and/or modify

foreign.h

@@ -0,0 +1,62 @@
+/**
+ * @file foreign.h
+ * @brief Defines a foreign clock record.
+ * @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.
+ */
+#ifndef HAVE_FOREIGN_H
+#define HAVE_FOREIGN_H
+
+#include <sys/queue.h>
+
+#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

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)

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.