1 files changed, 152 insertions, 27 deletions

diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index d42ede78a9dd..3076ee3ccc6c 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -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"
@@ -210,29 +212,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);
 }
@@ -251,22 +239,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);
         }
 }
 
@@ -479,14 +505,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);
 }