[patch 16/26] timekeeping: Update PTP timekeepers on clocksource change

[Thomas Gleixner <tglx@linutronix.de>]

Propagate a system clocksource change to the PTP timekeepers.

Signed-off-by: Thomas Gleixner <tglx@linutronix.de>

---
 kernel/time/timekeeping.c |   28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)
---
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -119,8 +119,10 @@ static struct tk_fast tk_fast_raw  ____c
 
 #ifdef CONFIG_POSIX_PTP_CLOCKS
 static __init void tk_ptp_setup(void);
+static void tk_ptp_update_clocksource(void);
 #else
 static inline void tk_ptp_setup(void) { }
+static inline void tk_ptp_update_clocksource(void) { }
 #endif
 
 unsigned long timekeeper_lock_irqsave(void)
@@ -1548,6 +1550,8 @@ static int change_clocksource(void *data
 		timekeeping_update_from_shadow(&tk_core, TK_UPDATE_ALL);
 	}
 
+	tk_ptp_update_clocksource();
+
 	if (old) {
 		if (old->disable)
 			old->disable(old);
@@ -2653,6 +2657,30 @@ EXPORT_SYMBOL(hardpps);
 #endif /* CONFIG_NTP_PPS */
 
 #ifdef CONFIG_POSIX_PTP_CLOCKS
+
+/* Bitmap for the activated PTP timekeepers */
+static unsigned long ptp_timekeepers;
+
+/* Invoked from timekeeping after a clocksource change */
+static void tk_ptp_update_clocksource(void)
+{
+	unsigned long active = READ_ONCE(ptp_timekeepers);
+	unsigned int id;
+
+	for_each_set_bit(id, &active, BITS_PER_LONG) {
+		struct tk_data *tkd = &timekeeper_data[id + TIMEKEEPER_PTP];
+		struct timekeeper *tks = &tkd->shadow_timekeeper;
+
+		guard(raw_spinlock_irqsave)(&tkd->lock);
+		if (!tks->clock_valid)
+			continue;
+
+		timekeeping_forward_now(tks);
+		tk_setup_internals(tks, tk_core.timekeeper.tkr_mono.clock);
+		timekeeping_update_from_shadow(tkd, TK_UPDATE_ALL);
+	}
+}
+
 static __init void tk_ptp_setup(void)
 {
 	for (int i = TIMEKEEPER_PTP; i <= TIMEKEEPER_PTP_LAST; i++)