hv_util: adjust system time smoothly

index 7c90e33..51528bf 100644
@@ -185,7 +185,7 @@ static void hv_set_host_time(struct work_struct *work)
 	struct adj_time_work	*wrk;
 	s64 host_tns;
 	u64 newtime;
-	struct timespec host_ts;
+	struct timespec64 host_ts;
 
 	wrk = container_of(work, struct adj_time_work, work);
 

@@ -204,9 +204,9 @@ static void hv_set_host_time(struct work_struct *work)
 		newtime += (current_tick - wrk->ref_time);
 	}
 	host_tns = (newtime - WLTIMEDELTA) * 100;
-	host_ts = ns_to_timespec(host_tns);
+	host_ts = ns_to_timespec64(host_tns);
 
-	do_settimeofday(&host_ts);
+	do_settimeofday64(&host_ts);
 }
 
 /*

index b371d0e..c224b7d 100644
@@ -99,6 +99,9 @@ static struct clocksource hyperv_cs_msr = {
 };
 
 static void *hypercall_pg;
+struct clocksource *hyperv_cs;
+EXPORT_SYMBOL_GPL(hyperv_cs);
+
 /*
  * This function is to be invoked early in the boot sequence after the
  * hypervisor has been detected.

@@ -141,10 +144,10 @@ void hyperv_init(void)
 		union hv_x64_msr_hypercall_contents tsc_msr;
 
 		tsc_pg = __vmalloc(PAGE_SIZE, GFP_KERNEL, PAGE_KERNEL);
-		if (!tsc_pg) {
-			clocksource_register_hz(&hyperv_cs_msr, NSEC_PER_SEC/100);
-			return;
-		}
+		if (!tsc_pg)
+			goto register_msr_cs;
+
+		hyperv_cs = &hyperv_cs_tsc;
 
 		rdmsrl(HV_X64_MSR_REFERENCE_TSC, tsc_msr.as_uint64);
 

@@ -161,6 +164,8 @@ void hyperv_init(void)
 	 * the partition counter.
 	 */
 
+register_msr_cs:
+	hyperv_cs = &hyperv_cs_msr;
 	if (ms_hyperv.features & HV_X64_MSR_TIME_REF_COUNT_AVAILABLE)
 		clocksource_register_hz(&hyperv_cs_msr, NSEC_PER_SEC/100);
 }

index f8dc370..d54833d 100644
@@ -3,6 +3,7 @@
 
 #include <linux/types.h>
 #include <linux/interrupt.h>
+#include <linux/clocksource.h>
 #include <asm/hyperv.h>
 
 /*

@@ -168,6 +169,8 @@ void hv_setup_crash_handler(void (*handler)(struct pt_regs *regs));
 void hv_remove_crash_handler(void);
 
 #if IS_ENABLED(CONFIG_HYPERV)
+extern struct clocksource *hyperv_cs;
+
 void hyperv_init(void);
 void hyperv_report_panic(struct pt_regs *regs);
 bool hv_is_hypercall_page_setup(void);

index 51528bf..f3c03b2 100644
@@ -27,6 +27,8 @@
 #include <linux/sysctl.h>
 #include <linux/reboot.h>
 #include <linux/hyperv.h>
+#include <linux/clockchips.h>
+#include <linux/ptp_clock_kernel.h>
 #include <asm/mshyperv.h>
 
 #include "hyperv_vmbus.h"

@@ -182,29 +184,15 @@ struct adj_time_work {
 
 static void hv_set_host_time(struct work_struct *work)
 {
-	struct adj_time_work	*wrk;
-	s64 host_tns;
-	u64 newtime;
+	struct adj_time_work *wrk;
 	struct timespec64 host_ts;
+	u64 reftime, newtime;
 
 	wrk = container_of(work, struct adj_time_work, work);
 
-	newtime = wrk->host_time;
-	if (ts_srv_version > TS_VERSION_3) {
-		/*
-		 * Some latency has been introduced since Hyper-V generated
-		 * its time sample. Take that latency into account before
-		 * using TSC reference time sample from Hyper-V.
-		 *
-		 * This sample is given by TimeSync v4 and above hosts.
-		 */
-		u64 current_tick;
-
-		hv_get_current_tick(current_tick);
-		newtime += (current_tick - wrk->ref_time);
-	}
-	host_tns = (newtime - WLTIMEDELTA) * 100;
-	host_ts = ns_to_timespec64(host_tns);
+	reftime = hyperv_cs->read(hyperv_cs);
+	newtime = wrk->host_time + (reftime - wrk->ref_time);
+	host_ts = ns_to_timespec64((newtime - WLTIMEDELTA) * 100);
 
 	do_settimeofday64(&host_ts);
 }

@@ -223,22 +211,60 @@ static void hv_set_host_time(struct work_struct *work)
  * to discipline the clock.
  */
 static struct adj_time_work  wrk;
