Recalibrate TSC timecounter with HPET and PM timer

If frequency of an invariant (non-stop) time stamp counter is measured
using an independent working timecounter that has a known frequency, we
can assume that the measured TSC frequency is as good as the resolution
of the timecounter that we use to perform the measurement. This lets us
switch from this high quality but expensive source to the cheaper TSC
without sacrificing precision on a wide range of modern CPUs.

From Adam Steen <adam@adamsteen.com.au> with tweaks from reyk@ and myself.

Tested by brynet@, sthen@ and others, OK mlarkin, sthen

7 files changed, 256 insertions, 76 deletions

diff --git a/sys/arch/amd64/amd64/identcpu.c b/sys/arch/amd64/amd64/identcpu.c
index a448b885ba7..892eb25ac7e 100644
--- a/sys/arch/amd64/amd64/identcpu.c
+++ b/sys/arch/amd64/amd64/identcpu.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: identcpu.c,v 1.87 2017/06/20 05:34:41 mlarkin Exp $	*/
+/*	$OpenBSD: identcpu.c,v 1.88 2017/10/06 13:33:53 mikeb Exp $	*/
 /*	$NetBSD: identcpu.c,v 1.1 2003/04/26 18:39:28 fvdl Exp $	*/
 
 /*
@@ -47,8 +47,8 @@
 #include <machine/cpufunc.h>
 
 void	replacesmap(void);
-u_int64_t cpu_tsc_freq(struct cpu_info *);
-u_int64_t cpu_tsc_freq_ctr(struct cpu_info *);
+uint64_t cpu_freq(struct cpu_info *);
+void	tsc_timecounter_init(struct cpu_info *);
 #if NVMM > 0
 void	cpu_check_vmm_cap(struct cpu_info *);
 #endif /* NVMM > 0 */
@@ -57,12 +57,6 @@ void	cpu_check_vmm_cap(struct cpu_info *);
 char cpu_model[48];
 int cpuspeed;
 
-u_int tsc_get_timecount(struct timecounter *tc);
-
-struct timecounter tsc_timecounter = {
-	tsc_get_timecount, NULL, ~0u, 0, "tsc", -1000, NULL
-};
-
 int amd64_has_xcrypt;
 #ifdef CRYPTO
 int amd64_has_pclmul;
@@ -387,13 +381,12 @@ via_update_sensor(void *args)
 }
 #endif
 
-u_int64_t
-cpu_tsc_freq_ctr(struct cpu_info *ci)
+uint64_t
+cpu_freq_ctr(struct cpu_info *ci)
 {
-	u_int64_t count, last_count, msr;
+	uint64_t count, last_count, msr;
 
 	if ((ci->ci_flags & CPUF_CONST_TSC) == 0 ||
-	    (ci->ci_flags & CPUF_INVAR_TSC) ||
 	    (cpu_perf_eax & CPUIDEAX_VERID) <= 1 ||
 	    CPUIDEDX_NUM_FC(cpu_perf_edx) <= 1)
 		return (0);
@@ -425,46 +418,12 @@ cpu_tsc_freq_ctr(struct cpu_info *ci)
 	return ((count - last_count) * 10);
 }
 
