Add clock sanity check.

Check the sanity of the synchronized clock by comparing its uncorrected
frequency with the system monotonic clock. When the measured frequency
offset is larger than the value of the sanity_freq_limit option (20% by
default), a warning message will be printed and the servo will be reset.
Setting the option to zero disables the check.

This is useful to detect when the clock is broken or adjusted by another
program.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>

clock.c

@@ -25,6 +25,7 @@
 #include "bmc.h"
 #include "clock.h"
 #include "clockadj.h"
+#include "clockcheck.h"
 #include "foreign.h"
 #include "mave.h"
 #include "missing.h"
@@ -96,6 +97,7 @@ struct clock {
 	struct clock_description desc;
 	struct clock_stats stats;
 	int stats_interval;
+	struct clockcheck *sanity_check;
 };
 
 struct clock the_clock;
@@ -643,6 +645,13 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count,
 		pr_err("failed to create stats");
 		return NULL;
 	}
+	if (dds->sanity_freq_limit) {
+		c->sanity_check = clockcheck_create(dds->sanity_freq_limit);
+		if (!c->sanity_check) {
+			pr_err("Failed to create clock sanity check");
+			return NULL;
+		}
+	}
 
 	c->dds = dds->dds;
 
@@ -1084,11 +1093,18 @@ enum servo_state clock_synchronize(struct clock *c,
 		clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset));
 		c->t1 = tmv_zero();
 		c->t2 = tmv_zero();
+		if (c->sanity_check) {
+			clockcheck_set_freq(c->sanity_check, -adj);
+			clockcheck_step(c->sanity_check,
+					-tmv_to_nanoseconds(c->master_offset));
+		}
 		break;
 	case SERVO_LOCKED:
 		clockadj_set_freq(c->clkid, -adj);
 		if (c->clkid == CLOCK_REALTIME)
 			sysclk_set_sync();
+		if (c->sanity_check)
+			clockcheck_set_freq(c->sanity_check, -adj);
 		break;
 	}
 	return state;
@@ -1204,3 +1220,12 @@ int clock_num_ports(struct clock *c)
 {
 	return c->nports;
 }
+
+void clock_check_ts(struct clock *c, struct timespec ts)
+{
+	if (c->sanity_check &&
+	    clockcheck_sample(c->sanity_check,
+			      ts.tv_sec * NS_PER_SEC + ts.tv_nsec)) {
+		servo_reset(c->servo);
+	}
+}

clock.h

@@ -238,4 +238,11 @@ struct clock_description *clock_description(struct clock *c);
  */
 int clock_num_ports(struct clock *c);
 
+/**
+ * Perform a sanity check on a time stamp made by a clock.
+ * @param c  The clock instance.
+ * @param ts The time stamp.
+ */
+void clock_check_ts(struct clock *c, struct timespec ts);
+
 #endif

clockcheck.c