-static inline void adj_guesttime(u64 hosttime, u64 reftime, u8 flags)
+
+/*
+ * The last time sample, received from the host. PTP device responds to
+ * requests by using this data and the current partition-wide time reference
+ * count.
+ */
+static struct {
+	u64				host_time;
+	u64				ref_time;
+	struct system_time_snapshot	snap;
+	spinlock_t			lock;
+} host_ts;
+
+static inline void adj_guesttime(u64 hosttime, u64 reftime, u8 adj_flags)
 {
+	unsigned long flags;
+	u64 cur_reftime;
 
 	/*
 	 * This check is safe since we are executing in the
 	 * interrupt context and time synch messages arre always
 	 * delivered on the same CPU.
 	 */
-	if (work_pending(&wrk.work))
-		return;
-
-	wrk.host_time = hosttime;
-	wrk.ref_time = reftime;
-	wrk.flags = flags;
-	if ((flags & (ICTIMESYNCFLAG_SYNC | ICTIMESYNCFLAG_SAMPLE)) != 0) {
+	if (adj_flags & ICTIMESYNCFLAG_SYNC) {
+		/* Queue a job to do do_settimeofday64() */
+		if (work_pending(&wrk.work))
+			return;
+
+		wrk.host_time = hosttime;
+		wrk.ref_time = reftime;
+		wrk.flags = adj_flags;
 		schedule_work(&wrk.work);
+	} else {
+		/*
+		 * Save the adjusted time sample from the host and the snapshot
+		 * of the current system time for PTP device.
+		 */
+		spin_lock_irqsave(&host_ts.lock, flags);
+
+		cur_reftime = hyperv_cs->read(hyperv_cs);
+		host_ts.host_time = hosttime;
+		host_ts.ref_time = cur_reftime;
+		ktime_get_snapshot(&host_ts.snap);
+
+		/*
+		 * TimeSync v4 messages contain reference time (guest's Hyper-V
+		 * clocksource read when the time sample was generated), we can
+		 * improve the precision by adding the delta between now and the
+		 * time of generation.
+		 */
+		if (ts_srv_version > TS_VERSION_3)
+			host_ts.host_time += (cur_reftime - reftime);
+
+		spin_unlock_irqrestore(&host_ts.lock, flags);
 	}
 }
 

@@ -471,14 +497,113 @@ static  struct hv_driver util_drv = {
 	.remove =  util_remove,
 };
 
+static int hv_ptp_enable(struct ptp_clock_info *info,
+			 struct ptp_clock_request *request, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static int hv_ptp_settime(struct ptp_clock_info *p, const struct timespec64 *ts)
+{
+	return -EOPNOTSUPP;
+}
+
+static int hv_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta)
+{
+	return -EOPNOTSUPP;
+}
+static int hv_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int hv_ptp_gettime(struct ptp_clock_info *info, struct timespec64 *ts)
+{
+	unsigned long flags;
+	u64 newtime, reftime;
+
+	spin_lock_irqsave(&host_ts.lock, flags);
+	reftime = hyperv_cs->read(hyperv_cs);
+	newtime = host_ts.host_time + (reftime - host_ts.ref_time);
+	*ts = ns_to_timespec64((newtime - WLTIMEDELTA) * 100);
+	spin_unlock_irqrestore(&host_ts.lock, flags);
+
+	return 0;
+}
+
+static int hv_ptp_get_syncdevicetime(ktime_t *device,
+				     struct system_counterval_t *system,
+				     void *ctx)
+{
+	system->cs = hyperv_cs;
+	system->cycles = host_ts.ref_time;
+	*device = ns_to_ktime((host_ts.host_time - WLTIMEDELTA) * 100);
+
+	return 0;
+}
+
+static int hv_ptp_getcrosststamp(struct ptp_clock_info *ptp,
+				 struct system_device_crosststamp *xtstamp)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&host_ts.lock, flags);
+
+	/*
+	 * host_ts contains the last time sample from the host and the snapshot
+	 * of system time. We don't need to calculate the time delta between
+	 * the reception and now as get_device_system_crosststamp() does the
+	 * required interpolation.
+	 */
+	ret = get_device_system_crosststamp(hv_ptp_get_syncdevicetime,
+					    NULL, &host_ts.snap, xtstamp);
+
+	spin_unlock_irqrestore(&host_ts.lock, flags);
+
+	return ret;
+}
+
+static struct ptp_clock_info ptp_hyperv_info = {
+	.name		= "hyperv",
+	.enable         = hv_ptp_enable,
+	.adjtime        = hv_ptp_adjtime,
+	.adjfreq        = hv_ptp_adjfreq,
+	.gettime64      = hv_ptp_gettime,
+	.getcrosststamp = hv_ptp_getcrosststamp,
+	.settime64      = hv_ptp_settime,
+	.owner		= THIS_MODULE,
+};
+
+static struct ptp_clock *hv_ptp_clock;
+
 static int hv_timesync_init(struct hv_util_service *srv)
 {
+	/* TimeSync requires Hyper-V clocksource. */
+	if (!hyperv_cs)
+		return -ENODEV;
+
 	INIT_WORK(&wrk.work, hv_set_host_time);
+
+	/*
+	 * ptp_clock_register() returns NULL when CONFIG_PTP_1588_CLOCK is
+	 * disabled but the driver is still useful without the PTP device
+	 * as it still handles the ICTIMESYNCFLAG_SYNC case.
+	 */
+	hv_ptp_clock = ptp_clock_register(&ptp_hyperv_info, NULL);
+	if (IS_ERR_OR_NULL(hv_ptp_clock)) {
+		pr_err("cannot register PTP clock: %ld\n",
+		       PTR_ERR(hv_ptp_clock));
+		hv_ptp_clock = NULL;
+	}
+
 	return 0;
 }
 
 static void hv_timesync_deinit(void)
 {
+	if (hv_ptp_clock)
+		ptp_clock_unregister(hv_ptp_clock);
 	cancel_work_sync(&wrk.work);
 }