3 files changed, 213 insertions, 0 deletions

diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index bdce33291161..384f661a6496 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -90,4 +90,16 @@ config PTP_1588_CLOCK_PCH
           To compile this driver as a module, choose M here: the module
           will be called ptp_pch.
 
+config PTP_1588_CLOCK_KVM
+        tristate "KVM virtual PTP clock"
+        depends on PTP_1588_CLOCK
+        depends on KVM_GUEST && X86
+        default y
+        help
+          This driver adds support for using kvm infrastructure as a PTP
+          clock. This clock is only useful if you are using KVM guests.
+
+          To compile this driver as a module, choose M here: the module
+          will be called ptp_kvm.
+
 endmenu
diff --git a/drivers/ptp/Makefile b/drivers/ptp/Makefile
index 8b58597298de..530736161a8b 100644
--- a/drivers/ptp/Makefile
+++ b/drivers/ptp/Makefile
@@ -6,3 +6,4 @@ ptp-y					:= ptp_clock.o ptp_chardev.o ptp_sysfs.o
 obj-$(CONFIG_PTP_1588_CLOCK)            += ptp.o
 obj-$(CONFIG_PTP_1588_CLOCK_IXP46X)     += ptp_ixp46x.o
 obj-$(CONFIG_PTP_1588_CLOCK_PCH)        += ptp_pch.o
+obj-$(CONFIG_PTP_1588_CLOCK_KVM)        += ptp_kvm.o
diff --git a/drivers/ptp/ptp_kvm.c b/drivers/ptp/ptp_kvm.c
new file mode 100644
index 000000000000..0a54e8326a90
--- /dev/null
+++ b/drivers/ptp/ptp_kvm.c
@@ -0,0 +1,200 @@
+/*
+ * Virtual PTP 1588 clock for use with KVM guests
+ *
+ * Copyright (C) 2017 Red Hat Inc.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ */
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <uapi/linux/kvm_para.h>
+#include <asm/kvm_para.h>
+#include <asm/pvclock.h>
+#include <asm/kvmclock.h>
+#include <uapi/asm/kvm_para.h>
+
+#include <linux/ptp_clock_kernel.h>
+
+struct kvm_ptp_clock {
+        struct ptp_clock *ptp_clock;
+        struct ptp_clock_info caps;
+};
+
+DEFINE_SPINLOCK(kvm_ptp_lock);
+
+static struct pvclock_vsyscall_time_info *hv_clock;
+
+static struct kvm_clock_pairing clock_pair;
+static phys_addr_t clock_pair_gpa;
+
+static int ptp_kvm_get_time_fn(ktime_t *device_time,
+                               struct system_counterval_t *system_counter,
+                               void *ctx)
+{
+        unsigned long ret;
+        struct timespec64 tspec;
+        unsigned version;
+        int cpu;
+        struct pvclock_vcpu_time_info *src;
+
+        spin_lock(&kvm_ptp_lock);
+
+        preempt_disable_notrace();
+        cpu = smp_processor_id();
+        src = &hv_clock[cpu].pvti;
+
+        do {
+                /*
+                 * We are using a TSC value read in the hosts
+                 * kvm_hc_clock_pairing handling.
+                 * So any changes to tsc_to_system_mul
+                 * and tsc_shift or any other pvclock
+                 * data invalidate that measurement.
+                 */
+                version = pvclock_read_begin(src);
+
+                ret = kvm_hypercall2(KVM_HC_CLOCK_PAIRING,
+                                     clock_pair_gpa,
+                                     KVM_CLOCK_PAIRING_WALLCLOCK);
+                if (ret != 0) {
+                        pr_err_ratelimited("clock pairing hypercall ret %lu\n", ret);
+                        spin_unlock(&kvm_ptp_lock);
+                        preempt_enable_notrace();
+                        return -EOPNOTSUPP;
+                }
+
+                tspec.tv_sec = clock_pair.sec;
+                tspec.tv_nsec = clock_pair.nsec;
+                ret = __pvclock_read_cycles(src, clock_pair.tsc);
+        } while (pvclock_read_retry(src, version));
+
+        preempt_enable_notrace();
+
+        system_counter->cycles = ret;
+        system_counter->cs = &kvm_clock;
+
+        *device_time = timespec64_to_ktime(tspec);
+
+        spin_unlock(&kvm_ptp_lock);
+
+        return 0;
+}
+
+static int ptp_kvm_getcrosststamp(struct ptp_clock_info *ptp,
+                                  struct system_device_crosststamp *xtstamp)
+{
+        return get_device_system_crosststamp(ptp_kvm_get_time_fn, NULL,
+                                             NULL, xtstamp);
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_kvm_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+        return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+        return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_settime(struct ptp_clock_info *ptp,
+                           const struct timespec64 *ts)
+{
+        return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+        unsigned long ret;
+        struct timespec64 tspec;
+
+        spin_lock(&kvm_ptp_lock);
+
+        ret = kvm_hypercall2(KVM_HC_CLOCK_PAIRING,
+                             clock_pair_gpa,
+                             KVM_CLOCK_PAIRING_WALLCLOCK);
+        if (ret != 0) {
+                pr_err_ratelimited("clock offset hypercall ret %lu\n", ret);
+                spin_unlock(&kvm_ptp_lock);
+                return -EOPNOTSUPP;
+        }
+
+        tspec.tv_sec = clock_pair.sec;
+        tspec.tv_nsec = clock_pair.nsec;
+        spin_unlock(&kvm_ptp_lock);
+
+        memcpy(ts, &tspec, sizeof(struct timespec64));
+
+        return 0;
+}
+
+static int ptp_kvm_enable(struct ptp_clock_info *ptp,
+                          struct ptp_clock_request *rq, int on)
+{
+        return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info ptp_kvm_caps = {
+        .owner          = THIS_MODULE,
+        .name           = "KVM virtual PTP",
+        .max_adj        = 0,
+        .n_ext_ts       = 0,
+        .n_pins         = 0,
+        .pps            = 0,
+        .adjfreq        = ptp_kvm_adjfreq,
+        .adjtime        = ptp_kvm_adjtime,
+        .gettime64      = ptp_kvm_gettime,
+        .settime64      = ptp_kvm_settime,
+        .enable         = ptp_kvm_enable,
+        .getcrosststamp = ptp_kvm_getcrosststamp,
+};
+
+/* module operations */
+
+static struct kvm_ptp_clock kvm_ptp_clock;
+
+static void __exit ptp_kvm_exit(void)
+{
+        ptp_clock_unregister(kvm_ptp_clock.ptp_clock);
+}
+
+static int __init ptp_kvm_init(void)
+{
+        clock_pair_gpa = slow_virt_to_phys(&clock_pair);
+        hv_clock = pvclock_pvti_cpu0_va();
+
+        if (!hv_clock)
+                return -ENODEV;
+
+        kvm_ptp_clock.caps = ptp_kvm_caps;
+
+        kvm_ptp_clock.ptp_clock = ptp_clock_register(&kvm_ptp_clock.caps, NULL);
+
+        if (IS_ERR(kvm_ptp_clock.ptp_clock))
+                return PTR_ERR(kvm_ptp_clock.ptp_clock);
+
+        return 0;
+}
+
+module_init(ptp_kvm_init);
+module_exit(ptp_kvm_exit);
+
+MODULE_AUTHOR("Marcelo Tosatti <mtosatti@redhat.com>");
+MODULE_DESCRIPTION("PTP clock using KVMCLOCK");
+MODULE_LICENSE("GPL");