1 files changed, 51 insertions, 10 deletions

diff --git a/arch/x86/kernel/tsc.c b/arch/x86/kernel/tsc.c
index e169e85db434..fb4302738410 100644
--- a/arch/x86/kernel/tsc.c
+++ b/arch/x86/kernel/tsc.c
@@ -25,6 +25,7 @@
 #include <asm/geode.h>
 #include <asm/apic.h>
 #include <asm/intel-family.h>
+#include <asm/i8259.h>
 
 unsigned int __read_mostly cpu_khz;     /* TSC clocks / usec, not used here */
 EXPORT_SYMBOL(cpu_khz);
@@ -363,6 +364,20 @@ static unsigned long pit_calibrate_tsc(u32 latch, unsigned long ms, int loopmin)
         unsigned long tscmin, tscmax;
         int pitcnt;
 
+        if (!has_legacy_pic()) {
+                /*
+                 * Relies on tsc_early_delay_calibrate() to have given us semi
+                 * usable udelay(), wait for the same 50ms we would have with
+                 * the PIT loop below.
+                 */
+                udelay(10 * USEC_PER_MSEC);
+                udelay(10 * USEC_PER_MSEC);
+                udelay(10 * USEC_PER_MSEC);
+                udelay(10 * USEC_PER_MSEC);
+                udelay(10 * USEC_PER_MSEC);
+                return ULONG_MAX;
+        }
+
         /* Set the Gate high, disable speaker */
         outb((inb(0x61) & ~0x02) | 0x01, 0x61);
 
@@ -487,6 +502,9 @@ static unsigned long quick_pit_calibrate(void)
         u64 tsc, delta;
         unsigned long d1, d2;
 
+        if (!has_legacy_pic())
+                return 0;
+
         /* Set the Gate high, disable speaker */
         outb((inb(0x61) & ~0x02) | 0x01, 0x61);
 
@@ -988,8 +1006,6 @@ static void __init detect_art(void)
 
 /* clocksource code */
 
-static struct clocksource clocksource_tsc;
-
 static void tsc_resume(struct clocksource *cs)
 {
         tsc_verify_tsc_adjust(true);
@@ -1040,12 +1056,31 @@ static void tsc_cs_tick_stable(struct clocksource *cs)
 /*
  * .mask MUST be CLOCKSOURCE_MASK(64). See comment above read_tsc()
  */
+static struct clocksource clocksource_tsc_early = {
+        .name                   = "tsc-early",
+        .rating                 = 299,
+        .read                   = read_tsc,
+        .mask                   = CLOCKSOURCE_MASK(64),
+        .flags                  = CLOCK_SOURCE_IS_CONTINUOUS |
+                                  CLOCK_SOURCE_MUST_VERIFY,
+        .archdata               = { .vclock_mode = VCLOCK_TSC },
+        .resume                 = tsc_resume,
+        .mark_unstable          = tsc_cs_mark_unstable,
+        .tick_stable            = tsc_cs_tick_stable,
+};
+
+/*
+ * Must mark VALID_FOR_HRES early such that when we unregister tsc_early
+ * this one will immediately take over. We will only register if TSC has
+ * been found good.
+ */
 static struct clocksource clocksource_tsc = {
         .name                   = "tsc",
         .rating                 = 300,
         .read                   = read_tsc,
         .mask                   = CLOCKSOURCE_MASK(64),
         .flags                  = CLOCK_SOURCE_IS_CONTINUOUS |
+                                  CLOCK_SOURCE_VALID_FOR_HRES |
                                   CLOCK_SOURCE_MUST_VERIFY,
         .archdata               = { .vclock_mode = VCLOCK_TSC },
         .resume                 = tsc_resume,
@@ -1169,8 +1204,8 @@ static void tsc_refine_calibration_work(struct work_struct *work)
         int cpu;
 
         /* Don't bother refining TSC on unstable systems */
-        if (check_tsc_unstable())
-                goto out;
+        if (tsc_unstable)
+                return;
 
         /*
          * Since the work is started early in boot, we may be
@@ -1222,9 +1257,13 @@ static void tsc_refine_calibration_work(struct work_struct *work)
                 set_cyc2ns_scale(tsc_khz, cpu, tsc_stop);
 
 out:
+        if (tsc_unstable)
+                return;
+
         if (boot_cpu_has(X86_FEATURE_ART))
                 art_related_clocksource = &clocksource_tsc;
         clocksource_register_khz(&clocksource_tsc, tsc_khz);
+        clocksource_unregister(&clocksource_tsc_early);
 }
 
 
@@ -1233,13 +1272,11 @@ static int __init init_tsc_clocksource(void)
         if (!boot_cpu_has(X86_FEATURE_TSC) || tsc_disabled > 0 || !tsc_khz)
                 return 0;
 
+        if (check_tsc_unstable())
+                return 0;
+
         if (tsc_clocksource_reliable)
                 clocksource_tsc.flags &= ~CLOCK_SOURCE_MUST_VERIFY;
-        /* lower the rating if we already know its unstable: */
-        if (check_tsc_unstable()) {
-                clocksource_tsc.rating = 0;
-                clocksource_tsc.flags &= ~CLOCK_SOURCE_IS_CONTINUOUS;
-        }
 
         if (boot_cpu_has(X86_FEATURE_NONSTOP_TSC_S3))
                 clocksource_tsc.flags |= CLOCK_SOURCE_SUSPEND_NONSTOP;
@@ -1252,6 +1289,7 @@ static int __init init_tsc_clocksource(void)
                 if (boot_cpu_has(X86_FEATURE_ART))
                         art_related_clocksource = &clocksource_tsc;
                 clocksource_register_khz(&clocksource_tsc, tsc_khz);
+                clocksource_unregister(&clocksource_tsc_early);
                 return 0;
         }
 
@@ -1356,9 +1394,12 @@ void __init tsc_init(void)
 
         check_system_tsc_reliable();
 
-        if (unsynchronized_tsc())
+        if (unsynchronized_tsc()) {
                 mark_tsc_unstable("TSCs unsynchronized");
+                return;
+        }
 
+        clocksource_register_khz(&clocksource_tsc_early, tsc_khz);
         detect_art();
 }