-u_int64_t
-cpu_tsc_freq(struct cpu_info *ci)
+uint64_t
+cpu_freq(struct cpu_info *ci)
 {
-	u_int64_t last_count, count;
-	uint32_t eax, ebx, khz, dummy;
+	uint64_t last_count, count;
 
-	if (!strcmp(cpu_vendor, "GenuineIntel") &&
-	    cpuid_level >= 0x15) {
-		eax = ebx = khz = dummy = 0;
-		CPUID(0x15, eax, ebx, khz, dummy);
-		khz /= 1000;
-		if (khz == 0) {
-			switch (ci->ci_model) {
-			case 0x4e: /* Skylake mobile */
-			case 0x5e: /* Skylake desktop */
-			case 0x8e: /* Kabylake mobile */
-			case 0x9e: /* Kabylake desktop */
-				khz = 24000; /* 24.0 Mhz */
-				break;
-			case 0x55: /* Skylake X */
-				khz = 25000; /* 25.0 Mhz */
-				break;
-			case 0x5c: /* Atom Goldmont */
-				khz = 19200; /* 19.2 Mhz */
-				break;
-			}
-		}
-		if (ebx == 0 || eax == 0)
-			count = 0;
-		else if ((count = khz * ebx / eax) != 0) {
-			/*
-			 * Using the CPUID-derived frequency increases
-			 * the quality of the TSC time counter.
-			 */
-			tsc_timecounter.tc_quality = 2000;
-			return (count * 1000);
-		}
-	}
-
-	count = cpu_tsc_freq_ctr(ci);
+	count = cpu_freq_ctr(ci);
 	if (count != 0)
 		return (count);
 
@@ -475,15 +434,10 @@ cpu_tsc_freq(struct cpu_info *ci)
 	return ((count - last_count) * 10);
 }
 
-u_int
-tsc_get_timecount(struct timecounter *tc)
-{
-	return rdtsc();
-}
-
 void
 identifycpu(struct cpu_info *ci)
 {
+	uint64_t freq = 0;
 	u_int32_t dummy, val;
 	char mycpu_model[48];
 	int i;
@@ -568,18 +522,18 @@ identifycpu(struct cpu_info *ci)
 			ci->ci_flags |= CPUF_INVAR_TSC;
 	}
 
-	ci->ci_tsc_freq = cpu_tsc_freq(ci);
+	freq = cpu_freq(ci);
 
 	amd_cpu_cacheinfo(ci);
 
 	printf("%s: %s", ci->ci_dev->dv_xname, mycpu_model);
 
-	if (ci->ci_tsc_freq != 0)
-		printf(", %llu.%02llu MHz", (ci->ci_tsc_freq + 4999) / 1000000,
-		    ((ci->ci_tsc_freq + 4999) / 10000) % 100);
+	if (freq != 0)
+		printf(", %llu.%02llu MHz", (freq + 4999) / 1000000,
+		    ((freq + 4999) / 10000) % 100);
 
 	if (ci->ci_flags & CPUF_PRIMARY) {
-		cpuspeed = (ci->ci_tsc_freq + 4999) / 1000000;
+		cpuspeed = (freq + 4999) / 1000000;
 		cpu_cpuspeed = cpu_amd64speed;
 	}
 
@@ -723,14 +677,7 @@ identifycpu(struct cpu_info *ci)
 #endif
 	}
 
-	if ((ci->ci_flags & CPUF_PRIMARY) &&
-	    (ci->ci_flags & CPUF_CONST_TSC) &&
-	    (ci->ci_flags & CPUF_INVAR_TSC)) {
-		printf("%s: TSC frequency %llu Hz\n",
-		    ci->ci_dev->dv_xname, ci->ci_tsc_freq);
-		tsc_timecounter.tc_frequency = ci->ci_tsc_freq;
-		tc_init(&tsc_timecounter);
-	}
+	tsc_timecounter_init(ci);
 
 	cpu_topology(ci);
 #if NVMM > 0
