Add support for write phase mode.

Recently the Linux kernel's PTP Hardware Clock interface was expanded
to include a "write phase" mode where the clock servo in implemented
in hardware.  This mode hearkens back to the tradition ntp_adjtime
interface, passing a measured offset into the kernel's servo.

This patch adds a new configuration option and logic to support the
write phase mode.

Because the hardware's adjustment bandwidth may be limited, this mode
is only activated when the servo reaches SERVO_LOCKED_STABLE state, in
order to achieve reasonably fast locking times.  Users may control the
SERVO_LOCKED_STABLE state by configuring 'servo_offset_threshold' and
'servo_num_offset_values' accordingly.

Example configuration file highlights:

  unicast_listen          1
  logSyncInterval         0
  logMinDelayReqInterval  0
  first_step_threshold    0.001000000
  step_threshold          0
  clock_servo             pi

  write_phase_mode        1
  servo_offset_threshold  50
  servo_num_offset_values 10
  tsproc_mode             raw

Signed-off-by: Vincent Cheng <vincent.cheng.xh@renesas.com>
Signed-off-by: Richard Cochran <richardcochran@gmail.com>

clock.c

@@ -22,6 +22,7 @@
 #include <poll.h>
 #include <stdlib.h>
 #include <string.h>
+#include <sys/ioctl.h>
 #include <sys/queue.h>
 
 #include "address.h"
@@ -102,6 +103,7 @@ struct clock {
 	int sde;
 	int free_running;
 	int freq_est_interval;
+	int write_phase_mode;
 	int grand_master_capable; /* for 802.1AS only */
 	int utc_timescale;
 	int utc_offset_set;
@@ -1028,6 +1030,7 @@ struct clock *clock_create(enum clock_type type, struct config *config,
 	c->config = config;
 	c->free_running = config_get_int(config, NULL, "free_running");
 	c->freq_est_interval = config_get_int(config, NULL, "freq_est_interval");
+	c->write_phase_mode = config_get_int(config, NULL, "write_phase_mode");
 	c->grand_master_capable = config_get_int(config, NULL, "gmCapable");
 	c->kernel_leap = config_get_int(config, NULL, "kernel_leap");
 	c->utc_offset = config_get_int(config, NULL, "utc_offset");
@@ -1076,6 +1079,12 @@ struct clock *clock_create(enum clock_type type, struct config *config,
 		   and return 0. Set the frequency back to make sure fadj is
 		   the actual frequency of the clock. */
 		clockadj_set_freq(c->clkid, fadj);
+
+		/* Disable write phase mode if not implemented by driver */
+		if (c->write_phase_mode && !phc_has_writephase(c->clkid)) {
+			pr_err("clock does not support write phase mode");
+			return NULL;
+		}
 	}
 	c->servo = servo_create(c->config, servo, -fadj, max_adj, sw_ts);
 	if (!c->servo) {
@@ -1632,10 +1641,22 @@ int clock_switch_phc(struct clock *c, int phc_index)
 	return 0;
 }
 
+static void clock_synchronize_locked(struct clock *c, double adj)
+{
+	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);
+	}
+}
+
 enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 {
-	double adj, weight;
 	enum servo_state state = SERVO_UNLOCKED;
+	double adj, weight;
+	int64_t offset;
 
 	c->ingress_ts = ingress;
 
@@ -1659,19 +1680,11 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 		return clock_no_adjust(c, ingress, origin);
 	}
 
-	adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset),
+	offset = tmv_to_nanoseconds(c->master_offset);
-			   tmv_to_nanoseconds(ingress), weight, &state);
+	adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ingress),
+			   weight, &state);
 	c->servo_state = state;
 
-	if (c->stats.max_count > 1) {
-		clock_stats_update(&c->stats, tmv_dbl(c->master_offset), adj);
-	} else {
-		pr_info("master offset %10" PRId64 " s%d freq %+7.0f "
-			"path delay %9" PRId64,
-			tmv_to_nanoseconds(c->master_offset), state, adj,
-			tmv_to_nanoseconds(c->path_delay));
-	}
-
 	tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c));
 
 	switch (state) {
@@ -1689,16 +1702,27 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 		tsproc_reset(c->tsproc, 0);
 		break;
 	case SERVO_LOCKED:
+		clock_synchronize_locked(c, adj);
+		break;
 	case SERVO_LOCKED_STABLE:
-		clockadj_set_freq(c->clkid, -adj);
+		if (c->write_phase_mode) {
-		if (c->clkid == CLOCK_REALTIME) {
+			clockadj_set_phase(c->clkid, -offset);
-			sysclk_set_sync();
+			adj = 0;
-		}
+		} else {
-		if (c->sanity_check) {
+			clock_synchronize_locked(c, adj);
-			clockcheck_set_freq(c->sanity_check, -adj);
 		}
 		break;
 	}
+
+	if (c->stats.max_count > 1) {
+		clock_stats_update(&c->stats, tmv_dbl(c->master_offset), adj);
+	} else {
+		pr_info("master offset %10" PRId64 " s%d freq %+7.0f "
+			"path delay %9" PRId64,
+			tmv_to_nanoseconds(c->master_offset), state, adj,
+			tmv_to_nanoseconds(c->path_delay));
+	}
+
 	return state;
 }
 

