[RFC PATCH v2 4/8] timekeeping: Add absolute reference for feed-forward clock discipline

[David Woodhouse]

From: David Woodhouse <dwmw@xxxxxxxxxxxx>

Add timekeeping_set_reference() which allows an external clock source
(such as a hypervisor vmclock) to provide an absolute time reference.
The reference defines a linear counter-to-time mapping that the kernel
uses to set both the frequency and phase of the system clock.

When timekeeping_set_reference() is called:
 - tick_length is computed from the reference period and set via
   ntp_set_tick_length(), keeping all NTP state consistent
 - A pending flag is set so that on the next tick (under the
   timekeeping lock), the phase error is set via ntp_set_time_offset()

The existing time_offset slew mechanism then converges the clock to
the reference, with the clawback fix ensuring ntp_error stays accurate
across mult changes.

Signed-off-by: David Woodhouse <dwmw@xxxxxxxxxxxx>
---
 include/linux/timekeeping_reference.h | 19 ++++++++++++
 kernel/time/ntp.c                     | 27 ++++++++++++++++
 kernel/time/ntp_internal.h            |  3 ++
 kernel/time/timekeeping.c             | 44 +++++++++++++++++++++++++++
 4 files changed, 93 insertions(+)
 create mode 100644 include/linux/timekeeping_reference.h

diff --git a/include/linux/timekeeping_reference.h b/include/linux/timekeeping_reference.h
new file mode 100644
index 000000000000..4c1d8a6c02f1
--- /dev/null
+++ b/include/linux/timekeeping_reference.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _LINUX_TIMEKEEPING_REFERENCE_H
+#define _LINUX_TIMEKEEPING_REFERENCE_H
+
+#include <linux/clocksource_ids.h>
+#include <linux/types.h>
+
+struct tk_reference {
+	enum clocksource_ids	cs_id;
+	u64			counter_value;
+	u64			time_sec;
+	u64			time_frac_sec;
+	u64			period_frac_sec;
+	u8			period_shift;
+};
+
+int timekeeping_set_reference(const struct tk_reference *ref);
+
+#endif
diff --git a/kernel/time/ntp.c b/kernel/time/ntp.c
index 2d6d00ae5bf7..79e76bb6942b 100644
--- a/kernel/time/ntp.c
+++ b/kernel/time/ntp.c
@@ -366,6 +366,33 @@ u64 ntp_tick_length(unsigned int tkid)
 	return tk_ntp_data[tkid].tick_length;
 }
 
+u64 ntp_tick_length_base(unsigned int tkid)
+{
+	return tk_ntp_data[tkid].tick_length_base;
+}
+
+void ntp_set_time_offset(unsigned int tkid, s64 offset_ns)
+{
+	struct ntp_data *ntpdata = &tk_ntp_data[tkid];
+
+	ntpdata->time_offset = div_s64((s64)offset_ns << NTP_SCALE_SHIFT,
+				       NTP_INTERVAL_FREQ);
+	ntpdata->time_adjust = 0;
+}
+
+void ntp_set_tick_length(unsigned int tkid, u64 tick_length)
+{
+	struct ntp_data *ntpdata = &tk_ntp_data[tkid];
+	u64 base;
+
+	base = (u64)(ntpdata->tick_usec * NSEC_PER_USEC * USER_HZ)
+		<< NTP_SCALE_SHIFT;
+	base += ntpdata->ntp_tick_adj;
+
+	ntpdata->time_freq = (s64)(tick_length * NTP_INTERVAL_FREQ - base);
+	ntp_update_frequency(ntpdata);
+}
+
 /**
  * ntp_get_next_leap - Returns the next leapsecond in CLOCK_REALTIME ktime_t
  * @tkid:	Timekeeper ID
diff --git a/kernel/time/ntp_internal.h b/kernel/time/ntp_internal.h
index 7084d839c207..44306ffe25ff 100644
--- a/kernel/time/ntp_internal.h
+++ b/kernel/time/ntp_internal.h
@@ -6,6 +6,9 @@ extern void ntp_init(void);
 extern void ntp_clear(unsigned int tkid);
 /* Returns how long ticks are at present, in ns / 2^NTP_SCALE_SHIFT. */
 extern u64 ntp_tick_length(unsigned int tkid);
+extern u64 ntp_tick_length_base(unsigned int tkid);
+extern void ntp_set_time_offset(unsigned int tkid, s64 offset_ns);
+extern void ntp_set_tick_length(unsigned int tkid, u64 tick_length);
 extern ktime_t ntp_get_next_leap(unsigned int tkid);
 extern int second_overflow(unsigned int tkid, time64_t secs);
 extern int ntp_adjtimex(unsigned int tkid, struct __kernel_timex *txc, const struct timespec64 *ts,
diff --git a/kernel/time/timekeeping.c b/kernel/time/timekeeping.c
index 050123fc179b..89fed9473c38 100644
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -2324,6 +2324,33 @@ static __always_inline void timekeeping_apply_adjustment(struct timekeeper *tk,
  * Adjust the timekeeper's multiplier to the correct frequency
  * and also to reduce the accumulated error value.
  */
+
+#include <linux/timekeeping_reference.h>
+
+static struct tk_reference tk_ref;
+static bool tk_ref_valid;
+static bool tk_ref_pending;
+
+int timekeeping_set_reference(const struct tk_reference *ref)
+{
+	u64 ci = tk_core.timekeeper.cycle_interval;
+	u64 new_tl;
+
+	tk_ref = *ref;
+
+	new_tl = mul_u64_u64_shr(ref->period_frac_sec,
+			(u64)ci * NSEC_PER_SEC,
+			32 + ref->period_shift);
+	ntp_set_tick_length(TIMEKEEPER_CORE, new_tl);
+
+	/* Ensure tk_ref fields are visible before tk_ref_valid/pending */
+	smp_wmb();
+	tk_ref_valid = true;
+	tk_ref_pending = true;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(timekeeping_set_reference);
+
 static void timekeeping_adjust(struct timekeeper *tk, s64 offset)
 {
 	u64 ntp_tl = ntp_tick_length(tk->id);
@@ -2339,6 +2366,23 @@ static void timekeeping_adjust(struct timekeeper *tk, s64 offset)
 		tk->ntp_tick = ntp_tl;
 		mult = div64_u64(tk->ntp_tick >> tk->ntp_error_shift,
 				 tk->cycle_interval);
+		if (tk_ref_pending && tk->cs_id == tk_ref.cs_id) {
+			u64 d = tk->tkr_mono.cycle_last - tk_ref.counter_value;
+			__uint128_t p = (__uint128_t)d * tk_ref.period_frac_sec;
+			u64 rf;
+			s64 ref_err;
+
+			p >>= tk_ref.period_shift;
+			p += tk_ref.time_frac_sec;
+			rf = (u64)p;
+			ref_err = (s64)mul_u64_u64_shr(rf,
+				(u64)NSEC_PER_SEC << tk->tkr_mono.shift, 64) -
+				(s64)tk->tkr_mono.xtime_nsec;
+			ntp_set_time_offset(tk->id,
+				ref_err >> tk->tkr_mono.shift);
+			tk->ntp_error = 0;
+			tk_ref_pending = false;
+		}
 	}
 
 	/*
-- 
2.51.0