diff --git a/sys/arch/amd64/amd64/tsc.c b/sys/arch/amd64/amd64/tsc.c
new file mode 100644
index 00000000000..ce91d5b95df
--- /dev/null
+++ b/sys/arch/amd64/amd64/tsc.c
@@ -0,0 +1,224 @@
+/*	$OpenBSD: tsc.c,v 1.1 2017/10/06 13:33:53 mikeb Exp $	*/
+/*
+ * Copyright (c) 2016,2017 Reyk Floeter <reyk@openbsd.org>
+ * Copyright (c) 2017 Adam Steen <adam@adamsteen.com.au>
+ * Copyright (c) 2017 Mike Belopuhov <mike@openbsd.org>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/sysctl.h>
+#include <sys/timetc.h>
+
+#include <machine/cpu.h>
+#include <machine/cpufunc.h>
+
+#define RECALIBRATE_MAX_RETRIES		5
+#define RECALIBRATE_SMI_THRESHOLD	50000
+#define RECALIBRATE_DELAY_THRESHOLD	20
+
+int		tsc_recalibrate;
+
+uint64_t	tsc_frequency;
+int		tsc_is_invariant;
+
+uint		tsc_get_timecount(struct timecounter *tc);
+
+struct timecounter tsc_timecounter = {
+	tsc_get_timecount, NULL, ~0u, 0, "tsc", -1000, NULL
+};
+
+uint64_t
+tsc_freq_cpuid(struct cpu_info *ci)
+{
+	uint64_t count;
+	uint32_t eax, ebx, khz, dummy;
+
+	if (!strcmp(cpu_vendor, "GenuineIntel") &&
+	    cpuid_level >= 0x15) {
+		eax = ebx = khz = dummy = 0;
+		CPUID(0x15, eax, ebx, khz, dummy);
+		khz /= 1000;
+		if (khz == 0) {
+			switch (ci->ci_model) {
+			case 0x4e: /* Skylake mobile */
+			case 0x5e: /* Skylake desktop */
+			case 0x8e: /* Kabylake mobile */
+			case 0x9e: /* Kabylake desktop */
+				khz = 24000; /* 24.0 Mhz */
+				break;
+			case 0x55: /* Skylake X */
+			case 0x5f: /* Atom Denverton */
+				khz = 25000; /* 25.0 Mhz */
+				break;
+			case 0x5c: /* Atom Goldmont */
+				khz = 19200; /* 19.2 Mhz */
+				break;
+			}
+		}
+		if (ebx == 0 || eax == 0)
+			count = 0;
+		else if ((count = (uint64_t)khz * (uint64_t)ebx / eax) != 0)
+			return (count * 1000);
+	}
+
+	return (0);
+}
+
+static inline int
+get_tsc_and_timecount(struct timecounter *tc, uint64_t *tsc, uint64_t *count)
+{
+	uint64_t n, tsc1, tsc2;
+	int i;
+
+	for (i = 0; i < RECALIBRATE_MAX_RETRIES; i++) {
+		tsc1 = rdtsc();
+		n = (tc->tc_get_timecount(tc) & tc->tc_counter_mask);
+		tsc2 = rdtsc();
+
+		if ((tsc2 - tsc1) < RECALIBRATE_SMI_THRESHOLD) {
+			*count = n;
+			*tsc = tsc2;
+			return (0);
+		}
+	}
+	return (1);
+}
+
+static inline uint64_t
+calculate_tsc_freq(uint64_t tsc1, uint64_t tsc2, int usec)
+{
+	uint64_t delta;
+
+	delta = (tsc2 - tsc1);
+	return (delta * 1000000 / usec);
+}
+
+static inline uint64_t
+calculate_tc_delay(struct timecounter *tc, uint64_t count1, uint64_t count2)
+{
+	uint64_t delta;
+
+	if (count2 < count1)
+		count2 += tc->tc_counter_mask;
+
+	delta = (count2 - count1);
+	return (delta * 1000000 / tc->tc_frequency);
+}
+
+uint64_t
+measure_tsc_freq(struct timecounter *tc)
+{
+	uint64_t count1, count2, frequency, min_freq, tsc1, tsc2;
+	u_long ef;
+	int delay_usec, i, err1, err2, usec;
+
+	/* warmup the timers */
+	for (i = 0; i < 3; i++) {
+		(void)tc->tc_get_timecount(tc);
+		(void)rdtsc();
+	}
+
+	min_freq = ULLONG_MAX;
+
+	delay_usec = 100000;
+	for (i = 0; i < 3; i++) {
+		ef = read_rflags();
+		disable_intr();
+
+		err1 = get_tsc_and_timecount(tc, &tsc1, &count1);
+		delay(delay_usec);
+		err2 = get_tsc_and_timecount(tc, &tsc2, &count2);
+
+		write_rflags(ef);
+
+		if (err1 || err2)
+			continue;
+
+		usec = calculate_tc_delay(tc, count1, count2);
+
+		if ((usec < (delay_usec - RECALIBRATE_DELAY_THRESHOLD)) ||
+		    (usec > (delay_usec + RECALIBRATE_DELAY_THRESHOLD)))
+			continue;
+
+		frequency = calculate_tsc_freq(tsc1, tsc2, usec);
+
+		min_freq = MIN(min_freq, frequency);
+	}
+
+	return (min_freq);
+}
+
+void
+calibrate_tsc_freq(void)
+{
+	struct timecounter *reference = tsc_timecounter.tc_priv;
+	uint64_t freq;
+
+	if (!reference || !tsc_recalibrate)
+		return;
+
+	if ((freq = measure_tsc_freq(reference)) == 0)
+		return;
+	tsc_frequency = freq;
+	tsc_timecounter.tc_frequency = freq;
+	if (tsc_is_invariant)
+		tsc_timecounter.tc_quality = 2000;
+
+	printf("%s: recalibrated TSC frequency %lld Hz\n",
+	    reference->tc_name, tsc_timecounter.tc_frequency);
+}
+
+void
+cpu_recalibrate_tsc(struct timecounter *tc)
+{
+	struct timecounter *reference = tsc_timecounter.tc_priv;
+
+	/* Prevent recalibration with a worse timecounter source */
+	if (reference && reference->tc_quality > tc->tc_quality)
+		return;
+
+	tsc_timecounter.tc_priv = tc;
+	calibrate_tsc_freq();
+}
+
+uint
+tsc_get_timecount(struct timecounter *tc)
+{
+	return rdtsc();
+}
+
+void
+tsc_timecounter_init(struct cpu_info *ci)
+{
+	if (!(ci->ci_flags & CPUF_PRIMARY) ||
+	    !(ci->ci_flags & CPUF_CONST_TSC) ||
+	    !(ci->ci_flags & CPUF_INVAR_TSC))
+		return;
+
+	tsc_frequency = tsc_freq_cpuid(ci);
+	tsc_is_invariant = 1;
+
+	/* Newer CPUs don't require recalibration */
+	if (tsc_frequency > 0) {
+		tsc_timecounter.tc_frequency = tsc_frequency;
+		tsc_timecounter.tc_quality = 2000;
+	} else {
+		tsc_recalibrate = 1;
+		calibrate_tsc_freq();
+	}
+
+	tc_init(&tsc_timecounter);
+}
diff --git a/sys/arch/amd64/conf/files.amd64 b/sys/arch/amd64/conf/files.amd64
index bfbd6153d7a..546ca9c9f6b 100644
--- a/sys/arch/amd64/conf/files.amd64
+++ b/sys/arch/amd64/conf/files.amd64
@@ -1,4 +1,4 @@
-#	$OpenBSD: files.amd64,v 1.89 2017/05/31 19:18:19 deraadt Exp $
+#	$OpenBSD: files.amd64,v 1.90 2017/10/06 13:33:53 mikeb Exp $
 
 maxpartitions 16
 maxusers 2 16 128