@@ -0,0 +1,128 @@
+/**
+ * @file clockcheck.c
+ * @note Copyright (C) 2013 Miroslav Lichvar <mlichvar@redhat.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 <stdlib.h>
+#include <time.h>
+
+#include "clockcheck.h"
+#include "print.h"
+
+#define CHECK_MIN_INTERVAL 100000000
+#define CHECK_MAX_FREQ 900000000
+
+struct clockcheck {
+	/* Sanity frequency limit */
+	int freq_limit;
+	/* Frequency was set at least once */
+	int freq_known;
+	/* Current frequency */
+	int current_freq;
+	/* Maximum and minimum frequency since last update */
+	int max_freq;
+	int min_freq;
+	uint64_t last_ts;
+	uint64_t last_mono_ts;
+};
+
+struct clockcheck *clockcheck_create(int freq_limit)
+{
+	struct clockcheck *cc;
+	cc = calloc(1, sizeof(*cc));
+	if (!cc)
+		return NULL;
+	cc->freq_limit = freq_limit;
+	cc->max_freq = -CHECK_MAX_FREQ;
+	cc->min_freq = CHECK_MAX_FREQ;
+	return cc;
+}
+
+int clockcheck_sample(struct clockcheck *cc, uint64_t ts)
+{
+	uint64_t mono_ts;
+	int64_t interval, mono_interval;
+	double max_foffset, min_foffset;
+	struct timespec now;
+	int ret = 0;
+
+	/* Check the sanity of the synchronized clock by comparing its
+	   uncorrected frequency with the system monotonic clock. If
+	   the synchronized clock is the system clock, the measured
+	   frequency offset will be the current frequency correction of
+	   the system clock. */
+
+	if (!cc->freq_known)
+		return ret;
+
+	interval = (int64_t)ts - cc->last_ts;
+	if (interval >= 0 && interval < CHECK_MIN_INTERVAL)
+		return ret;
+
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	mono_ts = now.tv_sec * 1000000000LL + now.tv_nsec;
+	mono_interval = (int64_t)mono_ts - cc->last_mono_ts;
+
+	if (mono_interval < CHECK_MIN_INTERVAL)
+		return ret;
+
+	if (cc->last_ts && cc->max_freq <= CHECK_MAX_FREQ) {
+		max_foffset = 1e9 * (interval /
+				     (1.0 + cc->min_freq / 1e9) /
+				     mono_interval - 1.0);
+		min_foffset = 1e9 * (interval /
+				     (1.0 + cc->max_freq / 1e9) /
+				     mono_interval - 1.0);
+
+		if (min_foffset > cc->freq_limit) {
+			pr_warning("clockcheck: clock jumped forward or"
+					" running faster than expected!");
+			ret = 1;
+		} else if (max_foffset < -cc->freq_limit) {
+			pr_warning("clockcheck: clock jumped backward or"
+					" running slower than expected!");
+			ret = 1;
+		}
+	}
+
+	cc->last_mono_ts = mono_ts;
+	cc->last_ts = ts;
+	cc->max_freq = cc->min_freq = cc->current_freq;
+
+	return ret;
+}
+
+void clockcheck_set_freq(struct clockcheck *cc, int freq)
+{
+	if (cc->max_freq < freq)
+		cc->max_freq = freq;
+	if (cc->min_freq > freq)
+		cc->min_freq = freq;
+	cc->current_freq = freq;
+	cc->freq_known = 1;
+}
+
+void clockcheck_step(struct clockcheck *cc, int64_t step)
+{
+	if (cc->last_ts)
+		cc->last_ts += step;
+}
+
+void clockcheck_destroy(struct clockcheck *cc)
+{
+	free(cc);
+}

clockcheck.h

@@ -0,0 +1,64 @@
+/**
+ * @file clockcheck.h
+ * @brief Implements clock sanity checking.
+ * @note Copyright (C) 2013 Miroslav Lichvar <mlichvar@redhat.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_CLOCKCHECK_H
+#define HAVE_CLOCKCHECK_H
+
+#include <stdint.h>
+
+/** Opaque type */
+struct clockcheck;
+
+/**
+ * Create a new instance of a clock sanity check.
+ * @param freq_limit The maximum allowed frequency offset between uncorrected
+ *                   clock and the system monotonic clock in ppb.
+ * @return A pointer to a new clock check on success, NULL otherwise.
+ */
+struct clockcheck *clockcheck_create(int freq_limit);
+
+/**
+ * Perform the sanity check on a time stamp.
+ * @param cc Pointer to a clock check obtained via @ref clockcheck_create().
+ * @param ts Time stamp made by the clock in nanoseconds.
+ * @return Zero if ts passed the check, non-zero otherwise.
+ */
+int clockcheck_sample(struct clockcheck *cc, uint64_t ts);
+
+/**
+ * Inform clock check about changes in current frequency of the clock.
+ * @param cc   Pointer to a clock check obtained via @ref clockcheck_create().
+ * @param freq Frequency correction applied to the clock in ppb.
+ */
+void clockcheck_set_freq(struct clockcheck *cc, int freq);
+
+/**
+ * Inform clock check that the clock was stepped.
+ * @param cc   Pointer to a clock check obtained via @ref clockcheck_create().
+ * @param step Step correction applied to the clock in nanoseconds.
+ */
+void clockcheck_step(struct clockcheck *cc, int64_t step);
+
+/**
+ * Destroy a clock check.
+ * @param cc Pointer to a clock check obtained via @ref clockcheck_create().
+ */
+void clockcheck_destroy(struct clockcheck *cc);
+
+#endif

config.c

@@ -373,6 +373,12 @@ static enum parser_result parse_global_setting(const char *option,
 			return r;
 		*cfg->pi_max_frequency = val;
 
+	} else if (!strcmp(option, "sanity_freq_limit")) {
+		r = get_ranged_int(value, &val, 0, INT_MAX);
+		if (r != PARSED_OK)
+			return r;
+		cfg->dds.sanity_freq_limit = val;
+
 	} else if (!strcmp(option, "ptp_dst_mac")) {
 		if (MAC_LEN != sscanf(value, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
 				      &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]))

config.h

