[SeaBIOS] [PATCH v2 4/4] kvm: add support for reading tsc frequency from kvmclock

[Gerd Hoffmann]

Signed-off-by: Gerd Hoffmann 
---
 src/fw/paravirt.h | 12 ++++++++++++
 src/fw/paravirt.c | 44 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+)

diff --git a/src/fw/paravirt.h b/src/fw/paravirt.h
index f7e1d4c551d9..4e2e993ba9d3 100644
--- a/src/fw/paravirt.h
+++ b/src/fw/paravirt.h
@@ -5,6 +5,18 @@
 #include "biosvar.h" // GET_GLOBAL
 #include "romfile.h" // struct romfile_s

+// kvmclock
+struct pvclock_vcpu_time_info {
+	u32   version;
+	u32   pad0;
+	u64   tsc_timestamp;
+	u64   system_time;
+	u32   tsc_to_system_mul;
+	s8    tsc_shift;
+	u8    flags;
+	u8    pad[2];
+} __attribute__((__packed__)); /* 32 bytes */
+
 // Types of paravirtualized platforms.
 #define PF_QEMU     (1<<0)
 #define PF_XEN      (1<<1)
diff --git a/src/fw/paravirt.c b/src/fw/paravirt.c
index 1e3d6012700b..119280c574fd 100644
--- a/src/fw/paravirt.c
+++ b/src/fw/paravirt.c
@@ -75,6 +75,48 @@ static void kvm_detect(void)
     }
 }

+#define KVM_FEATURE_CLOCKSOURCE           0
+#define KVM_FEATURE_CLOCKSOURCE2          3
+
+#define MSR_KVM_SYSTEM_TIME            0x12
+#define MSR_KVM_SYSTEM_TIME_NEW  0x4b564d01
+
+#define PVCLOCK_TSC_STABLE_BIT     (1 << 0)
+
+struct pvclock_vcpu_time_info *kvmclock;
+
+static void kvmclock_init(void)
+{
+    unsigned int eax, ebx, ecx, edx, msr;
+
+    if (!runningOnKVM())
+        return;
+
+    cpuid(KVM_CPUID_SIGNATURE + 0x01, &eax, &ebx, &ecx, &edx);
+    if (eax & (1 <<  KVM_FEATURE_CLOCKSOURCE2))
+        msr = MSR_KVM_SYSTEM_TIME_NEW;
+    else if (eax & (1 <<  KVM_FEATURE_CLOCKSOURCE))
+        msr = MSR_KVM_SYSTEM_TIME;
+    else
+        return;
+
+    kvmclock = memalign_low(sizeof(*kvmclock), 32);
+    memset(kvmclock, 0, sizeof(*kvmclock));
+    u32 value = (u32)(kvmclock);
+    dprintf(1, "kvmclock: at 0x%x (msr 0x%x)\n", value, msr);
+    wrmsr(msr, value | 0x01);
+
+    if (!(kvmclock->flags & PVCLOCK_TSC_STABLE_BIT))
+        return;
+    u32 MHz = (1000 << 16) / (kvmclock->tsc_to_system_mul >> 16);
+    if (kvmclock->tsc_shift < 0)
+        MHz <<= -kvmclock->tsc_shift;
+    else
+        MHz >>= kvmclock->tsc_shift;
+    dprintf(1, "kvmclock: stable tsc, %d MHz\n", MHz);
+    tsctimer_setfreq(MHz * 1000, "kvmclock");
+}
+
 static void qemu_detect(void)
 {
     if (!CONFIG_QEMU_HARDWARE)
@@ -163,6 +205,8 @@ qemu_platform_setup(void)
         return;
     }

+    kvmclock_init();
+
     // Initialize pci
     pci_setup();
     smm_device_setup();