[PATCH 4/4] wineboot: Initialize and calibrate user shared data Qpc frequency.

[Rémi Bernon]

Signed-off-by: Rémi Bernon <rbernon at codeweavers.com>
---
 programs/wineboot/wineboot.c | 117 +++++++++++++++++++++++++++++++++++
 1 file changed, 117 insertions(+)

diff --git a/programs/wineboot/wineboot.c b/programs/wineboot/wineboot.c
index 9e36b3c22dd..9333116e9af 100644
--- a/programs/wineboot/wineboot.c
+++ b/programs/wineboot/wineboot.c
@@ -245,10 +245,127 @@ static void initialize_xstate_features(struct _KUSER_SHARED_DATA *data)
 
 static void initialize_qpc_features(struct _KUSER_SHARED_DATA *data)
 {
+    int regs[4], cpuid_level, denom, numer, freq, tmp;
+
+    if (data->QpcBypassEnabled) return;
+
     data->QpcBypassEnabled = 0;
     data->QpcFrequency = TICKSPERSEC;
     data->QpcShift = 0;
     data->QpcBias = 0;
+
+    if (!data->ProcessorFeatures[PF_RDTSC_INSTRUCTION_AVAILABLE])
+    {
+        WARN("No RDTSC support, disabling QpcBypass\n");
+        return;
+    }
+
+    __cpuid(regs, 0x80000000);
+    if (regs[0] < 0x80000007)
+    {
+        WARN("Unable to check invariant TSC, disabling QpcBypass\n");
+        return;
+    }
+
+    /* check for invariant tsc bit */
+    __cpuid(regs, 0x80000007);
+    if (!(regs[3] & (1 << 8)))
+    {
+        WARN("No invariant TSC, disabling QpcBypass\n");
+        return;
+    }
+    data->QpcBypassEnabled |= SHARED_GLOBAL_FLAGS_QPC_BYPASS_ENABLED;
+
+    /* check for rdtscp support bit */
+    __cpuid(regs, 0x80000001);
+    if ((regs[3] & (1 << 27)))
+        data->QpcBypassEnabled |= SHARED_GLOBAL_FLAGS_QPC_BYPASS_USE_RDTSCP;
+    else if (data->ProcessorFeatures[PF_XMMI64_INSTRUCTIONS_AVAILABLE])
+        data->QpcBypassEnabled |= SHARED_GLOBAL_FLAGS_QPC_BYPASS_USE_LFENCE;
+    else
+        data->QpcBypassEnabled |= SHARED_GLOBAL_FLAGS_QPC_BYPASS_USE_MFENCE;
+
+    __cpuid(regs, 0);
+    tmp = regs[2];
+    regs[2] = regs[3];
+    regs[3] = tmp;
+
+    /* only available on some intel CPUs */
+    if (memcmp(regs + 1, "GenuineIntel", 12)) data->QpcFrequency = 0;
+    else if ((cpuid_level = regs[0]) < 0x15) data->QpcFrequency = 0;
+    else
+    {
+        __cpuid(regs, 0x15);
+        if (!(denom = regs[0]) || !(numer = regs[1])) data->QpcFrequency = 0;
+        else
+        {
+            if (!(freq = regs[2]) && cpuid_level >= 0x16)
+            {
+                __cpuid(regs, 0x16); /* eax is base freq in MHz */
+                freq = regs[0] * 1000 * denom / numer;
+            }
+
+            data->QpcFrequency = freq * numer / denom;
+        }
+
+        if (!data->QpcFrequency)
+            WARN("Failed to read TSC frequency from CPUID, falling back to calibration.\n");
+        else
+        {
+            data->QpcFrequency = (data->QpcFrequency + (1 << 10) - 1) >> 10;
+            data->QpcShift = 10;
+            data->QpcBias = 0;
+
+            TRACE("TSC frequency read from CPUID, freq %I64d, shift %d, bias %I64d\n",
+                  data->QpcFrequency, data->QpcShift, data->QpcBias);
+        }
+    }
+
+    if (!data->QpcFrequency)
+    {
+        LONGLONG time0, time1, tsc0, tsc1, tsc2, tsc3, freq0, freq1, error;
+        unsigned int aux;
+        UINT retries = 50;
+
+        data->QpcShift = 0;
+        data->QpcBias = 0;
+
+        do
+        {
+            tsc0 = __rdtscp(&aux);
+            time0 = RtlGetSystemTimePrecise();
+            tsc1 = __rdtscp(&aux);
+            Sleep(1);
+            tsc2 = __rdtscp(&aux);
+            time1 = RtlGetSystemTimePrecise();
+            tsc3 = __rdtscp(&aux);
+
+            freq0 = (tsc2 - tsc0) * 10000000 / (time1 - time0);
+            freq1 = (tsc3 - tsc1) * 10000000 / (time1 - time0);
+            error = llabs((freq1 - freq0) * 1000000 / min(freq1, freq0));
+        }
+        while (error > 100 && retries--);
+
+        if (!retries) WARN("TSC frequency calibration failed, unstable TSC?\n");
+        else
+        {
+            data->QpcFrequency = (freq0 + freq1 + (1 << 10) - 1) >> 11;
+            data->QpcShift = 10;
+            data->QpcBias = 0;
+
+            TRACE("TSC frequency calibration complete, freq %I64d, shift %d, bias %I64d\n",
+                  data->QpcFrequency, data->QpcShift, data->QpcBias);
+        }
+    }
+
+    if (!data->QpcFrequency)
+    {
+        WARN("Unable to calibrate TSC frequency, disabling QpcBypass.\n");
+        data->QpcBypassEnabled = 0;
+        data->QpcFrequency = TICKSPERSEC;
+        data->QpcShift = 0;
+        data->QpcBias = 0;
+    }
 }
 
 #else
-- 
2.30.2