@@ -86,6 +86,8 @@ struct config {
 	double *pi_f_offset_const;
 	int *pi_max_frequency;
 
+	int *sanity_freq_limit;
+
 	unsigned char *ptp_dst_mac;
 	unsigned char *p2p_dst_mac;
 	unsigned char *udp6_scope;

default.cfg

@@ -52,6 +52,7 @@ pi_offset_const		0.0
 pi_f_offset_const	0.0000001
 pi_max_frequency	900000000
 clock_servo		pi
+sanity_freq_limit	200000000
 #
 # Transport options
 #

ds.h

@@ -54,6 +54,7 @@ struct default_ds {
 	int freq_est_interval; /*log seconds*/
 	int stats_interval; /*log seconds*/
 	int kernel_leap;
+	int sanity_freq_limit;
 	int time_source;
 	struct clock_description clock_desc;
 };

gPTP.cfg

@@ -51,6 +51,7 @@ pi_offset_const		0.0
 pi_f_offset_const	0.0000001
 pi_max_frequency	900000000
 clock_servo		pi
+sanity_freq_limit	200000000
 #
 # Transport options
 #

makefile

@@ -23,9 +23,9 @@ VER     = -DVER=$(version)
 CFLAGS	= -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
 LDLIBS	= -lm -lrt $(EXTRA_LDFLAGS)
 PRG	= ptp4l pmc phc2sys hwstamp_ctl
-OBJ     = bmc.o clock.o clockadj.o config.o fault.o fsm.o ptp4l.o mave.o \
+OBJ     = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
- msg.o phc.o pi.o port.o print.o raw.o servo.o sk.o stats.o tlv.o tmtab.o \
+ fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o print.o raw.o servo.o \
- transport.o udp.o udp6.o uds.o util.o version.o
+ sk.o stats.o tlv.o tmtab.o transport.o udp.o udp6.o uds.o util.o version.o
 
 OBJECTS	= $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o
 SRC	= $(OBJECTS:.o=.c)
@@ -47,9 +47,9 @@ ptp4l: $(OBJ)
 pmc: msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o transport.o udp.o \
  udp6.o uds.o util.o version.o
 
-phc2sys: clockadj.o msg.o phc.o phc2sys.o pi.o pmc_common.o print.o servo.o \
+phc2sys: clockadj.o clockcheck.o msg.o phc.o phc2sys.o pi.o pmc_common.o \
- raw.o sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o uds.o util.o \
+ print.o raw.o servo.o sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o \
- version.o
+ uds.o util.o version.o
 
 hwstamp_ctl: hwstamp_ctl.o version.o
 

phc2sys.8

@@ -27,6 +27,8 @@ phc2sys \- synchronize two clocks
 ] [
 .BI \-N " clock-readings"
 ] [
+.BI \-L " freq-limit"
+] [
 .BI \-u " summary-updates"
 ] [
 .BI \-n " domain-number"
@@ -118,6 +120,13 @@ Specify the offset between the slave and master times in seconds.  See
 .B TIME SCALE USAGE
 below.
 .TP
+.BI \-L " freq-limit"
+The maximum allowed frequency offset between uncorrected clock and the system
+monotonic clock in parts per billion (ppb). This is used as a sanity check of
+the synchronized clock. When a larger offset is measured, a warning message
+will be printed and the servo will be reset. When set to 0, the sanity check is
+disabled. The default is 200000000 (20%).
+.TP
 .BI \-u " summary-updates"
 Specify the number of clock updates included in summary statistics. The
 statistics include offset root mean square (RMS), maximum absolute offset,

phc2sys.c

@@ -36,6 +36,7 @@
 #include <linux/ptp_clock.h>
 
 #include "clockadj.h"
+#include "clockcheck.h"
 #include "ds.h"
 #include "fsm.h"
 #include "missing.h"
@@ -143,6 +144,7 @@ struct clock {
 	int pmc_ds_idx;
 	int pmc_ds_requested;
 	uint64_t pmc_last_update;
+	struct clockcheck *sanity_check;
 };
 
 static void update_clock_stats(struct clock *clock,
@@ -193,6 +195,9 @@ static void update_clock(struct clock *clock,
 		offset += clock->sync_offset * NS_PER_SEC *
 			clock->sync_offset_direction;
 
+	if (clock->sanity_check && clockcheck_sample(clock->sanity_check, ts))
+		servo_reset(clock->servo);
+
 	ppb = servo_sample(clock->servo, offset, ts, &state);
 	clock->servo_state = state;
 
@@ -201,11 +206,15 @@ static void update_clock(struct clock *clock,
 		break;
 	case SERVO_JUMP:
 		clockadj_step(clock->clkid, -offset);
+		if (clock->sanity_check)
+			clockcheck_step(clock->sanity_check, -offset);
 		/* Fall through. */
 	case SERVO_LOCKED:
 		clockadj_set_freq(clock->clkid, -ppb);
 		if (clock->clkid == CLOCK_REALTIME)
 			sysclk_set_sync();
+		if (clock->sanity_check)
+			clockcheck_set_freq(clock->sanity_check, -ppb);
 		break;
 	}
 
@@ -559,6 +568,7 @@ static void usage(char *progname)
 		" -R [rate]      slave clock update rate in HZ (1.0)\n"
 		" -N [num]       number of master clock readings per update (5)\n"
 		" -O [offset]    slave-master time offset (0)\n"
+		" -L [limit]     sanity frequency limit in ppb (200000000)\n"
 		" -u [num]       number of clock updates in summary stats (0)\n"
 		" -w             wait for ptp4l\n"
 		" -n [num]       domain number (0)\n"
@@ -579,6 +589,7 @@ int main(int argc, char *argv[])
 	int c, domain_number = 0, phc_readings = 5, pps_fd = -1;
 	int max_ppb, r, wait_sync = 0, forced_sync_offset = 0;
 	int print_level = LOG_INFO, use_syslog = 1, verbose = 0;
+	int sanity_freq_limit = 200000000;
 	double ppb, phc_interval = 1.0, phc_rate;
 	struct timespec phc_interval_tp;
 	struct clock dst_clock = {
@@ -594,7 +605,7 @@ int main(int argc, char *argv[])
 	progname = strrchr(argv[0], '/');
 	progname = progname ? 1+progname : argv[0];
 	while (EOF != (c = getopt(argc, argv,
-				  "c:d:s:P:I:S:F:R:N:O:i:u:wn:xl:mqvh"))) {
+				  "c:d:s:P:I:S:F:R:N:O:L:i:u:wn:xl:mqvh"))) {
 		switch (c) {
 		case 'c':
 			dst_clock.clkid = clock_open(optarg);
@@ -649,6 +660,10 @@ int main(int argc, char *argv[])
 			dst_clock.sync_offset_direction = -1;
 			forced_sync_offset = 1;
 			break;
+		case 'L':
+			if (get_arg_val_i(c, optarg, &sanity_freq_limit, 0, INT_MAX))
+				return -1;
+			break;
 		case 'u':
 			if (get_arg_val_ui(c, optarg, &dst_clock.stats_max_count,
 					  0, UINT_MAX))
@@ -721,6 +736,13 @@ int main(int argc, char *argv[])
 			return -1;
 		}
 	}
+	if (sanity_freq_limit) {
+		dst_clock.sanity_check = clockcheck_create(sanity_freq_limit);
+		if (!dst_clock.sanity_check) {
+			fprintf(stderr, "failed to create clock check");
+			return -1;
+		}
+	}
 
 	print_set_progname(progname);
 	print_set_verbose(verbose);

port.c

@@ -2082,6 +2082,9 @@ enum fsm_event port_event(struct port *p, int fd_index)
 		msg_put(msg);
 		return EV_NONE;
 	}
+	if (msg->hwts.ts.tv_sec && msg->hwts.ts.tv_nsec) {
+		clock_check_ts(p->clock, msg->hwts.ts);
+	}
 	if (port_ignore(p, msg)) {
 		msg_put(msg);
 		return EV_NONE;

ptp4l.8

@@ -340,6 +340,13 @@ The maximum allowed frequency adjustment of the clock in parts per billion
 set to 0, the hardware limit will be used.
 The default is 900000000 (90%).
 .TP
+.B sanity_freq_limit
+The maximum allowed frequency offset between uncorrected clock and the system
+monotonic clock in parts per billion (ppb). This is used as a sanity check of
+the synchronized clock. When a larger offset is measured, a warning message
+will be printed and the servo will be reset. When set to 0, the sanity check is
+disabled. The default is 200000000 (20%).
+.TP
 .B ptp_dst_mac
 The MAC address where should be PTP messages sent.
 Relevant only with L2 transport. The default is 01:1B:19:00:00:00.

ptp4l.c

@@ -55,6 +55,7 @@ static struct config cfg_settings = {
 		.freq_est_interval = 1,
 		.stats_interval = 0,
 		.kernel_leap = 1,
+		.sanity_freq_limit = 200000000,
 		.time_source = INTERNAL_OSCILLATOR,
 		.clock_desc = {
 			.productDescription = {