clockadj.c

@@ -79,6 +79,18 @@ double clockadj_get_freq(clockid_t clkid)
 	return f;
 }
 
+void clockadj_set_phase(clockid_t clkid, long offset)
+{
+	struct timex tx;
+	memset(&tx, 0, sizeof(tx));
+
+	tx.modes = ADJ_OFFSET | ADJ_NANO;
+	tx.offset = offset;
+	if (clock_adjtime(clkid, &tx) < 0) {
+		pr_err("failed to set the clock offset: %m");
+	}
+}
+
 void clockadj_step(clockid_t clkid, int64_t step)
 {
 	struct timex tx;

clockadj.h

@@ -43,6 +43,13 @@ void clockadj_set_freq(clockid_t clkid, double freq);
  */
 double clockadj_get_freq(clockid_t clkid);
 
+/**
+ * Set clock's phase offset.
+ * @param clkid  A clock ID obtained using phc_open() or CLOCK_REALTIME.
+ * @param offset The phase offset in nanoseconds.
+ */
+void clockadj_set_phase(clockid_t clkid, long offset);
+
 /**
  * Step clock's time.
  * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME.

config.c

@@ -328,6 +328,7 @@ struct config_item config_tab[] = {
 	GLOB_ITEM_STR("userDescription", ""),
 	GLOB_ITEM_INT("utc_offset", CURRENT_UTC_OFFSET, 0, INT_MAX),
 	GLOB_ITEM_INT("verbose", 0, 0, 1),
+	GLOB_ITEM_INT("write_phase_mode", 0, 0, 1),
 };
 
 static struct unicast_master_table *current_uc_mtab;

configs/default.cfg

@@ -80,6 +80,7 @@ ntpshm_segment		0
 msg_interval_request	0
 servo_num_offset_values 10
 servo_offset_threshold  0
+write_phase_mode	0
 #
 # Transport options
 #

missing.h

@@ -24,6 +24,7 @@
 #define HAVE_MISSING_H
 
 #include <linux/ptp_clock.h>
+#include <linux/version.h>
 #include <sys/syscall.h>
 #include <sys/timex.h>
 #include <time.h>
@@ -75,9 +76,9 @@ enum {
 #define PTP_PEROUT_REQUEST2 PTP_PEROUT_REQUEST
 #endif
 
-#ifndef PTP_PIN_SETFUNC
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,8,0)
 
-/* from Linux kernel version 5.4 */
+/* from upcoming Linux kernel version 5.8 */
 struct compat_ptp_clock_caps {
 	int max_adj;   /* Maximum frequency adjustment in parts per billon. */
 	int n_alarm;   /* Number of programmable alarms. */
@@ -87,11 +88,17 @@ struct compat_ptp_clock_caps {
 	int n_pins;    /* Number of input/output pins. */
 	/* Whether the clock supports precise system-device cross timestamps */
 	int cross_timestamping;
-	int rsv[13];   /* Reserved for future use. */
+	/* Whether the clock supports adjust phase */
+	int adjust_phase;
+	int rsv[12];   /* Reserved for future use. */
 };
 
 #define ptp_clock_caps compat_ptp_clock_caps
 
+#endif /*LINUX_VERSION_CODE < 5.8*/
+
+#ifndef PTP_PIN_SETFUNC
+
 enum ptp_pin_function {
 	PTP_PF_NONE,
 	PTP_PF_EXTTS,

phc.c

@@ -127,3 +127,13 @@ int phc_has_pps(clockid_t clkid)
 		return 0;
 	return caps.pps;
 }
+
+int phc_has_writephase(clockid_t clkid)
+{
+	struct ptp_clock_caps caps;
+
+	if (phc_get_caps(clkid, &caps)) {
+		return 0;
+	}
+	return caps.adjust_phase;
+}

phc.h

@@ -77,4 +77,14 @@ int phc_pin_setfunc(clockid_t clkid, struct ptp_pin_desc *desc);
  */
 int phc_has_pps(clockid_t clkid);
 
+/**
+ * Checks whether the given PTP hardware clock device supports write phase mode.
+ *
+ * @param clkid A clock ID obtained using phc_open().
+ *
+ * @return Zero if write phase mode is not supported by the clock, non-zero
+ * otherwise.
+ */
+int phc_has_writephase(clockid_t clkid);
+
 #endif

ptp4l.8

@@ -763,6 +763,12 @@ The offset threshold used in order to transition from the SERVO_LOCKED
 to the SERVO_LOCKED_STABLE state.  The transition occurs once the last
 'servo_num_offset_values' offsets are all below the threshold value.
 The default value of offset_threshold is 0 (disabled).
+.TP
+.B write_phase_mode
+This option enables using the "write phase" feature of a PTP Hardware
+Clock.  If supported by the device, this mode uses the hardware's
+built in phase offset control instead of frequency offset control.
+The default value is 0 (disabled).
 
 .SH UNICAST DISCOVERY OPTIONS