@@ -10,6 +10,7 @@ file	arch/amd64/amd64/gdt.c			multiprocessor
 file	arch/amd64/amd64/machdep.c
 file	arch/amd64/amd64/hibernate_machdep.c	hibernate
 file	arch/amd64/amd64/identcpu.c
+file	arch/amd64/amd64/tsc.c
 file	arch/amd64/amd64/via.c
 file	arch/amd64/amd64/locore.S
 file	arch/amd64/amd64/aes_intel.S		crypto
diff --git a/sys/arch/amd64/include/cpu.h b/sys/arch/amd64/include/cpu.h
index 37e2b490eec..9f7926d69b5 100644
--- a/sys/arch/amd64/include/cpu.h
+++ b/sys/arch/amd64/include/cpu.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.h,v 1.114 2017/08/11 20:19:14 tedu Exp $	*/
+/*	$OpenBSD: cpu.h,v 1.115 2017/10/06 13:33:53 mikeb Exp $	*/
 /*	$NetBSD: cpu.h,v 1.1 2003/04/26 18:39:39 fvdl Exp $	*/
 
 /*-
@@ -138,7 +138,6 @@ struct cpu_info {
 	u_int32_t	ci_family;
 	u_int32_t	ci_model;
 	u_int32_t	ci_cflushsz;
-	u_int64_t	ci_tsc_freq;
 
 	int		ci_inatomic;
 
diff --git a/sys/arch/amd64/include/cpuvar.h b/sys/arch/amd64/include/cpuvar.h
index 24fc8fe880d..bd3b07bce0d 100644
--- a/sys/arch/amd64/include/cpuvar.h
+++ b/sys/arch/amd64/include/cpuvar.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpuvar.h,v 1.8 2016/07/28 21:57:57 kettenis Exp $	*/
+/*	$OpenBSD: cpuvar.h,v 1.9 2017/10/06 13:33:53 mikeb Exp $	*/
 /* 	$NetBSD: cpuvar.h,v 1.1 2003/03/01 18:29:28 fvdl Exp $ */
 
 /*-
@@ -96,5 +96,6 @@ void x86_ipi_init(int);
 void identifycpu(struct cpu_info *);
 void cpu_init(struct cpu_info *);
 void cpu_init_first(void);
+void cpu_adjust_tsc_freq(uint64_t (*)());
 
 #endif
diff --git a/sys/dev/acpi/acpihpet.c b/sys/dev/acpi/acpihpet.c
index 17f5e59facf..f3c0819dfd0 100644
--- a/sys/dev/acpi/acpihpet.c
+++ b/sys/dev/acpi/acpihpet.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: acpihpet.c,v 1.21 2015/10/06 20:49:32 matthew Exp $ */
+/* $OpenBSD: acpihpet.c,v 1.22 2017/10/06 13:33:53 mikeb Exp $ */
 /*
  * Copyright (c) 2005 Thorsten Lockert <tholo@sigmasoft.com>
  *
@@ -264,6 +264,10 @@ acpihpet_attach(struct device *parent, struct device *self, void *aux)
 	hpet_timecounter.tc_priv = sc;
 	hpet_timecounter.tc_name = sc->sc_dev.dv_xname;
 	tc_init(&hpet_timecounter);
+#if defined(__amd64__)
+	extern void cpu_recalibrate_tsc(struct timecounter *);
+	cpu_recalibrate_tsc(&hpet_timecounter);
+#endif
 	acpihpet_attached++;
 }
 
diff --git a/sys/dev/acpi/acpitimer.c b/sys/dev/acpi/acpitimer.c
index fb78acf2564..cdc8c99a17a 100644
--- a/sys/dev/acpi/acpitimer.c
+++ b/sys/dev/acpi/acpitimer.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: acpitimer.c,v 1.11 2015/03/14 03:38:46 jsg Exp $ */
+/* $OpenBSD: acpitimer.c,v 1.12 2017/10/06 13:33:53 mikeb Exp $ */
 /*
  * Copyright (c) 2005 Thorsten Lockert <tholo@sigmasoft.com>
  *
@@ -96,6 +96,10 @@ acpitimerattach(struct device *parent, struct device *self, void *aux)
 	acpi_timecounter.tc_priv = sc;
 	acpi_timecounter.tc_name = sc->sc_dev.dv_xname;
 	tc_init(&acpi_timecounter);
+#if defined(__amd64__)
+	extern void cpu_recalibrate_tsc(struct timecounter *);
+	cpu_recalibrate_tsc(&acpi_timecounter);
+#endif
 }